"""
pursuit_control
2点パシュートによる操舵量計算モジュール
多項式曲線上の近距離・遠距離の2点から操舵量と速度を算出する
"""

from dataclasses import dataclass

import numpy as np

from common import config
from pc.steering.base import SteeringBase, SteeringOutput
from pc.vision.line_detector import (
    ImageParams,
    detect_line,
    reset_valley_tracker,
)


@dataclass
class PursuitParams:
    """2点パシュート制御のパラメータ

    Attributes:
        near_ratio: 近い目標点の位置（0.0=上端，1.0=下端）
        far_ratio: 遠い目標点の位置（0.0=上端，1.0=下端）
        k_near: 近い目標点の操舵ゲイン
        k_far: 遠い目標点の操舵ゲイン
        max_steer_rate: 1フレームあたりの最大操舵変化量
        max_throttle: 直線での最大速度
        speed_k: カーブ減速係数（2点の差に対する係数）
    """
    near_ratio: float = 0.8
    far_ratio: float = 0.3
    k_near: float = 0.5
    k_far: float = 0.3
    max_steer_rate: float = 0.1
    max_throttle: float = 0.4
    speed_k: float = 2.0


class PursuitControl(SteeringBase):
    """2点パシュートによる操舵量計算クラス

    多項式曲線上の近い点と遠い点の2箇所のx座標偏差から
    操舵量を計算する．微分・曲率の計算が不要で直感的
    """

    def __init__(
        self,
        params: PursuitParams | None = None,
        image_params: ImageParams | None = None,
    ) -> None:
        self.params: PursuitParams = (
            params or PursuitParams()
        )
        self.image_params: ImageParams = (
            image_params or ImageParams()
        )
        self._prev_steer: float = 0.0
        self._last_result = None

    def compute(
        self, frame: np.ndarray,
    ) -> SteeringOutput:
        """カメラ画像から2点パシュートで操舵量を計算する

        Args:
            frame: グレースケールのカメラ画像

        Returns:
            計算された操舵量
        """
        p = self.params

        # 線検出
        result = detect_line(frame, self.image_params)
        self._last_result = result

        if not result.detected or result.poly_coeffs is None:
            return SteeringOutput(
                throttle=0.0, steer=0.0,
            )

        poly = np.poly1d(result.poly_coeffs)
        center_x = config.FRAME_WIDTH / 2.0
        h = float(config.FRAME_HEIGHT)

        # 2点の y 座標を計算
        near_y = h * p.near_ratio
        far_y = h * p.far_ratio

        # 2点の x 座標を多項式から取得
        near_x = poly(near_y)
        far_x = poly(far_y)

        # 各点の偏差（正: 線が左にある → 右に曲がる）
        near_err = (center_x - near_x) / center_x
        far_err = (center_x - far_x) / center_x

        # 操舵量
        steer = p.k_near * near_err + p.k_far * far_err
        steer = max(-1.0, min(1.0, steer))

        # レートリミッター
        delta = steer - self._prev_steer
        max_delta = p.max_steer_rate
        delta = max(-max_delta, min(max_delta, delta))
        steer = self._prev_steer + delta

        # 速度制御（2点の x 差でカーブ度合いを判定）
        curve = abs(near_x - far_x) / center_x
        throttle = p.max_throttle - p.speed_k * curve
        throttle = max(0.0, throttle)

        self._prev_steer = steer

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

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

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