#include <CommandParser.h>
#include <AccelStepper.h>

// Config

#define MOTOR_PIN_1 8   // -> IN1 -> ULN2003 driver
#define MOTOR_PIN_2 9   // -> IN2 -> ULN2003 driver
#define MOTOR_PIN_3 10  // -> IN3 -> ULN2003 driver
#define MOTOR_PIN_4 11  // -> IN4 -> ULN2003 driver

#define MOTOR_INTERFACE_TYPE 8

AccelStepper stepper = AccelStepper(
	MOTOR_INTERFACE_TYPE,
	MOTOR_PIN_1,
	MOTOR_PIN_3,
	MOTOR_PIN_2,
	MOTOR_PIN_4
);

typedef CommandParser<10, 5, 10, 15, 64> commandParser;
commandParser parser;

// Commands

// SPEED <uint>
void cmd_SetStepperDeviceSpeed(commandParser::Argument *args, char *response) {
	stepper.setSpeed((uint32_t) args[0].asUInt64);
	Serial.println("SPEED " + String((uint32_t) args[0].asUInt64));
	strlcpy(response, "OK", commandParser::MAX_RESPONSE_SIZE);
}

// ACCEL <uint>
void cmd_SetStepperDeviceAcceleration(commandParser::Argument *args, char *response) {
	stepper.setAcceleration((uint32_t) args[0].asUInt64);
	Serial.println("ACCEL " + String((uint32_t) args[0].asUInt64));
	strlcpy(response, "OK", commandParser::MAX_RESPONSE_SIZE);
}

// MOVE <uint>
void cmd_MoveStepperDeviceToNewPosition(commandParser::Argument *args, char *response) {
	Serial.println("MOVING");
	stepper.runToNewPosition((int32_t) args[0].asUInt64);
	Serial.println("POSITION " + String((int32_t) args[0].asUInt64));
	strlcpy(response, "OK", commandParser::MAX_RESPONSE_SIZE);
}

// STATUS
void cmd_Status(commandParser::Argument *args, char *response) {
	Serial.println("SPEED " + String(stepper.speed()));
	Serial.println("POSITION " + String(stepper.currentPosition()));  
	strlcpy(response, "OK", commandParser::MAX_RESPONSE_SIZE);
}

// RESET
void cmd_Reset(commandParser::Argument *args, char *response) {
	strlcpy(response, "OK", commandParser::MAX_RESPONSE_SIZE);
	asm volatile ("jmp 0");
}


void setup() {
	// Serial init
	Serial.begin(9600);
	while (!Serial);

	Serial.println("");
	Serial.println("INIT");

	stepper.setMaxSpeed(2500);
	stepper.setAcceleration(2500);

	// Register commands  
	parser.registerCommand("/SPEED", "u", &cmd_SetStepperDeviceSpeed);
	Serial.println("Register command: /SPEED u");

	parser.registerCommand("/ACCEL", "u", &cmd_SetStepperDeviceAcceleration);
	Serial.println("Register command: /ACCEL u");

	parser.registerCommand("/MOVE", "i", &cmd_MoveStepperDeviceToNewPosition);  
	Serial.println("Register command: /MOVE i");

	parser.registerCommand("/STATUS", "", &cmd_Status);
	Serial.println("Register command: /STATUS");  

	parser.registerCommand("/RESET", "", &cmd_Reset);
	Serial.println("Register command: /RESET");

	// Print commands
	Serial.println("REVISION 4");
	Serial.println("INIT COMPLETE");
}

void loop() {
	// Command handler
	if (Serial.available()) {
		char line[256];
		size_t lineLength = Serial.readBytesUntil('\n', line, 255);
		line[lineLength] = '\0';

		char response[commandParser::MAX_RESPONSE_SIZE];
		parser.processCommand(line, response);
		Serial.println(response);
	}
}