#include "EggDuino.h"

void initHardware()
{
	if (!initConfigStore())
	{
		g_iPenUpPos = 5;
		g_iPenDownPos = 20;
	}
	g_iPenState = g_iPenUpPos;

	g_stepEngine.init();
	g_pStepperRotate = g_stepEngine.stepperConnectToPin(step1);
	if (g_pStepperRotate)
	{
		// rotMotor.setMaxSpeed(2000.0);
		// rotMotor.setAcceleration(10000.0);

		g_pStepperRotate->setDirectionPin(dir1);
		g_pStepperRotate->setEnablePin(enableRotMotor);
		g_pStepperRotate->setAcceleration(10000);
		g_pStepperRotate->setAutoEnable(false);
	}

	// Stepper pen init
	g_pStepperPen = g_stepEngine.stepperConnectToPin(step2);
	if (g_pStepperPen)
	{
		// penMotor.setMaxSpeed(2000.0);
		// penMotor.setAcceleration(10000.0);
		g_pStepperPen->setDirectionPin(dir2);
		g_pStepperPen->setEnablePin(enablePenMotor);
		g_pStepperPen->setAcceleration(10000);
		g_pStepperPen->setAutoEnable(false);
	}

	motorsOff();
	penServo.attach(servoPin);
	penServo.write(g_iPenState);
}

void storePenUpPosInEE()
{
	saveConfigToFile();
}

void storePenDownPosInEE()
{
	saveConfigToFile();
}

void sendAck()
{
   Log(__FUNCTION__);
	Serial.print("OK\r\n");
}

void sendError()
{
   Log(__FUNCTION__);
	Serial.print("unknown CMD\r\n");
}

void motorsOff()
{
   Log(__FUNCTION__);
	g_pStepperPen->disableOutputs();
	g_pStepperRotate->disableOutputs();
	g_bMotorsEnabled = 0;
}

void motorsOn()
{
   Log(__FUNCTION__);
	g_pStepperPen->enableOutputs();
	g_pStepperRotate->enableOutputs();
	g_bMotorsEnabled = 1;
}

void toggleMotors()
{
   Log(__FUNCTION__);
	if (g_bMotorsEnabled)
	{
		motorsOff();
	}
	else
	{
		motorsOn();
	}
}

bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB)
{
	char *arg1;
	char *arg2;
	char *arg3;
	arg1 = SCmd.next();
	if (arg1 != NULL)
	{
		*duration = atoi(arg1);
		arg2 = SCmd.next();
	}
	if (arg2 != NULL)
	{
		*penStepsEBB = atoi(arg2);
		arg3 = SCmd.next();
	}
	if (arg3 != NULL)
	{
		*rotStepsEBB = atoi(arg3);

		return true;
	}

	return false;
}

void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
{
	if (!g_bMotorsEnabled)
	{
		motorsOn();
	}


		if ((1 == fROT_STEP_CORRECTION) && (1 == fPEN_STEP_CORRECTION))
		{ // if coordinatessystems are identical
			// set Coordinates and Speed
			g_pStepperRotate->setSpeedInTicks(abs((float)rotStepsEBB * (float)1000 / (float)duration));
			g_pStepperRotate->move(rotStepsEBB);

			g_pStepperPen->setSpeedInTicks(abs((float)penStepsEBB * (float)1000 / (float)duration));
			g_pStepperPen->move(penStepsEBB);
		}
	else
	{
		// incoming EBB-Steps will be multiplied by 16, then Integer-maths is done, result will be divided by 16
		//  This make thinks here really complicated, but floating point-math kills performance and memory, believe me... I tried...
		long rotSteps = ((long)rotStepsEBB * 16 / fROT_STEP_CORRECTION) + (long)g_iRotStepError; // correct incoming EBB-Steps to our microstep-Setting and multiply  by 16 to avoid floatingpoint...
		long penSteps = ((long)penStepsEBB * 16 / fPEN_STEP_CORRECTION) + (long)g_iPenStepError;

		int rotStepsToGo = (int)(rotSteps / 16); // Calc Steps to go, which are possible on our machine
		int penStepsToGo = (int)(penSteps / 16);

		g_iRotStepError = (long)rotSteps - ((long)rotStepsToGo * (long)16); // calc Position-Error, if there is one
		g_iPenStepError = (long)penSteps - ((long)penStepsToGo * (long)16);

		long temp_rotSpeed = ((long)rotStepsToGo * (long)1000 / (long)duration); // calc Speed in Integer Math
		long temp_penSpeed = ((long)penStepsToGo * (long)1000 / (long)duration);

		float rotSpeed = (float)abs(temp_rotSpeed); // type cast
			float penSpeed = (float)abs(temp_penSpeed);

			// set Coordinates and Speed
			g_pStepperRotate->setSpeedInTicks(rotSpeed);
			g_pStepperRotate->move(rotStepsToGo);

			g_pStepperPen->setSpeedInTicks(penSpeed);
			g_pStepperPen->move(penStepsToGo);
		}
	}

void moveOneStep()
{
	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning());
}

void moveToDestination()
{
	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning());
}

void setprgButtonState()
{
	g_bPrgButtonState = 1;
}