#include "EggDuino.h"

namespace
{
bool g_bPrintSessionActive = false;

void captureSerialProtocolLine(const char *line)
{
	captureIncomingPrintLine(line, PROTOCOL_TRANSPORT_SERIAL);
}

#ifdef ESP32
void captureBleProtocolLine(const char *line)
{
	captureIncomingPrintLine(line, PROTOCOL_TRANSPORT_BLE);
}

void captureWifiProtocolLine(const char *line)
{
	captureIncomingPrintLine(line, PROTOCOL_TRANSPORT_WIFI);
}
#endif

void startPrintSessionIfNeeded()
{
	if (!g_bPrintSessionActive)
	{
		clearIncomingPrintLog();
		g_bPrintSessionActive = true;
	}
}

void stopPrintSession()
{
	g_bPrintSessionActive = false;
}
}

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

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

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

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

void queryNodeCount()
{
	protocolWrite(String(g_uiNodeCount) + "\r\n");
	sendAck();
}

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

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

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

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

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

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

	startPrintSessionIfNeeded();
	
	prepareMove(duration, penStepsEBB, rotStepsEBB);
	moveToDestination();
	sendAck();
}

void setPen()
{
   Log(__FUNCTION__);
	int cmd;
	char *arg;

	// moveToDestination();

	arg = nextCommandArg();
	if (arg != NULL)
	{
		cmd = atoi(arg);
		switch (cmd)
		{
		case 0:
			movePenServoTo(g_iPenUpPos);
			break;

		case 1:
			movePenServoTo(g_iPenDownPos);
			break;

		default:
			sendError();
		}
	}
	char *val;
	val = nextCommandArg();
	if (val != NULL)
	{
		(void)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__);
	(void)nextCommandArg();

	doTogglePen();
	sendAck();

}

void doTogglePen()
{
   Log(__FUNCTION__);
	if (g_iPenState == g_iPenUpPos)
	{
		movePenServoTo(g_iPenDownPos);
	}
	else
	{
		movePenServoTo(g_iPenUpPos);
	}
}

void enableMotors()
{
   Log(__FUNCTION__);
	int cmd;
	int value;
	char *arg;
	char *val;
	arg = nextCommandArg();
	if (arg != NULL)
		cmd = atoi(arg);
	val = nextCommandArg();
	if (val != NULL)
		value = atoi(val);
	// values parsed
	if ((arg != NULL) && (val == NULL))
	{
		switch (cmd)
		{
		case 0:
			stopPrintSession();
			motorsOff();
			sendAck();
			break;
		case 1:
			startPrintSessionIfNeeded();
			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:
			stopPrintSession();
			motorsOff();
			sendAck();
			break;
		case 1:
			startPrintSessionIfNeeded();
			motorsOn();
			sendAck();
			break;
		default:
			sendError();
		}
	}
}

void stepperModeConfigure()
{
   Log(__FUNCTION__);
	int cmd;
	int value;
   int iTmpPenPos;
	char *arg;
	arg = nextCommandArg();
	if (arg != NULL)
		cmd = atoi(arg);
	char *val;
	val = nextCommandArg();
	if (val != NULL)
		value = atoi(val);
	if ((arg != NULL) && (val != NULL))
	{
		switch (cmd)
		{
		case 4:
			iTmpPenPos = (int)((float)(value - 6000) / (float)133.3); // transformation from EBB to PWM-Servo
         if (g_iPenDownPos != iTmpPenPos)
         {
            g_iPenDownPos = iTmpPenPos;
            storePenDownPosInEE();
         }
			sendAck();
			break;
		case 5:
			iTmpPenPos = (int)((float)(value - 6000) / (float)133.3); // transformation from EBB to PWM-Servo
         if (g_iPenUpPos != iTmpPenPos)
         {
            g_iPenUpPos = iTmpPenPos;
            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__);
	protocolWrite(initSting);
	protocolWrite("\r\n");
}

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

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

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

	registerCommands(SCmd);
	SCmd.setLineHandler(captureSerialProtocolLine);
#ifdef ESP32
	registerCommands(g_BLECmd);
	g_BLECmd.setLineHandler(captureBleProtocolLine);
	registerCommands(g_WifiCmd);
	g_WifiCmd.setLineHandler(captureWifiProtocolLine);
#endif
}
