#!/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 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 = 1800.0          # RPM
        self.target_rpm = 2200.0   # Target RPM for smooth transitions
        self.speed = 45.0          # km/h
        self.coolant_temp = 88.0   # °C
        self.oil_temp = 92.0       # °C
        self.trans_temp = 82.0     # °C (Ford T6 221E1C)
        self.load = 32.0           # % (0-100)
        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.lock = threading.Lock()

    def update_simulation(self):
        """Simulates driving telemetry with smooth sinusoidal variation"""
        t = time.time()
        with self.lock:
            if self.auto_simulate:
                # RPM smoothly moves between 1000 and 3600
                self.rpm = 2100.0 + 1200.0 * math.sin(t * 0.8) + 200.0 * math.cos(t * 1.7)
                if self.rpm < 800:
                    self.rpm = 800
                # MAP pressure fluctuates with RPM
                self.map_psi = 12.0 + 10.0 * ((self.rpm - 800.0) / 3000.0) + 1.5 * math.sin(t * 2.0)
                # Engine load correlates with RPM
                self.load = 20.0 + 45.0 * ((self.rpm - 800.0) / 3000.0)
                # Vehicle speed correlates with RPM/gear
                self.speed = 25.0 + 65.0 * ((self.rpm - 800.0) / 2600.0) + 5.0 * math.sin(t * 0.7)
                if self.speed < 0:
                    self.speed = 0.0
                # Temperatures fluctuate slightly around operating values
                self.coolant_temp = 92.0 + 3.0 * math.sin(t * 0.1)
                self.oil_temp = 93.0 + 1.5 * math.sin(t * 0.12)
                self.trans_temp = 82.0 + 1.0 * math.cos(t * 0.09)
                self.pcm_volt = 14.2 + 0.15 * math.sin(t * 0.5)

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}")


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.4:
                        last_display_time = now
                        cmd_display = cmd if cmd else "<CR>"
                        print(f"\r[ECT: {state.coolant_temp:3.0f}°C | RPM: {state.rpm:4.0f} | SPEED: {state.speed:3.0f} km/h | Volt: {state.pcm_volt:4.1f}V | CMD: {cmd_display:6s} -> {response.strip()}]", 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():
    print("=" * 65)
    print("   ESP32 CYD OBD2 Bluetooth Emulator (ELM327 v1.5)")
    print("=" * 65)

    # 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}")

    # 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], [], [], 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()
    except KeyboardInterrupt:
        print("\nDeteniendo emulador...")
    finally:
        try:
            profile_manager.UnregisterProfile(profile_path)
        except Exception:
            pass
        server_sock.close()
        print("Emulador finalizado.")


if __name__ == "__main__":
    run_emulator()