File content as of revision 32:9a4046224b11:

//
// commander.cpp
// Created by Chandan Siyag on 14/11/2015.
#include <iostream>
#include <string>
#include "commander.h"

using namespace std;

const int kCommandValueBufferSize = 80;
const int kObjectBufferSize = 20;

/// All objects that can recieve commands
string CommandObjectValue [6] = { "mbed", "pc", "colour_sensor", "servos", "port", "break_beam" };
/// All object(fist column) and their possible commands(respective row).
string CommandObjectCommandsValue [6][kMaxCommandCount] = {
    {"mbed", "haz-block", "read-current-block", "mode", "sort", "reading-count", "", "", "", ""},
    {"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
    {"colour_sensor", "i-time", "preview", "value", "block-value", "test", "", "", "", ""},
    {"servos", "test", "reset", "toggle", "", "", "", "", "", ""},
    {"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
    {"break_beam", "test", "", "", "", "", "", "", "", ""},
};

/// Creates new commander object
Commander::Commander()
{
    replyCommands = false;
    this->resetVariables();
}
/// Desctructor
Commander::~Commander()
{

}

/// Recieves and decodes the command
void Commander::decodeCommand(CommandTypeRaw type)
{
    this->resetVariables();
    this->typeRaw = type;
    this->typeChar = CommandTypeValue[type];

    this->readCommandObject();
    if (this->objectRaw == InvalidObject) {
        pc.printf("DEBUG:Invalid command object.\n");
        return;
    }

    if (this->readCommand(this->objectRaw) == false) {
        pc.printf("DEBUG:Invalid command.\n");
        return;
    } else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
        this->executeCommand();
        if (this->replyCommands == true) {
            pc.printf("VALUE:%s:VALUE\n", this->description().c_str());
        }
        //TODO: Send request with response descriptor.
        //        pc.("DEBUG:Finished executine command");
        return;
    }

    pc.printf("INFO:Not connected to PC. %i\n", this->commandIndex[0]);
    return;
}

/// Get the information about the parsed command in a readable manner.
//TODO: Fix format
std::string Commander::description()
{
    string str;
    str.append("Type:\t\t");
    str.append(&this->typeChar);
    str.append("\nObject:\t\t" + this->object);

    for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
        if (this->command[i] == "") {
            break;
        }
        str.append("\nCommand:\t" + this->command[i] + "\nValue:\t\t" + this->commandValue[i].c_str() + " \n");
    }
    return str;
}

/// Reads and assignes the imminent commanding object.
void Commander::readCommandObject()
{
    char objectInitiator = '<';
    char objectTerminator = '>';

    char nextChar = '\0';

    do {
        nextChar = pc.getc();
    } while (nextChar != objectInitiator);

    char objectCharArray [kObjectBufferSize] = "";
    int i = 0;
    while (i < kObjectBufferSize) {
        nextChar = pc.getc();
        if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
            continue;
        }
        if (nextChar == objectTerminator)
            break;
        objectCharArray[i] = nextChar;
        i++;
    }
    string tempStr(objectCharArray);
    this->object = tempStr;

    for (int i = 0; i < (sizeof(CommandObjectValue)/sizeof(*CommandObjectValue)); i++) {
        if (CommandObjectValue[i] == this->object) {
            this->objectRaw = static_cast<CommandObjectRaw>(i);
            return;
        }
    }

    this->objectRaw = InvalidObject;
    return;
}

/// Reads the command for the previouly read object.
/// Return true if is a possible command, false otherwise.
bool Commander::readCommand(CommandObjectRaw objectRaw)
{
    char nextChar = '\0';
    char commandCharArray [kMaxCommandCount - 1][kCommandValueBufferSize] = { '\0' };
    char commandValueArray [kMaxCommandCount -1][kCommandValueBufferSize] = { '\0' };

    int charIndex = 0;
    int valueCharIndex = 0;
    int commandValueIndex = 0;
    bool commandComplete = false;
    while (charIndex < kCommandValueBufferSize - 1 && valueCharIndex < kCommandValueBufferSize - 1) {
        nextChar = pc.getc();

        if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
            continue;
        } else if (nextChar == kCommandTerminator) {
            break;
        } else if (nextChar == '=') {
            commandComplete = true;
        } else if (nextChar == ',') {
            commandComplete = false;
            commandValueIndex++;
            charIndex = 0;
            valueCharIndex = 0;
        }

        if (commandComplete == false && nextChar != ',') {
            commandCharArray[commandValueIndex][charIndex] = nextChar;
            charIndex++;
        } else if (commandComplete == true && nextChar != '=') {
            commandValueArray[commandValueIndex][valueCharIndex] = nextChar;
            valueCharIndex++;
        }
    }

    for (int i = 0; i < kMaxCommandCount - 1; i++) {
        //        pc.printf("i: %i\n", i);
        if (commandCharArray[i][0] == '\0') {
            break;
        }
        string tempCommandStr(commandCharArray[i]);
        string tempValueStr(commandValueArray[i]);
        int row = this->objectRaw;
        int column = 1;
        for (; column < kMaxCommandCount - 1; column++) {
            if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
                this->command[i] = tempCommandStr;
                this->commandIndex[i] = column;
                this->commandValue[i] = tempValueStr;
                break;
            }
        }
        if (this->commandIndex[i] == -1) {
            return false;
        }
    }

    return true;
}

/// Executes the previously read command with the object.
void Commander::executeCommand()
{
    switch (this->objectRaw) {
        case MBED:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    if (this->typeRaw == Set) {
                        if (this->commandValue[i] == "start") {
                            hazBlock(this->typeRaw);
                        } else if (this->commandValue[i] == "pause") {
                            pc.printf("DEBUG: Exited set new haz block mode.\n");
                            setNewHazBlock = false;
                        }
                    } else if (this->typeRaw == Query) {
                    hazBlock(this->typeRaw);
                    }
                } else if (this->commandIndex[i] == 2) {
                    getCurrentBlock(this->typeRaw);
                } else if (this->commandIndex[i] == 3) {
                    if (this->commandValue[i] == "maintanence") {
                        currentMode = Maintanence;
                        pc.printf("INFO:Running in maintanence mode.\n");
                    } else if (this->commandValue[i] == "normal") {
                        currentMode = Normal;
                        pc.printf("INFO:Running in nomal mode.\n");
                    } else if (this->commandValue[i] == "none") {
                        currentMode = None;
                        pc.printf("INFo:Running in none mode.\n");
                    }
                } else if (this->commandIndex[i] == 4 && currentMode == Normal) {
                    if (this->commandValue[i] == "start") {
                        currentState = Start;
                        pc.printf("INFO:Starting sorting.\n");
                    } else if (this->commandValue[i] == "pause") {
                        currentState = Pause;
                        pc.printf("INFO:Pausing sorting.\n");
                    }
                } else if (this->commandIndex[i] == 5) {
                    int readingCount = 0;
                    sscanf(this->commandValue[i].c_str(), "%i", readingCount);
                    hazReadingCount = readingCount;
                }
            }
            break;
        case PC:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    connectToPC(this->typeRaw);
                } else if (this->commandIndex[i] == 2) {
                    disconnectToPC(this->typeRaw);
                } else if (this->commandIndex[i] == 8) {
                    if (this->commandValue[i] == "ON") {
                        this->replyCommands = true;
                    } else if (this->commandValue[i] == "OFF") {
                        this->replyCommands = false;
                    }
                }
            }
            break;
        case ColourSensor:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    float integrationTime;
                    sscanf(this->commandValue[i].c_str(), "%f", &integrationTime);
                    if (integrationTime < 2.4 || integrationTime > 600) {
                        pc.printf("ERROR:Integration Time invalid: %.3f\n", integrationTime);
                        continue;
                    }
                    setIntegrationTimeTo(integrationTime);
                } else if (this->commandIndex[i] == 2) {
                    if (this->commandValue[i] == "ON") {
                        previewOnPC(true);
                    } else if (this->commandValue[i] == "OFF") {
                        previewOnPC(false);
                    }
                } else if (this->commandIndex[i] == 3 && runColourSensorTest == true && getColourSensorValue == false) {
                    getColourSensorValue = true;
                } else if (this->commandIndex[i] == 4 && runColourSensorTest == true && getBlockColourValue == false) {
                    getBlockColourValue = true;
                } else if (this->commandIndex[i] == 5) {
                    if (this->commandValue[i] == "start") {
                        testColourSensor(Start);
                    } else if (this->commandValue[i] == "pause") {
                        testColourSensor(Pause);
                    }
                }
            }
            break;
        case Servos:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    if (this->commandValue[i] == "start") {
                        testServos(Start);
                    } else if (this->commandValue[i] == "pause") {
                        testServos(Pause);
                    }
                } else if (this->commandIndex[i] == 2) {
                    resetServos();
                } else if (this->commandIndex[i] == 3) {
                    if (this->commandValue[i] == "1") {
                        pc.printf("INFO: Moving top servo.\n");
                        gToggleServoNumber = 1;
                    } else if (this->commandValue[i] == "2") {
                        pc.printf("INFO: Moving bottom servo.\n");
                        gToggleServoNumber = 2;
                    } else if (this->commandValue[i] == "3") {
                        pc.printf("INFO: Moving both servos.\n");
                        gToggleServoNumber = 3;
                    }
                }
            }
            break;
        case Port:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    getPortInfo();
                } else if (this->commandIndex[i] == 2) {
                    int baudRate;
                    sscanf(this->commandValue[i].c_str(), "%i", &baudRate);
                    setPortBaudRate(baudRate);
                } else if (this->commandIndex[i] == 3) {
                    int parity = 0;
                    sscanf(this->commandValue[i].c_str(), "%i", &parity);
                    setPortParity(parity);
                }
            }
            break;
        case BreakBeam:
            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
                    break;
                }
                if (this->commandIndex[i] == 1) {
                    if (this->commandValue[i] == "start") {
                        testBreakBeams(Start);
                    } else if (this->commandValue[i] == "pause") {
                        testBreakBeams(Pause);
                    }
                }
            }
        default:
            break;
    }
    return;
}

// Cleans all the properties, ready to recieved next command.
void Commander::resetVariables()
{
    this->object = "";
    this->objectRaw = InvalidObject;
    for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i ++) {
        this->command[i].clear();
        this->commandValue[i].clear();
        this->commandIndex[i] = -1;
    }
    this->typeRaw = InvalidType;
    this->typeChar = '\0';
}