#ifndef CONTROLADORES_H
#define CONTROLADORES_H
#include "math.h"
#define RaioRoda 0.04
#define RaioRobo 0.07
#include <PID_v1.h>
 
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double Input2, Output2, Setpoint2;
 
 
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, 15 , 21 ,0.5, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2, 5 , 21 ,0.5, DIRECT);
 
unsigned long serialTime; //this will help us know when to talk with processing
 
int enc=12;
int enc2=13;
 
void SerialSend()
{
  Serial.print("PID ");
  Serial.print(Setpoint);   
  Serial.print(" ");
  Serial.print(Input);   
  Serial.print(" ");
  Serial.print(Output);   
  Serial.print(" ");
  Serial.print(myPID.GetKp());   
  Serial.print(" ");
  Serial.print(myPID.GetKi());   
  Serial.print(" ");
  Serial.print(myPID.GetKd());   
  Serial.print(" ");
  if(myPID.GetMode()==AUTOMATIC) Serial.print("Automatic");
  else Serial.print("Manual");  
  Serial.print(" ");
  if(myPID.GetDirection()==DIRECT) Serial.println("Direct");
  else Serial.println("Reverse");
}
 
  int controlador1(float velang,float velref, float sinal){
 
       //pid-related code
       Input = velang;
       Setpoint = velref;
       myPID.Compute();
 
       return constrain(Output, 0, 255);
 
 }
 
   int controlador2(float velang,float velref, float sinal){
 
       //pid-related code
       Input2 = velang;
       Setpoint2 = velref;
       myPID2.Compute();
 
       return constrain(Output2, 0, 255);
 
 }
 
 float Leitura_Encoder1(){
 
 
   int estado;
 
   static int pulso = 0;
 
   static long transicao = 0;
 
   static int estadoant;
 
 
   static long rotacao=0;
 
   static long transicaoant = 0;
 
 
   static long tempotr = millis();
 
 
   static long tempotr_ant = tempotr;
 
   static float velang = 0;
   static float vellin = 0;
 
 
   static int dif = 0;
 
 
 
  estado = analogRead(enc);
 
  if (estado >= 1000 && estadoant < 1000){
    pulso = 1;
    transicao++;
    tempotr_ant = tempotr;
    tempotr = millis();
    dif = tempotr - tempotr_ant;
    if (dif >= 0.15){
      velang = 0;
      vellin = 0;
     }
      if(transicao > 1 && dif!=0){
        velang = (PI/3)/((dif)*pow(10,-3));
        vellin = velang * RaioRoda;
      }
      else{
        velang = 0;
        vellin = 0;
 
      }
  }   
  else if(estado < 1000 && estadoant >= 1000){
    pulso = 0;
    transicao++;
  }
  estadoant = estado;
 
   if (transicao % 12 == 0 && transicaoant == (transicao - 1))
  {rotacao++;
  }
 
  transicaoant = transicao;
 
  return velang;
 }
 
 float Leitura_Encoder2(){
 
 
   int estado2;
 
   static int pulso2 = 0;
 
   static long transicao2 = 0;
 
   static int estadoant2;
 
 
   static long rotacao2=0;
 
   static long transicaoant2 = 0;
 
 
 
   static long tempotr2 = millis();
 
 
   static long tempotr_ant2 = tempotr2;
 
   static float velang2 = 0;
   static float vellin2 = 0;
 
 
   static int dif2 = 0;
 
 
 
    estado2 = analogRead(enc2);
 
    if (estado2 >= 1000 && estadoant2 < 1000){
    pulso2 = 1;
    transicao2++;
    tempotr_ant2 = tempotr2;
    tempotr2 = millis();
    dif2 = tempotr2 - tempotr_ant2;
      if(transicao2 > 1 && dif2 != 0){
        velang2 = (PI/3)/((dif2)*pow(10,-3));
        vellin2 = velang2 * RaioRoda;
      }
       else{
        velang2 = 0;
        vellin2 = 0;
      }
  }   
  else if(estado2 < 1000 && estadoant2 >= 1000){
    pulso2 = 0;
    transicao2++;
 
  }
  estadoant2 = estado2;
 
   if (transicao2 % 12 == 0 && transicaoant2 == (transicao2 - 1))
  {rotacao2++;
  }
 
  transicaoant2 = transicao2;
 
  return velang2;
 
 }
 
/********************************************
 * Serial Communication functions / helpers
 ********************************************/
 
 
union {                // This Data structure lets
  byte asBytes[24];    // us take the byte array
  float asFloat[6];    // sent from processing and
}                      // easily convert it to a
foo;                   // float array
 
 
 
 
void SerialReceive()
{
 
  // read the bytes sent from Processing
  int index=0;
  byte Auto_Man = -1;
  byte Direct_Reverse = -1;
  while(Serial.available()&&index<26)
  {
    if(index==0) Auto_Man = Serial.read();
    else if(index==1) Direct_Reverse = Serial.read();
    else foo.asBytes[index-2] = Serial.read();
    index++;
  } 
 
  // if the information we got was in the correct format, 
  // read it into the system
  if(index==26  && (Auto_Man==0 || Auto_Man==1)&& (Direct_Reverse==0 || Direct_Reverse==1))
  {
    Setpoint=double(foo.asFloat[0]);
                                          // * the user has the ability to send the 
                                          //   value of "Input"  in most cases (as 
                                          //   in this one) this is not needed.
    if(Auto_Man==0)                       // * only change the output if we are in 
    {                                     //   manual mode.  otherwise we'll get an
      Output=double(foo.asFloat[2]);      //   output blip, then the controller will 
    }                                     //   overwrite.
 
    double p, i, d;                       // * read in and set the controller tunings
    p = double(foo.asFloat[3]);           //
    i = double(foo.asFloat[4]);           //
    d = double(foo.asFloat[5]);           //
    myPID.SetTunings(p, i, d);            //
 
    if(Auto_Man==0) myPID.SetMode(MANUAL);// * set the controller mode
    else myPID.SetMode(AUTOMATIC);             
 
    if(Direct_Reverse==0) myPID.SetControllerDirection(DIRECT);
    else myPID.SetControllerDirection(REVERSE);          
  }
  Serial.flush();                         
}
 
 
#endif