#include "EggDuino.h"

void initHardware()
{
	if (!initConfigStore())
	{
		penUpPos = 5;
		penDownPos = 20;
	}
	penState = penUpPos;

	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(penState);
}

void storePenUpPosInEE()
{
	saveConfigToFile();
}

void storePenDownPosInEE()
{
	saveConfigToFile();
}

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

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

void motorsOff()
{
	// digitalWrite(enableRotMotor, HIGH);
	// digitalWrite(enablePenMotor, HIGH);
	g_pStepperPen->disableOutputs();
	g_pStepperRotate->disableOutputs();
	motorsEnabled = 0;
}

void motorsOn()
{
	// digitalWrite(enableRotMotor, LOW) ;
	// digitalWrite(enablePenMotor, LOW) ;
	g_pStepperPen->enableOutputs();
	g_pStepperRotate->enableOutputs();
	motorsEnabled = 1;
}

void toggleMotors()
{
	if (motorsEnabled)
	{
		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 (!motorsEnabled)
	{
		motorsOn();
	}

	if (duration == 0)
	{
		duration = 1;
	}

	if (penStepCorrection == 0)
	{
		penStepCorrection = 1;
	}

	if (rotStepCorrection == 0)
	{
		rotStepCorrection = 1;
	}

	if ((1 == rotStepCorrection) && (1 == penStepCorrection))
	{ // if coordinatessystems are identical
		// set Coordinates and Speed
		// rotMotor.move(rotStepsEBB);
		// rotMotor.setSpeed(abs((float)rotStepsEBB * (float)1000 / (float)duration));
		g_pStepperRotate->move(rotStepsEBB);
		g_pStepperRotate->setSpeedInTicks(abs((float)rotStepsEBB * (float)1000 / (float)duration));

		// penMotor.move(penStepsEBB);
		// penMotor.setSpeed(abs((float)penStepsEBB * (float)1000 / (float)duration));
		g_pStepperPen->move(penStepsEBB);
		g_pStepperPen->setSpeedInTicks(abs((float)penStepsEBB * (float)1000 / (float)duration));
	}
	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 / rotStepCorrection) + (long)rotStepError; // correct incoming EBB-Steps to our microstep-Setting and multiply  by 16 to avoid floatingpoint...
		long penSteps = ((long)penStepsEBB * 16 / penStepCorrection) + (long)penStepError;

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

		rotStepError = (long)rotSteps - ((long)rotStepsToGo * (long)16); // calc Position-Error, if there is one
		penStepError = (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
		// rotMotor.move(rotStepsToGo); // finally, let us set the target position...
		// rotMotor.setSpeed(rotSpeed); // and the Speed!
		g_pStepperRotate->move(rotStepsToGo);
		g_pStepperRotate->setSpeedInTicks(rotSpeed);

		// penMotor.move(penStepsToGo);
		// penMotor.setSpeed(penSpeed);
		g_pStepperPen->move(penStepsToGo);
		g_pStepperPen->setSpeedInTicks(penSpeed);
	}
}

void moveOneStep()
{
	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning())
		;
	// if (penMotor.distanceToGo() || rotMotor.distanceToGo())
	// {
	// 	penMotor.runSpeedToPosition(); // Moving.... moving... moving....
	// 	rotMotor.runSpeedToPosition();
	// }
}

void moveToDestination()
{
	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning())
		;
	// while (penMotor.distanceToGo() || rotMotor.distanceToGo())
	// {
	// 	penMotor.runSpeedToPosition(); // Moving.... moving... moving....
	// 	rotMotor.runSpeedToPosition();
	// }
}

void setprgButtonState()
{
	prgButtonState = 1;
}