"""
pd_control
PD 制御による操舵量計算モジュール
近方・遠方の2領域偏差から操舵量と速度を算出する
"""

import time
from dataclasses import dataclass, field

import numpy as np

from pc.steering.base import SteeringBase, SteeringOutput
from pc.vision.line_detector import detect_line


@dataclass
class PdParams:
    """PD 制御のパラメータ

    Attributes:
        kp: 比例ゲイン
        kd: 微分ゲイン
        alpha: 近方偏差の重み
        beta: 遠方偏差の重み
        max_throttle: 直線での最大速度
        speed_k: 減速係数
    """
    kp: float = 0.5
    kd: float = 0.1
    alpha: float = 0.6
    beta: float = 0.4
    max_throttle: float = 0.4
    speed_k: float = 0.3


class PdControl(SteeringBase):
    """PD 制御による操舵量計算クラス"""

    def __init__(
        self, params: PdParams | None = None,
    ) -> None:
        self.params: PdParams = params or PdParams()
        self._prev_error: float = 0.0
        self._prev_time: float = 0.0
        self._last_result = None

    def compute(
        self, frame: np.ndarray,
    ) -> SteeringOutput:
        """カメラ画像から PD 制御で操舵量を計算する

        Args:
            frame: BGR 形式のカメラ画像

        Returns:
            計算された操舵量
        """
        # 線検出
        result = detect_line(frame)
        self._last_result = result

        # 線が検出できなかった場合は停止
        if result.near_x is None and result.far_x is None:
            return SteeringOutput(throttle=0.0, steer=0.0)

        # 片方のみ検出できた場合はもう片方の値で代用
        near_error = result.near_error
        far_error = result.far_error
        if result.near_x is None:
            near_error = far_error
        if result.far_x is None:
            far_error = near_error

        # 制御用偏差の合成
        p = self.params
        error = p.alpha * near_error + p.beta * far_error

        # 時間差分の計算
        now = time.time()
        dt = now - self._prev_time if self._prev_time > 0 else 0.033
        dt = max(dt, 0.001)

        # PD 制御
        derivative = (error - self._prev_error) / dt
        steer = p.kp * error + p.kd * derivative

        # 操舵量のクランプ
        steer = max(-1.0, min(1.0, steer))

        # 速度制御（操舵量連動）
        throttle = p.max_throttle - p.speed_k * abs(steer)
        throttle = max(0.0, throttle)

        # 状態の更新
        self._prev_error = error
        self._prev_time = now

        return SteeringOutput(
            throttle=throttle, steer=steer,
        )

    def reset(self) -> None:
        """内部状態をリセットする"""
        self._prev_error = 0.0
        self._prev_time = 0.0
        self._last_result = None

    @property
    def last_detect_result(self):
        """直近の線検出結果を取得する"""
        return self._last_result