//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};
int ir_value_ant[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     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 
  #if debug
    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();
  #endif
}

//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 300
#define LIMIT_THRESHOLD 10

void loop() {

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

  void stop_move()
  {
    if ((ir_value[0]<=LIMIT)&&(avanza_state))
     {
       motorGo(0, BRAKE, 0);
       motorGo(1, BRAKE, 0);
       avanza_state = false;
       #if debug
        Serial.println("Stop Avanza");
       #endif
     }
    if ((ir_value[4]<=LIMIT)&&(retrocede_state))
     {
       motorGo(0, BRAKE, 0);
       motorGo(1, BRAKE, 0);
       retrocede_state = false;
       #if debug
        Serial.println("Stop Retrocede");
       #endif
     }
  }
  
void manual_control()
  {
   char user_input;   
   while(Serial.available())
    {
      user_input = Serial.read(); //Read user input and trigger appropriate function
      if (user_input =='p')
      {
         Stop();
         Serial.write(user_input); 
      }
      else if(user_input =='j')
      {
        Forward();
        Serial.write(user_input); 
      }
      else if(user_input =='k')
      {
        Reverse();
        Serial.write(user_input); 
      }
      else if(user_input =='l')
      {
        for(int i=0; i<5; i++)
          {
            float voltage = 0;
            for(int j=0; j<N; j++)
              {
                voltage = analogRead(CS[i])*(5000/1023.) + voltage;
              }
            #if debug
              Serial.print(voltage/(N*0.13));
              Serial.print(' ');
            #endif
          }   
          Serial.write(user_input); 
        #if debug
          Serial.println();
        #endif
      }
      else if(user_input =='+')
      {
        IncreaseSpeed();
        Serial.write(user_input); 
      }
      else if(user_input =='-')
      {
        DecreaseSpeed();
        Serial.write(user_input); 
      }
      else if(user_input =='q')
      {
        front(100);
        Serial.write(user_input); 
      }
      else if(user_input =='w')
      {
        avanza(100);
        Serial.write(user_input); 
      }
      else if(user_input =='s')
      {
        retrocede(100);
        Serial.write(user_input); 
      }
          else if(user_input =='a')
      {
        izquierda(100);
        Serial.write(user_input); 
      }
      else if(user_input =='d')
      {
        derecha(100);
        Serial.write(user_input); 
      }
      else
      {
        Serial.print('?');
      }
    }
  }
//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;
}

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

#define RES 50
void ir_read() 
  {
    for(int i=0; i<8; i++)
      {
        ir_value[i] = average(i);
        if ((ir_value[i]<=(ir_value_ant[i]-RES))||(ir_value[i]>=(ir_value_ant[i]+RES)))
          {
            ir_value_ant[i]= ir_value[i];
            Serial.print("IR");
            Serial.print(i);
            Serial.print(":");
            Serial.println(ir_value[i]);
          }
      }
  }
  
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()
{
  #if debug
    Serial.println("Forward");
  #endif
  usMotor_Status = CW;
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
}

void Reverse()
{
  #if debug
    Serial.println("Reverse");
  #endif
  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;  
  }
  #if debug
    Serial.print("Speed +: ");
    Serial.println(usSpeed);
  #endif
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
}

void DecreaseSpeed()
{
  usSpeed = usSpeed - 10;
  if(usSpeed < 0)
  {
    usSpeed = 0;  
  }
  #if debug
    Serial.print("Speed -: ");
    Serial.println(usSpeed);
  #endif
  for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed); 
}

void motorGo(uint8_t motor, uint8_t direct, uint8_t value)         //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (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], value);
}

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

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

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

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

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