#include <Arduino.h>
#include "proto.h"
#include "QList.h"
#include "mode.h"
#include "amptimer.h"


void init_proto()
{
	Serial.begin(115200);

	Serial.println("Piezo Control started.");
	Serial.println("Commands:");
	Serial.println("  amp: [0 - 1]");
	Serial.println("  freq: [0.1 - 200]");
	Serial.println("  mode: [const][sine][tri]");
	Serial.println("  sweep:[start;end;rate][no]");
	Serial.println("------------------------");
}


#define COMMAND_BUFFER_SIZE			256
#define CR                            0x0D
#define LF                            0x0A



void executeCommand(const char* cmd_str);


char commandString[COMMAND_BUFFER_SIZE];
size_t cmdStrIndex = 0;


#define CMD_ARG_OFFSET		7

unsigned long lastAutoStatus = 0;


QList<String> arrCmd;


void serialEvent()
{
	while (Serial.available() > 0)
	{
		if (cmdStrIndex >= COMMAND_BUFFER_SIZE)
		{
			// Drop data.
			memset(commandString, 0, sizeof(commandString));
			cmdStrIndex = 0;
			//	return;
		}


		char inChar = (char)Serial.read();

		commandString[cmdStrIndex] = inChar;

//			Serial1.print(inChar);

		if (inChar == CR || inChar == LF)
			commandString[cmdStrIndex] = char(0);

		cmdStrIndex++;

		if (inChar == LF)
		{
		//	executeCommand(commandString);

			arrCmd.push_back(String(commandString));

			cmdStrIndex = 0;

//			break;
		}
	}



	if (arrCmd.size() > 0)
	{
		executeCommand(arrCmd.at(0).c_str());

		arrCmd.pop_front();
	}


}


const char* _szCmdFreq = "freq:";
const char* _szCmdAmp = "amp:";
const char* _szCmdMode = "mode:";
const char* _szCmdSweep = "sweep:";


void executeCommand(const char* cmd_str)
{

//	Serial.print("cmd: ");
//	Serial.println(cmd_str);

	if (strstr(cmd_str, _szCmdFreq) != NULL)
	{
		const char* str_arg = cmd_str + strlen(_szCmdFreq);

		if (strstr(str_arg, "?") == NULL)
		{
			double freq = atof(str_arg);

			setFreq(freq);
		}

		Serial.print(_szCmdFreq );
		Serial.println(getFreq(), 3);
	}
	else if (strstr(cmd_str, _szCmdAmp) != NULL)
	{
		const char* str_arg = cmd_str + strlen(_szCmdAmp);

		if (strstr(str_arg, "?") == NULL)
		{
			double ampnew = atof(str_arg);

			setAmp(ampnew);
		}

		Serial.print(_szCmdAmp);
		Serial.println(getAmp(), 2);
	}
	else if (strstr(cmd_str, _szCmdMode) != NULL)
	{
		const char* str_arg = cmd_str + strlen(_szCmdMode);

		setMode(str_arg);

		Serial.print(_szCmdMode);
		Serial.println(getCurrentModeName());
	}
	else if (strstr(cmd_str, _szCmdSweep) != NULL)
	{
		const char* str_arg = cmd_str + strlen(_szCmdSweep);

		setSweep(str_arg);

		//Serial.println(str_mode);

		Serial.print(_szCmdSweep);
		Serial.println(getSweepParams());
	}


}

void processCommand()
{
	serialEvent();


}