// blade moter pins
const byte BladeRightPwm = 5; // not hooked up
const byte BladeRight = 4; // direction enable
const byte BladeLeft = 7; 
const byte BladeLeftPwm = 6; // not hooked up

// Analogue Inputs
const byte bladeCurrentPin = A2;   // 0 to 1024 simulates message recieved
const byte driveCurrentPin = A1; 
const byte batteryVoltagePin = A0; 

const byte pwmOutOne = 11; // ununsed pwm out
//Wheels
//need two pwm outputs and two direction pins
const byte WheelLeftPwm = 9;  // servo pin 1 0 to 255
const byte WheelRightPwm = 10;  // servo
const byte WheelLeft = 8;  // LED connected to digital pin 8
const byte WheelRight = 12; // LED connected to digital pin 12
const byte WheelLeftTick =2;  // only two and three cam be interupts
const byte WheelRightTick = 3;
//5, 6, 9, 10, 11 are pwm output pins
//pins 5 and 6: 980 Hz

char Motor = 'A';
char Direction ='+';
volatile byte WheelDirection = LOW;
const byte serialRate = 9600;
volatile byte BladeDirection = LOW;
const int UPDATE_DELAY = 1000; 
unsigned long lastUpdate = 0;       // Last Sensor Msg Update time

int speed = 0;                    // variable to store temp

//Debonce time in ms
const int DEBOUNCE_DELAY = 250;   // increase if the output flickers
unsigned long lastDebounceTimeLeft = 0;  // the last time the output pin was toggled
unsigned long lastDebounceTimeRight = 0;  // the last time the output pin was toggled
String leftTickMsg = "LeftTick:1";
String rightTickMsg = "RightTick:1";


//Serial Handling
String inputString = "";      // a String to hold incoming data

bool stringComplete = false;  // whether the string is complete
int isForward = HIGH; // Direction Flag
bool isLeft = true; // Motor Flag
bool driveReady = false;
int driveValue = 0;


void setup() {

  pinMode(WheelRight, OUTPUT);  // sets the pin as output
  pinMode(WheelLeft, OUTPUT);  // sets the pin as output
  pinMode(BladeLeft, OUTPUT);
  pinMode(BladeRight, OUTPUT);

  pinMode(WheelLeft, OUTPUT);  // sets the pin as output
  pinMode(WheelRight, OUTPUT);  // sets the pin as output
  pinMode(WheelLeftTick, INPUT_PULLUP); //pin seven
  pinMode(WheelRightTick, INPUT_PULLUP); //pin eight 
  attachInterrupt(digitalPinToInterrupt(WheelLeftTick), tickLeft, RISING);
  attachInterrupt(digitalPinToInterrupt(WheelRightTick), tickRight, RISING);
  lastUpdate= millis();
  Serial.begin(9600);
  inputString.reserve(20);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB
  }
  
}

void loop() {
 
  if ((millis() - lastUpdate) > UPDATE_DELAY){
    // sending the latest voltage and current readings
    //Serial.println("BladeCurrent:"+String(analogRead(bladeCurrentPin)));
    //Serial.println("WheelCurrent:"+String(analogRead(driveCurrentPin)));
    //Serial.println("BatteryVoltage:"+String(analogRead(batteryVoltagePin)));
    //Serial.println("Status: "+String(speed));
    lastUpdate = millis();
  }
  if (stringComplete) {
    // clear the string:
    inputString = "";
    stringComplete = false;
  }

}

void tickLeft() {
  if ((millis() - lastDebounceTimeLeft) > DEBOUNCE_DELAY) {
    Serial.println(leftTickMsg);
    lastDebounceTimeLeft = millis();
  }
}

void tickRight() {
  
  if ((millis() - lastDebounceTimeRight) > DEBOUNCE_DELAY) {
    Serial.println(rightTickMsg);
    lastDebounceTimeRight = millis();
  }
}

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    if (isDigit(inChar) ) {
      // add it to the string if it is number:
      inputString += inChar;
    } else {
      if (inChar == 'L'){Motor = inChar;} 
      if (inChar == 'R') {Motor = inChar;}
      if (inChar == 'B'){Motor = inChar;}
      if (inChar=='+'){Direction = inChar;} 
      if (inChar=='-'){Direction = inChar;}
    }
    
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:

    if (inChar == '\n') {
      speed = inputString.toInt();
      if (Direction == '+'){
        WheelDirection = HIGH;
      } else{
        WheelDirection = LOW;
      }
      if (Motor=='L'){
        Serial.println("LEFT:"+String(Direction)+":"+String(speed));
        digitalWrite(WheelLeft, ! WheelDirection);
        analogWrite(WheelLeftPwm, speed); // analogWrite values from 0 to 255
      } 
      if (Motor=='R') {
        Serial.println("RIGHT:"+String(Direction)+":"+String(speed));
        digitalWrite(WheelRight, WheelDirection);
        analogWrite(WheelRightPwm, speed);
      }
      if (Motor=='B') {
        Serial.println("BLADE:"+String(Direction)+":"+String(speed));
        if (Direction=='+'){
            BladeDirection = 1;
        } else {
          BladeDirection = 0;
        }
        if (speed>0){
          digitalWrite(BladeRight, BladeDirection);
          digitalWrite(BladeLeft, ! BladeDirection);
          if (BladeDirection == 1){
            analogWrite(BladeRightPwm, speed);
            analogWrite(BladeLeftPwm, 0);   
          } else {
            analogWrite(BladeRightPwm,0);
            analogWrite(BladeLeftPwm, speed);             
          }
       
        } else {
          digitalWrite(BladeRightPwm, 0);
          digitalWrite(BladeLeftPwm, 0);
          digitalWrite(BladeRight, 0);
          digitalWrite(BladeLeft, 0);          
        }
      }
      //Best to flag this condition at the end the handler
      //so that the conversions etc are done first  
      stringComplete = true;
    }
  }
}