#include <Servo.h>

Servo IRcontrol; //creates servo object

//CHANNEL A: LEFT SIDE
//CHANNEL B: RIGHT SIDE

int AD = 12;  //channel A direction
int AB = 9;   //channel A brake
int AS = 3;   //channel A speed
int BD = 13;  //channel B direction
int BB = 8;   //channel B brake
int BS = 11;  //channel B speed
int LS = 7;   //left switch
int RS = 6;   //right switch
int QT = 500; //quarter turn (500 ticks)  TEST
int HT = 1000;//half turn (1000 ticks)    TEST
int RT = 750; //reverse time for turns
int RevS = 100;//reverse speed
int TS = 255; //turn speed
int RL = 5;   // right LED
int LL = 4;    //left LED
int POT = 2;  //potentiometer
int TD = 0;   //turn dependency
int TT = 0;   //turn time
int MCS = 2;  //motor control switch
int servopot = 2; //servo position control
int servopos =0;
int IRsen = 4; //IR sensor

void setup(){
  Serial.begin(9600);//for debugging
  IRcontrol.attach(17);  //assigns pin to servo object
  pinMode(LS, INPUT);
  pinMode(RS, INPUT);
  pinMode(RL, OUTPUT);
  pinMode(LL, OUTPUT);
  pinMode(MCS, INPUT);
  pinMode(IRsen, INPUT);

  //Channel A
  pinMode(AD, OUTPUT);
  pinMode(AB, OUTPUT);
  pinMode(AS, OUTPUT);
  //Channel B
  pinMode(BD, OUTPUT);
  pinMode(BB, OUTPUT);
  pinMode(BS, OUTPUT);
}

void loop()
{
  int IRval = analogRead(IRsen);
  int obsdist = pow((2667.7/IRval),1.335113485); //converts ir sensor input to distance value
  int RSS = digitalRead(RS); //Right switch state
  int LSS = digitalRead(LS); //Left switch state
  int MCSS = digitalRead(MCS); //motor control switch state
  Serial.println(servopos);//debug  
 
// BELOW HERE IS NOT WORKING
 
  if(MCSS == HIGH)
  {
    for(servopos = 0; servopos<180; servopos +=1)
    {
      IRcontrol.write(servopos);
      forwardmove(75);
      if(LSS == HIGH)reactleft(QT);
      if(RSS == HIGH)reactright(QT);
      if(obsdist<=25)
      {
        if(servopos<90)
        {
          TT = map(servopos,0,90, 0,500);
          reactright(TT);
        }
        if(servopos>90)
        {
          TT = map(servopos,180,90, 0,500);
          reactleft(TT);
        }
      
      }
      delay(10);
      
    }

// ABOVE HERE IS NOT WORKING   


// works with no IR
//    forwardmove(100);
//    if(LSS == HIGH)reactleft(QT);
//    if(RSS == HIGH)reactright(QT);


  }
  else if(MCSS==LOW)
  {
    forwardmove(0);
    int servopotval = analogRead(servopot);
    int servopos = map(servopotval, 0 ,1023, 1, 179);
    IRcontrol.write(servopos);
    delay(5);
    if(obsdist<25){
      digitalWrite(LL, HIGH);
      digitalWrite(RL, HIGH);
    }
  }
}

int forwardmove(int movespeed){
  digitalWrite(AD, HIGH);
  digitalWrite(BD, HIGH);
  digitalWrite(AB, LOW);
  digitalWrite(BB, LOW);
  analogWrite(AS, movespeed);
  analogWrite(BS, movespeed);
  digitalWrite(LL, LOW);
  digitalWrite(RL, LOW);
}

int reactleft(int TT){//TA is turn angle
  digitalWrite(AD, LOW);
  digitalWrite(BD, LOW);
  digitalWrite(AB, LOW);
  digitalWrite(BB, LOW);
  analogWrite(AS, RevS);
  analogWrite(BS, RevS);
  digitalWrite(LL, HIGH);
  delay(RT);
  digitalWrite(AB, LOW);
  digitalWrite(BB, LOW);
  digitalWrite(AD, HIGH);
  digitalWrite(BD, LOW);
  analogWrite(AS, TS);
  analogWrite(BS, TS);
  delay(TT);
}

int reactright(int TT){//TA is turn angle
  digitalWrite(AD, LOW);
  digitalWrite(BD, LOW);
  digitalWrite(AB, LOW);
  digitalWrite(BB, LOW);
  analogWrite(AS, RevS);
  analogWrite(BS, RevS);
  digitalWrite(RL, HIGH);
  delay(RT);
  digitalWrite(AB, LOW);
  digitalWrite(BB, LOW);
  digitalWrite(AD, LOW);
  digitalWrite(BD, HIGH);
  analogWrite(AS, TS);
  analogWrite(BS, TS);
  delay(TT);
}