import mpu6050
from machine import Pin, I2C, sleep
import math
import time
import network
import utime
import usocket
import machine

# Inicializar el sensor MPU6050
i2c = I2C (scl =Pin(22), sda = Pin(21))
mpu = mpu6050.accel(i2c)

# Variables para almacenar los valores de los sensores
ax, ay, az = 0, 0, 0
gx, gy, gz = 0, 0, 0

# Variables para los ángulos de inclinación
ang_x, ang_y = 0, 0
ang_x_prev, ang_y_prev = 0, 0
tiempo_prev = time.ticks_ms()

def updateFiltered():
    global ang_x, ang_y, ang_x_prev, ang_y_prev, tiempo_prev

    dt = (time.ticks_ms() - tiempo_prev) / 1000.0
    tiempo_prev = time.ticks_ms()

    # Leer los valores de los sensores
    temps = (mpu.get_values())
    #print(temps)
    
    az = temps ["AcZ"] # aceleracion en z 
    ay = temps ["AcY"] # acelaracion en y
    ax = temps ["AcX"] # aceleracion en x
   
    gy = temps ["GyY"] # velocidad angular en y
    gx = temps ["GyX"] # velocidad angular en x 
    gz = temps ["GyZ"] # velocidad angular en z

    # Calcular los ángulos con el acelerómetro
    accel_ang_x = math.atan(ay / math.sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / math.pi)
    accel_ang_y = math.atan(-ax / math.sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / math.pi)

    # Calcular el ángulo de rotación con el giroscopio y el filtro complementario
    ang_x = 0.98 * (ang_x_prev + (gx / 131) * dt) + 0.02 * accel_ang_x
    ang_y = 0.98 * (ang_y_prev + (gy / 131) * dt) + 0.02 * accel_ang_y

    ang_x_prev = ang_x
    ang_y_prev = ang_y
    
def trans_tcp():
    global ang_x, ang_y
    """
    #Conection with WiFi----------------------------------------------------------
    wf = network.WLAN(network.STA_IF)  # # Obtener el objeto WLAN (Wireless LAN) en modo estación
    wf.active(True) # Activar la interfaz WLAN
    #wf.connect("nombre de red","contraseña") # Conectar a la red WiFi especificada
    wf.connect("Comunidad Innovadores","INn0V4-2K23!*")
    
    while not wf.isconnected():
        print(".")
        utime.sleep(1)
    print(wf.ifconfig()[0]) #192.168.0.10 IP  # Imprimir la dirección IP asignada al ESP32

    s = usocket.socket()  # Crear un nuevo objeto de socket
    s.bind(("10.200.172.148",2020))#Parametros "direccion IP", "puerto"
    s.listen(2) # Escuchar conexiones entrantes, con un máximo de 10 conexiones en espera

    print("Servidor iniciado, esperando conexion")
    """
    server = True
    
    while server:
        """
        # TCP comunication------------------------
        (sc,addr) =s.accept()  # Aceptar una nueva conexión entrante
        print(addr)  # Imprimir la dirección del cliente que se ha conectado
        """
        time.sleep(2)
        while True:
            updateFiltered() #Sensor value
            mensaje_enviar = "RX" + str(ang_x)
            print(mensaje_enviar)
            #sc.send(mensaje_enviar.encode())
            time.sleep(0.1)
            mensaje_enviar = "RY" + str(ang_y)
            print(mensaje_enviar)
            #sc.send(mensaje_enviar.encode())            
            time.sleep(0.1)
        #-----------------------------------------
        #sc.close() # Cerrar el socket servidor
        #s.close() # Cerrar el socket servidor

trans_tcp()