#!/usr/bin/env python3
"""
Simulador GPS NMEA para ESP32 CYD (Cluster Ford Ka)
Transmite sentencias NMEA 0183 ($GPRMC, $GPGGA, $GPVTG) a 9600 bps
a través del puerto serie USB (/dev/ttyUSB0).

Permite probar y calibrar la telemetría GPS de la pantalla CYD
(Velocímetro GPS, Reloj Satelital, Brújula, Altitud, Coordenadas, Satélites)
sin requerir un módulo GPS físico ni modificar el firmware del ESP32.
"""

import sys
import time
import math
import serial
import argparse
from datetime import datetime, timezone

def nmea_checksum(sentence: str) -> str:
    """Calcula el checksum XOR estándar NMEA 0183"""
    c = 0
    for ch in sentence:
        c ^= ord(ch)
    return f"{c:02X}"

def build_nmea(body: str) -> bytes:
    """Envuelve la trama con '$' y '*CHECKSUM\\r\\n'"""
    chk = nmea_checksum(body)
    return f"${body}*{chk}\r\n".encode("ascii")

def run_gps_emulator(port="/dev/ttyUSB0", baud=9600, rate_hz=2.0, speed_sim=True, fixed_speed=70.0):
    print("=" * 60)
    print("  SIMULADOR GPS NMEA PARA ESP32 CYD")
    print("=" * 60)
    print(f"Puerto:   {port}")
    print(f"Baudrate: {baud} bps (8N1)")
    print(f"Tasa:     {rate_hz} Hz ({1.0/rate_hz:.2f}s por trama)")
    print(f"Modo:     {'Ciclo dinámico de velocidad' if speed_sim else f'Velocidad fija {fixed_speed} km/h'}")
    print("-" * 60)

    try:
        ser = serial.Serial()
        ser.port = port
        ser.baudrate = baud
        ser.timeout = 1
        ser.dtr = False
        ser.rts = False
        ser.open()
        print(f"[OK] Conectado exitosamente a {port}")
    except Exception as e:
        print(f"[ERROR] No se pudo abrir {port}: {e}")
        print("Verifica que la placa CYD esté conectada por USB y que tengas permisos.")
        sys.exit(1)

    # Coordenadas iniciales simuladas (Buenos Aires, Argentina)
    # -34.6037 S, -58.3816 W
    base_lat = -34.6037
    base_lon = -58.3816
    altitude = 35.0  # metros
    heading = 45.0   # grados (Noreste)
    satellites = 9

    t_start = time.time()
    interval = 1.0 / rate_hz

    try:
        while True:
            t = time.time()
            dt = t - t_start

            # 1. Simulación de velocidad
            if speed_sim:
                # Ciclo de 30 segundos: aceleración, crucero, desaceleración
                phase = dt % 30.0
                if phase < 5.0:
                    current_speed = 0.0
                elif phase < 15.0:
                    # Acelera de 0 a 85 km/h
                    current_speed = (phase - 5.0) / 10.0 * 85.0
                elif phase < 22.0:
                    # Crucero a 85 km/h con pequeña variación
                    current_speed = 85.0 + math.sin(phase) * 3.0
                else:
                    # Frena de 85 a 0 km/h
                    current_speed = max(0.0, 85.0 * (1.0 - (phase - 22.0) / 8.0))
            else:
                current_speed = fixed_speed

            # Variación suave del rumbo según movimiento
            if current_speed > 1.0:
                heading = (heading + 1.5 * interval) % 360.0

            # Velocidad en nudos para NMEA (1 km/h = 0.539957 nudos)
            speed_knots = current_speed * 0.539957

            # 2. Hora y fecha UTC actual
            now = datetime.now(timezone.utc)
            time_str = now.strftime("%H%M%S.00")
            date_str = now.strftime("%d%m%y")

            # Formato de coordenadas NMEA: DDMM.MMMM
            lat_deg = int(abs(base_lat))
            lat_min = (abs(base_lat) - lat_deg) * 60.0
            lat_dir = "S" if base_lat < 0 else "N"
            lat_nmea = f"{lat_deg:02d}{lat_min:07.4f}"

            lon_deg = int(abs(base_lon))
            lon_min = (abs(base_lon) - lon_deg) * 60.0
            lon_dir = "W" if base_lon < 0 else "E"
            lon_nmea = f"{lon_deg:03d}{lon_min:07.4f}"

            # 3. Construcción de sentencias NMEA
            # A. $GPRMC: Sentencia recomendada mínima (Hora, Estado A, Lat, Lon, Nudos, Rumbo, Fecha)
            rmc = f"GPRMC,{time_str},A,{lat_nmea},{lat_dir},{lon_nmea},{lon_dir},{speed_knots:.1f},{heading:.1f},{date_str},,,A"

            # B. $GPGGA: Datos de fijación GPS (Hora, Lat, Lon, Calidad 1, Satélites, HDOP, Altura)
            gga = f"GPGGA,{time_str},{lat_nmea},{lat_dir},{lon_nmea},{lon_dir},1,{satellites:02d},0.9,{altitude:.1f},M,16.0,M,,"

            # C. $GPVTG: Rumbo y velocidad respecto al fondo (Rumbo, T, Nudos, N, Km/h, K)
            vtg = f"GPVTG,{heading:.1f},T,,M,{speed_knots:.1f},N,{current_speed:.1f},K,A"

            # Enviar ráfaga NMEA al ESP32 CYD
            ser.write(build_nmea(rmc))
            ser.write(build_nmea(gga))
            ser.write(build_nmea(vtg))
            ser.flush()

            # Feedback en consola en una sola línea interactiva
            print(f"\r[GPS Sim] Satélites: {satellites} | Vel: {current_speed:5.1f} km/h | Rumbo: {heading:5.1f}° | Alt: {altitude:4.1f}m | Hora UTC: {now.strftime('%H:%M:%S')}", end="", flush=True)

            time.sleep(interval)

    except KeyboardInterrupt:
        print("\n\n[INFO] Deteniendo simulador GPS...")
    finally:
        ser.close()
        print("[OK] Puerto serie cerrado.")

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="Simulador GPS NMEA para ESP32 CYD")
    parser.add_argument("--port", default="/dev/ttyUSB0", help="Puerto serie de la placa CYD (default: /dev/ttyUSB0)")
    parser.add_argument("--baud", type=int, default=9600, help="Baudrate serie (default: 9600)")
    parser.add_argument("--rate", type=float, default=2.0, help="Frecuencia en Hz (default: 2.0 Hz)")
    parser.add_argument("--speed", type=float, default=None, help="Velocidad fija en km/h (desactiva ciclo)")
    args = parser.parse_args()

    sim_speed = (args.speed is None)
    fix_spd = args.speed if args.speed is not None else 70.0
    run_gps_emulator(port=args.port, baud=args.baud, rate_hz=args.rate, speed_sim=sim_speed, fixed_speed=fix_spd)