#define INPUT1_PIN 6   // PWMB
#define INPUT2_PIN 10  // DIRB  ---  right
#define INPUT4_PIN 9   // PWMA
#define INPUT3_PIN 5   // DIRA  ---  left

byte comdata;
String readString;
int driveSpeed = 100;
int driveDuration = 500;
long int stop1;

enum DM
{
  DRIVE,
  TURN,
  STOP,
  PAUSE
}Drive_Mode=PAUSE;

enum DIR
{
  FORWARD,
  BACKWARD
}Drive_Direction=FORWARD;

enum TURN
{
  LEFT,
  RIGHT
}Turn_Direction=LEFT;


void go_Left(int val){
  analogWrite(INPUT1_PIN, LOW);
  analogWrite(INPUT2_PIN, val); //the speed value of motorA is val
  analogWrite(INPUT3_PIN, LOW);
  analogWrite(INPUT4_PIN, val); //the speed value of motorA is val
}

void go_Right(int val){
  analogWrite(INPUT1_PIN, val);
  analogWrite(INPUT2_PIN, LOW); //the speed value of motorA is val
  analogWrite(INPUT3_PIN, val);
  analogWrite(INPUT4_PIN, LOW); //the speed value of motorA is val
}

void go_Advance(int val){
  analogWrite(INPUT1_PIN, val);
  analogWrite(INPUT2_PIN, LOW); //the speed value of motorA is val
  analogWrite(INPUT3_PIN, LOW);
  analogWrite(INPUT4_PIN, val); //the speed value of motorA is val
}

void go_Back(int val){
  analogWrite(INPUT1_PIN, LOW);
  analogWrite(INPUT2_PIN, val); //the speed value of motorA is val
  analogWrite(INPUT3_PIN, val);
  analogWrite(INPUT4_PIN, LOW); //the speed value of motorA is val
}

void stop_Stop(){
  analogWrite(INPUT1_PIN, LOW);
  analogWrite(INPUT2_PIN, LOW); //the speed value of motorA is val
  analogWrite(INPUT3_PIN, LOW);
  analogWrite(INPUT4_PIN, LOW); //the speed value of motorA is val
}



void lets_Drive()
{
    switch (Drive_Mode)
    {
      case DRIVE:
          switch (Drive_Direction){
            case FORWARD:
              go_Advance(driveSpeed);
              break;
            case BACKWARD:
              go_Back(driveSpeed);
              break;
            default:
              break;
          }
          Serial.println("DRIVE");
          break;
      case TURN:
          switch (Turn_Direction){
            case LEFT:
              go_Left(driveSpeed);
              break;
            case RIGHT:
              go_Right(driveSpeed);
              break;
            default:
              break;
          }
          Serial.println("TURN");
          break;
      case STOP:
          stop_Stop();
          Serial.println("STOP");
          break;
       default:
          break;

    }
}


void setup()
 {
     Serial.begin(9600);
     Serial.println("JDY-16 Test");

     pinMode(INPUT1_PIN, OUTPUT);
     digitalWrite(INPUT1_PIN, LOW); // When not sending PWM, we want it low
     pinMode(INPUT2_PIN, OUTPUT);
     digitalWrite(INPUT2_PIN, LOW); // When not sending PWM, we want it low
     pinMode(INPUT3_PIN, OUTPUT);
     digitalWrite(INPUT3_PIN, LOW); // When not sending PWM, we want it low
     pinMode(INPUT4_PIN, OUTPUT);
     digitalWrite(INPUT4_PIN, LOW);
 }


boolean speedData = false;
String speedString;
boolean durationData = false;
String durationString;

void loop()
 {

   while (Serial.available()) {
    comdata = Serial.read();  //gets one byte from serial buffer
    readString += (char)comdata; //makes the String readString
    //Serial.println(comdata);
    delay(2);  //slow looping to allow buffer to fill with next character
  }



         if(readString.length() > 9){
          //if(readString.length() > 0){
          Serial.println("Data received as <> ");
          Serial.println(readString);
            switch (readString.charAt(2))
            {
              case 'D':Drive_Mode=DRIVE;Serial.println("Drive Mode");break;
              case 'T':Drive_Mode=TURN;Serial.println("Turn Mode");break;
              case 'S':Drive_Mode=STOP;Serial.println("Stop Mode:");break;
              default:break;
            }

         if(readString.charAt(2) != 'S'){
            speedData = true;
            durationData = true;
            Serial.println("Not stop mode:");
            switch (readString.charAt(6))
            {
              case 'F':Drive_Direction=FORWARD;Serial.println("Forward Direction");break;
              case 'B':Drive_Direction=BACKWARD;Serial.println("Backward Direction");break;
              case 'L':Turn_Direction=LEFT;Serial.println("Left Direction");break;
              case 'R':Turn_Direction=RIGHT;Serial.println("Right Direction");break;
              default:break;
            }

//            int speedIndex;
//            for (int x = 10; x < readString.length(); x++)
//            {
//              if (speedData) {
//                speedString += readString.charAt(x);
//                if(x == (readString.length()-1)){
//                  speedData = false;
//                  driveSpeed = speedString.toInt();
//                  Serial.println("Speed: ");
//                  Serial.print(driveSpeed);
//                  Serial.println("");
//                  speedString = "";
//                }
//              }
//            }


            int speedIndex;
            for (int x = 10; x < readString.length(); x++)
            {
              if (speedData) {
                if(isDigit(readString.charAt(x))){
                  speedString += readString.charAt(x);
                }else{
                  speedData = false;
                  driveSpeed = speedString.toInt();
                  Serial.println("Drive Speed: ");
                  Serial.println(driveSpeed);
                  speedIndex = x+2;
                  speedString = "";
                }
              }
            }
            Serial.println("speedIndex is ");
            Serial.println(speedIndex);

            Serial.println("total received string length");
            Serial.println(readString.length());

            for (int x = speedIndex; x < readString.length(); x++)
            {
              if (durationData) {
                if(isDigit(readString.charAt(x))){
                  durationString += readString.charAt(x);
                }
                if(readString.length() == (x+1)){
                  durationData = false;
                  driveDuration = durationString.toInt();
                  Serial.println("Drive Duration: ");
                  Serial.println(driveDuration);
                  if(driveDuration > 0){
                    stop1=millis()+driveDuration;
                  }
                  durationString = "";
                }
              }
            }



         }
          readString="";
         lets_Drive();
         }

         if(driveDuration > 0 && stop1 > 0){
            if (millis()>=stop1) {
              Serial.println("Timer over, stoping the car!");
              stop_Stop();
            }
         }


     }