"""Kinematics: a seeded patrol route and the drone's motion along it."""

from __future__ import annotations

import math
import random
from dataclasses import dataclass, replace

from .config import Config


@dataclass(frozen=True)
class Pose:
    ts_ns: int
    x: float
    y: float
    z: float
    roll: float
    pitch: float
    yaw: float
    vx: float
    vy: float
    vz: float


def make_waypoints(cfg: Config, rng: random.Random) -> list[tuple[float, float]]:
    """Perimeter patrol with per-drone jitter so drones do not overlap."""
    s = cfg.area_size_m
    jitter = rng.uniform(0.05, 0.20) * s
    corners = [(jitter, jitter), (s - jitter, jitter), (s - jitter, s - jitter), (jitter, s - jitter)]
    start = rng.randrange(4)
    return corners[start:] + corners[:start]


def initial_pose(cfg: Config, rng: random.Random, ts_ns: int) -> Pose:
    wps = make_waypoints(cfg, rng)
    x, y = wps[0]
    return Pose(ts_ns=ts_ns, x=x, y=y, z=cfg.cruise_alt_m, roll=0.0, pitch=0.0, yaw=0.0, vx=0.0, vy=0.0, vz=0.0)


def step(pose: Pose, target: tuple[float, float], cfg: Config, rng: random.Random, dt: float, ts_ns: int) -> tuple[Pose, bool]:
    """Advance one tick toward the target waypoint. Returns (pose, reached)."""
    dx, dy = target[0] - pose.x, target[1] - pose.y
    dist = math.hypot(dx, dy)
    reached = dist < cfg.cruise_speed_ms * dt * 2

    speed = cfg.cruise_speed_ms * rng.uniform(0.9, 1.1)
    if dist > 1e-6:
        vx, vy = dx / dist * speed, dy / dist * speed
    else:
        vx, vy = 0.0, 0.0

    yaw = math.degrees(math.atan2(vy, vx))
    # Small attitude noise around the commanded values
    roll = rng.gauss(0.0, 1.5)
    pitch = rng.gauss(2.0, 1.0)
    z = cfg.cruise_alt_m + rng.gauss(0.0, 0.5)
    vz = (z - pose.z) / dt if dt > 0 else 0.0

    return (
        replace(
            pose,
            ts_ns=ts_ns,
            x=pose.x + vx * dt,
            y=pose.y + vy * dt,
            z=z,
            roll=roll,
            pitch=pitch,
            yaw=yaw,
            vx=vx,
            vy=vy,
            vz=vz,
        ),
        reached,
    )