#include <Arduino.h>
#include <math.h>

// Constantes
const double UNIVERSE_RANGE[] = {0, 1};
const int REGLAS_SIZE = 9;
const float DETENTE = 0;
const float MEDIO = 0.5;
const float ADELANTE = 1;
const float REGLAS[9][2][2] = {
  {{0.0, 0.0}, {ADELANTE, ADELANTE}},   //1
  {{2.0, 2.0}, {DETENTE, DETENTE}},     //2         
  {{1.0, 1.0}, {ADELANTE, ADELANTE}},   //3       
  {{0.0, 2.0}, {ADELANTE, MEDIO}},      //4          
  {{2.0, 0.0}, {MEDIO, ADELANTE}},      //5          
  {{0.0, 1.0}, {ADELANTE, MEDIO}},      //6      
  {{1.0, 0.0}, {MEDIO, ADELANTE}},      //7          
  {{1.0, 2.0}, {ADELANTE, MEDIO}},      //8          
  {{2.0, 1.0}, {MEDIO, ADELANTE}}       //9      
};

// Función para la defuzzificación por centroide
double centroid_defuzzification(double output_values[], int output_size, const double universe_range[]) {
  double numerator = 0.0;
  double denominator = 0.0;

  if (output_size == 1) {
      return output_values[0];
  } else {
      for (int i = 0; i < output_size; i++) {
          double crisp_value = universe_range[0] + i * (universe_range[1] - universe_range[0]) / (output_size - 1);
          numerator += output_values[i] * crisp_value;
          denominator += output_values[i];
      }

      if (denominator != 0) {
          return numerator / denominator;
      } else {
          return 0.0;
      }
  }
}

// Función para calcular la membresía difusa
double msp(double xx, double aa, double bb, double cc, double dd) {
  if (fabs(aa - bb) < 0.0001 && fabs(xx - aa) < 0.0001) {  // Triángulo izquierdo
      return 1;
  } else if (fabs(cc - dd) < 0.0001 && fabs(xx - cc) < 0.001) {  // Triángulo derecho
      return 1;
  } else if (fabs(xx - bb) < 0.0001 || fabs(xx - cc) < 0.0001) {  // Triángulo
      return 1;
  } else {
      if (aa <= xx && xx <= bb) {
          return (xx - aa) / (bb - aa);
      } else if (bb < xx && xx <= cc) {
          return 1;
      } else if (cc < xx && xx <= dd) {
          return (dd - xx) / (dd - cc);
      } else {
          return 0;
      }
  }
}

// Función para obtener índices mayores que cero
void obtener_indices_mayores_que_cero(double lista_numeros[], int size, int indices[], int *indices_size) {
  *indices_size = 0;
  for (int i = 0; i < size; i++) {
      if (lista_numeros[i] > 0) {
          indices[(*indices_size)++] = i;
      }
  }
}

// Función para calcular las salidas
void salidas(int fuzz_entradas[2][3], int fuzz_sizes[2], const float lista_reglas[][2][2], int reglas_size, float lista_salidas_derecha[], float lista_salidas_izquierda[], int *salidas_size) {
  *salidas_size = 0;
  for (int i = 0; i < reglas_size; i++) {
      for (int j = 0; j < fuzz_sizes[0]; j++) {
          for (int k = 0; k < fuzz_sizes[1]; k++) {
              if (lista_reglas[i][0][0] == fuzz_entradas[0][j] && lista_reglas[i][0][1] == fuzz_entradas[1][k]) {
                  lista_salidas_derecha[(*salidas_size)] = lista_reglas[i][1][0];
                  lista_salidas_izquierda[(*salidas_size)++] = lista_reglas[i][1][1];
              }
          }
      }
  }
}

// Función principal para calcular los valores de los motores
void calcularValoresMotores(double valor_sensor_A, double valor_sensor_B, double *resultant_value_derecha, double *resultant_value_izquierda) {
  double sensor_a[] = {
      msp(valor_sensor_A, 0, 0, 0.125, 0.25),
      msp(valor_sensor_A, 0.2, 0.4, 0.4, 0.6),
      msp(valor_sensor_A, 0.5, 0.55, 1, 1)
  };

  double sensor_b[] = {
      msp(valor_sensor_B, 0, 0, 0.125, 0.25),
      msp(valor_sensor_B, 0.2, 0.4, 0.4, 0.6),
      msp(valor_sensor_B, 0.5, 0.55, 1, 1)
  };

  int fuzz_a[3], fuzz_b[3];
  int fuzz_a_size, fuzz_b_size;

  obtener_indices_mayores_que_cero(sensor_a, 3, fuzz_a, &fuzz_a_size);
  obtener_indices_mayores_que_cero(sensor_b, 3, fuzz_b, &fuzz_b_size);

  int fuzz_sensores[2][3] = {{0}};
  for (int i = 0; i < fuzz_a_size; i++) {
      fuzz_sensores[0][i] = fuzz_a[i];
  }
  for (int i = 0; i < fuzz_b_size; i++) {
      fuzz_sensores[1][i] = fuzz_b[i];
  }

  int fuzz_sizes[2] = {fuzz_a_size, fuzz_b_size};

  float lista_salidas_derecha[9] = {0.0};
  float lista_salidas_izquierda[9] = {0.0};
  int salidas_size;

  salidas(fuzz_sensores, fuzz_sizes, REGLAS, REGLAS_SIZE, lista_salidas_derecha, lista_salidas_izquierda, &salidas_size);

  // Convertir listas de salidas a tipo double
  double lista_salidas_derecha_d[9];
  double lista_salidas_izquierda_d[9];
  for (int i = 0; i < salidas_size; i++) {
      lista_salidas_derecha_d[i] = (double)lista_salidas_derecha[i];
      lista_salidas_izquierda_d[i] = (double)lista_salidas_izquierda[i];
  }

  *resultant_value_derecha = centroid_defuzzification(lista_salidas_derecha_d, salidas_size, UNIVERSE_RANGE);
  *resultant_value_izquierda = centroid_defuzzification(lista_salidas_izquierda_d, salidas_size, UNIVERSE_RANGE);
}

void setup() {
  Serial.begin(115200);

  // Guardar el tiempo de finalización
  unsigned long fin = millis();

  // Calcular la duración total
  double duracion = (double)fin / 1000.0;
  Serial.printf("El programa tardó %.6f segundos en ejecutarse.\n", duracion);
}

void loop() {
  double valor_sensor_A = (double)analogRead(27)/4095; // sensor derecho
  double valor_sensor_B = (double)analogRead(14)/4095; // sensor izquierdo
  double resultant_value_derecha, resultant_value_izquierda;

  // Llamar a la función para calcular los valores de los motores
  calcularValoresMotores(valor_sensor_A, valor_sensor_B, &resultant_value_derecha, &resultant_value_izquierda);
  
  Serial.printf("izquierda: %.3f, derecha: %.3f\n", valor_sensor_B, valor_sensor_A);
  Serial.printf("Resultant value izquierda: %.3f, derecha: %.3f\n", resultant_value_izquierda, resultant_value_derecha);
  delay(700);
}