#include <NewPing.h>
#include <Wire.h>
#include <Adafruit_AMG88xx.h>
#include <IRremote.h>

#define Boton_2 0xE718FF00  // adelante
#define Boton_8 0xAD52FF00  // atrás
#define Boton_4 0xF708FF00  // izquierda
#define Boton_6 0xA55AFF00  // derecha
#define Boton_5 0xE31CFF00
#define Boton_7 0xBD42FF00
#define Boton_9 0xB54AFF00

int SENSOR = 44;
int mode = 0;

int continua;


Adafruit_AMG88xx amg;

int derecho_r_en = 2;
int derecho_l_en = 3;

int izquierdo_r_en = 8;
int izquierdo_l_en = 9;

//Use PWM pins
int derecho_r_pwm = 5;
int derecho_l_pwm = 6;

int izquierdo_r_pwm = 11;
int izquierdo_l_pwm = 12;


int trigPin = 22;
int echoPin = 24;
NewPing sonar(trigPin, echoPin, 200);


bool bGoForward = false;

int distance = 1;  // Used to hold distance from sonar reading.

float pixels[AMG88xx_PIXEL_ARRAY_SIZE];  //libreria thermal
float max_value;
float min_value;
int max_index;



void setup() {

  Serial.begin(9600);
  IrReceiver.begin(SENSOR, DISABLE_LED_FEEDBACK);



  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);

  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);

  float pixels[AMG88xx_PIXEL_ARRAY_SIZE];  //libreria thermal
  float max_value;
  float min_value;
  int max_index;

  bool status;
  status = amg.begin();                                                      //libreria
  if (!status) {                                                             //libreria
    Serial.println("Could not find a valid AMG88xx sensor, check wiring!");  //libreria
    while (1)
      ;  //libreria
  }

  delay(100);  // let sensor boot up
}

void loop() {

  //To enable forward and backward
  digitalWrite(derecho_r_en, HIGH);
  digitalWrite(derecho_l_en, HIGH);

  digitalWrite(izquierdo_r_en, HIGH);
  digitalWrite(izquierdo_l_en, HIGH);

  if (mode == 0) {

    if (IrReceiver.decode()) {                                       // si existen datos ya decodificados
      Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);  // imprime valor en hexadecimal en monitor
      if (IrReceiver.decodedIRData.decodedRawData == Boton_2)        // si codigo recibido es igual a Boton_1
        forward();
      IrReceiver.resume();  // enciende o apaga componente rojo

      if (IrReceiver.decodedIRData.decodedRawData == Boton_8)  // si codigo recibido es igual a Boton_2
        reverse();
      IrReceiver.resume();  // enciende o apaga componente verde

      if (IrReceiver.decodedIRData.decodedRawData == Boton_4)  // si codigo recibido es igual a Boton_3
        left();
      IrReceiver.resume();  // enciende o apaga componente azul

      if (IrReceiver.decodedIRData.decodedRawData == Boton_6)  // si codigo recibido es igual a Boton_3
        right();
      IrReceiver.resume();

      if (IrReceiver.decodedIRData.decodedRawData == Boton_5)  // si codigo recibido es igual a Boton_3
        stop();
      IrReceiver.resume();  // resume la adquisicion de datos
    } else if (IrReceiver.decodedIRData.decodedRawData == Boton_7) {
      mode = 1;
      //stop();
      //IrReceiver.resume();
    }
  }

  if (mode == 1) {



    amg.readPixels(pixels);
    min_value = amg.readThermistor();  //lee temperatura ambiente y le asigna el valor minimo


    max_value = -1.0;
    min_value = 100.0;
    max_index = -1;



    for (int i = 0; i < AMG88xx_PIXEL_ARRAY_SIZE; i++) {  //libreria genera matriz array
      if (pixels[i] < min_value) {                        //coge del array el valor mínimo, como dimos uno muy alto 100 arriba siempres se cumplirá
        min_value = pixels[i];
        max_value = pixels[i] + 3;  //ojo, cosa mia
      }
    }
    for (int i = 0; i < AMG88xx_PIXEL_ARRAY_SIZE; i++) {  //libreria genera matriz array

      if (pixels[i] > max_value && pixels[i] > min_value + 3.0) {  // solo actua si hay variaciones de +3 entre max y min
        max_value = pixels[i];                                     //anota valor maximo
        max_index = i;                                             //posicion en el array del valor maximo, izq son valores bajos y derecha altos en caso de amg8833 tumbado
      }
    }



    //Serial.println(max_index);



    distance = sonar.ping_cm();
    //Serial.println(distance);
    if (distance != 0 && distance < 60) {
      bGoForward = false;
      reverse();
      Serial.println("retrocede");
      delay(30);
      stop();
    }
    void (*resetFunc)(void) = 0;  //declare reset function @ address 0

    if (IrReceiver.decode()) {  // si existen datos ya decodificados
      Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);
      IrReceiver.resume();

      if (IrReceiver.decodedIRData.decodedRawData == Boton_9) {
        Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);
        resetFunc();
      }
    }

    distance = sonar.ping_cm();
    if (distance != 0 && distance < 60) {
      bGoForward = false;
      reverse();
      Serial.println("reverse");
      delay(1500);
      stop();
    }

    if (max_index >= 1 && max_index <= 16) {
      right();
      continua = 1;
      Serial.println("derecha");
      delay(1500);
    }
    if (max_index >= 49 && max_index <= 64) {
      left();
      continua = 2;
      Serial.println("izquierda");
      delay(1500);
    }

    if (max_index >= 17 && max_index <= 48) {
      forward();
      Serial.println("avanza");
      delay(1500);
    }
    if (max_value < min_value + 1 && continua == 1) {
      right();
      Serial.println("derecha");
      delay(1500);
    }
    if (max_value < min_value + 1 && continua == 2) {
      left();
      Serial.println("izquierda");
      delay(1500);
    }

    distance = sonar.ping_cm();
    if (distance != 0 && distance < 60) {
      bGoForward = false;
      reverse();
      Serial.println("reverse");
      delay(1500);
      stop();
    }
  }
}



void stop() {
  analogWrite(derecho_r_pwm, 0);  // girara en un sentido, si se cambia a l_pwm cambiará el sentido
  analogWrite(derecho_l_pwm, 0);  // numero aqui hacia delante

  analogWrite(izquierdo_r_pwm, 0);  //al reves, importe aqui gira hacia delante
  analogWrite(izquierdo_l_pwm, 0);
}

void left() {
  analogWrite(derecho_r_pwm, 0);   // girara en un sentido, si se cambia a l_pwm cambiará el sentido
  analogWrite(derecho_l_pwm, 50);  // numero aqui hacia delante

  analogWrite(izquierdo_r_pwm, 0);  //al reves, importe aqui gira hacia delante
  analogWrite(izquierdo_l_pwm, 50);
}

void right() {
  analogWrite(derecho_r_pwm, 50);  // girara en un sentido, si se cambia a l_pwm cambiará el sentido
  analogWrite(derecho_l_pwm, 0);   // numero aqui hacia delante

  analogWrite(izquierdo_r_pwm, 50);  //al reves, importe aqui gira hacia delante
  analogWrite(izquierdo_l_pwm, 0);
}

void forward() {
  analogWrite(derecho_r_pwm, 0);   // girara en un sentido, si se cambia a l_pwm cambiará el sentido
  analogWrite(derecho_l_pwm, 50);  // numero aqui hacia delante

  analogWrite(izquierdo_r_pwm, 50);  //al reves, importe aqui gira hacia delante
  analogWrite(izquierdo_l_pwm, 0);
}

void reverse() {
  analogWrite(derecho_r_pwm, 50);  // girara en un sentido, si se cambia a l_pwm cambiará el sentido
  analogWrite(derecho_l_pwm, 0);   // numero aqui hacia delante

  analogWrite(izquierdo_r_pwm, 0);  //al reves, importe aqui gira hacia delante
  analogWrite(izquierdo_l_pwm, 50);
}