#!/usr/bin/env python3
"""
ELM327 WiFi Simulator — Simulates a WiFi OBD-II adapter for development/testing.

Usage:
    python3 elm327-simulator.py [--port PORT] [--verbose]

Default port: 35000 (standard ELM327 WiFi port)

From the Android emulator, connect to:
    IP:   10.0.2.2  (host loopback from emulator)
    Port: 35000

From a physical device on the same network:
    IP:   <your Mac's local IP>
    Port: 35000

The simulator responds to:
    - AT commands (ATZ, ATI, ATE0, ATS1, ATSP6, etc.)
    - Mode 01 PIDs (RPM, speed, coolant, MAP, baro, etc.)
    - Mode 22 PIDs (Ford EcoBoost: boost MAP, baro, oil temp)
    - ATRV (battery voltage)

Simulated engine behavior:
    - RPM cycles smoothly between idle (~850) and ~5500
    - Speed follows RPM with gear ratio simulation
    - Coolant/oil temps rise under load, fall at idle
    - Boost pressure tracks RPM * throttle
    - Voltage dips slightly under load

Press Ctrl+C to stop the server.
"""

import socket
import threading
import time
import math
import argparse
import random
import sys


# ──────────────────────────────────────────────
#  Simulated engine state
# ──────────────────────────────────────────────

class EngineState:
    """Simulates a Ford Fiesta ST Mk7 1.6 EcoBoost engine."""

    def __init__(self):
        self.tick = 0.0
        self.rpm = 850.0
        self.speed_kph = 0.0
        self.coolant_c = 78.0
        self.oil_c = 86.0
        self.map_kpa = 99.0       # manifold absolute pressure
        self.baro_kpa = 101.3     # barometric pressure
        self.voltage = 14.1
        self.throttle = 0.0
        self.gear = 1
        self.lock = threading.Lock()

    def update(self, dt=0.1):
        """Advance the simulation by dt seconds."""
        with self.lock:
            self.tick += dt

            # Throttle: smooth sine pattern (~30s cycle)
            raw = math.sin(self.tick * 0.07) * 0.5 + 0.5
            noise = math.sin(self.tick * 0.29) * 0.05
            target_throttle = max(0.0, min(1.0, raw + noise))
            self.throttle += (target_throttle - self.throttle) * 0.12

            # RPM
            target_rpm = 850 + self.throttle * 5200
            rate = 0.10 if target_rpm > self.rpm else 0.07
            self.rpm += (target_rpm - self.rpm) * rate

            # Gear shifts
            gear_ratios = [3.6, 2.1, 1.4, 1.0, 0.75, 0.62]
            if self.rpm > 5800 and self.gear < 6:
                self.gear += 1
            if self.rpm < 1400 and self.gear > 1:
                self.gear -= 1
            ratio = gear_ratios[self.gear - 1]

            # Speed
            target_speed = max(0, min(220, self.rpm / ratio / 35.0))
            self.speed_kph += (target_speed - self.speed_kph) * 0.08

            # Boost (turbo)
            spool = max(0, min(1, (self.rpm - 1500) / 3000))
            if self.throttle > 0.15:
                target_boost = spool * self.throttle * 120  # kPa above baro
            else:
                target_boost = -8  # vacuum
            boost_rate = 0.06 if target_boost > (self.map_kpa - self.baro_kpa) else 0.15
            boost = (self.map_kpa - self.baro_kpa)
            boost += (target_boost - boost) * boost_rate
            self.map_kpa = max(20, self.baro_kpa + boost)

            # Coolant
            load = self.throttle * (self.rpm / 6000)
            target_coolant = 78 + load * 18
            self.coolant_c += (target_coolant - self.coolant_c) * 0.008
            self.coolant_c = max(70, min(105, self.coolant_c))

            # Oil
            target_oil = self.coolant_c + 8 + load * 10
            self.oil_c += (target_oil - self.oil_c) * 0.006
            self.oil_c = max(76, min(130, self.oil_c))

            # Voltage
            target_v = 14.1 - (self.rpm / 6000) * 0.3 - load * 0.15
            self.voltage += (target_v - self.voltage) * 0.08
            self.voltage = max(13.2, min(14.5, self.voltage))


# ──────────────────────────────────────────────
#  ELM327 command handler
# ──────────────────────────────────────────────

class Elm327Handler:
    """
    Processes AT and OBD commands, returns ELM327-formatted responses.

    Supported AT commands:
        ATZ       - Reset (returns version string)
        ATI       - Adapter info
        ATE0/ATE1 - Echo off/on
        ATL0/ATL1 - Linefeeds off/on
        ATS0/ATS1 - Spaces off/on
        ATH0/ATH1 - Headers off/on
        ATSP0-9   - Set protocol
        ATAT0-2   - Adaptive timing
        ATST XX   - Set timeout
        ATSH XXX  - Set header
        ATRV      - Read voltage
        ATDP      - Describe protocol
        AT@1      - Device description

    Supported OBD PIDs:
        Mode 01:
            0100 - Supported PIDs [01-20]
            0105 - Coolant temperature
            010B - Intake MAP
            010C - Engine RPM
            010D - Vehicle speed
            0133 - Barometric pressure
        Mode 22 (Ford EcoBoost):
            221440 - MAP (high-res)
            221442 - Baro (high-res)
            220461 - Oil temperature
    """

    VERSION = "ELM327 v2.1"

    def __init__(self, engine: EngineState, verbose: bool = False):
        self.engine = engine
        self.verbose = verbose
        self.echo = False
        self.spaces = True
        self.headers = False
        self.linefeed = False
        self.protocol = 6  # ISO 15765-4 CAN 11-bit 500k

    def handle(self, raw_cmd: str) -> str:
        """Process a command string and return the ELM327 response."""
        cmd = raw_cmd.strip().upper().replace(" ", "")

        if not cmd:
            return ">"

        if self.verbose:
            print(f"  <- {cmd}")

        # Simulate real adapter latency (50-150ms)
        time.sleep(0.05 + 0.1 * (hash(cmd) % 100) / 100)

        # AT commands
        if cmd.startswith("AT"):
            resp = self._handle_at(cmd[2:])
        # OBD Mode 22 (Ford specific)
        elif cmd.startswith("22"):
            resp = self._handle_mode22(cmd)
        # OBD Mode 01
        elif cmd.startswith("01"):
            resp = self._handle_mode01(cmd)
        # OBD Mode 09 (VIN etc.)
        elif cmd.startswith("09"):
            resp = "NO DATA"
        else:
            resp = "?"

        if self.verbose:
            print(f"  -> {resp}")

        # Append prompt
        terminator = "\r\r>" if not self.linefeed else "\r\n\r\n>"
        return resp + terminator

    # ── AT commands ──

    def _handle_at(self, at: str) -> str:
        if at == "Z":
            # Reset
            self.echo = False
            self.spaces = True
            self.headers = False
            self.linefeed = False
            return "\r\r" + self.VERSION
        if at == "I":
            return self.VERSION
        if at == "E0":
            self.echo = False
            return "OK"
        if at == "E1":
            self.echo = True
            return "OK"
        if at == "L0":
            self.linefeed = False
            return "OK"
        if at == "L1":
            self.linefeed = True
            return "OK"
        if at == "S0":
            self.spaces = False
            return "OK"
        if at == "S1":
            self.spaces = True
            return "OK"
        if at == "H0":
            self.headers = False
            return "OK"
        if at == "H1":
            self.headers = True
            return "OK"
        if at.startswith("SP"):
            proto = at[2:]
            if proto in ("0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "A", "B"):
                self.protocol = int(proto, 16)
                return "OK"
            return "?"
        if at.startswith("AT"):
            # Adaptive timing AT0, AT1, AT2
            return "OK"
        if at.startswith("ST"):
            # Set timeout
            return "OK"
        if at.startswith("SH"):
            # Set header
            return "OK"
        if at == "RV":
            return f"{self.engine.voltage:.1f}V"
        if at == "DP":
            return "ISO 15765-4 (CAN 11/500)"
        if at == "@1":
            return "RevWatch ELM327 Simulator"
        if at.startswith("FC"):
            return "OK"
        if at.startswith("CRA") or at.startswith("CF") or at.startswith("CM"):
            return "OK"
        # Unknown AT → OK (clones do this)
        return "OK"

    # ── Mode 01 ──

    def _handle_mode01(self, cmd: str) -> str:
        if len(cmd) < 4:
            return "?"

        pid = int(cmd[2:4], 16)
        sep = " " if self.spaces else ""

        with self.engine.lock:
            if pid == 0x00:
                # Supported PIDs 01-20: 0C, 0D, 05, 0B → bits
                # Bit layout: PID 01=MSB of byte 1, PID 20=LSB of byte 4
                bitmap = 0
                for p in [0x05, 0x0B, 0x0C, 0x0D, 0x11, 0x13, 0x1C, 0x20]:
                    bitmap |= (1 << (0x20 - p))
                b = f"{bitmap:08X}"
                return sep.join(["41", "00", b[0:2], b[2:4], b[4:6], b[6:8]])

            if pid == 0x05:
                # Coolant temp: value + 40
                val = int(self.engine.coolant_c + 40)
                return sep.join(["41", "05", f"{val:02X}"])

            if pid == 0x0B:
                # Intake MAP (kPa), 1 byte
                val = int(self.engine.map_kpa)
                return sep.join(["41", "0B", f"{val:02X}"])

            if pid == 0x0C:
                # RPM: (A*256+B)/4
                raw = int(self.engine.rpm * 4)
                a = (raw >> 8) & 0xFF
                b = raw & 0xFF
                return sep.join(["41", "0C", f"{a:02X}", f"{b:02X}"])

            if pid == 0x0D:
                # Speed (km/h), 1 byte
                val = int(self.engine.speed_kph)
                return sep.join(["41", "0D", f"{val:02X}"])

            if pid == 0x11:
                # Throttle position: value * 100 / 255
                val = int(self.engine.throttle * 255)
                return sep.join(["41", "11", f"{val:02X}"])

            if pid == 0x33:
                # Barometric pressure (kPa), 1 byte
                val = int(self.engine.baro_kpa)
                return sep.join(["41", "33", f"{val:02X}"])

        return "NO DATA"

    # ── Mode 22 (Ford EcoBoost) ──

    def _handle_mode22(self, cmd: str) -> str:
        if len(cmd) < 6:
            return "?"

        pid = int(cmd[2:6], 16)
        pid_hi = f"{(pid >> 8) & 0xFF:02X}"
        pid_lo = f"{pid & 0xFF:02X}"
        sep = " " if self.spaces else ""

        with self.engine.lock:
            if pid == 0x1440:
                # MAP high-res: (A*256+B) * 0.25 kPa
                raw = int(self.engine.map_kpa / 0.25)
                a = (raw >> 8) & 0xFF
                b = raw & 0xFF
                return sep.join(["62", pid_hi, pid_lo, f"{a:02X}", f"{b:02X}"])

            if pid == 0x1442:
                # Baro high-res: (A*256+B) * 0.25 kPa
                raw = int(self.engine.baro_kpa / 0.25)
                a = (raw >> 8) & 0xFF
                b = raw & 0xFF
                return sep.join(["62", pid_hi, pid_lo, f"{a:02X}", f"{b:02X}"])

            if pid == 0x0461:
                # Oil temp: (A*256+B) / 64 degC  →  raw = temp * 64
                raw = int(self.engine.oil_c * 64)
                # Signed: A is high byte (can be negative for sub-zero, but we're always positive here)
                a = (raw >> 8) & 0xFF
                b = raw & 0xFF
                return sep.join(["62", pid_hi, pid_lo, f"{a:02X}", f"{b:02X}"])

        return "NO DATA"


# ──────────────────────────────────────────────
#  TCP server
# ──────────────────────────────────────────────

def handle_client(conn: socket.socket, addr, handler: Elm327Handler, verbose: bool):
    """Handle a single ELM327 TCP client connection."""
    print(f"[+] Client connected: {addr[0]}:{addr[1]}")
    conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
    conn.settimeout(60.0)

    buf = b""
    try:
        while True:
            try:
                data = conn.recv(1024)
            except socket.timeout:
                continue
            if not data:
                break

            buf += data

            # ELM327 commands are terminated by \r (0x0D)
            while b"\r" in buf:
                line, buf = buf.split(b"\r", 1)
                cmd_str = line.decode("ascii", errors="ignore").strip()
                if cmd_str:
                    response = handler.handle(cmd_str)
                    conn.sendall(response.encode("ascii"))
    except (ConnectionResetError, BrokenPipeError, OSError):
        pass
    finally:
        print(f"[-] Client disconnected: {addr[0]}:{addr[1]}")
        conn.close()


def run_engine_loop(engine: EngineState):
    """Background thread: update engine state at ~10 Hz."""
    while True:
        engine.update(dt=0.1)
        time.sleep(0.1)


def main():
    parser = argparse.ArgumentParser(
        description="ELM327 WiFi Simulator for RevWatch development"
    )
    parser.add_argument(
        "--port", type=int, default=35000,
        help="TCP port to listen on (default: 35000)"
    )
    parser.add_argument(
        "--verbose", "-v", action="store_true",
        help="Print all commands and responses"
    )
    args = parser.parse_args()

    engine = EngineState()

    # Start engine simulation in background
    engine_thread = threading.Thread(target=run_engine_loop, args=(engine,), daemon=True)
    engine_thread.start()

    # Start TCP server
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server.bind(("0.0.0.0", args.port))
    server.listen(2)

    print(f"=== ELM327 WiFi Simulator ===")
    print(f"Listening on port {args.port}")
    print(f"")
    print(f"Connect from Android emulator:")
    print(f"  IP:   10.0.2.2")
    print(f"  Port: {args.port}")
    print(f"")
    print(f"Connect from physical device (same network):")
    print(f"  IP:   <your Mac's local IP>")
    print(f"  Port: {args.port}")
    print(f"")
    print(f"Simulating: Ford Fiesta ST Mk7 — 1.6 EcoBoost")
    print(f"Press Ctrl+C to stop.")
    print()

    try:
        while True:
            conn, addr = server.accept()
            handler = Elm327Handler(engine, verbose=args.verbose)
            client_thread = threading.Thread(
                target=handle_client,
                args=(conn, addr, handler, args.verbose),
                daemon=True
            )
            client_thread.start()
    except KeyboardInterrupt:
        print("\nShutting down.")
        server.close()
        sys.exit(0)


if __name__ == "__main__":
    main()
