/* Eggduino-Firmware by Joachim Cerny, 2014

   Thanks for the nice libs ACCELSTEPPER and SERIALCOMMAND, which made this project much easier.
   Thanks to the Eggbot-Team for such a funny and enjoable concept!
   Thanks to my wife and my daughter for their patience. :-)

 */

// implemented Eggbot-Protocol-Version v13
// EBB-Command-Reference, I sourced from: http://www.schmalzhaus.com/EBB/EBBCommands.html
// no homing sequence, switch-on position of pen will be taken as reference point.
// No collision-detection!!
// Supported Servos: I do not know, I use Arduino Servo Lib with TG9e- standard servo.
// Note: Maximum-Speed in Inkscape is 1000 Steps/s. You could enter more, but then Pythonscript sends nonsense.
// EBB-Coordinates are coming in for 16th-Microstepmode. The Coordinate-Transforms are done in weired integer-math. Be careful, when you diecide to modify settings.

/* TODOs:
   1	collision control via penMin/penMax
   2	implement homing sequence via microswitch or optical device
 */

#include "EggDuino.h"
#include <ServoEasing.hpp>

// EXTRAFEATURES - UNCOMMENT TO USE THEM -------------------------------------------------------------------

// #define prgButton 2 // PRG button
// #define penToggleButton 12 // pen up/down button
// #define motorsButton 4 // motors enable button

//-----------------------------------------------------------------------------------------------------------

FastAccelStepperEngine g_stepEngine = FastAccelStepperEngine();
FastAccelStepper *g_pStepperRotate = NULL;
FastAccelStepper *g_pStepperPen = NULL;

// make Objects
ServoEasing penServo;
SerialCommand SCmd;
#ifdef ESP32
SerialCommand g_BLECmd;
SerialCommand g_WifiCmd;
#endif

namespace
{
SerialCommand *g_pActiveParser = &SCmd;
ProtocolTransport g_activeTransport = PROTOCOL_TRANSPORT_SERIAL;
}

// create Buttons
#ifdef prgButton
Button prgButtonToggle(prgButton, setprgButtonState);
#endif
#ifdef penToggleButton
Button penToggle(penToggleButton, doTogglePen);
#endif
#ifdef motorsButton
Button motorsToggle(motorsButton, toggleMotors);
#endif

// Variables... be careful, by messing around here, everything has a reason and crossrelations...
int g_iPenUpPos = 5;	   // can be overwritten from EBB-Command SC
int g_iPenDownPos = 20;   // can be overwritten from EBB-Command SC
int g_iServoRateUp = 0;   // from EBB-Protocol, mapped to ServoEasing speed (up movement)
int g_iServoRateDown = 0; // from EBB-Protocol, mapped to ServoEasing speed (down movement)
long g_iRotStepError = 0;
long g_iPenStepError = 0;
int g_iPenState = g_iPenUpPos;
uint32_t g_uiNodeCount = 0;
unsigned int g_uiLayer = 0;
boolean g_bPrgButtonState = 0;
boolean g_bMotorsEnabled = 0;
int g_iRotDirPin = kDefaultRotDirPin;
int g_iRotEnablePin = kDefaultRotEnablePin;
int g_iRotStepPin = kDefaultRotStepPin;
int g_iPenStepPin = kDefaultPenStepPin;
int g_iPenDirPin = kDefaultPenDirPin;
int g_iPenEnablePin = kDefaultPenEnablePin;
int g_iServoPin = kDefaultServoPin;
int g_iRotMicrostep = kDefaultRotMicrostep;
int g_iPenMicrostep = kDefaultPenMicrostep;
float fROT_STEP_CORRECTION = 16.0 / kDefaultRotMicrostep; // devide EBB-Coordinates by this factor to get EGGduino-Steps
float fPEN_STEP_CORRECTION = 16.0 / kDefaultPenMicrostep; // devide EBB-Coordinates by this factor to get EGGduino-Steps
int g_iMaxAcclSpeed = 10000;
String g_sHostname = "EggDuino";
String g_sWifiSsid = "";
String g_sWifiPassword = "";

void setActiveProtocolContext(SerialCommand *parser, ProtocolTransport transport)
{
	if (parser != NULL)
	{
		g_pActiveParser = parser;
	}
	g_activeTransport = transport;
}

char *nextCommandArg()
{
	return g_pActiveParser->next();
}

void protocolWrite(const char *message)
{
	if (message == NULL)
	{
		return;
	}

#ifdef ESP32
	if (g_activeTransport == PROTOCOL_TRANSPORT_BLE)
	{
		if (bleProtocolWrite(message))
		{
			return;
		}
	}
	if (g_activeTransport == PROTOCOL_TRANSPORT_WIFI)
	{
		if (wifiProtocolWrite(message))
		{
			return;
		}
	}
#endif

	Serial.print(message);
}

void protocolWrite(const String &message)
{
	protocolWrite(message.c_str());
}

// Stepper Test
#ifdef TEST
// #define dirPinStepper 16
// #define enablePinStepper 12
// #define stepPinStepper 26
#endif

void setup()
{
	Serial.begin(115200);
	Serial.println("Starting...");
	startWebInterface();
	makeComInterface();
	initHardware();
	startBleInterface();
}

uint8_t g_uiState = 0;
unsigned long g_uiLastTim = millis();

void loop()
{
#ifdef TEST

	unsigned long uiNow = millis();
	motorsOn();

	if (uiNow - g_uiLastTim > 5000)
	{
		g_uiLastTim = uiNow;
		switch (g_uiState)
		{
		case 0:
			Log(String(g_uiState));
			g_pStepperRotate->setSpeedInUs(10); // the parameter is us/step !!!
			g_pStepperRotate->setAcceleration(10000);
			g_pStepperRotate->move(1000);
			g_uiState++;
			break;

		case 1:
			Log(String(g_uiState));
			g_pStepperRotate->setSpeedInUs(10); // the parameter is us/step !!!
			g_pStepperRotate->setAcceleration(10000);
			g_pStepperRotate->move(-1000);
			g_uiState++;
			break;

		case 2:
			Log(String(g_uiState));
			g_pStepperPen->setSpeedInUs(10); // the parameter is us/step !!!
			g_pStepperPen->setAcceleration(10000);
			g_pStepperPen->move(1000);
			g_uiState++;
			break;

		case 3:
			Log(String(g_uiState));
			g_pStepperPen->setSpeedInUs(10); // the parameter is us/step !!!
			g_pStepperPen->setAcceleration(10000);
			g_pStepperPen->move(-1000);
			g_uiState = 0;
			break;

		default:
			break;
		}
		Log("Alive");
	}
#else
	// moveOneStep();
	setActiveProtocolContext(&SCmd, PROTOCOL_TRANSPORT_SERIAL);
	SCmd.readSerial();
	handleBleInterface();
	handleWebInterface();
	handleWifiProtocolInterface();
#endif

#ifdef penToggleButton
	penToggle.check();
#endif

#ifdef motorsButton
	motorsToggle.check();
#endif

#ifdef prgButton
	prgButtonToggle.check();
#endif
}
