"""
P-controller line-follower demo (Tkinter GUI).

The bot has a heading that persists and builds up over time (inertia),
which is what lets a P-only controller overshoot and oscillate around
the line instead of just smoothly decaying to it:

    error           = 0 - y
    steering command = Kp * error
    heading  += steering command * dt, clamped to +/- 60 deg
    y        += speed * sin(heading) * dt
"""

import math
import tkinter as tk

DT = 0.05                       # simulation step, seconds
SPEED = 5.0                     # forward speed, m/s
MAX_STEER = math.radians(60)    # steering angle clamp
PIXELS_PER_METER = 12
CANVAS_W, CANVAS_H = 800, 400
MID_Y = CANVAS_H // 2
TRAIL_DT_PX = 4                 # how many px the trail advances per frame
BOT_SCREEN_X = CANVAS_W - 60    # bot always drawn here, trail scrolls left


class LineFollowerSim:
    def __init__(self, root):
        self.root = root
        root.title("P-controller line follower (y = 0)")

        # --- state ---
        self.y = 3.0         # lateral position (the error is -y)
        self.theta = 0.0     # heading angle, persists/integrates over time
        self.trail = []      # pixel y-values, oldest first

        # --- canvas ---
        self.canvas = tk.Canvas(root, width=CANVAS_W, height=CANVAS_H, bg="white")
        self.canvas.pack(padx=10, pady=10)
        self.canvas.create_line(0, MID_Y, CANVAS_W, MID_Y, fill="#2ca02c", width=2)
        self.canvas.create_text(8, MID_Y - 10, anchor="w", fill="#2ca02c", text="y = 0 (target line)")

        # --- Kp slider ---
        slider_frame = tk.Frame(root)
        slider_frame.pack(fill="x", padx=10)
        tk.Label(slider_frame, text="Kp").pack(side="left")
        self.kp = tk.DoubleVar(value=1.0)
        tk.Scale(
            slider_frame, from_=-50, to=50, resolution=0.1, orient="horizontal",
            variable=self.kp, length=500, showvalue=True,
        ).pack(side="left", fill="x", expand=True, padx=8)

        # --- readouts ---
        readout_frame = tk.Frame(root)
        readout_frame.pack(fill="x", padx=10, pady=(4, 0))
        self.error_var = tk.StringVar()
        self.steer_var = tk.StringVar()
        self.kp_var = tk.StringVar()
        for var in (self.error_var, self.steer_var, self.kp_var):
            tk.Label(readout_frame, textvariable=var, font=("Consolas", 11), anchor="w").pack(fill="x")

        # --- equation ---
        eq_frame = tk.Frame(root)
        eq_frame.pack(fill="x", padx=10, pady=(6, 4))
        tk.Label(eq_frame, text="steering command = Kp × error, integrated into heading (clamped to ±60°)",
                 font=("Consolas", 13, "bold")).pack(anchor="w")
        self.eq_live_var = tk.StringVar()
        tk.Label(eq_frame, textvariable=self.eq_live_var, font=("Consolas", 11), fg="#555").pack(anchor="w")

        # --- controls ---
        btn_frame = tk.Frame(root)
        btn_frame.pack(fill="x", padx=10, pady=(0, 10))
        tk.Button(btn_frame, text="Reset", command=self.reset).pack(side="left")

        self.step()

    def reset(self):
        self.y = 3.0
        self.theta = 0.0
        self.trail.clear()
        self.canvas.delete("trail", "bot")

    def step(self):
        kp = self.kp.get()
        error = -self.y  # target (0) minus current position
        steer_cmd = kp * error

        self.theta += steer_cmd * DT
        self.theta = max(-MAX_STEER, min(MAX_STEER, self.theta))
        self.y += SPEED * math.sin(self.theta) * DT

        self.error_var.set(f"error            : {error:+7.3f} m")
        self.steer_var.set(f"steering angle   : {math.degrees(self.theta):+7.2f} deg")
        self.kp_var.set(f"current Kp (P)   : {kp:+6.2f}")
        self.eq_live_var.set(
            f"= {kp:+.2f} × {error:+.2f} m -> heading = {math.degrees(self.theta):+.2f} deg"
        )

        self.trail.append((MID_Y - self.y * PIXELS_PER_METER, self.theta))
        max_points = CANVAS_W // TRAIL_DT_PX + 1
        if len(self.trail) > max_points:
            self.trail.pop(0)

        self.draw()
        self.root.after(int(DT * 1000), self.step)

    def draw(self):
        self.canvas.delete("trail", "bot")

        n = len(self.trail)
        points = []
        for i, (py, _) in enumerate(self.trail):
            px = BOT_SCREEN_X - (n - 1 - i) * TRAIL_DT_PX
            if px < 0:
                continue
            points.extend((px, py))
        if len(points) >= 4:
            self.canvas.create_line(*points, fill="#1f77b4", width=2, tags="trail")

        bot_py, bot_steer = self.trail[-1]
        nose = self._rotate(12, 0, bot_steer)
        left = self._rotate(-8, 6, bot_steer)
        right = self._rotate(-8, -6, bot_steer)
        poly = []
        for dx, dy in (nose, left, right):
            poly.extend((BOT_SCREEN_X + dx, bot_py - dy))
        self.canvas.create_polygon(*poly, fill="#d62728", tags="bot")

    @staticmethod
    def _rotate(lx, ly, theta):
        c, s = math.cos(theta), math.sin(theta)
        return lx * c - ly * s, lx * s + ly * c


if __name__ == "__main__":
    root = tk.Tk()
    LineFollowerSim(root)
    root.mainloop()