#ifndef CONTROLADORES_H
#define CONTROLADORES_H
#include "funcoes.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;
 
long t_atual = 0;
long t_start = 0;
int maxcalibrado = 0;
 
//Specify the links and initial tuning parameters
 
PID myPID(&Input, &Output, &Setpoint, 13.872,47.515,1.0863, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2, 15.226559 , 54.445344 ,1.2191, DIRECT);
 
unsigned long serialTime; //this will help us know when to talk with processing
 
 
int enc=12;
int enc2=13;
 
 
 
//float kp = 0.2;
//float kd = 1;
 
 
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){
 
//    int erro = 0;
//    static int erro_ant = 0;
//    int sinal_controle;
//
//    // float P_velref = 36.096*velref +  6.785; //valor adquirido por linearinizacao scydavis
//    //float P_velang = 36.096*velang + 6.785;
//     erro = (velref - velang);
//     sinal_controle = (kp*erro) + (kd*(erro - erro_ant));
//     erro_ant = erro;
//     float P_sinal = 36.096*sinal_controle + 6.785;
//
//     return constrain(sinal + int(P_sinal), 0, 210);
 
       //pid-related code
       Input = velang;
       Setpoint = velref;
       myPID.Compute();
 
 
       return constrain(Output, 30, 255);
 
 
 
 }
 
   int controlador2(float velang,float velref, float sinal){
 
//    int erro = 0;
//    static int erro_ant = 0;
//    int sinal_controle;
//
//    // float P_velref = 36.096*velref +  6.785; //valor adquirido por linearinizacao scydavis
//    //float P_velang = 36.096*velang + 6.785;
//     erro = (velref - velang);
//     sinal_controle = (kp*erro) + (kd*(erro - erro_ant));
//     erro_ant = erro;
//     float P_sinal = 36.096*sinal_controle + 6.785;
//
//     return constrain(sinal + int(P_sinal), 0, 210);
 
       //pid-related code
       Input2 = velang;
       Setpoint2 = velref;
       myPID2.Compute();
 
       return constrain(Output2, 70, 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 tempoatr=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(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++;
    //tempotr_ant = tempotr;
    //tempotr = millis();
    //dif = tempotr - tempotr_ant;
    //velang = (PI/6)/((dif)*pow(10,-3));
    //vellin = velang * RaioRoda;
  }
  estadoant = estado;
  tempoatr = millis();
  dif = tempoatr - tempotr;
   if (dif >= 1000){
      velang = 0;
      vellin = 0;
     }
 
   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 tempoatr2=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++;
    //tempotr_ant = tempotr;
    //tempotr = millis();
    //dif = tempotr - tempotr_ant;
    //velang = (PI/6)/((dif)*pow(10,-3));
    //vellin = velang * RaioRoda;
  }
  estadoant2 = estado2;
  tempoatr2 = millis();
  dif2 = tempoatr2 - tempotr2;
  if (dif2 >= 1000){
      velang2 = 0;
      vellin2 = 0;
     }
 
   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
 
 
 
// getting float values from processing into the arduino
// was no small task.  the way this program does it is
// as follows:
//  * a float takes up 4 bytes.  in processing, convert
//    the array of floats we want to send, into an array
//    of bytes.
//  * send the bytes to the arduino
//  * use a data structure known as a union to convert
//    the array of bytes back into an array of floats
 
//  the bytes coming from the arduino follow the following
//  format:
//  0: 0=Manual, 1=Auto, else = ? error ?
//  1: 0=Direct, 1=Reverse, else = ? error ?
//  2-5: float setpoint
//  6-9: float input
//  10-13: float output  
//  14-17: float P_Param
//  18-21: float I_Param
//  22-245: float D_Param
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]);
    //Input=double(foo.asFloat[1]);       // * 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);// * set the controller Direction
    else myPID.SetControllerDirection(REVERSE);          //
  }
  Serial.flush();                         // * clear any random data from the serial buffer
}
 
// unlike our tiny microprocessor, the processing ap
// has no problem converting strings into floats, so
// we can just send strings.  much easier than getting
// floats from processing to here no?
 
 
#endif