/*
    * Control 2 BiPolar Stepper Motors
    *
    * Receive Commands
    * Use those commands to move the motors
    * Return "ok" string to say done
    *
    * Use as you wish, give me credit of what you use :)
    * Tim Jackson
    */

    #include <Stepper.h>
        #include <Servo.h>

            #ifndef NULL
            #define NULL (0)
            #endif
            #define VERSION        (1)      // firmware version
            #define MAX_BUF        (64)     // What is the longest message Arduino can store?

            // Motors used are from DVD/CD Drive
            // The motors have 20 step/revolution
            // Total revolutions 12
            // 20 * 12 = 240 this is its max movement
            // Using 2 SN754410NE to drive them
            // motor 1 coil 1 pins 4, 5.
            // motor 1 coil 2 pins 6, 7.
            // motor 2 coil 1 pins 11, 10.
            // motor 2 coil 2 pins 13, 12.
            Stepper stepperX(20, 4, 5, 6, 7);
            Stepper stepperY(20, 13, 12, 11, 10);
            Servo penServo;

            // Command line variables
            char  buffer[MAX_BUF];  // message
            int   sofar;            // size of buffer
            char mode_abs = 1;      // absolute mode
            float fr = 0;           // feed rate
            float unitMultiplyer = 240 / 36; // 240 steps = 36mm of movement, if inches then: 240 / 1.4173
            float previousX = 0;
            float previousY = 0;
            int valZ = 0;
            int valT = 0;
            int valC = 0;
            int ToolPin = 3;
            int ServerPin = 9;
            int posUp = 2000;// 160 deg
            int posDown = 700;// 15 deg
            int maxSteps = 0;
            int no_data = 0;
            bool isComment = false;


            void feedrate(float nfr) {
            // to do at some time
            }
            void pause(long ms) {
            delay(ms / 1000);
            delayMicroseconds(ms % 1000);
            }
            void setOrigin(float this_X, float this_Y) {

            previousX = previousX - this_X;
            previousY = previousY - this_Y;

            }
            void motorsOff() {
            // if motors need to be turned off.
            }
            void Info() {
            Serial.print("Tim's Bipolar XY Plotter. Version: ");
            Serial.println(VERSION);
            Serial.println("Commands:");
            Serial.println("G00 [X(steps)] [Y(steps)]; - goX goY");
            Serial.println("G01 [X(steps)] [Y(steps)] [F(feedrate)]; - travel to XY");
            Serial.println("G02 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - clockwise arc");
            Serial.println("G03 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - counter-clockwise arc");
            Serial.println("G90; - absolute mode");
            Serial.println("G91; - relative mode");
            Serial.println("G92 [X(steps)] [Y(steps)]; - change logical position");
            Serial.println("M100; - this Info message");
            Serial.println("M101; - Reset Plotter");
            Serial.println("All commands must end with a newline. /n");
            delay(100);
            }
            float parsenumber(char code, float val) {
            char *p = strchr(buffer, code);
            if (*p == code) {
            return atof(p + 1);
            }
            return val;
            }
            void checkTool() {
            // do +-Z for Tool State on or off
            valZ = parsenumber('Z', valZ);
            //Serial.print(" valZ:");
            //Serial.println(valZ);
            if (valZ > 0) {
            ChangeTool(0);
            }
            else {
            ChangeTool(1);
            }
            }
            void processCommand() {

            int cmd = parsenumber('G', -1);
            switch (cmd) {
            case  0:
            case  1: { // line
            //checkTool();
            feedrate(parsenumber('F', fr));
            MoveMotors_LINE(parsenumber('X', previousX) * unitMultiplyer, parsenumber('Y', previousY) * unitMultiplyer);
            break;
            }
            case 2:
            case 3: {  // arc
            int Roation = 0;
            if (cmd == 3) { Roation = 1; }
            //checkTool();
            feedrate(parsenumber('F', fr));
            MoveMotors_ARC(parsenumber('X', previousX) * unitMultiplyer, parsenumber('Y', previousY) * unitMultiplyer, parsenumber('I', previousX) * unitMultiplyer, parsenumber('J', previousY) * unitMultiplyer, Roation);
            break;
            }
            case  4:  pause(parsenumber('P', 0) * 1000);  break;  // dwell
            case 20:  unitMultiplyer = 240.0 / 1.4173;  break;  // Inch mode
            case 21:  unitMultiplyer = 240.0 / 36.0;  break;  // mm mode
            case 90:  mode_abs = 1;  break;  // absolute mode
            case 91:  mode_abs = 0;  break;  // relative mode
            case 92:  setOrigin(parsenumber('X', 0), parsenumber('Y', 0)); break;// set origin
            default:  break;
            }

            cmd = parsenumber('M', -1);
            switch (cmd) {
            case 3: ChangeTool(1); break;
            case 4: ChangeTool(2); break;
            case 5: ChangeTool(0); break;

            //None Standard Commands
            case 18: motorsOff();    break;
            case 100:  Info(); break;
            case 101:  ResetMotors(); break;
            case 114:    SendPositionToCom(previousX, previousY, valT, valC);  break;// Send current position to serial
            default:  break;
            }
            //Serial.println("ok");
            }
            void init_process_string()
            {
            //init our command
            for (byte i = 0; i < MAX_BUF; i++)
            buffer[i] = 0;
            sofar = 0;
            Serial.flush();
            Serial.println("ok");
            }

            void setup() {
            Serial.begin(28800);  //9600 14400 19200 28800* 38400 56000 57600** 115200 *Good **max

            // set the speed of the motor to 1000 RPMs max
            stepperX.setSpeed(900);
            stepperY.setSpeed(900);
            // set pin to switch tool on/off
            // pinMode(ToolPin, OUTPUT);
            //penServo.attach(ServerPin);
            ResetMotors();
            Info();  // show message
            init_process_string();



            }
            void loop() {

            char c;
            //read in characters if we got them.
            if (Serial.available() > 0)
            {
            c = Serial.read();
            no_data = 0;
            //newlines are ends of commands.
            if (c != '\n' || c != '\r')
            {
            if (c == 0x18) {
            Serial.println("Tim");
            }
            else {
            if (c == '(') {
            isComment = true;
            }
            // If we're not in isComment mode, add it to our array.
            if (!isComment)
            {
            buffer[sofar] = c;
            sofar++;
            }
            if (c == ')') {
            isComment = false; // End of isComment - start listening again
            }
            }
            }
            }
            else
            {
            no_data++;
            delayMicroseconds(100);//100

            //if theres a pause or we got a real command, do it
            if (sofar && (c == '\n' || c == '\r' || no_data > 100))
            {
            processCommand();  // Process Command
            init_process_string();//clear ommand.
            }
            }
            }

            void MoveMotors_LINE(int this_X, int this_Y) {

            int startX = previousX;
            int startY = previousY;

            //Serial.print("Start X:");
            //Serial.print(startX);
            //Serial.print("  Y:");
            //Serial.println(startY);
            //Serial.print("End X:");
            //Serial.print(this_X);
            //Serial.print("  Y:");
            //Serial.println(this_Y);

            float numOfStepsX = this_X - startX;
            float numOfStepsY = this_Y - startY;
            float stepX = 1;
            float stepY = 1;

            //Serial.print("Number Of Steps X:");
            //Serial.print(numOfStepsX);
            //Serial.print("   Y:");
            //Serial.println(numOfStepsY);

            if (abs(numOfStepsX) >= abs(numOfStepsY)) {
            //Serial.println("X is=|| Greater");
            maxSteps = numOfStepsX;

            if (numOfStepsX != 0) {
            if (this_X > previousX) { stepX = 1; }
            else { stepX = -1; }
            }
            else {
            stepX = 0;
            }

            if (numOfStepsY != 0) {
            if (this_Y > previousY) { stepY = abs(numOfStepsY / numOfStepsX); }
            else { stepY = -abs(numOfStepsY / numOfStepsX); }
            }
            else {
            stepY = 0;
            }

            //Serial.print("Step X:");
            //Serial.print(stepX);
            //Serial.print("  Y:");
            //Serial.println(stepY);

            for (size_t i = 0; i < abs(numOfStepsX); i++) {

            int newSepsY = ((round(stepY * (i + 1))) + startY) - previousY;

            stepperY.step(newSepsY);
            previousY = previousY + newSepsY;


            //stepperY.step(round(stepY));
            //previousY = previousY + stepY;

            stepperX.step(stepX);
            previousX = previousX + stepX;

            // Send current position to serial
            SendPositionToCom(previousX, previousY, valT, valC);

            }

            }
            else {
            //Serial.println("Y is=|| Greater");
            maxSteps = numOfStepsY;

            if (numOfStepsY != 0) {
            if (this_Y >= previousY) { stepY = 1; }
            else { stepY = -1; }
            }
            else {
            stepY = 0;
            }
            if (numOfStepsX != 0) {
            if (this_X >= previousX) { stepX = abs(numOfStepsX / numOfStepsY); }
            else { stepX = -abs(numOfStepsX / numOfStepsY); }
            }
            else {
            stepX = 0;
            }

            //Serial.print("Step X:");
            //Serial.print(stepX);
            //Serial.print("  Y:");
            //Serial.println(stepY);

            for (size_t i = 0; i < abs(numOfStepsY); i++) {

            int newSepsX = ((round(stepX * (i + 1))) + startX) - previousX;

            stepperX.step(newSepsX);
            previousX = previousX + newSepsX;

            stepperY.step(stepY);
            previousY = previousY + stepY;

            // Send current position to serial
            SendPositionToCom(previousX, previousY, valT, valC);

            }

            }

            }
            void MoveMotors_ARC(int this_X, int this_Y, int this_I, int this_J, int rotation) {

            float startX = previousX;
            float startY = previousY;
            float endX = this_X;
            float endY = this_Y;
            float startToCentX = this_I;
            float startToCentY = this_J;
            float centerX = startX + startToCentX;
            float centerY = startY + startToCentY;

            float endToCentX = this_X - centerX;
            float endToCentY = this_Y - centerY;

            float radius = sqrt((startToCentX*startToCentX) + (startToCentY*startToCentY));
            float startAngle = 0;
            float endAngle = 0;
            float arcAngle = 360;

            if (startX >= centerX && startY <= centerY) {
            if (startToCentY != 0 && startToCentX != 0) {
            startAngle = ((atan(abs(startToCentY) / abs(startToCentX))) * 180 / PI);
            }
            else if (startToCentY == 0) {
            startAngle = 0;
            }
            else if (startToCentX == 0) {
            startAngle = 0;
            }
            }
            if (startX <= centerX && startY <= centerY) {
            if (startToCentY != 0 && startToCentX != 0) {
            startAngle = 180 - ((atan(abs(startToCentY) / abs(startToCentX))) * 180 / PI);
            }
            else if (startToCentY == 0) {
            startAngle = 180;
            }
            else if (startToCentX == 0) {
            startAngle = 90;
            }
            }
            if (startX <= centerX && startY >= centerY) {
            if (startToCentY != 0 && startToCentX != 0) {
            startAngle = 180 + ((atan(abs(startToCentY) / abs(startToCentX))) * 180 / PI);
            }
            else if (startToCentY == 0) {
            startAngle = 180;
            }
            else if (startToCentX == 0) {
            startAngle = 270;
            }
            }
            if (startX >= centerX && startY >= centerY) {
            if (startToCentY != 0 && startToCentX != 0) {
            startAngle = 360 - ((atan(abs(startToCentY) / abs(startToCentX))) * 180 / PI);
            }
            else if (startToCentY == 0) {
            startAngle = 0;
            }
            else if (startToCentX == 0) {
            startAngle = 270;
            }
            }

            if (endX >= centerX && endY <= centerY) {
            if (endToCentY != 0 && endToCentX != 0) {
            endAngle = ((atan(abs(endToCentY) / abs(endToCentX))) * 180 / PI);
            }
            else if (endToCentY == 0) {
            endAngle = 0;
            }
            else if (endToCentX == 0) {
            endAngle = 0;
            }
            }
            if (endX <= centerX && endY <= centerY) {
            if (endToCentY != 0 && endToCentX != 0) {
            endAngle = 180 - ((atan(abs(endToCentY) / abs(endToCentX))) * 180 / PI);
            }
            else if (endToCentY == 0) {
            endAngle = 0;
            }
            else if (endToCentX == 0) {
            endAngle = 90;
            }
            }
            if (endX <= centerX && endY >= centerY) {
            if (endToCentY != 0 && endToCentX != 0) {
            endAngle = 180 + ((atan(abs(endToCentY) / abs(endToCentX))) * 180 / PI);
            }
            else if (endToCentY == 0) {
            endAngle = 180;
            }
            else if (endToCentX == 0) {
            endAngle = 270;
            }
            }
            if (endX >= centerX && endY >= centerY) {
            if (endToCentY != 0 && endToCentX != 0) {
            endAngle = 360 - ((atan(abs(endToCentY) / abs(endToCentX))) * 180 / PI);
            }
            else if (endToCentY == 0) {
            endAngle = 0;
            }
            else if (endToCentX == 0) {
            endAngle = 270;
            }
            }

            if (startAngle > endAngle) {
            arcAngle = 360 - startAngle + endAngle;
            }
            else {
            arcAngle = endAngle - startAngle;
            }
            arcAngle = arcAngle / 180 * M_PI;

            if (startAngle == 0 && endAngle == 0) {
            arcAngle = 2 * M_PI;
            }

            float radAngleStep = asin(1 / radius); // =asin(1/2)  =0.5235 rad
            float NumberOfSteps = ceil(arcAngle / radAngleStep); // =floor(6.28 rad / 0.436 rad) = 12

            if (NumberOfSteps > 360) {
            NumberOfSteps = 360;
            }

            radAngleStep = arcAngle / NumberOfSteps; // = 0.5235 rad
            float radStartAngle = 0;
            if (startAngle > 0) {
            radStartAngle = startAngle / 180 * M_PI;
            }

            float newPointX = previousX;
            float newPointY = previousY;
            int currentX = previousX;
            int currentY = previousY;
            float newRadAngle = 0.0;

            float x2 = 0;
            float y2 = 0;

            for (int i = 0; i < NumberOfSteps; i++) {
            newRadAngle = radAngleStep * i;

            x2 = cos(radStartAngle + newRadAngle) * radius;
            y2 = sin(radStartAngle + newRadAngle) * radius;
            newPointX = centerX + x2;
            newPointY = centerY - y2;

            MoveMotors_LINE(newPointX, newPointY);

            }

            }

            void ChangeTool(int toolValue) {
            valT = toolValue;
            valZ = 1;
            penServo.attach(ServerPin);

            if (valT == 0) {
            //digitalWrite(ToolPin, LOW);
            //Serial.println("Tool Off");
            penServo.writeMicroseconds(posUp);
            }
            else if (valT == 1) {
            //digitalWrite(ToolPin, HIGH);
            //Serial.println("Tool Forward");
            penServo.writeMicroseconds(posDown);
            valZ = -1;
            }
            else if (valT == 2) {
            //digitalWrite(ToolPin, HIGH);
            //Serial.println("Tool Reverse");
            valZ = -1;
            }
            else if (valT == 3) {
            //digitalWrite(ToolPin, HIGH);
            //Serial.println("Tool On");
            valZ = -1;
            }
            delay(400);
            penServo.detach();

            SendPositionToCom(previousX, previousY, valT, valC);

            }
            void ResetMotors() {

            penServo.attach(ServerPin);
            penServo.writeMicroseconds(posUp);
            delay(400);
            penServo.detach();

            stepperX.step(242);
            stepperY.step(242);
            previousX = 242;
            previousY = 242;
            MoveMotors_LINE(0, 0);

            digitalWrite(ToolPin, LOW);
            valT = 0;
            valC = 0;

            SendPositionToCom(previousX, previousY, valT, valC);
            Serial.println("============");
            Serial.println("System Reset");
            Serial.println("============");
            //init_process_string();
            }
            void SendPositionToCom(int this_X, int this_Y, int this_T, int this_C) {

            Serial.print("Status: ");
            Serial.print("X:");
            Serial.print(this_X / unitMultiplyer);
            Serial.print(" Y:");
            Serial.print(this_Y / unitMultiplyer);
            Serial.print(" T:");
            Serial.print(this_T);
            Serial.print(" S:");
            Serial.print(fr);
            Serial.print(" C:");
            Serial.println(this_C);
            }