#ifndef SENSORES_H
#define SENSORES_H
 
#include "controladores.h"
#include "funcoes.h"
 
 
//##################################### IDENTIFICA CORES ####################################################
int RED = 23;
int GREEN = 25;
int BLUE = 27;
 
// porta analogica do LDR
int LDR = 9;
int LDR2 = 8;
 
int matrizPinodosLEDS[] = {RED,GREEN,BLUE}; 
 
// Uma variável booleana para indicar se o sensor foi calibrado 
boolean balanco = false; 
 
//Matriz de floats para armazenar o valor das cores 
float matrizCores[] = {   0,0,0}; 
float matrizBranco[] = {   0,0,0}; 
float matrizPreto[] = {   0,0,0}; 
 
//Variável para armazenar a média das leitura 
int mediaLeituras;
 
void mediaSensor(int vezes){   
  int leituras;   
  int somatorio=0;   
  for(int i = 0;i < vezes;i++){     
    leituras = analogRead(LDR);     
    somatorio = leituras + somatorio;     
    delay(10);   
  }   
  mediaLeituras = (somatorio)/vezes; 
} 
 
void confBalanco(){   
 
  //Calibrando o branco!   
  Serial.println("Calibrando o branco");   
  delay(5000);   
 
  for(int i = 0;i<=2;i++){     
    digitalWrite(matrizPinodosLEDS[i],HIGH);     
    delay(100);     
    mediaSensor(5);               
    matrizBranco[i] = mediaLeituras;     
    digitalWrite(matrizPinodosLEDS[i],LOW);     
    delay(100);   
  }   
 
  //Calibrando o preto!   
  Serial.println("Calibrando o preto");   
  delay(5000);   
 
  for(int i = 0;i<=2;i++){     
    digitalWrite(matrizPinodosLEDS[i],HIGH);     
    delay(100);     
    mediaSensor(5);     
    matrizPreto[i] = mediaLeituras;     
    digitalWrite(matrizPinodosLEDS[i],LOW);     
    delay(100);   
  }   
 
  //Avisa que a calibragem foi feita   
  Serial.println("Sensor Calibrado");   
  balanco = true;   
  delay(3000); 
} 
 
 
void checaBalanco() {   
  //checa se o balanço de branco foi feito   
 
  if(balanco == false){     
    confBalanco();   
  } 
} 
 
 
 
//Checa a cor 
void checaCores(){   
  for(int i = 0;i<=2;i++){     
    digitalWrite(matrizPinodosLEDS[i],HIGH);     
    delay(100);     
    mediaSensor(5);                       
    matrizCores[i] = mediaLeituras;             
    float cinzaDif = matrizBranco[i] - matrizPreto[i];     
    matrizCores[i] = (matrizCores[i] - matrizPreto[i])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[i],LOW);     
    delay(100);   
  } 
 
} 
 
void mostraCores(){
  int r = int (matrizCores[0]);
  int g = int (matrizCores[1]);
  int b = int (matrizCores[2]);
  Serial.println("  -----------------------------  ");   
   if (r < 30 && g < 30 && b < 30){   
       lcd.setCursor(0,0);  
       lcd.print("Preto           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((30<=r<=600)&&(r-20>g)&&(r-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Vermelho        ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
      else if((r>200)&& (g>200) &&(r>b)&&(g>b)){
       lcd.setCursor(0,0);  
       lcd.print("Amarelo         ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((20<=g<=600)&&(g-20>r)&&(g-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Verde           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((20<=b<=700)&&(b-20>r)&&(b-20>g)){
       lcd.setCursor(0,0);  
       lcd.print("Azul            ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
}
 
// ########################################### LOCALIZA #################################################################
 
 
void Aponta_max(int maxi){
 
  int l1;
  int l2;
  int diferenca;
  long tempoinicial = millis();
  long tempoatual = tempoinicial;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
 
  while(1){
 
  l1 = analogRead(LDR);
  l2 = analogRead(LDR2);
  diferenca = (l1 - l2);
  if (tempoatual<tempoinicial+3000){
    if (diferenca >= maxi)
     {break;}
 
  }
  else if (diferenca >= maxi-5)
  {break;}
  }
 
  motor1->run(RELEASE);
  motor2->run(RELEASE);
}
 
 
int giramax_controle(int angulo){
 
  int l1;
  int l2;
  int diferenca;
  int a=0;
  int maxi = 0;
  float velangdes =1.5;
  float velin= velangdes*RaioRoda;
  float t =(((PI*RaioRobo)/180)*angulo)/(velin); 
  float velang;
  float velang2;
  int u=60;
  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);
 
 
 
 
    l1 = analogRead(LDR);
    l2 = analogRead(LDR2);
    diferenca = abs(l1 - l2);
    Serial.println(diferenca);
    Serial.println("------------------------------------------------------");  
 
    if(diferenca > maxi){
      maxi = diferenca;
 
    }           
 
    motor1->setSpeed(u);
    motor2->setSpeed(u2); 
    tempoatual = millis();
    }
 
 
  return maxi;
}
 
 
// ########################################### LOCALIZA  #################################################################
 
 
int giramax(int angle){
  int maximo = 0;
 
  int medida = abs(analogRead(LDR) - analogRead(LDR2));
  if(medida > maximo){
    maximo = medida;
  }
  return maximo;
}
 
void giraeaponta(int a){
  int medida = 0;
  int medidanova = 0;
  while(1){
    int diferenca = abs(analogRead(LDR) - analogRead(LDR2));
    if(diferenca > (a -10)){
      motor1->run(RELEASE);
      motor2->run(RELEASE);
      break;
    }
  }
}
 
void ldr(){
  int max1 = giramax_controle(180);
  int max2 = giramax_controle(180);
  if(max1 > max2){
    giraeaponta(max1);
  }
  else{
    giraeaponta(max2);
  }
}
 
#endif