#ifndef FUNCOES_H
#define FUNCOES_H
 
#include <Adafruit_MotorShield.h>
#include "controladores.h"
#define RaioRoda 0.04
#define RaioRobo 0.07
#include <LiquidCrystal.h>
 
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
 
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);//motor lego
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
 
int v1 = 0; // usado para aumentar a velocidade do motor 1
int v2 = 0; // usado para aumentar a velocidade do motor 2
float t_gir180 = 2.1;
float t_dir90 = 1.025;
float t_esq90 = 0.8;
float t_dir45 = 1.5;
float t_frente = 2.3;
float t_re = 2;
 
int vel1 = 100; // velocidade inicial do motor 1 - 140
int vel2 = 135;// velocidade inicial do motor 2 - 160
 
void gira_esq (int angulo){
  float velangdes =2.0;
  float velin= velangdes*RaioRoda;
  float t =(((PI*RaioRobo)/180)*angulo)/(velin); 
  float velang;
  float velang2;
  int u=40;
  int u2=120;
  motor1->setSpeed(u);
  motor2->setSpeed(u2);
  long  tempoinicial = millis();
  long  tempoatual = tempoinicial;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  while(tempoatual < tempoinicial + (t*1000)){
    velang = Leitura_Encoder1();
    velang2 = Leitura_Encoder2();
    u = controlador1(velang,velangdes, u);
    u2 = controlador2(velang2,velangdes,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
     lcd.setCursor(0,0); 
        lcd.print("                "); 
        lcd.setCursor(0,0); 
        lcd.print(t);
        lcd.print(" ");
        lcd.print(" ");
        lcd.setCursor(0,1); 
        lcd.print(velang);
        lcd.print(" ");
        lcd.print(velang2);
        lcd.print("   ");
 
 
    tempoatual = millis();
  }
  motor1->run(RELEASE);
  motor2->run(RELEASE);
 
  }
 
 
void gira_dir (int angulo){
 float velangdes =2.0;
  float velin= velangdes*RaioRoda;
  float t =(((PI*RaioRobo)/180)*angulo)/(velin); 
  float velang;
  float velang2;
  int u=40;
  int u2=120;
  motor1->setSpeed(u);
  motor2->setSpeed(u2);
  long  tempoinicial = millis();
  long  tempoatual = tempoinicial;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  while(tempoatual < tempoinicial + (t*1000)){
    velang = Leitura_Encoder1();
    velang2 = Leitura_Encoder2();
    u = controlador1(velang,velangdes, u);
    u2 = controlador2(velang2,velangdes,u2);
 
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
     lcd.setCursor(0,0); 
        lcd.print("                "); 
        lcd.setCursor(0,0); 
        lcd.print(t);
        lcd.print(" ");
        lcd.print(" ");
        lcd.setCursor(0,1); 
        lcd.print(velang);
        lcd.print(" ");
        lcd.print(velang2);
        lcd.print("   ");
 
    tempoatual = millis();
  }
    motor1->run(RELEASE);
  motor2->run(RELEASE);
}
 
void para(){
  motor1->run(RELEASE);
  motor2->run(RELEASE);
  delay(500);
}
 
void frente(float d){
    float velangdes = 2.0;
    float velangdes2 = 2.0;
    float velang;
    float velang2;
    float velindes = velangdes * RaioRoda;
    float t = d/velindes;
    int u=40;
    int u2=120;
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
    long  tempoinicial = millis();
    long  tempoatual = tempoinicial;
    motor1->run(FORWARD);
    motor2->run(FORWARD);
 
    while(tempoatual<tempoinicial+(t*1000)){
 
        velang = Leitura_Encoder1();
        velang2 = Leitura_Encoder2();
 
 
        u = controlador1(velang,velangdes, u);
        u2 = controlador2(velang2,velangdes2,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
           //send-receive with processing if it's time
          if(millis()>serialTime)
          {
            SerialReceive();
            SerialSend();
            serialTime+=500;
          }
 
 
        lcd.setCursor(0,0); 
        lcd.print("                "); 
        lcd.setCursor(0,0); 
        lcd.print(t);
        lcd.print(" ");
        lcd.print(" ");
        lcd.setCursor(0,1); 
        lcd.print(velang);
        lcd.print(" ");
        lcd.print(velang2);
        lcd.print("   ");
 velang = 0;
 velang2 = 0;  
 tempoatual = millis();     
}
  motor1->run(RELEASE);
  velang = 0;
  motor2->run(RELEASE);
  velang2 = 0;
}
 
 
void re(float d){
    float velangdes = 2.2;
    float velangdes2 = 2.2;
    float velang;
    float velang2;
    float t = d/velangdes;
    int u=15;
    int u2=90;
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
    long  tempoinicial = millis();
    long  tempoatual = tempoinicial;
    motor1->run(BACKWARD);
    motor2->run(BACKWARD);
    while(tempoatual<tempoinicial+(t*1000)){
        velang = Leitura_Encoder1();
        velang2 = Leitura_Encoder2();
        u = controlador1(velang,velangdes, u);
        u2 = controlador2(velang,velangdes2,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
           //send-receive with processing if it's time
          if(millis()>serialTime)
          {
            SerialReceive();
            SerialSend();
            serialTime+=500;
          }
 
 
        lcd.setCursor(0,0); 
        lcd.print("                "); 
        lcd.setCursor(0,0); 
        lcd.print(vel1);
        lcd.print(" ");
        lcd.print(vel2);
        lcd.setCursor(0,1); 
        lcd.print(velang);
        lcd.print(" ");
        lcd.print(velang2);
        lcd.print("   ");
 
}
  motor1->run(RELEASE);
  motor2->run(RELEASE);
}
 
 
void direita(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(RELEASE);
  delay(t);
}
 
void esquerda(float t){
  t = t*1000;
  motor1->run(RELEASE);
  motor2->run(BACKWARD);
  delay(t);
}
 
// gira horario
void gira(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void giraesquerda(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
//gira antihorario
void giraanti(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
void esquerda(){
  int tempo=0;
 
  para();
  esquerda(20);
  para();
 
}
 
void reta(){
  frente(t_frente);
  para();
  gira(t_gir180);
  para();
  frente(t_frente);
  para();
}
 
void triangulo(){
  frente(t_frente);
  para();
  gira(t_dir45);
  para();
  float t = 2.8;
  frente(t);
  para();
  gira(t_dir45);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
void quadrado(){
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
// Aumenta velocidade motor 2
void AumentaVel2() {
  if (vel2 <= 256){
    vel2 = vel2 + 1;
 
  }
  else{
    vel2 = 100;
 }
} 
 
void AumentaVel1() {
  if (vel1 < 256){
    vel1 = vel1 + 1;
 
  }
  else{
    vel1 = 100;
 }
} 
 
// DIMINUI velocidade motor 2
void DiminuiVel2() {
 
    vel2 = vel2 - 1;
 
 
 
} 
 
void DiminuiVel1() {
  if (vel1 < 210){
    vel1 = vel1 - 1;
  }
  else{
    vel1 = 100;
 }
} 
 
 
void Teste_Encoder()  
{  
  int pulso = 0;
  int pulso2 = 0;
  long transicao = 0;
  long transicao2 = 0;
  int estadoant;
  int estado;
  int estadoant2;
  int estado2;
  long rotacao=0;
  long rotacao2=0;
  long transicaoant = 0;
  long transicaoant2 = 0;
 
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  long tempoinicial = millis();
  long tempotr = tempoinicial;
  long tempoatual = tempoinicial;
  long tempotr2 = tempoinicial;
  long tempoatual2 = tempoinicial;
  long tempotr_ant;
  long tempotr_ant2;
  float velang = 0;
  float vellin = 0;
  float velang2 = 0;
  float vellin2 = 0;
 
  int dif = 0;
  int dif2 = 0;
 
 
  while(1){
 
  estado = analogRead(enc);  
  estado2 = analogRead(enc2);
 
  //-------------------M1------------------------------------  
  if (estado >= 1000 && estadoant < 1000){
    pulso = 1;
    transicao++;
    tempotr_ant = tempotr;
    tempotr = millis();
    dif = tempotr - tempotr_ant;
    velang = (PI/3)/((dif)*pow(10,-3));
    vellin = velang * RaioRoda;
  }
  else if(estado < 1000 && estadoant >= 1000){
    pulso = 0;
    transicao++;
  }
  estadoant = estado;
 
   if (transicao % 12 == 0 && transicaoant == (transicao - 1))
  {rotacao++;
  }
 
  transicaoant = transicao;
  //-------------------M2------------------------------------  
  if (estado2 >= 1000 && estadoant2 < 1000){
    pulso2 = 1;
    transicao2++;
    tempotr_ant2 = tempotr2;
    tempotr2 = millis();
    dif2 = tempotr2 - tempotr_ant2;
    velang2 = (PI/3)/((dif2)*pow(10,-3));
    vellin2 = velang * RaioRoda;
  }
  else if(estado2 < 1000 && estadoant2 >= 1000){
    pulso2 = 0;
    transicao2++;
 
  }
  estadoant2 = estado2;
 
  if (transicao2 % 12 == 0 && transicaoant2 == (transicao2 - 1))
    {rotacao2++;
    }
 
  transicaoant2 = transicao2;
 
 
 
//---------------------------------aumenta e diminui velocidades------------------//  
  int bot;  
  bot = analogRead (0);
 
  if (bot < 100) {  
    // ---------------------- BOTAO RIGHT -----------------
    AumentaVel1();
    motor1->setSpeed(vel1);
    //delay(300);
  } 
  else if (bot < 200) {
    // ---------------------- BOTAO UP -----------------
    AumentaVel2();
    motor2->setSpeed(vel2);
 
  }  
  else if (bot < 400){ 
    // -------------------- BOTAO DOWN --------------  
    DiminuiVel2() ;
    motor2->setSpeed(vel2);
 
  }
    else if (bot < 600){ 
    // -------------------- BOTAO LEFT -------------- 
    DiminuiVel1() ;
    motor1->setSpeed(vel1);
  }
//---------------------------printa LCD------------------------// 
  lcd.setCursor(0,0); 
  lcd.print("                "); 
  lcd.setCursor(0,0); 
  lcd.print(vel1);
  lcd.print(" ");
  lcd.print(vel2);
  lcd.setCursor(0,1); 
  lcd.print("                ");    
  lcd.setCursor(0,1); 
  lcd.print(velang);
  lcd.print(" ");
  lcd.print(velang2);
 
 
  tempoatual = millis();
  }
 
  motor1->run(RELEASE);
  motor2->run(RELEASE);
  }
 
#endif