#!/usr/bin/env python3
"""
OBD-II / ELM327 Bluetooth Emulator for ESP32 CYD Gauge
Simulates an ELM327 v1.5 Bluetooth adapter and vehicle ECU.
"""

import sys
import os
import time
import math
import select
import socket
import threading
import argparse
import dbus
import dbus.service
from dbus.mainloop.glib import DBusGMainLoop
from gi.repository import GLib

# Vehicle Simulation State
class VehicleState:
    def __init__(self):
        self.rpm = 2250.0          # RPM (Sweet spot default)
        self.target_rpm = 2200.0   # Target RPM for smooth transitions
        self.speed = 70.0          # km/h
        self.coolant_temp = 90.0   # °C
        self.oil_temp = 93.0       # °C
        self.trans_temp = 82.0     # °C (Ford T6 221E1C)
        self.load = 58.0           # % (Sweet spot: 45% - 82%)
        self.map_psi = 14.7        # psi (Manifold Absolute Pressure)
        self.pcm_volt = 14.2       # Volts (MUST be > 12.5V or gauge will sleep)
        self.auto_simulate = True
        self.mode = "cycle"        # "cycle" or "eco" (modo fijo)
        self.lock = threading.Lock()

    def update_simulation(self):
        """Simulates driving telemetry with clear transitions through color zones and holds at max scale"""
        t = time.time()
        with self.lock:
            if not self.auto_simulate:
                return

            if self.mode == "eco":
                # MODO ECO FIJO: Bloquea telemetría permanentemente en el punto dulce óptimo
                self.pcm_volt = 14.2
                self.rpm = 2250.0          # Rango ECO: 1900 - 2550 RPM
                self.load = 58.0           # Rango ECO: 45% - 82%
                self.speed = 70.0          # km/h
                self.coolant_temp = 90.0   # °C
                self.oil_temp = 93.0
                self.map_psi = 14.5
                self.trans_temp = 82.0
                return

            # Ciclo de demostración de 36 segundos con FASE ECO dedicada y prolongada:
            #   0 -  4s: FASE 1 - Batería Baja Crítica (<12V) -> ROJO con alarma destellante (10.8V)
            #   4 -  8s: FASE 2 - Ralentí y reposo (12.4V, 850 RPM, Carga 28%)
            #   8 - 18s: FASE 3 - CRUCERO ÓPTIMO [ ECO ] (10 seg continuos: 2200 RPM, Carga 58%, 70 km/h, 14.2V) -> BADGE ACTIVO
            #  18 - 24s: FASE 4 - Exigido / autopista (3800 - 4800 RPM, Carga 88%, 120 km/h, 102°C) -> Sale de ECO
            #  24 - 30s: FASE 5 - MÁXIMO ABSOLUTO (100% escala: 6000 RPM, 160 km/h, 120°C, 16.0V) -> Todo en ROJO con alarma
            #  30 - 36s: FASE 6 - Descenso suave de vuelta al inicio
            cycle_period = 36.0
            phase = t % cycle_period

            if phase < 4.0:
                # Batería baja crítica (<12V) -> ROJO destellante
                self.pcm_volt = 10.8
                self.rpm = 800.0
                self.speed = 0.0
                self.coolant_temp = 86.0
                self.load = 25.0
            elif phase < 8.0:
                # Batería en reposo / baja carga (12.0 - 13.2V) -> AMARILLO
                self.pcm_volt = 12.4
                self.rpm = 850.0
                self.speed = 0.0
                self.coolant_temp = 88.0
                self.load = 28.0
            elif phase < 18.0:
                # FASE ECO: Marcha y crucero óptimo durante 10 segundos continuos (RPM: 2150-2300, Carga: 56%-62%)
                p = (phase - 8.0) / 10.0
                self.pcm_volt = 14.2
                self.rpm = 2200.0 + math.sin(p * math.pi) * 100.0
                self.load = 58.0 + math.sin(p * math.pi) * 4.0
                self.speed = 65.0 + p * 10.0
                self.coolant_temp = 90.0 + p * 2.0
            elif phase < 24.0:
                # Exigido / autopista -> AMARILLO
                p = (phase - 18.0) / 6.0
                self.pcm_volt = 14.9 + p * 0.2
                self.rpm = 3800.0 + p * (4800.0 - 3800.0)
                self.load = 85.0 + p * 8.0
                self.speed = 112.0 + p * (128.0 - 112.0)
                self.coolant_temp = 99.0 + p * (104.0 - 99.0)
            elif phase < 30.0:
                # MÁXIMO ABSOLUTO (100% de escala) -> ROJO con alarma destellante
                self.pcm_volt = 16.0
                self.rpm = 6000.0
                self.load = 96.0
                self.speed = 160.0
                self.coolant_temp = 120.0
            else:
                # Retorno progresivo
                p = (phase - 30.0) / 6.0
                self.pcm_volt = 16.0 - p * (16.0 - 10.8)
                self.rpm = 6000.0 - p * (6000.0 - 800.0)
                self.load = 96.0 - p * (96.0 - 25.0)
                self.speed = 160.0 - p * 160.0
                self.coolant_temp = 120.0 - p * (120.0 - 86.0)

            # Correlated auxiliary telemetry
            self.map_psi = 10.0 + 16.0 * ((self.rpm - 800.0) / 5200.0)
            self.oil_temp = self.coolant_temp + 3.0
            self.trans_temp = 82.0 + 1.0 * math.cos(t * 0.09)

state = VehicleState()

# BlueZ Profile for Serial Port Profile (SPP)
class SPPProfile(dbus.service.Object):
    def __init__(self, bus, path):
        super().__init__(bus, path)

    @dbus.service.method('org.bluez.Profile1', in_signature='', out_signature='')
    def Release(self):
        print("[BlueZ] Profile released")

    @dbus.service.method('org.bluez.Profile1', in_signature='oha{sv}', out_signature='')
    def NewConnection(self, path, fd, properties):
        raw_fd = fd.take()
        print(f"\n[Bluetooth D-Bus] ¡Nueva conexión aceptada desde {path}! (fd={raw_fd})")
        try:
            client_sock = socket.fromfd(raw_fd, socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
            t = threading.Thread(target=handle_client_stream, args=(client_sock,), daemon=True)
            t.start()
        except Exception as e:
            print(f"[Bluetooth] Error al inicializar socket desde fd: {e}")

    @dbus.service.method('org.bluez.Profile1', in_signature='o', out_signature='')
    def RequestDisconnection(self, path):
        print(f"[BlueZ] RequestDisconnection from {path}")


class AutoAgent(dbus.service.Object):
    """BlueZ Agent that automatically accepts pairing and SPP authorization"""
    @dbus.service.method("org.bluez.Agent1", in_signature="", out_signature="")
    def Release(self):
        pass

    @dbus.service.method("org.bluez.Agent1", in_signature="os", out_signature="")
    def AuthorizeService(self, device, uuid):
        print(f"\n[BlueZ Agent] Conexión de servicio autorizada para {device}")
        return

    @dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="s")
    def RequestPinCode(self, device):
        print(f"\n[BlueZ Agent] PIN solicitado para {device} -> 1234")
        return "1234"

    @dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="u")
    def RequestPasskey(self, device):
        return dbus.UInt32(0)

    @dbus.service.method("org.bluez.Agent1", in_signature="ouq", out_signature="")
    def DisplayPasskey(self, device, passkey, entered):
        pass

    @dbus.service.method("org.bluez.Agent1", in_signature="os", out_signature="")
    def DisplayPinCode(self, device, pincode):
        pass

    @dbus.service.method("org.bluez.Agent1", in_signature="ou", out_signature="")
    def RequestConfirmation(self, device, passkey):
        return

    @dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="")
    def RequestAuthorization(self, device):
        return

    @dbus.service.method("org.bluez.Agent1", in_signature="", out_signature="")
    def Cancel(self):
        pass


def format_elm_response(cmd: str) -> str:
    """Computes the ELM327 response string for a given command"""
    cmd = cmd.strip().upper()

    if not cmd:
        return "OK\r\r>"

    # AT Commands
    if cmd.startswith("AT"):
        if cmd == "ATZ" or cmd == "ATWS":
            return "ELM327 v1.5\r\r>"
        elif cmd in ["ATE0", "ATE1", "ATL0", "ATL1", "ATH0", "ATH1", "ATSP0", "ATAT2", "ATBRD23", "ATLP", "ATPP0CSV35", "ATPP0CON"]:
            return "OK\r\r>"
        elif cmd == "ATDP" or cmd == "ATDPN":
            return "AUTO, ISO 15765-4 (CAN 11/500)\r\r>"
        elif cmd == "ATRV":
            return f"{state.pcm_volt:.1f}V\r\r>"
        elif cmd == "ATIGN":
            return "ON\r\r>"
        else:
            return "OK\r\r>"

    # OBD2 PIDs
    with state.lock:
        rpm = state.rpm
        load = state.load
        coolant = state.coolant_temp
        speed = state.speed
        oil = state.oil_temp
        trans = state.trans_temp
        map_p = state.map_psi
        volt = state.pcm_volt

    if cmd == "0100":
        # PIDs 01-20 supported
        return "41 00 BE 3F B8 10\r\r>"

    elif cmd == "0104":
        # Engine Load (formula: A * 100 / 255)
        a = int(min(255, max(0, load * 255.0 / 100.0)))
        return f"41 04 {a:02X}\r\r>"

    elif cmd == "0105":
        # Engine Coolant Temperature (formula: A - 40)
        a = int(min(255, max(0, coolant + 40.0)))
        return f"41 05 {a:02X}\r\r>"

    elif cmd == "010B":
        # MAP - Manifold Absolute Pressure in psi (formula in code: A * 0.145)
        # So A = psi / 0.145
        a = int(min(255, max(0, map_p / 0.145)))
        return f"41 0B {a:02X}\r\r>"

    elif cmd == "010C":
        # Engine RPM (formula: (256 * A + B) / 4.0)
        # So 256*A + B = RPM * 4
        val = int(min(65535, max(0, rpm * 4.0)))
        a = (val >> 8) & 0xFF
        b = val & 0xFF
        return f"41 0C {a:02X} {b:02X}\r\r>"

    elif cmd == "010D":
        # Vehicle Speed in km/h (formula: A)
        a = int(min(255, max(0, speed)))
        return f"41 0D {a:02X}\r\r>"

    elif cmd == "010F":
        # Intake Air Temp (formula: A - 40)
        a = int(min(255, max(0, coolant - 5.0 + 40.0)))
        return f"41 0F {a:02X}\r\r>"

    elif cmd == "0142":
        # Control Module Voltage / PCM Volt (formula: (256 * A + B) / 1000.0)
        val = int(min(65535, max(0, volt * 1000.0)))
        a = (val >> 8) & 0xFF
        b = val & 0xFF
        return f"41 42 {a:02X} {b:02X}\r\r>"

    elif cmd == "015C":
        # Engine Oil Temperature (formula: A - 40)
        a = int(min(255, max(0, oil + 40.0)))
        return f"41 5C {a:02X}\r\r>"

    elif cmd == "221E1C":
        # Ford T6 Transmission Fluid Temp (formula: (256 * A + B) / 16.0)
        val = int(min(65535, max(0, trans * 16.0)))
        a = (val >> 8) & 0xFF
        b = val & 0xFF
        return f"62 1E 1C {a:02X} {b:02X}\r\r>"

    elif cmd == "221674":
        # Ford T5 Transmission Temp
        val = int(min(65535, max(0, (trans + 18.0) * 72.0 / 5.0)))
        a = (val >> 8) & 0xFF
        b = val & 0xFF
        return f"62 16 74 {a:02X} {b:02X}\r\r>"

    elif cmd == "0902":
        # VIN request (Ford format expected by elm327.h)
        # Sample simulated VIN: 1FTFW1ED4NFC12345
        return "014 \r0: 49 02 01 31 46 54 \r1: 46 57 31 45 44 34 4E \r2: 46 43 31 32 33 34 35 \r\r>"

    elif cmd == "04":
        # Clear DTC
        return "44\r\r>"

    elif cmd == "03" or cmd == "07" or cmd == "0A":
        # Trouble codes: 43 00 = 0 DTCs
        return "43 00\r\r>"

    # Fallback
    return "NO DATA\r\r>"


def handle_client_stream(sock):
    """Handles communication with connected ESP32 CYD"""
    print("[OBD2] ESP32 conectado! Enviando respuestas...")
    buffer = ""
    last_display_time = 0

    try:
        sock.settimeout(0.5)
        while True:
            try:
                data = sock.recv(256)
                if not data:
                    print("[OBD2] Cliente desconectado (EOF).")
                    break
                
                text = data.decode("latin1", errors="replace")
                buffer += text

                while "\r" in buffer or "\n" in buffer:
                    # Find split point
                    r_idx = buffer.find("\r")
                    n_idx = buffer.find("\n")
                    if r_idx != -1 and n_idx != -1:
                        split_idx = min(r_idx, n_idx)
                    elif r_idx != -1:
                        split_idx = r_idx
                    else:
                        split_idx = n_idx

                    cmd = buffer[:split_idx].strip()
                    buffer = buffer[split_idx+1:]

                    response = format_elm_response(cmd)
                    sock.sendall(response.encode("latin1"))

                    now = time.time()
                    if now - last_display_time > 0.25:
                        last_display_time = now
                        cmd_display = cmd if cmd else "<CR>"
                        with state.lock:
                            c_rpm = state.rpm
                            c_load = state.load
                            c_temp = state.coolant_temp
                            c_spd = state.speed
                            c_volt = state.pcm_volt
                        is_eco = (1900.0 <= c_rpm <= 2550.0 and 45.0 <= c_load <= 82.0)
                        eco_badge = "\033[92;1m[ECO ACTIVO]\033[0m" if is_eco else "\033[90m[ECO OFF]\033[0m"
                        print(f"\r[RPM: {c_rpm:4.0f} | Carga: {c_load:2.0f}% {eco_badge} | {c_temp:3.0f}°C | {c_spd:3.0f} km/h | {c_volt:4.1f}V | CMD: {cmd_display:5s} -> {response.strip():14s}]   ", end="", flush=True)

            except socket.timeout:
                pass
            except BlockingIOError:
                time.sleep(0.01)

    except Exception as e:
        print(f"\n[OBD2] Error en socket: {e}")
    finally:
        try:
            sock.close()
        except Exception:
            pass
        print("\n[OBD2] Sesión cerrada.")


def simulation_worker():
    """Background thread to continuously update simulated values"""
    while True:
        state.update_simulation()
        time.sleep(0.05)


def run_emulator():
    parser = argparse.ArgumentParser(description="OBD-II Bluetooth Emulator con soporte ECO Badge (Ford Ka Endura 1.3)")
    parser.add_argument("--mode", "-m", choices=["cycle", "eco"], default="cycle", help="Modo inicial: 'cycle' (demostración) o 'eco' (fijo)")
    parser.add_argument("--eco", "-e", action="store_true", help="Iniciar directamente en modo ECO fijo (RPM=2250, Carga=58%%)")
    args = parser.parse_args()

    if args.eco or args.mode == "eco":
        state.mode = "eco"

    print("=" * 70)
    print("   ESP32 CYD OBD2 Bluetooth Emulator (ELM327 v1.5)")
    print("   Soporte para Badge Táctico ECO / OPT (Ford Ka Endura 1.3)")
    print("=" * 70)
    if state.mode == "eco":
        print(">>> MODO ACTUAL: ECO FIJO (RPM: 2250, Carga: 58%, Vel: 70 km/h) <<<")
    else:
        print(">>> MODO ACTUAL: CICLO DINÁMICO (Fase ECO activa entre 8s y 18s) <<<")
    print("Comandos interactivos en consola:")
    print("  - Escribe 'e' + [Enter]: Activar MODO ECO FIJO (permanente)")
    print("  - Escribe 'c' + [Enter]: Activar MODO CICLO AUTOMÁTICO")
    print("  - Presiona Ctrl+C o escribe 'q': Salir\n")

    # Start simulation thread
    sim_thread = threading.Thread(target=simulation_worker, daemon=True)
    sim_thread.start()

    # Set up D-Bus loop for BlueZ SPP registration
    DBusGMainLoop(set_as_default=True)
    bus = dbus.SystemBus()
    profile_path = "/org/bluez/obd2_profile"

    profile = SPPProfile(bus, profile_path)
    profile_manager = dbus.Interface(
        bus.get_object("org.bluez", "/org/bluez"),
        "org.bluez.ProfileManager1"
    )

    opts = {
        "Name": "SerialPort",
        "Channel": dbus.UInt16(1),
        "Role": "server",
        "Service": "00001101-0000-1000-8000-00805F9B34FB",
        "AutoConnect": True,
    }

    try:
        profile_manager.RegisterProfile(profile_path, "00001101-0000-1000-8000-00805F9B34FB", opts)
        print("[BlueZ] Perfil SPP registrado con éxito.")
    except Exception as e:
        print(f"[BlueZ] Aviso al registrar perfil SPP: {e}")

    # Register BlueZ Agent for auto-pairing and authorization
    agent_path = "/org/bluez/obd2_agent"
    agent = AutoAgent(bus, agent_path)
    agent_manager = None
    try:
        agent_manager = dbus.Interface(
            bus.get_object("org.bluez", "/org/bluez"),
            "org.bluez.AgentManager1"
        )
        agent_manager.RegisterAgent(agent_path, "NoInputNoOutput")
        agent_manager.RequestDefaultAgent(agent_path)
        print("[BlueZ] Agente de autenticación y emparejamiento registrado.")
    except Exception as e:
        print(f"[BlueZ] Aviso al registrar agente: {e}")

    # Configure adapter properties (Discoverable, Pairable, Alias)
    try:
        manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.freedesktop.DBus.ObjectManager")
        objects = manager.GetManagedObjects()
        for path, ifaces in objects.items():
            if "org.bluez.Adapter1" in ifaces:
                props = dbus.Interface(bus.get_object("org.bluez", path), "org.freedesktop.DBus.Properties")
                props.Set("org.bluez.Adapter1", "Alias", dbus.String("OBDII"))
                props.Set("org.bluez.Adapter1", "DiscoverableTimeout", dbus.UInt32(0))
                props.Set("org.bluez.Adapter1", "Discoverable", dbus.Boolean(True))
                props.Set("org.bluez.Adapter1", "Pairable", dbus.Boolean(True))
                print(f"[BlueZ] Adaptador configurado en {path} (Nombre: OBDII, Visible y Emparejable).")
                break
    except Exception as e:
        print(f"[BlueZ] Aviso al configurar adaptador: {e}")

    # Start GLib main loop in background thread for D-Bus signals and methods
    glib_loop = GLib.MainLoop()
    dbus_thread = threading.Thread(target=glib_loop.run, daemon=True)
    dbus_thread.start()

    # Create native RFCOMM socket fallback
    server_sock = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
    try:
        server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server_sock.bind(("00:00:00:00:00:00", 1))
        server_sock.listen(1)
        print("[Bluetooth] Servidor RFCOMM escuchando en canal 1.")
    except Exception as e:
        print(f"[Bluetooth] Servidor RFCOMM: {e}")

    print("\n[Listo] Esperando conexión de la pantalla CYD...")
    print("Asegúrate de que tu CYD esté encendida buscando 'OBDII'.")
    print("Presiona Ctrl+C para salir.\n")

    try:
        while True:
            rlist, _, _ = select.select([server_sock, sys.stdin], [], [], 1.0)
            if server_sock in rlist:
                client_sock, client_info = server_sock.accept()
                print(f"\n[Bluetooth Socket] Conexión entrante aceptada desde {client_info}!")
                t = threading.Thread(target=handle_client_stream, args=(client_sock,), daemon=True)
                t.start()
            if sys.stdin in rlist:
                line = sys.stdin.readline().strip().lower()
                if line in ["e", "eco"]:
                    with state.lock:
                        state.mode = "eco"
                    print("\n\n>>> MODO ECO FIJO ACTIVADO: RPM=2250, Carga=58% (Badge [ ECO ] permanente) <<<\n")
                elif line in ["c", "cycle"]:
                    with state.lock:
                        state.mode = "cycle"
                    print("\n\n>>> MODO CICLO AUTOMÁTICO ACTIVADO (Fase ECO activa entre 8s y 18s) <<<\n")
                elif line in ["q", "quit", "exit"]:
                    break
    except KeyboardInterrupt:
        print("\nDeteniendo emulador...")
    finally:
        try:
            profile_manager.UnregisterProfile(profile_path)
        except Exception:
            pass
        if agent_manager:
            try:
                agent_manager.UnregisterAgent(agent_path)
            except Exception:
                pass
        server_sock.close()
        print("Emulador finalizado.")


if __name__ == "__main__":
    run_emulator()