#include <Servo.h>
#include <MsTimer2.h> // Incluir la biblioteca MsTimer2

// Definición de pines
const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;
const int buttonPin1 = 6;
const int servoPin1 = 7;
const int servoPin2 = 8;
const int trigPin3 = 9;
const int echoPin3 = 10;
const int trigPin4 = 11;
const int echoPin4 = 12;

Servo servo1;
Servo servo2;

class Sensor {
public:
    int trigPin;
    int echoPin;

    Sensor(int tPin, int ePin) : trigPin(tPin), echoPin(ePin) {
        pinMode(trigPin, OUTPUT);
        pinMode(echoPin, INPUT);
    }

    long readDistance() {
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);

        long duration = pulseIn(echoPin, HIGH);
        return duration * 0.034 / 2; // Convertir a cm
    }
};

class MachineState {
public:
    enum State {
        IDLE,
        SENSING,
        SENSING2,
        SENSING3,
        SENSING4,
        ACTION
    };

    State currentState;

    MachineState() : currentState(IDLE) {}

    void updateState(Sensor& sensor1, Sensor& sensor2, Sensor& sensor3, Sensor& sensor4, bool buttonPressed) {
        long distance1 = sensor1.readDistance();
        long distance2 = sensor2.readDistance();
        long distance3 = sensor3.readDistance();
        long distance4 = sensor4.readDistance();

        switch (currentState) {
            case IDLE:
                if (distance1 <= 20) {
                    Serial.println("Colectivo aproximandose");
                    currentState = SENSING;
                }
                break;

            case SENSING:
                if (distance1 > 20 && distance2 <= 20) {
                    Serial.println("Dar paso");
                    currentState = SENSING2;
                }
                break;
                
            
            case SENSING2:
                if (distance2 <= 20 && buttonPressed == HIGH) { 
                    Serial.println("Abriendo barreras...");
                    servo1.write(0);
                    delay(1000); // Esperar un segundo
                    servo2.write(0);
                    delay(1000);
                    currentState = SENSING3;
                }
                break;

            case SENSING3:
                if (distance2 > 20 && distance3 <= 20) {
                    Serial.println("Cruzando...");
                    servo1.write(90);
                    currentState = SENSING4;
                }
                break;

             case SENSING4:
                if (distance3 > 20 && distance4 <= 20) {
                    Serial.println("Cerrando barreras");
                    servo1.write(90);
                    delay(1000); // Esperar un segundo
                    servo2.write(90);
                    delay(1000);
                    currentState =ACTION;
                }
                break;        

            case ACTION:
                // Esperar un tiempo o resetear a IDLE
                if (distance3 > 20 && distance4 > 20) {
                    reset();
                }
                break;
        }
    }

    void reset() {
        servo1.write(90);
        servo2.write(90);
        currentState = IDLE;
    }
};

MachineState fsm;
Sensor sensor1(trigPin1, echoPin1);
Sensor sensor2(trigPin2, echoPin2);
Sensor sensor3(trigPin3, echoPin3);
Sensor sensor4(trigPin4, echoPin4);

void updateFSM() {
    bool buttonPressed = digitalRead(buttonPin1) == LOW; // Activo cuando se presiona
    fsm.updateState(sensor1, sensor2, sensor3, sensor4, buttonPressed);
}

void setup() {
    Serial.begin(9600);
    servo1.attach(servoPin1);
    servo2.attach(servoPin2);
    pinMode(buttonPin1, INPUT_PULLUP);

    // Iniciar el temporizador para que llame a updateFSM cada 100 ms (0.1 segundos)
    MsTimer2::set(100, updateFSM); 
    MsTimer2::start(); // Iniciar el temporizador
}

void loop() {
    // El loop se deja vacío, ya que el temporizador llamará a updateFSM periódicamente
}