"""
line_detector
カメラ画像から黒線の位置を検出するモジュール
二値化画像の白ピクセルに多項式をフィッティングして
線の位置・傾き・曲率を算出する
"""

from dataclasses import dataclass

import cv2
import numpy as np

from common import config

# 検出領域の y 範囲（画像全体）
DETECT_Y_START: int = 0
DETECT_Y_END: int = config.FRAME_HEIGHT

# フィッティングに必要な最小ピクセル数
MIN_FIT_PIXELS: int = 50


@dataclass
class ImageParams:
    """画像処理パラメータ

    Attributes:
        clahe_clip: CLAHE のコントラスト増幅上限
        clahe_grid: CLAHE の局所領域分割数
        blur_size: ガウシアンブラーのカーネルサイズ（奇数）
        binary_thresh: 二値化の閾値
        open_size: オープニングのカーネルサイズ（孤立ノイズ除去）
        close_width: クロージングの横幅（途切れ補間）
        close_height: クロージングの高さ
    """
    clahe_clip: float = 2.0
    clahe_grid: int = 8
    blur_size: int = 5
    binary_thresh: int = 80
    open_size: int = 5
    close_width: int = 25
    close_height: int = 3


@dataclass
class LineDetectResult:
    """線検出の結果を格納するデータクラス

    Attributes:
        detected: 線が検出できたか
        position_error: 画像下端での位置偏差（-1.0～+1.0）
        heading: 線の傾き（dx/dy，画像下端での値）
        curvature: 線の曲率（d²x/dy²）
        poly_coeffs: 多項式の係数（描画用，未検出時は None）
        binary_image: 二値化後の画像（デバッグ用）
    """
    detected: bool
    position_error: float
    heading: float
    curvature: float
    poly_coeffs: np.ndarray | None
    binary_image: np.ndarray | None


def detect_line(
    frame: np.ndarray,
    params: ImageParams | None = None,
) -> LineDetectResult:
    """画像から黒線の位置を検出する

    二値化後の白ピクセルに2次多項式をフィッティングし，
    線の位置・傾き・曲率を算出する

    Args:
        frame: BGR 形式のカメラ画像
        params: 画像処理パラメータ（None の場合はデフォルト）

    Returns:
        線検出の結果
    """
    if params is None:
        params = ImageParams()

    # グレースケール変換
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # CLAHE でコントラスト強調
    clahe = cv2.createCLAHE(
        clipLimit=params.clahe_clip,
        tileGridSize=(
            params.clahe_grid, params.clahe_grid,
        ),
    )
    enhanced = clahe.apply(gray)

    # ガウシアンブラー（カーネルサイズは奇数に補正）
    blur_k = params.blur_size | 1
    blurred = cv2.GaussianBlur(
        enhanced, (blur_k, blur_k), 0,
    )

    # 固定閾値で二値化（黒線を白，背景を黒に反転）
    _, binary = cv2.threshold(
        blurred, params.binary_thresh, 255,
        cv2.THRESH_BINARY_INV,
    )

    # オープニング（収縮→膨張で孤立ノイズを除去）
    if params.open_size >= 3:
        open_k = params.open_size | 1
        open_kernel = cv2.getStructuringElement(
            cv2.MORPH_ELLIPSE, (open_k, open_k),
        )
        binary = cv2.morphologyEx(
            binary, cv2.MORPH_OPEN, open_kernel,
        )

    # 横方向クロージング（反射で途切れた線を左右からつなぐ）
    if params.close_width >= 3:
        close_h = max(params.close_height | 1, 1)
        close_kernel = cv2.getStructuringElement(
            cv2.MORPH_ELLIPSE,
            (params.close_width, close_h),
        )
        binary = cv2.morphologyEx(
            binary, cv2.MORPH_CLOSE, close_kernel,
        )

    # 検出領域内の白ピクセル座標を取得
    region = binary[DETECT_Y_START:DETECT_Y_END, :]
    ys_local, xs = np.where(region > 0)

    if len(xs) < MIN_FIT_PIXELS:
        return LineDetectResult(
            detected=False,
            position_error=0.0,
            heading=0.0,
            curvature=0.0,
            poly_coeffs=None,
            binary_image=binary,
        )

    # 全体画像座標に変換
    ys = ys_local + DETECT_Y_START

    # 2次多項式フィッティング: x = f(y) = ay² + by + c
    coeffs = np.polyfit(ys, xs, 2)
    poly = np.poly1d(coeffs)

    # 画像中心
    center_x = config.FRAME_WIDTH / 2.0

    # 画像下端（近方）での位置偏差
    x_bottom = poly(DETECT_Y_END)
    position_error = (center_x - x_bottom) / center_x

    # 傾き: dx/dy（画像下端での値）
    poly_deriv = poly.deriv()
    heading = float(poly_deriv(DETECT_Y_END))

    # 曲率: d²x/dy²（2次多項式では定数 = 2a）
    poly_deriv2 = poly_deriv.deriv()
    curvature = float(poly_deriv2(DETECT_Y_END))

    return LineDetectResult(
        detected=True,
        position_error=position_error,
        heading=heading,
        curvature=curvature,
        poly_coeffs=coeffs,
        binary_image=binary,
    )