/*
 Mini CNC Plotter firmware, based in TinyCNC https://github.com/MakerBlock/TinyCNC-Sketches
 Send GCODE to this Sketch using gctrl.pde https://github.com/damellis/gctrl
 Convert SVG to GCODE with MakerBot Unicorn plugin for Inkscape available here https://github.com/martymcguire/inkscape-unicorn

 More information about the Mini CNC Plotter here (german, sorry): http://www.makerblog.at/2015/02/projekt-mini-cnc-plotter-aus-alten-cddvd-laufwerken/
  */

#include <Servo.h>
#include <AFMotor.h>

/* Structures, global variables    */
struct point {
  float x;
  float y;
  float z;
} actuatorPos;

// Current position of plothead
//struct point actuatorPos;

/*****************
 * *** Defines ***
 * ***************/

//#define debug			// Debug

#define LINE_BUFFER_LENGTH 1024
#define LED 13			// LED pin on Arduino Uno
#define zeroX 14		// pin finecorsa X
#define zeroY 15		// pin finecorsa Y
#define ena 2			// enable

/***************
 * *** Servo ***
 * *************/
#define penZUp 70		// Servo position for Up
#define penZDown 85	// Servo position for Down
#define penServoPin 10	// Servo on PWM pin 10
Servo penServo;			// create servo object to control a servo

/*****************
 * *** Stepper ***
 * ***************/
#define stepsPerRevolution_X 20
#define stepsPerRevolution_Y 48
#define rpm_X 500			// Speed
#define rpm_Y 200     // Speed
#define STEP_Y MICROSTEP	// Stepper Command mode (SINGLE, DOUBLE, INTERLEAVE, MICROSTEP)
#define STEP_X DOUBLE
AF_Stepper myStepperY(stepsPerRevolution_Y,1); // Initialize steppers for Y-axis
AF_Stepper myStepperX(stepsPerRevolution_X,2); // Initialize steppers for X-axis

/****************************************
 * *** Drawing settings, should be OK ***
 * **************************************/
float StepInc = 1;
int StepDelay = 5;
int LineDelay = 0;
int penDelay = 50;

/*****************************************
 * *** Motor steps to go 1 millimeter. ***
 * ***************************************/
// Use test sketch to go 100 steps. Measure the length of line.
// Calculate steps per mm. Enter here.
float StepsPerMillimeterX = 40.8163;  //100.0; 4.7619*2
float StepsPerMillimeterY = 108.1081; //6.6667*2;  //100.0;

/*************************************
 * *** Drawing robot limits, in mm ***
 * ***********************************/
// OK to start with. Could go up to 50 mm if calibrated well.
float Xmin = 0;
float Xmax = 48;
float Ymin = 0;
float Ymax = 35; //40;
float Zmin = 0;
float Zmax = 1;

float Xpos = Xmin;
float Ypos = Ymin;
float Zpos = Zmax;

//  Needs to interpret
//  G1 for moving
//  G4 P300 (wait 150ms)
//  M300 S30 (pen down)
//  M300 S50 (pen up)
//  Discard anything with a (
//  Discard any other command!

void Home() {
	while (digitalRead(zeroX) == HIGH) {
		myStepperX.step(1, BACKWARD, STEP_X);	//go to home X
    }
	while (digitalRead(zeroY) == HIGH) {
		myStepperY.step(1, BACKWARD, STEP_Y);		//go to home Y
	}
	digitalWrite(LED, HIGH);
}
/*********************************
 * Draw a line from (x0;y0) to (x1;y1).
 * Bresenham algo from https://www.marginallyclever.com/blog/2013/08/how-to-build-an-2-axis-arduino-cnc-gcode-interpreter/
 * int (x1;y1) : Starting coordinates
 * int (x2;y2) : Ending coordinates
 **********************************/
void drawLine(float x1, float y1) {
#ifdef debug
	Serial.print("fx1, fy1: ");
	Serial.print(x1);
	Serial.print(",");
	Serial.print(y1);
	Serial.println("");
#endif
	//  Bring instructions within limits
	if (x1 >= Xmax) {
		x1 = Xmax;
	}
	if (x1 <= Xmin) {
		x1 = Xmin;
	}
	if (y1 >= Ymax) {
		y1 = Ymax;
	}
	if (y1 <= Ymin) {
		y1 = Ymin;
	}

#ifdef debug
	Serial.print("Xpos, Ypos: ");
	Serial.print(Xpos);
	Serial.print(",");
	Serial.print(Ypos);
	Serial.println("");

	Serial.print("x1, y1: ");
	Serial.print(x1);
	Serial.print(",");
	Serial.print(y1);
	Serial.println("");
#endif

	//  Convert coordinates to steps
	x1 = (int)(x1*StepsPerMillimeterX);
	y1 = (int)(y1*StepsPerMillimeterY);
	float x0 = Xpos;
	float y0 = Ypos;

	//  Let's find out the change for the coordinates
	long dx = abs(x1-x0);
	long dy = abs(y1-y0);
	int sx = x0<x1 ? StepInc : -StepInc;
	int sy = y0<y1 ? StepInc : -StepInc;

	long i;
	long over = 0;

	if (dx > dy) {
		for (i=0; i<dx; ++i) {
		myStepperX.onestep(sx,STEP_X);
		//myStepperX.step(1,sx,STEP);
		over+=dy;
		if (over>=dx) {
			over-=dx;
			myStepperY.onestep(sy,STEP_Y);
			//myStepperY.step(1,sy,STEP);
		}
		delay(StepDelay);
		}
	}
	else {
		for (i=0; i<dy; ++i) {
		myStepperY.onestep(sy,STEP_Y);
		//myStepperY.step(1,sy,STEP);
		over+=dx;
		if (over>=dy) {
			over-=dy;
			myStepperX.onestep(sx,STEP_X);
			//myStepperX.step(1,sx,STEP);
		}
		delay(StepDelay);
		}
	}

#ifdef debug
	Serial.print("dx, dy:");
	Serial.print(dx);
	Serial.print(",");
	Serial.print(dy);
	Serial.println("");

	Serial.print("Going to (");
	Serial.print(x0);
	Serial.print(",");
	Serial.print(y0);
	Serial.println(")");
#endif

	//  Delay before any next lines are submitted
	delay(LineDelay);
	//  Update the positions
	Xpos = x1;
	Ypos = y1;
	if ((Xpos==0)&&(Ypos==0)){
		digitalWrite(LED, HIGH);
	}
	else {
		digitalWrite(LED, LOW);
	}
}

//  Raises pen
void penUp() {
	penServo.write(penZUp);
	delay(penDelay);
	Zpos=Zmax;
	//digitalWrite(15, LOW);
	//digitalWrite(16, HIGH);
#ifdef debug
	Serial.println("Pen up!");
#endif
}
//  Lowers pen
void penDown() {
	penServo.write(penZDown);
	delay(penDelay);
	Zpos=Zmin;
	//digitalWrite(15, HIGH);
	//digitalWrite(16, LOW);
#ifdef debug
	Serial.println("Pen down.");
#endif
}

void processIncomingLine( char* line, int charNB ) {
	int currentIndex = 0;
	char buffer[ 64 ];                                 // Hope that 64 is enough for 1 parameter
	struct point newPos;

	newPos.x = 0.0;
	newPos.y = 0.0;

	//  Needs to interpret
	//  G1 for moving
	//  G4 P300 (wait 150ms)
	//  G1 X60 Y30
	//  G1 X30 Y50
	//  M300 S30 (pen down)
	//  M300 S50 (pen up)
	//  Discard anything with a (
	//  Discard any other command!

	while( currentIndex < charNB ) {
		switch ( line[ currentIndex++ ] ) {              	// Select command, if any
			case 'U':
				penUp();
				break;
			case 'D':
				penDown();
				break;
			case 'G':
				buffer[0] = line[ currentIndex++ ];        	// /!\ Dirty - Only works with 2 digit commands
				//buffer[1] = line[ currentIndex++ ];
				//buffer[2] = '\0';
				buffer[1] = '\0';

				switch ( atoi( buffer ) ){                 	// Select G command
					case 0:                                	// G00 & G01 - Movement or fast movement. Same here
					case 1:
					// /!\ Dirty - Suppose that X is before Y
						char* indexX = strchr( line+currentIndex, 'X' );  // Get X/Y position in the string (if any)
						char* indexY = strchr( line+currentIndex, 'Y' );
						if ( indexY <= 0 ) {
							newPos.x = atof( indexX + 1);
							newPos.y = actuatorPos.y;
						}
						else if ( indexX <= 0 ) {
							newPos.y = atof( indexY + 1);
							newPos.x = actuatorPos.x;
						}
						else {
							newPos.y = atof( indexY + 1);
							indexY = '\0';
							newPos.x = atof( indexX + 1);
						}
						drawLine(newPos.x, newPos.y );
						actuatorPos.x = newPos.x;
						actuatorPos.y = newPos.y;
						break;
				}
				break;
			case 'M':
				buffer[0] = line[ currentIndex++ ];			// /!\ Dirty - Only works with 3 digit commands
				buffer[1] = line[ currentIndex++ ];
				buffer[2] = line[ currentIndex++ ];
				buffer[3] = '\0';
				switch ( atoi( buffer ) ){
					case 300: {
						char* indexS = strchr( line+currentIndex, 'S' );
						float Spos = atof( indexS + 1);
						//         Serial.println("ok");
						if (Spos == 30) {
							penDown();
						}
						if (Spos == 50) {
							penUp();
						}
						break;
					}
					case 114:                                // M114 - Repport position
						Serial.print( "Absolute position : X = " );
						Serial.print( actuatorPos.x );
						Serial.print( "  -  Y = " );
						Serial.println( actuatorPos.y );
						break;
					default:
						Serial.print( "Command not recognized : M");
						Serial.println( buffer );
				}

		}
	}
}


/**********************************
 * void setup() - Initialisations
 **********************************/
void setup() {
	//  Setup
	pinMode(LED, OUTPUT);
	pinMode(zeroX,INPUT_PULLUP);
	pinMode(zeroY,INPUT_PULLUP);
	pinMode(ena,INPUT_PULLUP);

	// Decrease if necessary
	myStepperX.setSpeed(rpm_X);
	myStepperY.setSpeed(rpm_Y);

	//  Set & move to initial default position
	penServo.attach(penServoPin);
	penServo.write(penZUp);
	Home();

	//  Notifications!!!
	Serial.begin( 9600 );
	Serial.println("Paolo's Mini CNC Plotter alive and kicking!");
	Serial.print("X range is from ");
	Serial.print(Xmin);
	Serial.print(" to ");
	Serial.print(Xmax);
	Serial.println(" mm.");
	Serial.print("Y range is from ");
	Serial.print(Ymin);
	Serial.print(" to ");
	Serial.print(Ymax);
	Serial.println(" mm.");
}

/**********************
 * void loop() - Main loop
 ***********************/
void loop()
{
	delay(100);
	char line[ LINE_BUFFER_LENGTH ];
	char c;
	int lineIndex;
	bool lineIsComment, lineSemiColon;

	lineIndex = 0;
	lineSemiColon = false;
	lineIsComment = false;

	while (1) {
		// Serial reception - Mostly from Grbl, added semicolon support
		while ( Serial.available()>0 ) {
			c = Serial.read();
			if (( c == '\n') || (c == '\r') ) {             // End of line reached
				if ( lineIndex > 0 ) {                        // Line is complete. Then execute!
					line[ lineIndex ] = '\0';                   // Terminate string
#ifdef debug
					Serial.print( "Received : ");
					Serial.println( line );
#endif
					processIncomingLine( line, lineIndex );
					lineIndex = 0;
				}
				else {
					// Empty or comment line. Skip block.
				}
				lineIsComment = false;
				lineSemiColon = false;
				Serial.println("ok");
			}
			else {
				if ( (lineIsComment) || (lineSemiColon) ) {   // Throw away all comment characters
					if ( c == ')' )  lineIsComment = false;     // End of comment. Resume line.
				}
				else {
					if ( c <= ' ' ) {                           // Throw away whitepace and control characters
					}
					else if ( c == '/' ) {                    // Block delete not supported. Ignore character.
					}
					else if ( c == '(' ) {                    // Enable comments flag and ignore all characters until ')' or EOL.
						lineIsComment = true;
					}
					else if ( c == ';' ) {
						lineSemiColon = true;
					}
					else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) {
						Serial.println( "ERROR - lineBuffer overflow" );
						lineIsComment = false;
						lineSemiColon = false;
					}
					else if ( c >= 'a' && c <= 'z' ) {        // Upcase lowercase
						line[ lineIndex++ ] = c-'a'+'A';
					}
					else {
						line[ lineIndex++ ] = c;
					}
				}
			}
		}
	}
}