"""
ts_pd_control
Theil-Sen 直線近似による PD 制御モジュール
行中心点に Theil-Sen 直線をフィッティングし，
位置偏差・傾き・微分項から操舵量を算出する
"""

import time
from dataclasses import dataclass

import numpy as np

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


@dataclass
class TsPdParams:
    """Theil-Sen PD 制御のパラメータ

    Attributes:
        kp: 位置偏差ゲイン
        kh: 傾き（Theil-Sen slope）ゲイン
        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 = 2.0


class TsPdControl(SteeringBase):
    """Theil-Sen 直線近似による PD 制御クラス

    行中心点から Theil-Sen 直線近似を行い，
    画像下端での位置偏差と直線の傾きから PD 制御で操舵量を計算する
    """

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

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

        Args:
            result: 線検出の結果

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

        p = self.params
        centers = result.row_centers

        # 有効な点（NaN でない行）を抽出
        valid = ~np.isnan(centers)
        ys = np.where(valid)[0].astype(float)
        xs = centers[valid]

        if len(ys) < 2:
            return SteeringOutput(
                throttle=0.0, steer=0.0,
            )

        # Theil-Sen 直線近似: x = slope * y + intercept
        slope, intercept = theil_sen_fit(ys, xs)

        center_x = config.FRAME_WIDTH / 2.0
        h = len(centers)

        # 画像下端での位置偏差
        bottom_x = slope * (h - 1) + intercept
        position_error = (center_x - bottom_x) / center_x

        # 操舵量: P 項（位置偏差）+ Heading 項（傾き）
        error = p.kp * position_error + p.kh * slope

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

        # D 項（微分項）
        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(slope)
        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