# Hecho por Juan Leiva
import time
from ssd1306 import SSD1306_I2C
from hcsr04 import HCSR04
from machine import I2C, Pin

import math

detecting = False

TRIGGER_PIN = 4
ECHO_PIN = 5

LED_PIN = 14
I2C_SDA = 23
I2C_SCL = 22
HIGH = 1
LOW = 0

MIN_DISTANCE = 10  # Distancia mínima para detectar mosquitos (cm)
SCAN_DURATION = 3  # Duración de cada escaneo (segundos)

trigger_pin = Pin(TRIGGER_PIN, Pin.OUT)
hcsr04 = HCSR04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN)

try:
    i2c = I2C(0, scl=Pin(22), sda=Pin(23))
    oled = SSD1306_I2C(128, 64, i2c)
    led = Pin(LED_PIN, Pin.OUT)
    # Eliminamos la inicialización del servo
    # servo = SERVO(SERVO_PIN)
    print("Hardware inicializado correctamente.")
except Exception as e:
    print(f"Error en la inicialización del hardware: {e}")
    oled = None
    led = None

def send_trigger_pulse():
    trigger_pin.on()
    time.sleep(0.00001)
    trigger_pin.off()

def measure_distance():
    try:
        distance_cm = hcsr04.distance_cm()
        return distance_cm
    except Exception as e:
        print(f"Error al medir la distancia: {e}")
        return 0

def draw_radar(angle, distance):
    """Dibuja el radar en la pantalla OLED."""
    try:
        if oled is not None:
            oled.fill(0)
            x = int(distance * math.cos(angle * math.pi / 180))
            y = int(distance * math.sin(angle * math.pi / 180))
            display_width = 128
            display_height = 64
            oled.circle((display_width // 2, display_height // 2), 3, color=0xFFFFFF)
            oled.line((display_width // 2, display_height // 2), (x + display_width // 2, y + display_height // 2), color=0xFFFFFF)
            oled.text(f"Distancia: {distance} cm", 0, 0, color=0xFFFFFF)
            oled.show()
    except Exception as e:
        print(f"Error al dibujar en la pantalla OLED: {e}")

def scan_for_mosquitos():
    global detecting

    detecting = False

    for angle in range(0, 360, 3):
        distance_cm = measure_distance()

        if distance_cm <= MIN_DISTANCE:
            detecting = True
            break

        draw_radar(angle, distance_cm)
        time.sleep(0.1)

    if detecting and led is not None:
        led.on()
    else:
        if led is not None:
            led.off()


while True:
    try:
        scan_for_mosquitos()
        # Eliminamos la función set_servo_angle ya que no se usa servo
        # time.sleep(0.1)
    except Exception as e:
        print(f"Error en el bucle principal: {e}")

def digitalWrite(pin, value):
    if value == HIGH:
        gpio.output(pin, 1)
    else:
        gpio.output(pin, 0)

def digitalRead(pin):
    return gpio.input(pin)

def sleep(seconds):
    time.sleep(0.1)