#include <Arduino.h>

// ---------------- CONFIG ----------------

const int LED_PIN = 15;
const int BUZZER_PIN = 14;
const int SERVO_PIN = 13;
const int TRIGGER_PIN = 2;
const int ECHO_PIN = 1;

// Posiciones del servo (valores para PWM en Raspberry Pi Pico)
// En MicroPython duty_u16 usa valores de 0-65535
const int POS_SMALL = 2000;
const int POS_MEDIUM = 4000;
const int POS_LARGE = 6000;
const int POS_HOME = 4000;

int current_position = POS_HOME;

// ---------------- FUNCIONES ----------------

void mover_suave(int destino) {
    int paso = 50;
    
    if (destino > current_position) {
        while (current_position < destino) {
            current_position += paso;
            analogWrite(SERVO_PIN, current_position);
            delay(10);
        }
    } else {
        while (current_position > destino) {
            current_position -= paso;
            analogWrite(SERVO_PIN, current_position);
            delay(10);
        }
    }
}

void beep() {
    digitalWrite(BUZZER_PIN, HIGH);
    delay(100);
    digitalWrite(BUZZER_PIN, LOW);
}

float obtener_distancia() {
    digitalWrite(TRIGGER_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PIN, LOW);

    unsigned long timeout = micros();
    while (digitalRead(ECHO_PIN) == LOW) {
        if (micros() - timeout > 30000) {
            return -1;
        }
    }

    unsigned long inicio = micros();
    timeout = micros();

    while (digitalRead(ECHO_PIN) == HIGH) {
        if (micros() - timeout > 30000) {
            return -1;
        }
    }

    unsigned long fin = micros();

    unsigned long duracion = fin - inicio;
    float distancia = (duracion * 0.0343) / 2.0;
    return distancia;
}

// ---------------- SETUP ----------------

void setup() {
    Serial.begin(115200);
    
    pinMode(LED_PIN, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);
    pinMode(SERVO_PIN, OUTPUT);
    pinMode(TRIGGER_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    
    // Configurar frecuencia PWM para el servo (50Hz)
    analogWriteFreq(50);
    
    // Posición inicial
    analogWrite(SERVO_PIN, current_position);
    
    Serial.println("Sistema de Clasificación Automática Iniciado");
}

// ---------------- LOOP PRINCIPAL ----------------

void loop() {
    float d = obtener_distancia();

    if (d > 0 && d < 30) {  // Solo actúa si hay objeto cercano
        Serial.print("Distancia: ");
        Serial.print(d);
        Serial.println(" cm");

        digitalWrite(LED_PIN, HIGH);
        beep();

        if (d < 10) {
            Serial.println("Objeto PEQUEÑO");
            mover_suave(POS_SMALL);

        } else if (d < 20) {
            Serial.println("Objeto MEDIANO");
            mover_suave(POS_MEDIUM);

        } else {
            Serial.println("Objeto GRANDE");
            mover_suave(POS_LARGE);
        }

        // Espera antes de regresar
        delay(500);

        Serial.println("Regresando a posición inicial...");
        mover_suave(POS_HOME);

        digitalWrite(LED_PIN, LOW);

        delay(1000);  // Pequeña pausa antes de siguiente lectura
    }

    delay(200);
}