// Estados para las FSMs
enum estadosRecepcion {ESPERA_COORDENADAS, LEE_DATOS, CALCULA_DETERMINANTE, CONVERSION_B1_A_CANONICA, CONVERSION_CANONICA_A_B2, MOSTRAR_DATOS} ;
enum estadosPrincipal {ESPERA_SENAL, VERIFICA_COORD, VERIFICA_BASES, VERIFICA_SWITCH, ENVIA_SENAL} ;
enum estadosEjecucion {CALCULA_DISTANCIA, CALCULA_ANGULO, MOSTRAR_DISTANCIA_ANGULOS, ACTIVA_MOTORES, ESPERA_PELOTA} ;

// Máquinas de estados
int estadoRecepcion = ESPERA_COORDENADAS;
int estadoPrincipal = ESPERA_SENAL;
int estadoEjecucion = CALCULA_DISTANCIA;

// Variables para guardar las coordenadas y vectores
float u1_x, u1_y, v1_x, v1_y; 
float u2_x, u2_y, v2_x, v2_y; 
float x1_b1, y1_b1, x2_b1, y2_b1, x3_b1, y3_b1, x4_b1, y4_b1, x5_b1, y5_b1, x1_b2, y1_b2, x2_b2, y2_b2, x3_b2, y3_b2, x4_b2, y4_b2, x5_b2, y5_b2;
float x1_canonica, y1_canonica, x2_canonica, y2_canonica, x3_canonica, y3_canonica, x4_canonica, y4_canonica, x5_canonica, y5_canonica;
float determinante;

#include <TimerOne.h>
const int switchPin = 3; // Señal del interruptor (la pelota) en el pin 2
const int ledPin = LED_BUILTIN;
const int buzzerPin = 5; // Pin para el zumbador

volatile bool pelotaPresente = false; // Para comprobar si la pelota está presente
volatile unsigned long ultimaActive = 0; // Para el zumbador
volatile bool zumbadorActivo = false; // Buzzer activo o no


void setup()
{
  Serial.begin(9600);
  pinMode(switchPin, INPUT);
  pinMode(ledPin, OUTPUT); // Para verificar el estado del interruptor
  pinMode(buzzerPin, OUTPUT); // Para activar el zumbador
  
  // ¿Interesado en usar un timer? Use la biblioteca TimerOne y configure el timer para ejecutarse cada segundo
  Timer1.initialize(1000000); // Configura el timer para ejecutarse cada 1 segundo 
  Timer1.attachInterrupt(zumbador); // Atache la función zumbador para ejecutarse cuando el timer se activa

}


void zumbador() // Función para hacer sonar el zumbador después de 5 segundos
{
    if (digitalRead(switchPin) == HIGH) {
        ultimaActive = millis();
        if (zumbadorActivo) {
            zumbadorActivo = false;
            noTone(buzzerPin);
        } 
    }  else if (millis() - ultimaActive > 5000 && !zumbadorActivo) { // 5 sec después
        zumbadorActivo = true;
        tone(buzzerPin, 1000);
    } 
} 


void loop() {
  
  switch (estadoRecepcion) {
    case ESPERA_COORDENADAS:
      // Aquí iría el código para esperar a las coordenadas y luego cambiar el estado a LEER_DATOS
      // Solicitar al usuario que ingrese los vectores de la primera base no canónica B1
        Serial.println("Ingrese el vector u1 de la base no canonica B1 (componente x): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector u1 de la base no canonica B1 (componente y): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector v1 de la base no canonica B1 (componente x): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector v1 de la base no canonica B1 (componente y): ");
        while (!Serial.available());

        // Solicitar al usuario que ingrese los vectores de la segunda base no canonica B2
        Serial.println("Ingrese el vector u2 de la base no canónica B2 (componente x): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector u2 de la base no canonica B2 (componente y): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector v2 de la base no canonica B2 (componente x): ");
        while (!Serial.available());

        Serial.println("Ingrese el vector v2 de la base no canonica B2 (componente y): ");
        while (!Serial.available());

        // Solicitar al usuario que ingrese las coordenadas en la base B1
        Serial.println("Ingrese la coordenada x1 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada y1 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada x2 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada y2 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada x3 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada y3 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada x4 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada y4 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada x5 en la base B1: ");
        while (!Serial.available());

        Serial.println("Ingrese la coordenada y5 en la base B1: ");
        while (!Serial.available());
    	
    	estadoRecepcion = LEE_DATOS;
      break;
    case  LEE_DATOS:
      // Aquí iría el código para leer los datos de las bases y las coordenadas
        u1_x = Serial.parseFloat();
        u1_y = Serial.parseFloat();
        v1_x = Serial.parseFloat();
        v1_y = Serial.parseFloat();
        u2_x = Serial.parseFloat();
        u2_y = Serial.parseFloat();
        v2_x = Serial.parseFloat();
        v2_y = Serial.parseFloat();
        x1_b1 = Serial.parseFloat();
        y1_b1 = Serial.parseFloat();
        x2_b1 = Serial.parseFloat();
        y2_b1 = Serial.parseFloat();
        x3_b1 = Serial.parseFloat();
        y3_b1 = Serial.parseFloat();
        x4_b1 = Serial.parseFloat();
        y4_b1 = Serial.parseFloat();
        x5_b1 = Serial.parseFloat();
        y5_b1 = Serial.parseFloat();
    
    	estadoRecepcion = CALCULA_DETERMINANTE;
   	  break;
    case CALCULA_DETERMINANTE:
      // Aquí iría el código para calcular el determinante
      	determinante = u2_x * v2_y - u2_y * v2_x;
      	estadoRecepcion = CONVERSION_B1_A_CANONICA;
      break;
    case CONVERSION_B1_A_CANONICA:
      // Aquí iría el código para hacer la conversión de las coordenadas de la primera base no canónica B1 a la base canónica
        x1_canonica = u1_x * x1_b1 + v1_x * y1_b1;
        y1_canonica = u1_y * x1_b1 + v1_y * y1_b1;
        x2_canonica = u1_x * x2_b1 + v1_x * y2_b1;
        y2_canonica = u1_y * x2_b1 + v1_y * y2_b1;
        x3_canonica = u1_x * x3_b1 + v1_x * y3_b1;
        y3_canonica = u1_y * x3_b1 + v1_y * y3_b1;
        x4_canonica = u1_x * x4_b1 + v1_x * y4_b1;
        y4_canonica = u1_y * x4_b1 + v1_y * y4_b1;
        x5_canonica = u1_x * x5_b1 + v1_x * y5_b1;
        y5_canonica = u1_y * x5_b1 + v1_y * y5_b1;

        estadoRecepcion = CONVERSION_CANONICA_A_B2;
      break;
    case CONVERSION_CANONICA_A_B2:
      // Aquí iría el código para hacer la conversión de las coordenadas de la base canónica a la segunda base no canónica B2
      float x1_b2, y1_b2, x2_b2, y2_b2, x3_b2, y3_b2, x4_b2, y4_b2, x5_b2, y5_b2;
        x1_b2 = (x1_canonica * v2_y - y1_canonica * v2_x) / determinante;
        y1_b2 = (-x1_canonica * u2_y + y1_canonica * u2_x) / determinante;
        x2_b2 = (x2_canonica * v2_y - y2_canonica * v2_x) / determinante;
        y2_b2 = (-x2_canonica * u2_y + y2_canonica * u2_x) / determinante;
        x3_b2 = (x3_canonica * v2_y - y3_canonica * v2_x) / determinante;
        y3_b2 = (-x3_canonica * u2_y + y3_canonica * u2_x) / determinante;
        x4_b2 = (x4_canonica * v2_y - y4_canonica * v2_x) / determinante;
        y4_b2 = (-x4_canonica * u2_y + y4_canonica * u2_x) / determinante;
        x5_b2 = (x5_canonica * v2_y - y5_canonica * v2_x) / determinante;
        y5_b2 = (-x5_canonica * u2_y + y5_canonica * u2_x) / determinante;
    	
    	estadoRecepcion = MOSTRAR_DATOS;
      break;
    case MOSTRAR_DATOS:
        Serial.println("Coordenadas en la base canonica:");
        Serial.print("Punto 1: (");
        Serial.print(x1_canonica);
        Serial.print(", ");
        Serial.print(y1_canonica);
        Serial.println(")");
        Serial.print("Punto 2: (");
        Serial.print(x2_canonica);
        Serial.print(", ");
        Serial.print(y2_canonica);
        Serial.println(")");
        Serial.print("Punto 3: (");
        Serial.print(x3_canonica);
        Serial.print(", ");
        Serial.print(y3_canonica);
        Serial.println(")");
        Serial.print("Punto 4: (");
        Serial.print(x4_canonica);
        Serial.print(", ");
        Serial.print(y4_canonica);
        Serial.println(")");
        Serial.print("Punto 5: (");
        Serial.print(x5_canonica);
        Serial.print(", ");
        Serial.print(y5_canonica);
        Serial.println(")");

        Serial.println("Coordenadas en la base no canonica B2:");
        Serial.print("Punto 1: (");
        Serial.print(x1_b2);
        Serial.print(", ");
        Serial.print(y1_b2);
        Serial.println(")");
        Serial.print("Punto 2: (");
        Serial.print(x2_b2);
        Serial.print(", ");
        Serial.print(y2_b2);
        Serial.println(")");
        Serial.print("Punto 3: (");
        Serial.print(x3_b2);
        Serial.print(", ");
        Serial.print(y3_b2);
        Serial.println(")");
        Serial.print("Punto 4: (");
        Serial.print(x4_b2);
        Serial.print(", ");
        Serial.print(y4_b2);
        Serial.println(")");
        Serial.print("Punto 5: (");
        Serial.print(x5_b2);
        Serial.print(", ");
        Serial.print(y5_b2);
        Serial.println(")");
 
        estadoPrincipal = ESPERA_SENAL;
      break;  
  } 

  switch (estadoPrincipal) {
    case ESPERA_SENAL:
      // Aquí iría el código para esperar la señal procedente de la FSM de recepción
    	estadoPrincipal = VERIFICA_COORD;
      break;
    
    case VERIFICA_COORD:
      if (x1_canonica > 127 || x1_canonica < -127 || y1_canonica > 127 || y1_canonica < -127) {
    
    estadoRecepcion = ESPERA_COORDENADAS;
  }  else {
    // Si las coordenadas están dentro del rango, procedemos a verificar las bases.
    estadoPrincipal = VERIFICA_BASES;
  }  
      break;
    case VERIFICA_BASES:
      // Aquí iría el código para verificar que las bases estén dentro del rango
    	if (u1_x > 20 || u1_x < -20 || u1_y > 20 || u1_y < -20 || v1_x > 20 || v1_x < -20 || v1_y > 20 || v1_y < -20) {
      
    	estadoRecepcion = ESPERA_COORDENADAS; // por ejemplo, puedes volver al estado ESPERA_SENAL
        }  else {
      estadoPrincipal = VERIFICA_SWITCH;
  } 
    	
      break;
    case VERIFICA_SWITCH:
      // Aquí iría el código para verificar el estado del interruptor
    if(digitalRead(switchPin) == HIGH) {
        pelotaPresente = true;
      }  else {
        pelotaPresente = false;
      } 
      	estadoPrincipal = ENVIA_SENAL;
      break;
    
    case ENVIA_SENAL:
  		if (switchPin == HIGH) {
    	estadoEjecucion = CALCULA_DISTANCIA;
    // Espera a que estadoEjecucion indique que ha terminado un movimiento
    	while (estadoEjecucion != ESPERA_PELOTA) {
      delay(100); // Puede ajustar este retraso como sea necesario
    } 
  } 
  estadoPrincipal = ESPERA_SENAL; // Regresa a su estado inicial
  break;
  } 
		
   	 float d1, d2, d3, d4, d5, d6; //Distancias para cada coordenada incluyendo el regreso al origen
     float a1, a2, a3, a4, a5, a6; //Angulos de giro
  
  float x1_delta, y1_delta, x2_delta, y2_delta, x3_delta, y3_delta, x4_delta, y4_delta, x5_delta, y5_delta;
  
  switch (estadoEjecucion) { 
      
    case CALCULA_DISTANCIA:
    if (switchPin == HIGH)
    {
      // Aquí iría el código para calcular las distancias
       x1_delta = x1_canonica - 0;
  	   x2_delta = x2_canonica - x1_canonica;
  	   x3_delta = x3_canonica - x2_canonica;
  	   x4_delta = x4_canonica - x3_canonica;
  	   x5_delta = 0 - x5_canonica;
      
  	   y1_delta = y1_canonica - 0;
  	   y2_delta = y2_canonica - y1_canonica;
  	   y3_delta = y3_canonica - y2_canonica;
  	   y4_delta = y4_canonica - y3_canonica;
  	   y5_delta = 0 - y5_canonica;
    
      	d1 = sqrt(sq(x1_delta) + sq(y1_delta));
     
  	  	d2 = sqrt(sq(x2_delta) + sq(y2_delta));
  
  	  	d3 = sqrt(sq(x3_delta) + sq(y3_delta));
  
  	  	d4 = sqrt(sq(x4_delta) + sq(y4_delta));
  
  	  	d5 = sqrt(sq(x5_delta) + sq(y5_delta));
    
    	estadoEjecucion = CALCULA_ANGULO;
      break;
    }
    case CALCULA_ANGULO:
      // Aquí iría el código para calcular los angulos
        a1 = atan2(y1_delta, x1_delta)*180 / PI;

        a2 = atan2(y2_delta, x2_delta)*180 / PI;
        a2 = a2 - a1;

        a3 = atan2(y3_delta, x3_delta)*180 / PI;
        a3 = a3 - a2;

        a4 = atan2(y4_delta, x4_delta)*180 / PI;
        a4 = a4 - a3;

        a5 = atan2(y5_delta, x5_delta)*180 / PI;
        a5 = a5 - a4;
        
    	estadoEjecucion = MOSTRAR_DISTANCIA_ANGULOS;
      break;
    case MOSTRAR_DISTANCIA_ANGULOS:
        Serial.print("Distancia 1: ");
        Serial.println(d1);
        Serial.print("Angulo 1: ");
        Serial.println(a1);
        Serial.print("Distancia 2: ");
        Serial.println(d2);
        Serial.print("Angulo 2: ");
        Serial.println(a2);
        Serial.print("Distancia 3: ");
        Serial.println(d3);
        Serial.print("Angulo 3: ");
        Serial.println(a3);
        Serial.print("Distancia 4: ");
        Serial.println(d4);
        Serial.print("Angulo 4: ");
        Serial.println(a4);
        Serial.print("Distancia 5: ");
        Serial.println(d5);
        Serial.print("Angulo 5: ");
        Serial.println(a5);

        estadoEjecucion = ACTIVA_MOTORES;
      break;
    case ACTIVA_MOTORES:
  
      estadoEjecucion = ESPERA_PELOTA;
  	
      break;
  }
}