"""
recovery
コースアウト復帰のパラメータと判定ロジックを定義するモジュール
黒線を一定時間検出できなかった場合に，
最後に検出した方向へ旋回しながら走行して復帰する
"""

import time
from dataclasses import dataclass

from pc.steering.base import SteeringOutput


@dataclass
class RecoveryParams:
    """コースアウト復帰のパラメータ

    Attributes:
        enabled: 復帰機能の有効/無効
        timeout_sec: 線を見失ってから復帰動作を開始するまでの時間
        steer_amount: 復帰時の操舵量（0.0 ~ 1.0）
        throttle: 復帰時の速度（負: 後退，正: 前進）
    """
    enabled: bool = True
    timeout_sec: float = 0.5
    steer_amount: float = 0.5
    throttle: float = -0.3


class RecoveryController:
    """コースアウト復帰の判定と操舵量算出を行うクラス

    自動操縦中にフレームごとに呼び出し，
    線検出の成否を記録する．一定時間検出できなかった場合に
    復帰用の操舵量を返す
    """

    def __init__(
        self,
        params: RecoveryParams | None = None,
    ) -> None:
        self.params: RecoveryParams = (
            params or RecoveryParams()
        )
        self._last_detected_time: float = 0.0
        self._last_error_sign: float = 0.0
        self._is_recovering: bool = False

    def reset(self) -> None:
        """内部状態をリセットする

        自動操縦の開始時に呼び出す
        """
        self._last_detected_time = time.time()
        self._last_error_sign = 0.0
        self._is_recovering = False

    def update(
        self,
        detected: bool,
        position_error: float = 0.0,
    ) -> SteeringOutput | None:
        """検出結果を記録し，復帰が必要なら操舵量を返す

        毎フレーム呼び出す．線が検出できている間は内部状態を
        更新して None を返す．検出できない時間が timeout_sec を
        超えたら復帰用の SteeringOutput を返す

        Args:
            detected: 線が検出できたか
            position_error: 検出時の位置偏差（正: 線が左）

        Returns:
            復帰操舵量，または None（通常走行を継続）
        """
        if not self.params.enabled:
            return None

        now = time.time()

        if detected:
            self._last_detected_time = now
            if position_error != 0.0:
                self._last_error_sign = (
                    1.0 if position_error > 0 else -1.0
                )
            self._is_recovering = False
            return None

        # 線を見失ってからの経過時間を判定
        elapsed = now - self._last_detected_time
        if elapsed < self.params.timeout_sec:
            return None

        # 復帰モード: 最後に検出した方向へ旋回
        # position_error > 0（線が左）→ 左へ旋回（steer < 0）
        self._is_recovering = True
        steer = (
            -self._last_error_sign
            * self.params.steer_amount
        )
        return SteeringOutput(
            throttle=self.params.throttle,
            steer=steer,
        )

    @property
    def is_recovering(self) -> bool:
        """現在復帰動作中かどうかを返す"""
        return self._is_recovering