#include "EggDuino.h"

void queryPen()
{
   Log(__FUNCTION__);
	char state;
	if (g_iPenState == g_iPenUpPos)
		state = '1';
	else
		state = '0';
	Serial.print(String(state) + "\r\n");
	sendAck();
}

void queryButton()
{
	Serial.print(String(g_bPrgButtonState) + "\r\n");
	sendAck();
	g_bPrgButtonState = 0;
}

void queryLayer()
{
   Log(__FUNCTION__);
	Serial.print(String(g_uiLayer) + "\r\n");
	sendAck();
}

void setLayer()
{
   Log(__FUNCTION__);
	uint32_t value = 0;
	char *arg1;
	arg1 = SCmd.next();
	if (arg1 != NULL)
	{
		value = atoi(arg1);
		g_uiLayer = value;
		sendAck();
	}
	else
		sendError();
}

void queryNodeCount()
{
	Serial.print(String(g_uiNodeCount) + "\r\n");
	sendAck();
}

void setNodeCount()
{
   Log(__FUNCTION__);
	uint32_t value = 0;
	char *arg1;
	arg1 = SCmd.next();
	if (arg1 != NULL)
	{
		value = atoi(arg1);
		g_uiNodeCount = value;
		sendAck();
	}
	else
		sendError();
}

void nodeCountIncrement()
{
	g_uiNodeCount = g_uiNodeCount++;
	sendAck();
}

void nodeCountDecrement()
{
	g_uiNodeCount = g_uiNodeCount--;
	sendAck();
}

void stepperMove()
{
   Log(__FUNCTION__);
	uint16_t duration = 0; // in ms
	int penStepsEBB = 0;   // Pen
	int rotStepsEBB = 0;   // Rot

	moveToDestination();

	if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB))
	{
		sendError();
		return;
	}

	sendAck();

	if ((penStepsEBB == 0) && (rotStepsEBB == 0))
	{
		delay(duration);
		return;
	}

	prepareMove(duration, penStepsEBB, rotStepsEBB);
}

void setPen()
{
   Log(__FUNCTION__);
	int cmd;
	int value;
	char *arg;
	char cstrMsg[20];

	moveToDestination();

	arg = SCmd.next();
	if (arg != NULL)
	{
		cmd = atoi(arg);
		switch (cmd)
		{
		case 0:
			penServo.write(g_iPenUpPos);
			break;

		case 1:
			penServo.write(g_iPenDownPos);
			break;

		default:
			sendError();
		}
	}
	char *val;
	val = SCmd.next();
	if (val != NULL)
	{
		value = atoi(val);
		sendAck();
		delay(value);
	}
	if (val == NULL && arg != NULL)
	{
		sendAck();
		delay(500);
	}
	//	Serial.println("delay");
	if (val == NULL && arg == NULL)
		sendError();
}

void togglePen()
{
   Log(__FUNCTION__);
	int value;
	char *arg;

	moveToDestination();

	arg = SCmd.next();
	if (arg != NULL)
		value = atoi(arg);
	else
		value = 500;

	doTogglePen();
	sendAck();
	delay(value);
}

void doTogglePen()
{
   Log(__FUNCTION__);
	if (g_iPenState == g_iPenUpPos)
	{
		penServo.write(g_iPenDownPos);
		g_iPenState = g_iPenDownPos;
	}
	else
	{
		penServo.write(g_iPenUpPos);
		g_iPenState = g_iPenUpPos;
	}
}

void enableMotors()
{
   Log(__FUNCTION__);
	int cmd;
	int value;
	char *arg;
	char *val;
	arg = SCmd.next();
	if (arg != NULL)
		cmd = atoi(arg);
	val = SCmd.next();
	if (val != NULL)
		value = atoi(val);
	// values parsed
	if ((arg != NULL) && (val == NULL))
	{
		switch (cmd)
		{
		case 0:
			motorsOff();
			sendAck();
			break;
		case 1:
			motorsOn();
			sendAck();
			break;
		default:
			sendError();
		}
	}
	// the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument.
	if ((arg != NULL) && (val != NULL))
	{
		switch (value)
		{
		case 0:
			motorsOff();
			sendAck();
			break;
		case 1:
			motorsOn();
			sendAck();
			break;
		default:
			sendError();
		}
	}
}

void stepperModeConfigure()
{
   Log(__FUNCTION__);
	int cmd;
	int value;
	char *arg;
	arg = SCmd.next();
	if (arg != NULL)
		cmd = atoi(arg);
	char *val;
	val = SCmd.next();
	if (val != NULL)
		value = atoi(val);
	if ((arg != NULL) && (val != NULL))
	{
		switch (cmd)
		{
		case 4:
			g_iPenDownPos = (int)((float)(value - 6000) / (float)133.3); // transformation from EBB to PWM-Servo
			storePenDownPosInEE();
			sendAck();
			break;
		case 5:
			g_iPenUpPos = (int)((float)(value - 6000) / (float)133.3); // transformation from EBB to PWM-Servo
			storePenUpPosInEE();
			sendAck();
			break;
		case 6: // rotMin=value;    ignored
			sendAck();
			break;
		case 7: // rotMax=value;    ignored
			sendAck();
			break;
		case 11:
			g_iServoRateUp = value;
			sendAck();
			break;
		case 12:
			g_iServoRateDown = value;
			sendAck();
			break;
		default:
			sendError();
		}
	}
}

void sendVersion()
{
   Log(__FUNCTION__);
	Serial.print(initSting);
	Serial.print("\r\n");
}

void unrecognized(const char *command)
{
   Log(__FUNCTION__);
	sendError();
}

void ignore()
{
   Log(__FUNCTION__);
	sendAck();
}

void makeComInterface()
{
	SCmd.addCommand("v", sendVersion);
	SCmd.addCommand("EM", enableMotors);
	SCmd.addCommand("SC", stepperModeConfigure);
	SCmd.addCommand("SP", setPen);
	SCmd.addCommand("SM", stepperMove);
	SCmd.addCommand("SE", ignore);
	SCmd.addCommand("TP", togglePen);
	SCmd.addCommand("PO", ignore); // Engraver command, not implemented, gives fake answer
	SCmd.addCommand("NI", nodeCountIncrement);
	SCmd.addCommand("ND", nodeCountDecrement);
	SCmd.addCommand("SN", setNodeCount);
	SCmd.addCommand("QN", queryNodeCount);
	SCmd.addCommand("SL", setLayer);
	SCmd.addCommand("QL", queryLayer);
	SCmd.addCommand("QP", queryPen);
	SCmd.addCommand("QB", queryButton);	  //"PRG" Button,
	SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?")
}