mport sys
import os
from machine import Pin, PWM, ADC
import time
import math

# Constante de gravedad en m/s^2
GRAVEDAD = 9.81

# Ángulo de inclinación en grados
ANGULO_INCLINACION = 0  # Cambia este valor según el ángulo específico de la celda

class Board:
    class BoardType:
        PICO_W = 'Raspberry Pi Pico W'
        PICO = 'Raspberry Pi Pico'
        RP2040 = 'RP2040'
        ESP8266 = 'ESP8266'
        ESP32 = 'ESP32'
        UNKNOWN = 'Unknown'

    def __init__(self):
        self.type = self.detect_board_type()
        self.led_pin = None
        self.adc = None
        self.servo = None

    def detect_board_type(self):
        sysname = os.uname().sysname.lower()
        machine = os.uname().machine.lower()

        # Detectar Raspberry Pi Pico W y Pico
        if sysname == 'rp2' and 'pico w' in machine:
            return self.BoardType.PICO_W
        elif sysname == 'rp2' and 'pico' in machine:
            return self.BoardType.PICO
        elif sysname == 'rp2' and 'rp2040' in machine:
            return self.BoardType.RP2040

        # Detectar ESP8266
        elif sysname == 'esp8266':
            return self.BoardType.ESP8266

        # Detectar ESP32
        elif sysname == 'esp32' and 'esp32' in machine:
            return self.BoardType.ESP32

        # Desconocido
        else:
            return self.BoardType.UNKNOWN

    def configure_hardware(self):
        # Configuración del LED dependiendo del tipo de tarjeta
        if self.type == self.BoardType.PICO_W:
            self.led_pin = Pin("LED", Pin.OUT)  # LED interno para Raspberry Pi Pico W
            self.adc = ADC(Pin(26))  # ADC en el pin 26 para Pico W
            self.servo = PWM(Pin(15))  # PWM en el pin 15 para el servo en Pico W
        elif self.type == self.BoardType.PICO or self.type == self.BoardType.RP2040:
            self.led_pin = Pin(25, Pin.OUT)  # LED interno para Raspberry Pi Pico y RP2040
            self.adc = ADC(Pin(26))  # ADC en el pin 26 para Pico y RP2040
            self.servo = PWM(Pin(15))  # PWM en el pin 15 para el servo
        elif self.type == self.BoardType.ESP8266:
            self.led_pin = Pin(2, Pin.OUT)  # GPIO 2 para el ESP8266 (LED interno)
            self.adc = ADC(0)  # El ESP8266 solo tiene un ADC en el canal ADC0
            self.servo = PWM(Pin(14))  # PWM en el pin 14 para el servo en ESP8266
        elif self.type == self.BoardType.ESP32:
            self.led_pin = Pin(2, Pin.OUT)  # GPIO 2 para el ESP32 (LED integrado)
            self.adc = ADC(Pin(32))  # ADC en el pin 32 para ESP32
            self.servo = PWM(Pin(15))  # PWM en el pin 15 para el servo en ESP32
        else:
            print("Placa desconocida, configurando pines por defecto.")
            self.led_pin = Pin(2, Pin.OUT)  # LED en GPIO 2
            self.adc = ADC(Pin(26))  # ADC en el pin 26 por defecto
            self.servo = PWM(Pin(15))  # PWM en el pin 15 por defecto

        # Configurar la frecuencia del servo motor en 50 Hz
        self.servo.freq(50)

    def blink_led(self, blink_times=3, delay=0.5):
        if self.led_pin:
            for _ in range(blink_times):
                self.led_pin.value(1)  # Encender LED
                print("LED Encendido")
                time.sleep(delay)
                self.led_pin.value(0)  # Apagar LED
                print("LED Apagado")
                time.sleep(delay)
        else:
            print("No se pudo configurar el LED. Verifique el tipo de tarjeta.")

    def set_servo_angle(self, adc_value):
        # Mapear el valor del ADC (0-65535) al rango de 0-180 grados
        angle = (adc_value / 65535) * 180
        
        # Mapear el ángulo a ciclo de trabajo PWM (0.5ms a 2.5ms para 0° a 180°)
        duty = int((angle / 180) * (125 - 25) + 25)  # Ajuste para ciclos de trabajo en microsegundos (PWM)
        self.servo.duty_u16(duty)
        return angle

# Clase HX711 para manejar el módulo de celdas de carga
class HX711:
    def __init__(self, data_pin, clock_pin):
        self.data_pin = Pin(data_pin, Pin.IN, pull=Pin.PULL_DOWN)
        self.clock_pin = Pin(clock_pin, Pin.OUT)
        self.clock_pin.value(0)

    def read_raw(self):
        count = 0
        while self.data_pin.value() == 1:
            pass  # Espera a que el pin de datos sea bajo
        
        for i in range(24):  # Leer 24 bits de datos
            self.clock_pin.value(1)
            count = (count << 1) | self.data_pin.value()
            self.clock_pin.value(0)
        
        # Ajuste del bit de signo (para valores negativos)
        self.clock_pin.value(1)
        count ^= 0x800000
        self.clock_pin.value(0)
        return count

    def read_mass(self, offset=0, scale=1):
        # Calcular la masa en función de la lectura y la escala
        raw_value = self.read_raw()
        return (raw_value - offset) / scale

    def read_force(self, offset=0, scale=1):
        # Calcular la fuerza aplicando la fórmula F = m * g
        mass = self.read_mass(offset, scale)
        return mass * GRAVEDAD

def map_value(value, in_min, in_max, out_min, out_max):
    """Mapea el valor de un rango a otro rango."""
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Uso de la clase Board y HX711
board = Board()
print("Tipo de tarjeta detectada:", board.type)

# Configurar el hardware según la tarjeta detectada
board.configure_hardware()

# Parpadeo del LED integrado
print("Iniciando el parpadeo del LED")
board.blink_led(blink_times=1, delay=0.5)

# Configurar dos módulos HX711 para los ejes x y z
# Cambia los pines a los que tengas conectados cada HX711
hx711_z = HX711(data_pin=4, clock_pin=5)  # Configuración para eje z
print("Iniciando el parpadeo del LED")
# Calibración inicial
offset_z = hx711_z.read_raw()  # Leer el valor sin peso en z
scale_z = 420  # Factor de escala para el eje z; ajusta según tu calibración

print("Iniciando la lectura de vector de fuerza con inclinación y control de servo...")
try:
    while True:
        # Leer el valor del ADC
        adc_value = board.adc.read_u16()
        ANGULO_INCLINACION=board.set_servo_angle(adc_value)
        # Convertir el ángulo a radianes para las funciones trigonométricas
        angulo_radianes = math.radians(ANGULO_INCLINACION)
        # Convertir el ángulo al ciclo de trabajo PWM (0.5 ms a 2.5 ms en microsegundos)
        duty = map_value(ANGULO_INCLINACION, 0, 180, 400, 4500)  # Ajuste para un rango típico de servos
        
        # Establecer el ciclo de trabajo en el servo
        board.servo.duty_u16(duty)
        
        # Mostrar valores
        print("Potenciómetro:", adc_value, "Ángulo del Servo:", ANGULO_INCLINACION, "Duty:", duty)
        
        # Obtener fuerza en bruto en cada eje
        force_z_raw = hx711_z.read_force(offset_z, scale_z)
        
        # Ajustar según la inclinación
        force_z = force_z_raw * math.cos(angulo_radianes)

        print("ADC Value:", adc_value)
        print("Servo Angle:", ANGULO_INCLINACION)
        print("Fuerza en Z ajustada:", force_z, "N")
        print("Magnitud del vector de fuerza ajustado:", force_z, "N")
        board.blink_led(blink_times=1, delay=1)
except KeyboardInterrupt:
    print("Lectura finalizada.")