/*  AR4 Annin Robot Control Software Arduino Nano sketch
    Copyright (c) 2021, Chris Annin
    All rights reserved.

    You are free to share, copy and redistribute in any medium
    or format.  You are free to remix, transform and build upon
    this material.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

          Redistributions of source code must retain the above copyright
          notice, this list of conditions and the following disclaimer.
          Redistribution of this software in source or binary forms shall be
   free of all charges or fees to the recipient of this software.
          Redistributions in binary form must reproduce the above copyright
          notice, this list of conditions and the following disclaimer in the
          documentation and/or other materials provided with the distribution.
          you must give appropriate credit and indicate if changes were made.
   You may do so in any reasonable manner, but not in any way that suggests the
          licensor endorses you or your use.
          Selling AR2 software, robots, robot parts, or any versions of robots
   or software based on this work is strictly prohibited.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   ARE DISCLAIMED. IN NO EVENT SHALL CHRIS ANNIN BE LIABLE FOR ANY DIRECT,
   INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
   THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

    [email protected]

    Log:

*/
#include <AccelStepper.h>
#include <SoftwareSerial.h>



// Firmware version
const char* VERSION = "0.0.1";

String inData;


AccelStepper stepper0(1,8,9);

const int Input2 = 2;
const int Input3 = 3;
const int Input4 = 4;
const int Input5 = 5;
const int Input6 = 6;
const int Input7 = 7;

const int Output8 = 8;
const int Output9 = 9;
const int Output10 = 10;
const int Output11 = 11;
const int Output12 = 12;
const int Output13 = 13;


#define home_switch 1

const double degreesPerStep = 0.000340909090909091;
long initial_homing = -1;
int current_steps = 0;



void moveToPosition(int steps,AccelStepper stepper_to_use) {
   
    int steps_to_move = steps-current_steps;
    Serial.println(String(current_steps));
    stepper_to_use.moveTo(steps_to_move);
    stepper_to_use.runToPosition(); // Move stepper to new position
    current_steps = steps;
    Serial.println("Done");
}
void home(){


    // Home the stepper at startup
    while (digitalRead(home_switch)) {
        stepper0.moveTo(initial_homing);
        initial_homing--;
        stepper0.run();
        delay(5);
    }
}
void readGripperPos(AccelStepper stepper_to_use){
  
      Serial.println(String(current_steps));

}

void setup() {
      // run once:
    Serial.begin(9600);
    Serial.println("Welcome to the Serial Monitor!");
    Serial.println("---------------------------------");
    pinMode(home_switch, INPUT_PULLUP);
    stepper0.setMaxSpeed(1000.0);
    stepper0.setAcceleration(1000.0);
    // home();
    current_steps = 0;
   // stepper0.setCurrentPosition(0);

}





void loop() {
  // start loop


  while (Serial.available() > 0) {
    char recieved = Serial.read();
    inData += recieved;
    // Process message when new line character is recieved
    if (recieved == '\n') {
      String function = inData.substring(0, 2);

      if (function == "ST") {
        Serial.println(String(VERSION));
      }

      //-----COMMAND TO MOVE STEPPER-----
      if (function == "SV") {
        int SVstart = inData.indexOf('V');
        int POSstart = inData.indexOf('P');
        int stepperNum = inData.substring(SVstart + 1, POSstart).toInt();
        int stepperPOS = inData.substring(POSstart + 1).toInt();
        if (stepperNum == 0) {
             moveToPosition(stepperPOS,stepper0);
        }
        
      }

      //-----COMMAND TO READ stepper POSITION-----
      if (function == "SP") {
        int SPstart = inData.indexOf('P');
        int stepperNum = inData.substring(SPstart + 1).toInt();
        if (stepperNum == 0) {
          readGripperPos(stepper0);
        }
      }

      //-----COMMAND IF INPUT THEN JUMP-----
      if (function == "JF") {
        int IJstart = inData.indexOf('X');
        int IJTabstart = inData.indexOf('T');
        int IJInputNum = inData.substring(IJstart + 1, IJTabstart).toInt();
        if (digitalRead(IJInputNum) == HIGH) {
          Serial.println("T");
        }
        if (digitalRead(IJInputNum) == LOW) {
          Serial.println("F");
        }
      }

      //-----COMMAND SET OUTPUT ON-----
      if (function == "ON") {
        int ONstart = inData.indexOf('X');
        int outputNum = inData.substring(ONstart + 1).toInt();
        digitalWrite(outputNum, HIGH);
        Serial.println("Done");
      }
      //-----COMMAND SET OUTPUT OFF-----
      if (function == "OF") {
        int ONstart = inData.indexOf('X');
        int outputNum = inData.substring(ONstart + 1).toInt();
        digitalWrite(outputNum, LOW);
        Serial.println("Done");
      }
      //-----COMMAND TO WAIT INPUT ON-----
      if (function == "WI") {
        int WIstart = inData.indexOf('N');
        int InputNum = inData.substring(WIstart + 1).toInt();
        while (digitalRead(InputNum) == LOW) {
          delay(100);
        }
        Serial.println("Done");
      }
      //-----COMMAND TO WAIT INPUT OFF-----
      if (function == "WO") {
        int WIstart = inData.indexOf('N');
        int InputNum = inData.substring(WIstart + 1).toInt();

        // String InputStr =  String("Input" + InputNum);
        // uint8_t Input = atoi(InputStr.c_str ());
        while (digitalRead(InputNum) == HIGH) {
          delay(100);
        }
        Serial.println("Done");
      }
      //-----COMMAND ECHO TEST MESSAGE-----
      if (function == "TM") {
        String echo = inData.substring(2);
        Serial.println(echo);
      }

      inData = "";  // Clear received buffer
    }
  }
}