File content as of revision 28:7e4d29977d72:

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

using namespace std;
#define _DEBUG_

//const int kCommandBufferSize = 40;
const int kCommandValueBufferSize = 80;
const int kObjectBufferSize = 20;

string CommandObjectValue [6] = { "mbed", "pc", "colour_sensor", "servos", "port", "break_beam" };
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", "", "", "", "", "", "", "", ""},
};

Commander::Commander()
{
	replyCommands = false;
	this->resetVariables();
}

Commander::~Commander()
{

}

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) {
		//        this->description();
		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;
}

//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;
}

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;
}

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]);
		//		pc.printf("%s\n", tempCommandStr.c_str());
		int row = this->objectRaw;
		//        pc.printf("Row: %i\n", this->objectRaw);
		int column = 1;
		//        pc.printf("Column: %i\n", column);
		for (; column < kMaxCommandCount - 1; column++) {
			//            pc.printf("%i\n", column);
			if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
				//                pc.printf("Found matching command.\n");
				this->command[i] = tempCommandStr;
				//                pc.printf("%s\n", this->command[i].c_str());
				this->commandIndex[i] = column;
				//                pc.printf("%i\n", this->commandIndex[i]);
				this->commandValue[i] = tempValueStr;
				//                pc.printf("%s\n", this->commandValue[i].c_str());
				//                pc.printf("%s\n", this->description().c_str());
				break;
			}
		}
		if (this->commandIndex[i] == -1) {
			//            pc.printf("index = -1\n");
			return false;
		}
	}

	//	pc.printf("Returning\n");
	return true;
}

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) {
				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;
}


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';
}