#include <PID_v1.h>


///////////Motor Drivers//////////////////

const int rightMotorELPin = 4;
const int rightMotorSignalPin = 3;
const int rightMotorDirPin = 5;
const int rightMotorPWMPin = 6;

const int leftMotorELPin = 7;
const int leftMotorSignalPin = 2;
const int leftMotorDirPin = 8;
const int leftMotorPWMPin = 9;

double rightMotorPWM = 0;
double rightMotorSpeed = 0;
double rightMotorEncoder = 0;
double setRightMotorSpeed = 0;
unsigned long rightMotorEncoderTime = 0;
unsigned long rightMotorEncoderPvTime = 0;
int rightMotorDistance = 0;

double leftMotorPWM = 0;
double leftMotorSpeed = 0;
double leftMotorEncoder = 0;
double setLeftMotorSpeed = 0;
unsigned long leftMotorEncoderTime = 0;
unsigned long leftMotorEncoderPvTime = 0;
int leftMotorDistance = 0;

//Specify the links and initial tuning parameters
double Kp = 200, Ki = 100.0, Kd = 0.001;
PID rightMotorPID(&rightMotorSpeed, &rightMotorPWM, &setRightMotorSpeed, Kp, Ki, Kd, DIRECT);
PID leftMotorPID(&leftMotorSpeed, &leftMotorPWM, &setLeftMotorSpeed, Kp, Ki, Kd, DIRECT);


////////////ROS///////////





String inputString = "";         // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete

int i = 0;
char Char = "";

void setup()
{




  pinMode(rightMotorELPin , OUTPUT);
  pinMode(rightMotorDirPin , OUTPUT);
  pinMode(rightMotorPWMPin , OUTPUT);

  pinMode(leftMotorELPin , OUTPUT);
  pinMode(leftMotorDirPin , OUTPUT);
  pinMode(leftMotorPWMPin , OUTPUT);

  pinMode(rightMotorSignalPin, INPUT_PULLUP);
  pinMode(leftMotorSignalPin, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(rightMotorSignalPin), readRightMotorEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(leftMotorSignalPin), readLeftMotorEncoder, CHANGE);

  digitalWrite(leftMotorELPin, LOW);
  digitalWrite(rightMotorELPin, LOW);

  //turn the PID on
  rightMotorPID.SetOutputLimits(0, 100);
  rightMotorPID.SetMode(AUTOMATIC);

  leftMotorPID.SetOutputLimits(0, 100);
  leftMotorPID.SetMode(AUTOMATIC);

  // initialize serial:
  Serial.begin(9600);
  // reserve 200 bytes for the inputString:
  inputString.reserve(200);
}

void loop()
{

  if (i == 0) {
    digitalWrite(rightMotorDirPin, LOW);
    digitalWrite(leftMotorDirPin, HIGH);
    digitalWrite(leftMotorELPin, HIGH);
    digitalWrite(rightMotorELPin, HIGH);
    setRightMotorSpeed = 0.2;
    setLeftMotorSpeed = 0.2;
    Serial.println("Forward");
    i = 1;
  }
  if (i == 1 && rightMotorDistance >= 2000) {
    rightMotorDistance = 0;
    setRightMotorSpeed = 0;
    setLeftMotorSpeed = 0;
    i = 2;
  }
  if ( i == 2 ) {
    digitalWrite(rightMotorDirPin, LOW);
    digitalWrite(leftMotorDirPin, LOW);
    digitalWrite(leftMotorELPin, HIGH);
    digitalWrite(rightMotorELPin, HIGH);
    setRightMotorSpeed = 0.1;
    setLeftMotorSpeed = 0.1;
    Serial.println("Left");
    i = 3;
  }
  if (i == 3 && rightMotorDistance >= 300) {
    rightMotorDistance = 0;
    setRightMotorSpeed = 0;
    setLeftMotorSpeed = 0;
    i = 4;
  }
  if (i == 4) {
    digitalWrite(rightMotorDirPin, LOW);
    digitalWrite(leftMotorDirPin, HIGH);
    digitalWrite(leftMotorELPin, HIGH);
    digitalWrite(rightMotorELPin, HIGH);
    setRightMotorSpeed = 0.2;
    setLeftMotorSpeed = 0.2;
    Serial.println("Forward");
    i = 5;
  }
  if (i == 5 && rightMotorDistance >= 2000) {
    rightMotorDistance = 0;
    i = 6;
  }
  if (i == 6) {
    digitalWrite(leftMotorELPin, LOW);
    digitalWrite(rightMotorELPin, LOW);
    setRightMotorSpeed = 0;
    setLeftMotorSpeed = 0;
    Serial.println("Stop");
    i = 7;
  }


  Serial. print(i);
  Serial.print ("  ");
  Serial.println(rightMotorDistance);

  if (rightMotorEncoder != 0) {
    rightMotorSpeed = ((rightMotorEncoder * 6) / (rightMotorEncoderTime - rightMotorEncoderPvTime)); // speed in m/s
    rightMotorDistance += rightMotorEncoder * 6;
    rightMotorEncoder = 0;
    rightMotorEncoderPvTime = rightMotorEncoderTime;
  } else {
    rightMotorSpeed = 0;
  }

  if (leftMotorEncoder != 0) {
    leftMotorSpeed = ((leftMotorEncoder * 6) / (leftMotorEncoderTime - leftMotorEncoderPvTime)); // speed in m/s
    leftMotorDistance += leftMotorEncoder * 6;
    leftMotorEncoder = 0;
    leftMotorEncoderPvTime = leftMotorEncoderTime;
  } else {
    leftMotorSpeed = 0;
  }

  rightMotorPID.Compute();
  leftMotorPID.Compute();


  //update new speed
  analogWrite(rightMotorPWMPin, rightMotorPWM);
  analogWrite(leftMotorPWMPin, leftMotorPWM);


  delay(50);
}

void readRightMotorEncoder()
{
  rightMotorEncoder++;
  rightMotorEncoderTime = millis();
}

void readLeftMotorEncoder()
{
  leftMotorEncoder++;
  leftMotorEncoderTime = millis();
}

void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char Char = Serial.read();
    if (Char == 'w') {
      digitalWrite(rightMotorDirPin, LOW);
      digitalWrite(leftMotorDirPin, HIGH);
      digitalWrite(leftMotorELPin, HIGH);
      digitalWrite(rightMotorELPin, HIGH);
      setRightMotorSpeed = 0.2;
      setLeftMotorSpeed = 0.2;
      Serial.println("Forward");
    } else if (Char == 'x') {
      digitalWrite(rightMotorDirPin, HIGH);
      digitalWrite(leftMotorDirPin, LOW);
      digitalWrite(leftMotorELPin, HIGH);
      digitalWrite(rightMotorELPin, HIGH);
      setRightMotorSpeed = 0.2;
      setLeftMotorSpeed = 0.2;
      Serial.println("Backword");
    } else if (Char == 's') {
      digitalWrite(leftMotorELPin, LOW);
      digitalWrite(rightMotorELPin, LOW);
      setRightMotorSpeed = 0;
      setLeftMotorSpeed = 0;
      Serial.println("Stop");
    } else if (Char == 'a') {
      digitalWrite(rightMotorDirPin, LOW);
      digitalWrite(leftMotorDirPin, LOW);
      digitalWrite(leftMotorELPin, HIGH);
      digitalWrite(rightMotorELPin, HIGH);
      setRightMotorSpeed = 0.1;
      setLeftMotorSpeed = 0.1;
      Serial.println("Left");
    } else if (Char == 'd') {
      digitalWrite(rightMotorDirPin, HIGH);
      digitalWrite(leftMotorDirPin, HIGH);
      digitalWrite(leftMotorELPin, HIGH);
      digitalWrite(rightMotorELPin, HIGH);
      setRightMotorSpeed = 0.1;
      setLeftMotorSpeed = 0.1;
      Serial.println("Right");
    }
  }
}