#include "EggDuino.h"

void updateStepCorrectionFactors()
{
	if (g_iRotMicrostep <= 0)
	{
		g_iRotMicrostep = kDefaultRotMicrostep;
	}
	if (g_iPenMicrostep <= 0)
	{
		g_iPenMicrostep = kDefaultPenMicrostep;
	}
	fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep;
	fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
}

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

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

		g_pStepperRotate->setDirectionPin(g_iRotDirPin);
		g_pStepperRotate->setEnablePin(g_iRotEnablePin);
		g_pStepperRotate->setAcceleration(10000);
		g_pStepperRotate->setAutoEnable(false);
	}

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

	motorsOff();
	penServo.attach(g_iServoPin);
	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;
}
