"""A small helper for the DotBot Academy workshop, over the controller's REST API."""

import math
import time

import requests

DOTBOT = 0  # the `{application}` segment of a robot's routes
ARRIVED = 2  # the robot's waypoints_status once it reaches a goto() target
BASE = "http://localhost:8000/controller"


def connect(base_url="http://localhost:8000"):
    """Point every helper at a controller, e.g. http://192.168.1.10:8000."""
    global BASE
    BASE = base_url.rstrip("/") + "/controller"
    return all_bots()


class Bot:
    def __init__(self, address):
        self.address = address
        self.target = None
        self.threshold = None

    def __repr__(self):
        return f"Bot({self.address})"

    def _put(self, route, body):
        url = f"{BASE}/dotbots/{self.address}/{DOTBOT}/{route}"
        requests.put(url, json=body, timeout=2).raise_for_status()

    def led(self, red, green, blue):
        """Set the RGB LED, each channel 0-255."""
        self._put("rgb_led", {"red": red, "green": green, "blue": blue})

    def goto(self, x, y, threshold=60):
        """Drive on board to (x, y) in mm; stops within `threshold` mm.

        Sends the target and returns at once; use arrived() to wait.
        """
        self._put("waypoints", {"threshold": threshold, "waypoints": [{"x": x, "y": y}]})
        self.target = (x, y)
        self.threshold = threshold

    def arrived(self):
        """True once the robot has reached its last goto() target."""
        bot = requests.get(f"{BASE}/dotbots/{self.address}", timeout=2).json()
        pos = bot.get("lh2_position")
        if self.target is None or pos is None or bot.get("waypoints_status") != ARRIVED:
            return False
        # The status can still be the previous goto()'s, so check where it is too
        return distance((pos["x"], pos["y"]), self.target) <= self.threshold + 50

    def position(self):
        """The LH2 position as (x, y) in mm, or None if not localized."""
        bot = requests.get(f"{BASE}/dotbots/{self.address}", timeout=2).json()
        pos = bot.get("lh2_position")
        return (pos["x"], pos["y"]) if pos else None

    def drive(self, speed=100, seconds=1):
        """Drive straight at `speed` mm/s (negative backs up) for `seconds`, then stop."""
        self.wheels(speed, speed, seconds)

    def spin(self, speed=100, seconds=1):
        """Turn on the spot, clockwise on the map (negative: anticlockwise), then stop."""
        self.wheels(speed, -speed, seconds)

    def wheels(self, left, right, seconds):
        """Each wheel's speed in mm/s, -700..700, for `seconds`, then stop."""
        self._repeat("wheel_velocity", {"left_mm_s": left, "right_mm_s": right}, seconds)

    def drive_raw(self, left, right, seconds):
        """Motor duty per wheel, -127..127, no speed control; below about 51 a
        wheel at rest does not turn."""
        body = {"left_x": 0, "left_y": left, "right_x": 0, "right_y": right}
        self._repeat("move_raw", body, seconds)

    def stop(self):
        self._put("wheel_velocity", {"left_mm_s": 0, "right_mm_s": 0})

    def _repeat(self, route, body, seconds):
        # The robot stops by itself ~0.5 s after the last driving command
        end = time.time() + seconds
        while time.time() < end:
            self._put(route, body)
            time.sleep(0.1)
        self.stop()


def distance(a, b):
    """Distance in mm between two (x, y) points."""
    return math.dist(a, b)


def all_bots():
    """Every robot the controller knows."""
    bots = requests.get(f"{BASE}/dotbots", timeout=2).json()
    return [Bot(b["address"]) for b in bots]


def bot(label):
    """Your robot, from the 6 hex digits on its label (or a full address)."""
    label = label.upper()
    found = [b for b in all_bots() if b.address.upper().endswith(label)]
    if len(found) != 1:
        raise LookupError(f"{len(found)} robots match {label!r}, expected 1")
    return found[0]


def swarm(labels):
    """Your robots, from their labels."""
    return [bot(label) for label in labels]