#define ENA           5 //must be PWM (~)
#define IN1           2
#define IN2           3
#define IN3           4
#define IN4           7
#define ENB           6 //must be PWM (~)
#define SensorLeft    9
#define SensorMiddle  10
#define SensorRight   11
#define redLED        12

int mSpeed = 150; //0-255
int flag = 0; // Start/Close | ON/OFF
int L = 0;    // Move/Stop

byte com = 0; //reply from voice recognition

unsigned char SL;// Left   Sensor Value | 1 = Black | 0 = White
unsigned char SM;// Middle Sensor Value | 1 = Black | 0 = White
unsigned char SR;// Right  Sensor Value | 1 = Black | 0 = White

void setup() {
  Serial.begin(9600);
  IO_config();
  stopp();
  delay(5000);
  Serial.write(0xAA);
  Serial.write(0x37);//Switch to Compact Mode
  delay(2000);
  Serial.write(0xAA);
  Serial.write(0x23);//Import group 3 and be ready for voice instruction
  delay(2000);
}

void loop() {

  Sensor_Scan();
  if (flag == 1 && L == 1) {
    followLine();
  }
  while (Serial.available())
  {
    com = Serial.read();

    switch (com)
    {
      case 0x11:
        //START/ON
        flag = 1;
        digitalWrite(redLED, 1);
        break;
      case 0x12:
        //CLOSE/OFF
        flag = 0;
        L = 0;
        digitalWrite(redLED, 0);
        break;
      case 0x13:
        //MOVE
        if (flag == 1) {
          L = 1;
          //Sensor_Scan();
          //followLine();
        }
        break;
      case 0x14:
        //STOP
        L = 0;
        stopp();
        break;
      case 0x15:
        //G
        stopp();
        break;
    }// end of SWITCH
  }

}// end of LOOP

void right() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
}

void left() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

void front() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

void back() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
}

void stopp() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 0);
}


void IO_config(void) {
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(redLED, OUTPUT);
  digitalWrite(redLED, 0);
  analogWrite(ENA, mSpeed); //0-255
  analogWrite(ENB, mSpeed); //0-255
}

void Sensor_Scan(void) {
  SL = digitalRead(SensorLeft);
  SM = digitalRead(SensorMiddle);
  SR = digitalRead(SensorRight);
}

void followLine() {
  ///////////////////////////////////////////////////////////////////////////////
  if (SM == 1)// middle sensor in black area
  {
    if (SL == 0 & SR == 1) // black on left, white on right, turn left
    {
      right();
    }
    else if (SR == 0 & SL == 1) // white on left, black on right, turn right
    {
      left();
    }
    else if (SR == 1 & SL == 1) {// all sensors BLACK
      stopp() ;
    }
    else // white on both sides, going forward
    {
      front();
    }
  }
  ///////////////////////////////////////////////////////////////////////////////
  else // middle sensor on white area
  {
    if (SL == 0 & SR == 1) // black on left, white on right, turn left
    {
      right();
    }
    else if (SR == 0 & SL == 1) // white on left, black on right, turn right
    {
      left();
    }
    else // all white or counter is equal to 7, stop
    {
      stopp() ;
    }
  }
  ///////////////////////////////////////////////////////////////////////////////
}//end of function