"""Robot vacuum simulator: random bounce vs planned rows (boustrophedon + BFS).
TinyBot Forge - https://tinybotforge.com/guides/robot-vacuum-simulator-python/
pip install numpy matplotlib   ->   python vacuum_sim.py
"""
import math, collections
import numpy as np
import matplotlib.pyplot as plt

CELL, W, H = 0.05, 5.0, 4.0                 # 5 cm grid, 5 x 4 m room
SPEED, RADIUS, BRUSH = 0.30, 0.17, 0.15     # m/s, robot radius, brush half-width (m)
DT, MINUTES = 0.1, 15
GX, GY = int(W / CELL), int(H / CELL)
wall = np.zeros((GY, GX), bool)
wall[[0, -1], :] = wall[:, [0, -1]] = True
for x0, y0, x1, y1 in [(0, 0, 2.2, 0.8),          # sofa
                       (1.0, 1.5, 2.0, 2.1),      # coffee table
                       (3.5, 3.4, 5.0, 4.0)]:     # cabinet
    wall[int(y0 / CELL):int(y1 / CELL), int(x0 / CELL):int(x1 / CELL)] = True
for x, y in [(3.2, 1.6), (4.3, 1.6), (3.2, 2.4), (4.3, 2.4)]:   # table legs
    wall[int(y / CELL), int(x / CELL)] = True

def grow(mask, r):                               # cells within r metres of mask
    n, out = int(round(r / CELL)), mask.copy()
    for dy in range(-n, n + 1):
        for dx in range(-n, n + 1):
            if dx * dx + dy * dy <= n * n:
                out |= np.roll(np.roll(mask, dy, 0), dx, 1)
    return out

blocked = grow(wall, RADIUS)                     # where the robot's centre can't go
floor = grow(~blocked, BRUSH - CELL) & ~wall     # floor the brush can reach
cell = lambda x, y: (min(GY - 1, max(0, int(y / CELL))), min(GX - 1, max(0, int(x / CELL))))

def bfs(start, goal):                            # shortest path on free cells to goal(cell)
    prev, q = {start: None}, collections.deque([start])
    while q:
        c = q.popleft()
        if goal(c) and c != start:
            path = []
            while c: path.append(c); c = prev[c]
            return [((j + .5) * CELL, (i + .5) * CELL) for i, j in path[::-1][1:]]
        for di, dj in ((1, 0), (-1, 0), (0, 1), (0, -1)):
            n = (c[0] + di, c[1] + dj)
            if 0 <= n[0] < GY and 0 <= n[1] < GX and not blocked[n] and n not in prev:
                prev[n] = c; q.append(n)
    return []

LANES = []                                       # boustrophedon: north/south lanes one brush apart
for j in range(int(RADIUS / CELL), GX, int(1.8 * BRUSH / CELL)):
    run_start = None
    for i in range(GY + 1):
        free = i < GY and not blocked[i, j]
        if free and run_start is None: run_start = i
        if not free and run_start is not None:
            LANES.append([((j + .5) * CELL, (run_start + .5) * CELL), ((j + .5) * CELL, (i - .5) * CELL)])
            run_start = None

def run(strategy, seed=0):
    rng = np.random.default_rng(seed)
    x, y, th, path, lanes = 2.7, 1.1, math.pi / 2, [], [l[:] for l in LANES]
    done = False
    clean, yy, xx = np.zeros((GY, GX), bool), *np.mgrid[0:GY, 0:GX]
    curve, n = [], int(BRUSH / CELL)
    for step in range(int(MINUTES * 60 / DT)):
        if strategy == "planned":
            if not path and lanes:               # next lane: go to its nearest end, drive to the other
                k = min(range(len(lanes)), key=lambda k: min(math.dist((x, y), e) for e in lanes[k]))
                a, b = sorted(lanes.pop(k), key=lambda e: math.dist((x, y), e))
                path = bfs(cell(x, y), lambda c: c == cell(*a)) + [b]
            elif not path and not done:          # lanes done: mop up any missed spots
                target = grow(floor & ~clean, BRUSH - CELL)
                path = bfs(cell(x, y), lambda c: target[c]); done = not path
            if path:
                tx, ty = path[0]; d = math.hypot(tx - x, ty - y)
                if d < SPEED * DT: x, y = path.pop(0)
                else: x, y = x + SPEED * DT * (tx - x) / d, y + SPEED * DT * (ty - y) / d
        else:                                    # random bounce: straight until a bump, random turn
            nx, ny = x + SPEED * DT * math.cos(th), y + SPEED * DT * math.sin(th)
            if blocked[cell(nx, ny)]: th += math.pi / 2 + rng.uniform(0, math.pi)
            else: x, y = nx, ny
        i, j = cell(x, y)                        # brush paints a disc
        s = (slice(max(0, i - n), i + n + 1), slice(max(0, j - n), j + n + 1))
        clean[s] |= ((yy[s] - i) ** 2 + (xx[s] - j) ** 2 <= n * n) & floor[s]
        if step % 10 == 0: curve.append(clean.sum() / floor.sum())
    return np.array(curve), clean

minutes = lambda c: next((k / 60 for k, v in enumerate(c) if v >= .9), None)   # one point per second
results = {"planned": run("planned"), **{f"random #{s}": run("random", s) for s in range(3)}}
for name, (curve, _) in results.items():
    t90 = minutes(curve)
    print(f"{name:10s} 90% clean after: {f'{t90:.1f} min' if t90 else 'not reached':>12}"
          f"   coverage at {MINUTES} min: {curve[-1]:.1%}")

fig, ax = plt.subplots(1, 3, figsize=(15, 4.8))
for name, (curve, _) in results.items():
    ax[0].plot(np.arange(len(curve)) / 60, curve * 100, label=name, lw=2 if name == "planned" else 1)
ax[0].axhline(90, ls="--", c="gray"); ax[0].set(xlabel="minutes", ylabel="% of floor cleaned"); ax[0].legend()
for a, name in zip(ax[1:], ["planned", "random #0"]):
    a.imshow(np.where(wall, 0, np.where(results[name][1], 2, 1)), origin="lower", cmap="Greys_r")
    a.set_title(f"{name}: white = cleaned after {MINUTES} min"); a.axis("off")
plt.tight_layout(); plt.savefig("vacuum_coverage.png", dpi=110); print("saved vacuum_coverage.png")
