"""
pd_control
PD 制御による操舵量計算モジュール
多項式フィッティングの位置・傾き・曲率から操舵量と速度を算出する
"""

import time
from dataclasses import dataclass

import numpy as np

from common.steering.base import SteeringBase, SteeringOutput
from common.vision.line_detector import (
    ImageParams,
    LineDetectResult,
)


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

    Attributes:
        kp: 位置偏差ゲイン
        kh: 傾き（ヘディング）ゲイン
        kd: 微分ゲイン
        max_steer_rate: 1フレームあたりの最大操舵変化量
        max_throttle: 直線での最大速度
        speed_k: 曲率ベースの減速係数
    """
    kp: float = 0.5
    kh: float = 0.3
    kd: float = 0.1
    max_steer_rate: float = 0.1
    max_throttle: float = 0.4
    speed_k: float = 0.3


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

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

    def _compute_from_result(
        self, result: LineDetectResult,
    ) -> SteeringOutput:
        """PD 制御で操舵量を計算する

        Args:
            result: 線検出の結果

        Returns:
            計算された操舵量
        """
        if not result.detected:
            return SteeringOutput(
                throttle=0.0, steer=0.0,
            )

        p = self.params

        # 位置偏差 + 傾きによる操舵量
        error = (
            p.kp * result.position_error
            + p.kh * result.heading
        )

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

        # 微分項
        derivative = (error - self._prev_error) / dt
        steer = error + p.kd * derivative

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

        # 速度制御（曲率連動）
        throttle = (
            p.max_throttle
            - p.speed_k * abs(result.curvature)
        )
        throttle = max(0.0, throttle)

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

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

    def _max_steer_rate(self) -> float:
        return self.params.max_steer_rate

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