#include <SoftwareSerial.h>
SoftwareSerial mySerial(13, 12); // RX, TX
#define MAX_BUF   64 
char  serialBuffer[MAX_BUF];  // where we store the message until we get a newline
int   sofar;            // how much is in the buffer
#include <EEPROM.h>

float KP;
float KI;
float KD;
bool modoseguidor = false;
unsigned long tiempoAnterior;

#include <QTRSensors.h>
#define NUM_SENSORS   8     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   18
#define M1 160  // para que vaya mas rapido en las rectas o en control bajar  parametros M1 y M2
#define M2 160

QTRSensorsRC qtrrc((unsigned char[]) {7,8,11,12, 17, 16, 15, 14},NUM_SENSORS, TIMEOUT, EMITTER_PIN); 
unsigned int sensorValues[NUM_SENSORS];
int lastError = 0;
int in1Pin = 5;  //Ain1  5
int in2Pin = 6; //Ain2  6 
int in3Pin = 10; //Bin1  10
int in4Pin = 9;  //Bin2    9
int m1Speed;  
int m2Speed;
int errorant;
int Dif;
int suma;




void setup()
{
mySerial.begin(9600);

Serial.begin(9600);

delay(1000);

pinMode(in1Pin, OUTPUT);   //5  Ain1
pinMode(in2Pin, OUTPUT);   //6  Ain2
pinMode(in3Pin, OUTPUT);  // 10  Bin1
pinMode(in4Pin, OUTPUT);//    9  Bin2 
digitalWrite(in1Pin,LOW);  //motor 1
digitalWrite(in2Pin,LOW);
digitalWrite(in3Pin,LOW);
digitalWrite(in4Pin,LOW);
  delay(500);
  int i;
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);    // turn on LED to indicate we are in calibration mode
  for (i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }
  digitalWrite(13, LOW); 


tiempoAnterior = millis();
}
void loop(){
  if (mySerial.available()) {
   Serial.write(mySerial.read());
   char c=mySerial.read();  // get it
 if(sofar<MAX_BUF-1) serialBuffer[sofar++]=c;  // store it
   
    if((c=='\n') || (c == '\r')) {
           // entire message received
      serialBuffer[sofar]=0;  // end the buffer so string functions work right
         processCommand();  // do something with the command
        sofar=0;

}
  
  }
if (Serial.available()) {

        char c=Serial.read();  // get it
  //mySerial.write(c);
  
    Serial.print(c);  // repeat it back so I know you got the message
     if(sofar<MAX_BUF-1) serialBuffer[sofar++]=c;  // store it
   
    if((c=='\n') || (c == '\r')) {
           // entire message received
      serialBuffer[sofar]=0;  // end the buffer so string functions work right
         processCommand();  // do something with the command
        sofar=0;
     }
    
  } 
   
  

  /*if( (millis()- tiempoAnterior) >=  10000){
    
    tiempoAnterior = millis();
  }*/
}


float parseNumber(char code,float val) {
  char *ptr=serialBuffer;  // start at the beginning of buffer
  while((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer+sofar) {  // walk to the end
    if(*ptr==code) {  // if you find code on your walk,
      return atof(ptr+1);  // convert the digits that follow into a float and return it
    }
    ptr=strchr(ptr,' ')+1;  // take a step from here to the letter after the next space
  }
  return val;  // end reached, nothing found, return default val.
}

void processCommand() {
   int cmd = parseNumber('S',-1);
  //KP
    if(cmd == 1){
    if(KP > 3)KP = 0;
    KP=KP+0.05;
    Serial.print("Valor KP: ");
    Serial.print(KP);
    Serial.println("\n");

    }   
    
    if(cmd == 2){
    
    KP=KP-0.05;
    if(KP <= 0.05)KP = 0;
    Serial.print("Valor KP: ");
    Serial.print(KP);
    Serial.println("\n");  
    }

  //KI


    if(cmd == 3){
    KI=KI+0.05;
    if(KI > 3)KI = 0;
    Serial.print("Valor KI: ");
    Serial.print(KI);
    Serial.println("\n");

    }   
    
    if(cmd == 4){
    KI=KI-0.05;
    if(KI <= 0.05)KI = 0;
    Serial.print("Valor KI: ");
    Serial.print(KI);
    Serial.println("\n");  
    }

    //KD

    if(cmd == 5){
    KD=KD+0.05;
    if(KD > 3)KD = 0;

    Serial.print("Valor KD: ");
    Serial.print(KD);
    Serial.println("\n");

    }   
    
    if(cmd == 6){
    KD=KD-0.05;
    if(KD <= 0.05)KD = 0;
    Serial.print("Valor KD: ");
    Serial.print(KD);
    Serial.println("\n");  
    }
   
    if(cmd == 7){ 
    
//KP    
byte* puntero = (byte*)&KP;
  for (int i = 0; i < sizeof(float); i++) {
    EEPROM.write(0 + i, puntero[i]);
  }
  
  // Leer el valor float desde la EEPROM byte por byte
  float valorLeido;
  byte* punteroLeido = (byte*)&valorLeido;
  for (int i = 0; i < sizeof(float); i++) {
    punteroLeido[i] = EEPROM.read(0 + i);
  }

//KI
byte* puntero1 = (byte*)&KI;
  for (int i = 4; i < 8; i++) {
    EEPROM.write(i, puntero1[i]);
  }
  
  // Leer el valor float desde la EEPROM byte por byte
  float valorLeido1;
  byte* punteroLeido1 = (byte*)&valorLeido1;
  for (int i = 4; i < 8; i++) {
    punteroLeido1[i] = EEPROM.read(i);
  }

//KD
byte* puntero2 = (byte*)&KD;
  for (int i = 8; i < 12; i++) {
    EEPROM.write(i, puntero2[i]);
  }
  
  // Leer el valor float desde la EEPROM byte por byte
  float valorLeido2;
  byte* punteroLeido2 = (byte*)&valorLeido2;
  for (int i = 8; i < 12; i++) {
    punteroLeido2[i] = EEPROM.read(i);
  }
  
  // KP
  Serial.print("Valor guardado KP: ");
  Serial.println(KP);

  Serial.print("Valor leído KP: ");
  Serial.println(valorLeido);
  //KI
  Serial.print("Valor guardado KI: ");
  Serial.println(KI);

  Serial.print("Valor leído KI: ");
  Serial.println(valorLeido1);
  //KD  
  Serial.print("Valor guardado KD: ");
  Serial.println(KD);

  Serial.print("Valor leído KD: ");
  Serial.println(valorLeido2);
    
       
    }

  if(cmd == 8 || modoseguidor == true){
    modoseguidor = true;

    /*        0                           3500                          7000
     *   Ain1  pin5 motor izquierdo                                 Bin1   pin10    Motor derecho
     *   Ain2 pin 6    m1speed                                             Bin2  pin 9  m2speed
     *                    
     * 
     *     
     *           Si la posición del sensor tiende a ser cero
     *     Error= 0 -3500  = -3500     
     *     si Kd=0 u ki=0
     *      
     *      m1speed =120- 350=-230 ->   0                           

        Gira a máxima velocidad el motor izquierdo y el motor motor derecho no gira
             
              Si la posición del sensor tiende a 7000
              Error= 7000 -3500 = 3500
              si Kd=0 u ki=0
           m1speed=120+350= 470 -> 255                          m2speed =120- 350=-230 ->   0  
     Gira a máxima velocidad el motor derecho y el motor motor izquierdo no gira

     */


  
  unsigned int position = qtrrc.readLine(sensorValues);
 
  delay(0);

 int error = position - 3500; 
 suma=suma+error;

  Dif=error-errorant;
 float motorSpeed=KP*error+suma*KI+KD*Dif;


   m1Speed = M1 +(int)motorSpeed; 
   m2Speed = M2 -(int)motorSpeed;
  
  if (m1Speed < 130)   // Para aumentar la velocidad reducir el valor de m1speed y m2speed 
    m1Speed = 130;     // lo ideal es que esten en cero 
  if (m2Speed < 130) 
    m2Speed = 130; 
    
     if (m1Speed > 255) 
    m1Speed = 255; 
  if (m2Speed > 255) 
    m2Speed = 255; 

    
analogWrite(in2Pin, m1Speed);  // Ain2  6    
digitalWrite(in1Pin,HIGH);  //Ain1  5
digitalWrite(in3Pin,HIGH);   //Bin1  10
analogWrite(in4Pin,m2Speed); //  Bin2    9


errorant=error;


  }
  
 }