"""State-machine behaviour with the GPS geofence, dog-play, and dashboard controls.

Driven off-robot with a recording fake robot, a scripted fake GPS, and a fake
perception source. Stdlib only.
"""

from __future__ import annotations

import time
from typing import List, Optional, Tuple

from config import default_config
from gowelcome.geo.gps import GpsFix
from gowelcome.statemachine import GoWelcomeStateMachine
from gowelcome.types import Detection, PerceptionResult, RoadInfo, State
from gowelcome.robot.interface import RobotInterface

FRAME_W, FRAME_H = 640, 480
DT = 1.0 / 15.0


class FakeRobot(RobotInterface):
    def __init__(self) -> None:
        self.drives: List[Tuple[float, float, float]] = []
        self.stops = 0
        self.gestures: List[str] = []
        self.greetings = 0
        self.balance_stands = 0
        self.damps = 0

    def get_frame(self):
        return None

    def frame_size(self):
        return (FRAME_W, FRAME_H)

    def drive(self, vx, vy, vyaw):
        self.drives.append((vx, vy, vyaw))

    def stop(self):
        self.stops += 1

    def set_avoidance(self, on):
        pass

    def balance_stand(self):
        self.balance_stands += 1

    def stand_up(self):
        pass

    def damp(self):
        self.damps += 1

    def gesture(self, name):
        self.gestures.append(name)

    def play_greeting(self):
        self.greetings += 1

    def shutdown(self):
        pass


class FakeGps:
    """Returns a settable fix; records commanded motion."""

    def __init__(self, fix: Optional[GpsFix] = None) -> None:
        self._fix = fix
        self.commands: List[Tuple[float, float, float]] = []

    def set_fix(self, fix: Optional[GpsFix]) -> None:
        self._fix = fix

    def latest(self) -> Optional[GpsFix]:
        return self._fix

    def on_command(self, vx, vy, vyaw):
        self.commands.append((vx, vy, vyaw))


class FakePerception:
    def __init__(self, result: PerceptionResult) -> None:
        self._result = result

    def latest(self) -> PerceptionResult:
        self._result.ts = time.monotonic()
        return self._result


def _empty_perc() -> PerceptionResult:
    return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[], dangers=[],
                            road=None, ts=time.monotonic(), seq=1)


def _fix(lat, lon, course=None) -> GpsFix:
    return GpsFix(lat=lat, lon=lon, ts=time.monotonic(), course_deg=course,
                  speed_mps=0.3, num_sats=10, hdop=0.8)


# --------------------------------------------------------------------------- #
# Geofence
# --------------------------------------------------------------------------- #
def test_breach_enters_boundary_and_homes_forward():
    cfg = default_config()
    cfg.geofence.enabled = True
    cfg.geofence.center_mode = "onstart"
    robot = FakeRobot()
    gps = FakeGps(_fix(0.0, 0.0, course=0.0))   # at centre
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)

    sm.step(DT)                                  # captures centre, stays inside
    assert sm.state is State.WANDER

    gps.set_fix(_fix(0.0, 0.001, course=90.0))   # ~111 m east -> way outside
    state = sm.step(DT)
    assert state is State.BOUNDARY
    vx, _vy, vyaw = robot.drives[-1]
    assert vx > 0.0                              # driving back toward centre
    assert abs(vyaw) > 0.0                       # steering toward centre


def test_boundary_low_speed_ignores_garbage_course():
    """Near-stationary GPS course is noise -> homing must use the safe gentle
    re-acquire turn, not steer on the (possibly wrong-way) reported course."""
    import pytest
    cfg = default_config()
    cfg.geofence.enabled = True
    cfg.geofence.center_mode = "onstart"   # min_speed_for_course default 0.15
    robot = FakeRobot()
    gps = FakeGps(_fix(0.0, 0.0, course=0.0))
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
    sm.step(DT)                            # capture centre
    # Breached + near-stationary + a course pointing the WRONG way (east, away).
    gps.set_fix(GpsFix(lat=0.0, lon=0.001, ts=time.monotonic(), course_deg=90.0,
                       speed_mps=0.0, num_sats=10, hdop=0.8))
    sm.step(DT)
    assert sm.state is State.BOUNDARY
    vx, _vy, vyaw = robot.drives[-1]
    assert vx > 0.0
    # course gated out -> gentle re-acquire turn (~+0.4), NOT the garbage hard turn.
    assert vyaw == pytest.approx(0.4, abs=1e-6)


def test_gps_lost_holds():
    cfg = default_config()
    cfg.geofence.enabled = True
    cfg.geofence.no_fix_behavior = "stop"
    robot = FakeRobot()
    gps = FakeGps(_fix(0.0, 0.0))
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
    sm.step(DT)                                  # capture centre
    drives_before = len(robot.drives)
    gps.set_fix(None)                            # GPS lost
    sm.step(DT)
    assert robot.stops >= 1
    assert len(robot.drives) == drives_before    # never drove blind


def test_no_center_holds_until_first_fix():
    cfg = default_config()
    cfg.geofence.enabled = True
    robot = FakeRobot()
    gps = FakeGps(None)                          # never a fix
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
    sm.step(DT)
    assert robot.stops >= 1
    assert not robot.drives


def test_geofence_disabled_when_no_gps_source():
    cfg = default_config()
    robot = FakeRobot()
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
    sm.step(DT)
    assert sm.state is State.WANDER
    assert robot.drives and robot.drives[-1][0] > 0.0   # roams normally


# --------------------------------------------------------------------------- #
# Vision-only containment (default: no GPS) -- stay away from road + cars
# --------------------------------------------------------------------------- #
def _perc(road=None, dets=None) -> PerceptionResult:
    dets = dets or []
    dangers = [d for d in dets if d.height_ratio(FRAME_H) >= 0.25 and d.label != "person"]
    return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[],
                            dangers=dangers, detections=dets, road=road,
                            ts=time.monotonic(), seq=1)


def test_default_is_vision_only_no_gps_hold():
    """Default config has no GPS -> the dog roams (vision-only), never holds."""
    cfg = default_config()
    assert cfg.geofence.enabled is False and cfg.gps.enabled is False
    robot = FakeRobot()
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
    sm.step(DT)
    assert sm.state is State.WANDER
    assert robot.drives and robot.drives[-1][0] > 0.0   # cruising, not holding


def test_road_repulsion_steers_away_and_slows():
    """Soft band road on the LEFT -> veer RIGHT (vyaw<0) and slow down."""
    cfg = default_config()
    cfg.wander.yaw_sweep_rate = 0.0          # isolate the repulsion term
    robot = FakeRobot()
    # road heavier on the left, centre below the hard AVOID trigger (0.35)
    road = RoadInfo(coverage=0.2, left=0.30, center=0.2, right=0.03)
    sm = GoWelcomeStateMachine(robot, FakePerception(_perc(road=road)), cfg)
    sm.step(DT)
    assert sm.state is State.WANDER          # soft band -> not hard AVOID_DANGER
    vx, _vy, vyaw = robot.drives[-1]
    assert vyaw < 0.0                        # turn right, away from the left road
    assert 0.0 < vx < cfg.wander.forward_speed   # slowed but still moving


def test_car_repulsion_steers_away():
    """A (far/small) car on the RIGHT -> veer LEFT (vyaw>0) away from it."""
    cfg = default_config()
    cfg.wander.yaw_sweep_rate = 0.0
    robot = FakeRobot()
    car = Detection(label="car", conf=0.5, x1=520, y1=210, x2=600, y2=260)  # right, small
    sm = GoWelcomeStateMachine(robot, FakePerception(_perc(dets=[car])), cfg)
    sm.step(DT)
    assert sm.state is State.WANDER          # small/far car -> soft steer, not hard avoid
    vx, _vy, vyaw = robot.drives[-1]
    assert vyaw > 0.0                        # turn left, away from the right-side car


# --------------------------------------------------------------------------- #
# Dog-play + dashboard controls
# --------------------------------------------------------------------------- #
def test_play_fires_in_wander():
    cfg = default_config()
    cfg.play.moderate_interval = 0.0
    cfg.play.jitter = 0.0
    robot = FakeRobot()
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
    sm.step(DT)
    assert sm.state is State.WANDER
    assert robot.gestures, "a dog-play gesture should have fired"
    assert robot.gestures[-1] in cfg.play.actions


def test_set_play_mode_and_status():
    cfg = default_config()
    sm = GoWelcomeStateMachine(FakeRobot(), FakePerception(_empty_perc()), cfg)
    assert sm.play_mode() == "moderate"
    assert sm.set_play_mode("playful") is True
    assert sm.play_mode() == "playful"
    assert sm.set_play_mode("bogus") is False
    s = sm.status()
    assert s["play_mode"] == "playful"
    assert s["state"] in {st.value for st in State}


def test_pause_stops_robot():
    cfg = default_config()
    robot = FakeRobot()
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg)
    sm.set_paused(True)
    sm.step(DT)
    assert robot.stops >= 1 and not robot.drives
    sm.set_paused(False)
    sm.step(DT)
    assert robot.drives    # resumes


def test_estop_damps_and_stops():
    cfg = default_config()
    robot = FakeRobot()
    sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg)
    sm.request_estop()
    sm.step(DT)
    assert robot.damps >= 1
    assert robot.stops >= 1
    assert not robot.drives
    sm.clear_estop()
    sm.step(DT)
    assert robot.drives