"""
obstacle_twin.py - a 2D "digital twin" of an ESP32 + HC-SR04 + L298N obstacle-avoiding car.
TinyBot Forge - https://tinybotforge.com/guides/esp32-obstacle-avoiding-robot-code/

The four controllers below are line-by-line ports of STRATEGY 1-4 in obstacle_car_v2.ino:
each `yield` is one Arduino call (drive / delay / readDistanceCm), so time passes the
same way it does on the real board.

What is modelled (simplified on purpose - see the guide):
  * HC-SR04: 15 degree cone (datasheet), 2-400 cm, 25 ms pulseIn timeout, median of 3 pings,
    0.5 cm noise, and NO echo from a flat surface hit at more than THETA degrees from
    head-on (sound reflects away like light off a mirror).
  * Motors: speed proportional to PWM above a dead band, random +-5 % left/right mismatch,
    random 0.85-1.10 battery factor, so timed turns are never exact.
  * Body: 0.12 m radius circle. Touching an obstacle blocks forward motion (the wheels
    stall or slip); it can still back up and spin.
Not modelled: acceleration, wheel slip on turns, sensor cross-talk, soft furniture.

    pip install numpy matplotlib
    python obstacle_twin.py                      # 20 rooms, 40 degree echo limit
    python obstacle_twin.py --rooms 100 --theta 25
    python obstacle_twin.py --plot paths.png     # draw one room
"""
import argparse, math
import numpy as np

# ---- car (Kit 1 numbers: 65 mm wheels, 170 rpm motors at 6 V) ----
R_BODY = 0.12          # m, collision radius
TRACK = 0.19           # m, wheel to wheel
V_MAX = math.pi * 0.065 * 170 / 60   # 0.58 m/s at PWM 255
DEADBAND = 60          # PWM below this does not move the car
SENSOR_FWD = 0.09      # m, sensor ahead of the axle centre
CONE = math.radians(7.5)              # half-angle of the 15 degree beam
N_RAYS = 9
DT = 0.01              # s, physics step
CELL = 0.10            # m, coverage grid

# ---- firmware constants (same names as obstacle_car_v2.ino) ----
SPEED_CRUISE, SPEED_TURN = 170, 160
STOP_CM, BACK_MS, TURN_MS = 20.0, 350, 450
LOOK_MS = 300           # strategy 3/4: pivot time for one "look"
WEAVE = 0               # strategy 4: PWM swing while cruising (35 tested: no gain, see guide)
WEAVE_MS = 1600         # strategy 4: one left-right weave period
STALL_MS = 1500         # strategy 4: reading frozen this long = we are pinned


class Room:
    def __init__(self, rng):
        w, h = rng.uniform(3, 5), rng.uniform(3, 5)
        self.w, self.h = w, h
        segs = [((0, 0), (w, 0)), ((w, 0), (w, h)), ((w, h), (0, h)), ((0, h), (0, 0))]
        self.boxes = []
        for _ in range(rng.integers(3, 7)):            # furniture, at random angles
            bw, bh = rng.uniform(0.3, 0.9), rng.uniform(0.3, 0.9)
            cx, cy = rng.uniform(0.6, w - 0.6), rng.uniform(0.6, h - 0.6)
            a = rng.uniform(0, math.pi)
            c, s = math.cos(a), math.sin(a)
            pts = [(cx + c * x - s * y, cy + s * x + c * y)
                   for x, y in ((-bw/2, -bh/2), (bw/2, -bh/2), (bw/2, bh/2), (-bw/2, bh/2))]
            self.boxes.append(pts)
            segs += [(pts[i], pts[(i + 1) % 4]) for i in range(4)]
        posts = []
        for _ in range(rng.integers(0, 3)):            # chairs: four 3 cm legs
            cx, cy = rng.uniform(0.5, w - 0.5), rng.uniform(0.5, h - 0.5)
            posts += [(cx + dx, cy + dy) for dx in (-0.2, 0.2) for dy in (-0.2, 0.2)]
        S = np.array(segs, float)
        self.a, self.b = S[:, 0], S[:, 1]
        d = self.b - self.a
        self.n = np.stack([-d[:, 1], d[:, 0]], 1) / np.linalg.norm(d, axis=1, keepdims=True)
        self.posts = np.array(posts, float).reshape(-1, 2)
        self.post_r = 0.015
        self.bp = np.array(self.boxes, float).reshape(-1, 4, 2)     # corners, counter-clockwise
        self.be = np.roll(self.bp, -1, axis=1) - self.bp            # edge vectors
        # free cells for coverage
        gx, gy = np.meshgrid(np.arange(CELL/2, w, CELL), np.arange(CELL/2, h, CELL))
        self.grid = np.stack([gx.ravel(), gy.ravel()], 1)
        self.free = np.array([self.clear(p, R_BODY) for p in self.grid])

    def clear(self, p, r):
        p = np.asarray(p)
        ap = p - self.a; ab = self.b - self.a
        t = np.clip((ap * ab).sum(1) / (ab * ab).sum(1), 0, 1)
        dist = np.linalg.norm(self.a + ab * t[:, None] - p, axis=1)
        if (dist < r).any():
            return False
        if len(self.posts) and (np.linalg.norm(self.posts - p, axis=1) < r + self.post_r).any():
            return False
        rel = p - self.bp                               # inside a box?
        if ((self.be[..., 0] * rel[..., 1] - self.be[..., 1] * rel[..., 0]) > 0).all(1).any():
            return False
        return 0 < p[0] < self.w and 0 < p[1] < self.h

    def nearest(self, p):
        """Closest obstacle point to p (walls, box faces, chair legs)."""
        p = np.asarray(p)
        ab = self.b - self.a
        t = np.clip(((p - self.a) * ab).sum(1) / (ab * ab).sum(1), 0, 1)
        q = self.a + ab * t[:, None]
        d = np.linalg.norm(q - p, axis=1)
        i = np.argmin(d); best, bq = d[i], q[i]
        if len(self.posts):
            dp = np.linalg.norm(self.posts - p, axis=1) - self.post_r
            j = np.argmin(dp)
            if dp[j] < best:
                bq = self.posts[j] + (p - self.posts[j]) / max(1e-9, dp[j] + self.post_r) * self.post_r
        return bq

    def ping(self, x, y, th, theta_max, rng):
        """One HC-SR04 ping. Returns cm, or 999 when nothing echoes back."""
        o = np.array([x + SENSOR_FWD * math.cos(th), y + SENSOR_FWD * math.sin(th)])
        best = 999.0
        for da in np.linspace(-CONE, CONE, N_RAYS):
            u = np.array([math.cos(th + da), math.sin(th + da)])
            # walls / box faces
            d = self.b - self.a
            den = u[0] * d[:, 1] - u[1] * d[:, 0]
            ok = np.abs(den) > 1e-9
            ao = self.a - o
            t = np.where(ok, (ao[:, 0] * d[:, 1] - ao[:, 1] * d[:, 0]) / np.where(ok, den, 1), -1)
            s = np.where(ok, (ao[:, 0] * u[1] - ao[:, 1] * u[0]) / np.where(ok, den, 1), -1)
            hit = ok & (t > 0) & (s >= 0) & (s <= 1)
            if hit.any():
                i = np.argmin(np.where(hit, t, np.inf))
                inc = math.degrees(math.acos(min(1, abs(float(u @ self.n[i])))))
                if inc <= theta_max:                    # specular: steep hits never come back
                    best = min(best, t[i] * 100)
            # chair legs: round, so the echo always comes straight back
            if len(self.posts):
                op = self.posts - o
                tc = op @ u
                perp = np.abs(op[:, 0] * u[1] - op[:, 1] * u[0])
                m = (tc > 0) & (perp < self.post_r)
                if m.any():
                    best = min(best, (tc[m].min() - self.post_r) * 100)
        if best < 2 or best > 400:
            return 999.0
        return best + rng.normal(0, 0.5)


def ping_ms(cm):
    """How long pulseIn() blocks for one ping (round trip at 58 us/cm, or the 25 ms timeout)."""
    return 25.0 if cm >= 999 else cm * 0.058 + 0.5


# ---------- the firmware, as Python generators ----------
# yield ("drive", left, right) | ("delay", ms) | ("read",) -> distance in cm | ("millis",) -> ms

def read_distance():
    a = yield ("read",); yield ("delay", 10)
    b = yield ("read",); yield ("delay", 10)
    c = yield ("read",)
    return sorted([a, b, c])[1]


def strategy1(rng):          # our original tutorial code: back up, turn right, else left
    while True:
        cm = yield from read_distance()
        if cm < STOP_CM:
            yield ("drive", 0, 0); yield ("delay", 100)
            yield ("drive", -SPEED_CRUISE, -SPEED_CRUISE); yield ("delay", BACK_MS)
            yield ("drive", SPEED_TURN, -SPEED_TURN); yield ("delay", TURN_MS)
            yield ("drive", 0, 0); yield ("delay", 80)
            if (yield from read_distance()) <= STOP_CM * 1.5:
                yield ("drive", -SPEED_TURN, SPEED_TURN); yield ("delay", TURN_MS * 2)
                yield ("drive", 0, 0); yield ("delay", 80)
        else:
            yield ("drive", SPEED_CRUISE, SPEED_CRUISE)
        yield ("delay", 40)


def strategy2(rng):          # random direction, random angle
    while True:
        cm = yield from read_distance()
        if cm < STOP_CM:
            yield ("drive", 0, 0); yield ("delay", 100)
            yield ("drive", -SPEED_CRUISE, -SPEED_CRUISE); yield ("delay", BACK_MS)
            sgn = 1 if rng.random() < 0.5 else -1
            yield ("drive", sgn * SPEED_TURN, -sgn * SPEED_TURN)
            yield ("delay", int(rng.uniform(1.0, 2.0) * TURN_MS))
            yield ("drive", 0, 0); yield ("delay", 80)
        else:
            yield ("drive", SPEED_CRUISE, SPEED_CRUISE)
        yield ("delay", 40)


def look_both_ways():
    """Strategy 3/4 avoid(): back up, look left, look right, go to the more open side."""
    yield ("drive", 0, 0); yield ("delay", 100)
    yield ("drive", -SPEED_CRUISE, -SPEED_CRUISE); yield ("delay", BACK_MS)
    yield ("drive", -SPEED_TURN, SPEED_TURN); yield ("delay", LOOK_MS)        # look left
    yield ("drive", 0, 0); yield ("delay", 80)
    left = yield from read_distance()
    yield ("drive", SPEED_TURN, -SPEED_TURN); yield ("delay", 2 * LOOK_MS)    # look right
    yield ("drive", 0, 0); yield ("delay", 80)
    right = yield from read_distance()
    if max(left, right) < STOP_CM * 1.5:                                     # boxed in: turn around
        yield ("drive", SPEED_TURN, -SPEED_TURN); yield ("delay", 2 * TURN_MS)
    elif left > right:
        yield ("drive", -SPEED_TURN, SPEED_TURN); yield ("delay", 2 * LOOK_MS)
    yield ("drive", 0, 0); yield ("delay", 80)


def strategy3(rng):
    while True:
        cm = yield from read_distance()
        if cm < STOP_CM:
            yield from look_both_ways()
        else:
            yield ("drive", SPEED_CRUISE, SPEED_CRUISE)
        yield ("delay", 40)


def strategy4(rng):          # strategy 3 + frozen-reading watchdog (+ optional weave)
    last_cm, last_change = 999.0, (yield ("millis",))
    while True:
        cm = yield from read_distance()
        now = yield ("millis",)
        if abs(cm - last_cm) > 1.0:
            last_cm, last_change = cm, now
        if cm < STOP_CM or now - last_change > STALL_MS:
            yield from look_both_ways()
            last_cm, last_change = 999.0, (yield ("millis",))
        else:
            w = int(WEAVE * math.sin(2 * math.pi * now / WEAVE_MS))
            yield ("drive", SPEED_CRUISE + w, SPEED_CRUISE - w)
        yield ("delay", 40)


STRATEGIES = {1: ("1 Back up, turn right (our old code)", strategy1),
              2: ("2 Random turn", strategy2),
              3: ("3 Look both ways", strategy3),
              4: ("4 Look both ways + stall watchdog", strategy4)}


def run(room, strat, seconds, theta_max, seed, keep_path=False):
    rng = np.random.default_rng(seed)
    while True:                                         # random free start pose
        x, y = rng.uniform(0.3, room.w - 0.3), rng.uniform(0.3, room.h - 0.3)
        if room.clear((x, y), R_BODY + 0.1):
            break
    th = rng.uniform(-math.pi, math.pi)
    gain = rng.uniform(0.85, 1.10) * (1 + np.array([1, -1]) * rng.uniform(-0.05, 0.05))
    ctrl = STRATEGIES[strat][1](rng)
    t, cmd, wait, reply = 0.0, (0, 0), 0.0, None
    visited = np.zeros(len(room.grid), bool)
    hits, back, last_touch, stuck_t, path, marks = 0, 0, -1.0, 0.0, [], []
    while t < seconds:
        while wait <= 0:                                # run firmware until it blocks
            op = ctrl.send(reply); reply = None
            if op[0] == "drive":
                cmd = op[1:]
            elif op[0] == "delay":
                wait += op[1] / 1000
            elif op[0] == "millis":
                reply = t * 1000
            else:                                       # ping: costs real time
                reply = room.ping(x, y, th, theta_max, rng)
                wait += ping_ms(reply) / 1000
        # motors -> wheel speeds
        vl, vr = [0.0 if abs(c) < DEADBAND else V_MAX * c / 255 * g for c, g in zip(cmd, gain)]
        v, w = (vl + vr) / 2, (vr - vl) / TRACK
        th += w * DT
        nx, ny = x + v * math.cos(th) * DT, y + v * math.sin(th) * DT
        moved = room.clear((nx, ny), R_BODY)
        if not moved:                                   # contact: slide along the surface
            q = room.nearest((nx, ny))
            n = np.array([nx, ny]) - q
            n = n / max(1e-9, np.linalg.norm(n))
            sx, sy = q + n * (R_BODY + 1e-4)
            if room.clear((sx, sy), R_BODY):
                nx, ny, moved = sx, sy, True
            contact = True
        else:
            contact = False
        if moved:
            x, y = nx, ny
        if contact:                                     # a new hit = first touch after 0.5 s free
            if t - last_touch > 0.5:
                hits += 1
                back += v < 0
                if keep_path:
                    marks.append((x, y))
            last_touch = t
            stuck_t += DT
        visited |= np.hypot(room.grid[:, 0] - x, room.grid[:, 1] - y) < R_BODY
        if keep_path and int(t / DT) % 5 == 0:
            path.append((x, y))
        t += DT; wait -= DT
    cov = (visited & room.free).sum() / max(1, room.free.sum())
    return dict(hits=hits, back=back, stuck=stuck_t / seconds, coverage=cov, path=np.array(path), marks=np.array(marks).reshape(-1, 2))


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--rooms", type=int, default=20)
    ap.add_argument("--seconds", type=float, default=120)
    ap.add_argument("--theta", type=float, default=40, help="echo lost beyond this angle from head-on")
    ap.add_argument("--seed", type=int, default=1)
    ap.add_argument("--plot", help="draw room 0 with all four paths to this PNG")
    a = ap.parse_args()
    rooms = [Room(np.random.default_rng(a.seed * 1000 + i)) for i in range(a.rooms)]
    if a.plot:
        import matplotlib; matplotlib.use("Agg")
        import matplotlib.pyplot as plt
        fig, axs = plt.subplots(1, 4, figsize=(16, 4.4))
        for ax, k in zip(axs, STRATEGIES):
            r = run(rooms[0], k, a.seconds, a.theta, a.seed, keep_path=True)
            rm = rooms[0]
            ax.plot(*np.vstack([rm.a, rm.a[:1]]).T * 0, alpha=0)
            for p, q in zip(rm.a, rm.b):
                ax.plot([p[0], q[0]], [p[1], q[1]], color="#333", lw=2)
            if len(rm.posts):
                ax.scatter(rm.posts[:, 0], rm.posts[:, 1], s=8, color="#333")
            ax.plot(r["path"][:, 0], r["path"][:, 1], color="#d08a2c", lw=0.8)
            ax.set_title(f"{STRATEGIES[k][0][:28]}\ncoverage {r['coverage']:.0%}, {r['hits']} hits",
                         fontsize=9)
            ax.set_aspect("equal"); ax.set_xticks([]); ax.set_yticks([])
        fig.tight_layout(); fig.savefig(a.plot, dpi=120)
        print("saved", a.plot)
        return
    print(f"{a.rooms} rooms x {a.seconds:.0f} s, echo lost beyond {a.theta:.0f} deg, seed {a.seed}")
    print(f"{'strategy':44s} {'coverage':>9s} {'hits/min':>9s} {'time pinned':>12s}")
    for k, (name, _) in STRATEGIES.items():
        rs = [run(r, k, a.seconds, a.theta, a.seed * 7 + i) for i, r in enumerate(rooms)]
        cov = np.mean([r["coverage"] for r in rs])
        hpm = np.mean([r["hits"] for r in rs]) / (a.seconds / 60)
        stk = np.mean([r["stuck"] for r in rs])
        print(f"{name:44s} {cov:9.0%} {hpm:9.2f} {stk:12.0%}")


if __name__ == "__main__":
    main()
