"""Smooth joint control — block until target reached.

Helper for :class:`lib.robot.Robot`. Drives both followers in parallel,
plans the per-move duration from a peak-velocity budget, and reports
planned vs actual timing.

Standalone so it can be unit-tested or used outside the Robot context.
"""
from __future__ import annotations

import threading
import time
from dataclasses import dataclass
from typing import TYPE_CHECKING

import numpy as np

from raiden.robot.controller import smooth_move_joints

if TYPE_CHECKING:
    from .robot import Robot


DOF = 7


@dataclass
class MoveResult:
    delta_rad: float          # max abs per-joint delta from start to target
    planned_s: float          # duration computed from velocity budget
    actual_s: float           # wall-clock time the move actually took
    achieved_rad_s: float     # delta_rad / actual_s


def plan_duration(
    current14: np.ndarray,
    target14: np.ndarray,
    vel_rad_s: float,
    min_move_s: float,
) -> tuple[float, float]:
    """Return ``(duration_s, delta_rad)`` for a smooth move.

    Sized by whichever joint moves the furthest — both arms get the same
    duration so they finish in sync.
    """
    delta = float(np.max(np.abs(target14 - current14)))
    duration = max(min_move_s, delta / max(vel_rad_s, 1e-6))
    return duration, delta


def smooth_move(
    robot: "Robot",
    target14: np.ndarray,
    vel_rad_s: float,
    min_move_s: float = 0.2,
    steps: int = 100,
) -> MoveResult:
    """Drive both arms to ``target14``, blocking until reached.

    Uses raiden's ``smooth_move_joints`` per arm in parallel threads.
    """
    current = robot.joints14
    duration, delta = plan_duration(current, target14, vel_rad_s, min_move_s)

    threads = []
    if robot.follower_l is not None:
        threads.append(threading.Thread(
            target=smooth_move_joints,
            args=(robot.follower_l, target14[:DOF]),
            kwargs={"time_interval_s": duration, "steps": steps},
        ))
    if robot.follower_r is not None:
        threads.append(threading.Thread(
            target=smooth_move_joints,
            args=(robot.follower_r, target14[DOF:2 * DOF]),
            kwargs={"time_interval_s": duration, "steps": steps},
        ))

    t0 = time.perf_counter()
    for t in threads:
        t.start()
    for t in threads:
        t.join()
    actual = time.perf_counter() - t0

    return MoveResult(
        delta_rad=delta,
        planned_s=duration,
        actual_s=actual,
        achieved_rad_s=delta / actual if actual > 0 else 0.0,
    )