#!/usr/bin/env python3
"""
PID Tuner for Self-Balancing Car
Connects over Bluetooth (HC-05) and allows real-time PID tuning.

Usage:
    python3 pid_tuner.py [port]

Default port: /dev/ttyUSB0 or /dev/rfcomm0
"""

import sys
import time
import serial
import argparse
import threading


class BalanceCar:
    def __init__(self, port: str, baud: int = 115200):
        self.ser = serial.Serial(port, baud, timeout=0.1)
        self.running = True
        self.last_state = ""

    def send(self, cmd: str):
        """Send command to car."""
        self.ser.write((cmd + '\r\n').encode())
        time.sleep(0.05)

    def set_angle_pid(self, kp: float, ki: float, kd: float):
        """Set angle PID gains."""
        self.send(f"p={kp:.2f} i={ki:.2f} d={kd:.2f}")

    def set_speed_pid(self, kp: float, ki: float):
        """Set speed PID gains."""
        self.send(f"sp={kp:.2f} si={ki:.2f}")

    def set_speed_ref(self, speed: float):
        """Set speed reference."""
        self.send(f"s={speed:.1f}")

    def start(self):
        """Start balancing."""
        self.send("start")

    def stop(self):
        """Emergency stop."""
        self.send("stop")

    def get_state(self):
        """Request current state."""
        self.send("?state")

    def read_line(self) -> str:
        """Read one line from serial."""
        try:
            line = self.ser.readline().decode().strip()
            return line
        except:
            return ""

    def close(self):
        self.running = False
        self.ser.close()


def reader_thread(car: BalanceCar):
    """Background thread to print car responses."""
    while car.running:
        line = car.read_line()
        if line:
            print(f"[CAR] {line}")
        else:
            time.sleep(0.01)


def main():
    parser = argparse.ArgumentParser(description="PID Tuner for Self-Balancing Car")
    parser.add_argument("port", nargs="?", default="/dev/ttyUSB0",
                        help="Serial port (default: /dev/ttyUSB0)")
    parser.add_argument("--baud", type=int, default=115200,
                        help="Baud rate (default: 115200)")
    args = parser.parse_args()

    print(f"Connecting to {args.port} @ {args.baud}...")
    car = BalanceCar(args.port, args.baud)

    # Start reader thread
    thread = threading.Thread(target=reader_thread, args=(car,), daemon=True)
    thread.start()

    print("Connected. Commands:")
    print("  ap <Kp> <Ki> <Kd>    — Set angle PID")
    print("  sp <Kp> <Ki>         — Set speed PID")
    print("  s <speed>             — Set speed reference")
    print("  start                 — Start balancing")
    print("  stop                  — Emergency stop")
    print("  state                 — Request state")
    print("  q                     — Quit")
    print()

    try:
        while True:
            cmd = input("> ").strip()
            if not cmd:
                continue
            if cmd == "q":
                break
            elif cmd == "start":
                car.start()
            elif cmd == "stop":
                car.stop()
            elif cmd == "state":
                car.get_state()
            elif cmd.startswith("ap "):
                parts = cmd.split()
                kp, ki, kd = float(parts[1]), float(parts[2]), float(parts[3])
                car.set_angle_pid(kp, ki, kd)
            elif cmd.startswith("sp "):
                parts = cmd.split()
                kp, ki = float(parts[1]), float(parts[2])
                car.set_speed_pid(kp, ki)
            elif cmd.startswith("s "):
                speed = float(cmd.split()[1])
                car.set_speed_ref(speed)
            else:
                car.send(cmd)
    except KeyboardInterrupt:
        pass
    finally:
        car.close()
        print("Disconnected.")


if __name__ == "__main__":
    main()