//int ir[8] = { A2, A3, A4, A5, A6, A7, A8, A9 };
int ir[8] = { A9, A8, A7, A6, A5, A4, A3, A2 };
int fc = A10;
int INA[5] = {  5,   6,  14,  18,  20};
int INB[5] = {  4,   3,   2,  17,  21};
int EN[5]  = {  8,   7,  15,  16,  19};
int CS[5]  = {A11, A12, A13, A14, A15};
int PWM[5] = { 13,  12,  11,  10,   9};
int ir_value[8] = {0, 0, 0, 0, 0, 0, 0, 0};


#define BRAKE 0
#define CW    1
#define CCW   2
#define CS_THRESHOLD 15   // Definition of safety current (Check: "1.3 Monster Shield Example").
#define N    500

#define PWM_MOTOR_1 13
#define PWM_MOTOR_2 12

#define MOTOR_1 0
#define MOTOR_2 1

#define debug false

short usSpeed = 150;  //default motor speed
unsigned short usMotor_Status = BRAKE;

bool avanza_state = false;
bool retrocede_state = false;

void setup() { 
  pinMode(fc, INPUT);
  for(int i=0; i<5; i++)
    {
      pinMode(INA[i], OUTPUT);
      pinMode(INB[i], OUTPUT);
      pinMode(EN[i], OUTPUT);
      digitalWrite(EN[i], HIGH);
      //pinMode(CS[i], INPUT);
    }
    
  Serial.begin(115200);              // Initiates the serial to do the monitoring 
  Serial.println("Begin motor control");
  Serial.println(); //Print function list for user selection
  Serial.println("Enter number for control option:");
  Serial.println("1. STOP");
  Serial.println("2. FORWARD");
  Serial.println("3. REVERSE");
  Serial.println("4. READ CURRENT");
  Serial.println("+. INCREASE SPEED");
  Serial.println("-. DECREASE SPEED");
  Serial.println("q. FRONT");
  Serial.println("w. AVANZA");
  Serial.println("s. RETROCEDE");
  Serial.println("a. IZQUIERDA");
  Serial.println("d. DERECHA");
  Serial.println();
  //delay(4000);
}

//ir 0 frontal
//ir 1 frontal izquierdo
//ir 2 izquierdo
//ir 3 trasero izquierdo
//ir 4 trasero
//ir 5 trasero derecho
//ir 6 derecho
//ir 7 frontal derecho
#define LIMIT 600
#define LIMIT_THRESHOLD 10

void loop() {

  
  //if(!digitalRead(fc)) motorGo(4, BRAKE, 0);
  
  manual_control();
  ir_read();
  stop_move();
  search();
//  reposo();
  
}

void stop_move()
  {
    if ((ir_value[0]<=LIMIT)&&(avanza_state))
     {
       motorGo(0, BRAKE, 0);
       motorGo(1, BRAKE, 0);
       avanza_state = false;
     }
    if ((ir_value[4]<=LIMIT)&&(retrocede_state))
     {
       motorGo(0, BRAKE, 0);
       motorGo(1, BRAKE, 0);
       retrocede_state = false;
     }
  }

unsigned long time_escape = millis();
unsigned long time_stop = millis();
bool stop_time = false;
bool stop_escape = false;

void search()
  {
//    if (((millis()-time_escape)<4000)&&(!stop_time)) stop_escape = false;
//    else stop_escape = true;
//    
//    if (((millis()-time_stop)<5000)&&(stop_escape)) stop_time = true;
//    else
//      {
//        time_escape = millis();
//        stop_time = false;
//      }
//       
//    for (int i=0; i<8; i++)
//      {
//        if ((!equal_max(ir_value[i]))&&(stop_escape))
//          {
//            Serial.println("En espera");
//            stop_time=true;
//            time_stop = millis();
//            break;
//          }
//      }
//      
//    if ((!equal_max(ir_value[0])&&((!stop_escape)||(!stop_time)))) 
//      {       
//        avanza(100);
//      }
//    else Stop();
//      
//    //else if ((equal_max(ir_value[0]))&&((millis()-time_escape)>=10000)) retrocede(100);
  }
  
void reposo()
  {
    if (!equal_max(ir_value[0])&&(equal_max(ir_value[4]))) retrocede(100);
    else if((equal_max(ir_value[0]))&&(!equal_max(ir_value[4]))||(!equal_max(ir_value[3]))||(!equal_max(ir_value[5]))) avanza(100);
    else if((!equal_max(ir_value[1]))||(!equal_max(ir_value[2]))) derecha(100);
    else if((!equal_max(ir_value[6]))||(!equal_max(ir_value[7]))) izquierda(100);
    else Stop();
  }

bool equal_max(int value)
  {
    if ((value>=(LIMIT - LIMIT_THRESHOLD))&&(value<=(LIMIT + LIMIT_THRESHOLD))||(value<=(LIMIT - LIMIT_THRESHOLD)))
      return true;
    else 
      return false;
  }

//bool search_min(int value)
//  {
//    if ((value>=(LIMIT - LIMIT_THRESHOLD))&&(value<=(LIMIT + LIMIT_THRESHOLD)))
//      return true;
//    else 
//      return false;
//  }

//bool equal_min(int value)
//  {
//    if ((value<=(LIMIT - LIMIT_THRESHOLD))&&((value>=(LIMIT + LIMIT_THRESHOLD))))
//      return true;
//    else 
//      return false;
//  }
  
void manual_control()
  {
   char user_input;   
   while(Serial.available())
    {
      user_input = Serial.read(); //Read user input and trigger appropriate function
      if (user_input =='1')
      {
         Stop();
      }
      else if(user_input =='2')
      {
        Forward();
      }
      else if(user_input =='3')
      {
        Reverse();
      }
      else if(user_input =='4')
      {
        for(int i=0; i<5; i++)
          {
            float voltage = 0;
            for(int j=0; j<N; j++)
              {
                voltage = analogRead(CS[i])*(5000/1023.) + voltage;
              }
            Serial.print(voltage/(N*0.13));
            Serial.print(' ');
          }   
        Serial.println();
      }
      else if(user_input =='+')
      {
        IncreaseSpeed();
      }
      else if(user_input =='-')
      {
        DecreaseSpeed();
      }
      else if(user_input =='q')
      {
        front(100);
      }
      else if(user_input =='w')
      {
        avanza(100);
      }
      else if(user_input =='s')
      {
        retrocede(100);
      }
          else if(user_input =='a')
      {
        izquierda(100);
      }
      else if(user_input =='d')
      {
        derecha(100);
      }
      else
      {
        Serial.println("Invalid option entered.");
      }
        
    }
  }
//convert voltage to millimeters
int convertIRvoltsToMM(float v) { 
    float value = -0.00003983993846*v*v*v+ 0.0456899769 *v*v - 17.48535575 * v + 2571.052715; 
    if (value < 200) value = 200;
    else if (value > 1500) value = 1500;
    return value;
}

#define N 1
float average(int pin)
  {
    float val = 0;
    for(int i=0; i<N; i++) 
      {
        val = val + analogRead(ir[pin]);
        delay(10);
      }
    
    return convertIRvoltsToMM(val/N);
  }


void ir_read() 
  {
    //int val = 0;
    for(int i=0; i<8; i++)
      {
        ir_value[i] = average(i);
        #if debug
          Serial.print(" ");
          if (ir_value[i]<10) Serial.print("   ");
          else if (ir_value[i]<100) Serial.print("  ");
          else if (ir_value[i]<1000) Serial.print(" ");
          Serial.print(ir_value[i]);
        #endif
      }
    #if debug
      Serial.println();
    #endif
  }
  
void Stop()
{
  #if debug
     Serial.println("Stop");
  #endif
  usMotor_Status = BRAKE;
  for(int i=0; i<5; i++) motorGo(i, usMotor_Status, 0);
}

void Forward()
{
  Serial.println("Forward");
  usMotor_Status = CW;
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
}

void Reverse()
{
  Serial.println("Reverse");
  usMotor_Status = CCW;
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
}

void IncreaseSpeed()
{
  usSpeed = usSpeed + 10;
  if(usSpeed > 255)
  {
    usSpeed = 255;  
  }
  
  Serial.print("Speed +: ");
  Serial.println(usSpeed);
  
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
}

void DecreaseSpeed()
{
  usSpeed = usSpeed - 10;
  if(usSpeed < 0)
  {
    usSpeed = 0;  
  }
  
  Serial.print("Speed -: ");
  Serial.println(usSpeed);

  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed); 
}

void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
    if(direct == CW)
    {
      digitalWrite(INA[motor], LOW); 
      digitalWrite(INB[motor], HIGH);
    }
    else if(direct == CCW)
    {
      digitalWrite(INA[motor], HIGH);
      digitalWrite(INB[motor], LOW);      
    }
    else
    {
      digitalWrite(INA[motor], LOW);
      digitalWrite(INB[motor], LOW);            
    }
    analogWrite(PWM[motor], pwm);
}

void front(uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
   Serial.println("Front");
   while(digitalRead(fc)) motorGo(4, CW, pwm);
   motorGo(4, BRAKE, 0);
}

void avanza(uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
  if (ir_value[0]>LIMIT)
   {
     Serial.println("Avanza");
     motorGo(0, CCW, pwm);
     motorGo(1, CCW, pwm);
     if (pwm>0) avanza_state = true;
     else avanza_state = false;
   }
  else 
   {
     Serial.println("Stop Avanza");
     motorGo(0, BRAKE, 0);
     motorGo(1, BRAKE, 0);
     avanza_state = false;
   }

}

void retrocede(uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
   if (ir_value[4]>LIMIT)
    {
      Serial.println("Retrocede");
      motorGo(0, CW, pwm);
      motorGo(1, CW, pwm);
      if (pwm>0) retrocede_state = true;
      else retrocede_state = false;
    }
   else 
    {
      Serial.println("Stop Retrocede");
      motorGo(0, BRAKE, 0);
      motorGo(1, BRAKE, 0);
      retrocede_state = false;
    }
}

void izquierda(uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
   Serial.println("Izquierda");
   motorGo(0, CW, pwm);
   motorGo(1, CCW, pwm);
}

void derecha(uint8_t pwm)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
   Serial.println("Derecha");
   motorGo(0, CCW, pwm);
   motorGo(1, CW, pwm);
}