"""
test_path_edges.py — pytest tests for _path_edges.py (no ROS2 required).

Tests cover:
  - build_homography: output shape, identity-like mapping
  - apply_homography: empty input, single point, batch
  - canny_edges: output shape, dtype, uniform image produces no edges
  - hough_lines: empty edge map returns []
  - classify_lines: slope filtering, left/right split
  - average_line: empty → None, single line, multi-line average
  - warp_segments: empty list, segment endpoint ordering
  - process_frame: smoke test on synthetic image
"""

from __future__ import annotations

import math

import cv2
import numpy as np
import pytest

from saltybot_bringup._path_edges import (
    PathEdgeConfig,
    PathEdgesResult,
    apply_homography,
    average_line,
    build_homography,
    canny_edges,
    classify_lines,
    hough_lines,
    process_frame,
    warp_segments,
)


# ── Helpers ───────────────────────────────────────────────────────────────────

def _solid_bgr(h: int = 100, w: int = 200, color=(128, 128, 128)) -> np.ndarray:
    img = np.zeros((h, w, 3), dtype=np.uint8)
    img[:] = color
    return img


def _default_H(roi_w: int = 200, roi_h: int = 100) -> np.ndarray:
    cfg = PathEdgeConfig()
    return build_homography(cfg.birdseye_src, roi_w, roi_h, cfg.birdseye_size)


# ── build_homography ──────────────────────────────────────────────────────────

class TestBuildHomography:
    def test_shape(self):
        H = _default_H()
        assert H.shape == (3, 3)

    def test_dtype(self):
        H = _default_H()
        assert H.dtype == np.float64

    def test_bottom_left_maps_near_left(self):
        """Bottom-left source trapezoid point should map near left of bird-eye."""
        cfg = PathEdgeConfig()
        H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size)
        # src[3] = [0.05, 0.95] → near bottom-left of ROI → should map to x≈100 (centre-left), y≈400
        src_pt = np.array([[0.05 * 200, 0.95 * 100]], dtype=np.float32)
        dst = apply_homography(H, src_pt)
        assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.25, abs=5)
        assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5)

    def test_bottom_right_maps_near_right(self):
        cfg = PathEdgeConfig()
        H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size)
        # src[2] = [0.95, 0.95] → bottom-right → maps to x≈300, y≈400
        src_pt = np.array([[0.95 * 200, 0.95 * 100]], dtype=np.float32)
        dst = apply_homography(H, src_pt)
        assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.75, abs=5)
        assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5)


# ── apply_homography ──────────────────────────────────────────────────────────

class TestApplyHomography:
    def test_empty_input(self):
        H = _default_H()
        out = apply_homography(H, np.empty((0, 2), dtype=np.float32))
        assert out.shape == (0, 2)

    def test_single_point_roundtrip(self):
        """Warping then inverse-warping should recover the original point."""
        H = _default_H(200, 100)
        H_inv = np.linalg.inv(H)
        pt = np.array([[50.0, 40.0]], dtype=np.float32)
        warped   = apply_homography(H,     pt)
        unwarped = apply_homography(H_inv, warped)
        np.testing.assert_allclose(unwarped, pt, atol=1e-3)

    def test_batch_output_shape(self):
        H = _default_H()
        pts = np.random.rand(5, 2).astype(np.float32) * 100
        out = apply_homography(H, pts)
        assert out.shape == (5, 2)
        assert out.dtype == np.float32


# ── canny_edges ───────────────────────────────────────────────────────────────

class TestCannyEdges:
    def test_output_shape(self):
        roi = _solid_bgr(80, 160)
        edges = canny_edges(roi, low=50, high=150, ksize=5)
        assert edges.shape == (80, 160)

    def test_output_dtype(self):
        roi = _solid_bgr()
        edges = canny_edges(roi, low=50, high=150, ksize=5)
        assert edges.dtype == np.uint8

    def test_uniform_image_no_edges(self):
        """A solid-colour image should produce no edges."""
        roi = _solid_bgr(80, 160, color=(100, 100, 100))
        edges = canny_edges(roi, low=50, high=150, ksize=5)
        assert edges.max() == 0

    def test_strong_edge_detected(self):
        """A sharp horizontal boundary should produce edges."""
        roi = np.zeros((100, 200, 3), dtype=np.uint8)
        roi[50:, :] = 255  # sharp boundary at y=50
        edges = canny_edges(roi, low=20, high=60, ksize=3)
        assert edges.max() == 255

    def test_ksize_even_skips_blur(self):
        """Even ksize should not crash (blur is skipped for even kernels)."""
        roi = _solid_bgr()
        edges = canny_edges(roi, low=50, high=150, ksize=4)
        assert edges.shape == (100, 200)


# ── hough_lines ───────────────────────────────────────────────────────────────

class TestHoughLines:
    def test_empty_edge_map(self):
        edge_map = np.zeros((100, 200), dtype=np.uint8)
        lines = hough_lines(edge_map, threshold=30, min_len=40, max_gap=20)
        assert lines == []

    def test_returns_list_of_tuples(self):
        """Draw a diagonal line and verify hough returns tuples of 4 floats."""
        edge_map = np.zeros((100, 200), dtype=np.uint8)
        cv2.line(edge_map, (0, 0), (199, 99), 255, 2)
        lines = hough_lines(edge_map, threshold=10, min_len=20, max_gap=5)
        assert isinstance(lines, list)
        if lines:
            assert len(lines[0]) == 4
            assert all(isinstance(v, float) for v in lines[0])

    def test_line_detected_on_drawn_segment(self):
        """A drawn line segment should be detected."""
        edge_map = np.zeros((200, 400), dtype=np.uint8)
        cv2.line(edge_map, (50, 100), (350, 150), 255, 2)
        lines = hough_lines(edge_map, threshold=10, min_len=30, max_gap=10)
        assert len(lines) >= 1


# ── classify_lines ────────────────────────────────────────────────────────────

class TestClassifyLines:
    def test_empty_returns_two_empty_lists(self):
        left, right = classify_lines([], min_slope=0.3)
        assert left == []
        assert right == []

    def test_negative_slope_goes_left(self):
        # slope = (y2-y1)/(x2-x1) = (50-0)/(0-100) = -0.5 → left
        lines = [(100.0, 0.0, 0.0, 50.0)]
        left, right = classify_lines(lines, min_slope=0.3)
        assert len(left) == 1
        assert right == []

    def test_positive_slope_goes_right(self):
        # slope = (50-0)/(100-0) = 0.5 → right
        lines = [(0.0, 0.0, 100.0, 50.0)]
        left, right = classify_lines(lines, min_slope=0.3)
        assert left == []
        assert len(right) == 1

    def test_near_horizontal_discarded(self):
        # slope = 0.1 → |slope| < 0.3 → discard
        lines = [(0.0, 0.0, 100.0, 10.0)]
        left, right = classify_lines(lines, min_slope=0.3)
        assert left == []
        assert right == []

    def test_vertical_line_skipped(self):
        # dx ≈ 0 → skip
        lines = [(50.0, 0.0, 50.0, 100.0)]
        left, right = classify_lines(lines, min_slope=0.3)
        assert left == []
        assert right == []

    def test_mixed_lines(self):
        lines = [
            (100.0, 0.0, 0.0, 50.0),    # slope -0.5 → left
            (0.0, 0.0, 100.0, 50.0),    # slope +0.5 → right
            (0.0, 0.0, 100.0, 5.0),     # slope +0.05 → discard
        ]
        left, right = classify_lines(lines, min_slope=0.3)
        assert len(left) == 1
        assert len(right) == 1


# ── average_line ──────────────────────────────────────────────────────────────

class TestAverageLine:
    def test_empty_returns_none(self):
        assert average_line([], roi_height=100) is None

    def test_single_line_extrapolated(self):
        # slope=0.5, intercept=0: x = y/0.5 = 2y
        # At y=99: x=198; at y=0: x=0
        lines = [(0.0, 0.0, 200.0, 100.0)]  # slope = 100/200 = 0.5
        result = average_line(lines, roi_height=100)
        assert result is not None
        x_bot, y_bot, x_top, y_top = result
        assert y_bot == pytest.approx(99.0, abs=1)
        assert y_top == pytest.approx(0.0, abs=1)

    def test_two_parallel_lines_averaged(self):
        # Both have slope=1.0, intercepts -50 and +50 → avg intercept=0
        # x = (y - 0) / 1.0 = y
        lines = [
            (50.0, 0.0, 100.0, 50.0),   # slope=1, intercept=-50
            (0.0, 50.0, 50.0, 100.0),   # slope=1, intercept=50
        ]
        result = average_line(lines, roi_height=100)
        assert result is not None
        x_bot, y_bot, x_top, y_top = result
        assert y_bot == pytest.approx(99.0, abs=1)
        # avg intercept = 0, m_avg=1 → x_bot = 99
        assert x_bot == pytest.approx(99.0, abs=2)

    def test_vertical_only_returns_none(self):
        # dx == 0 → skip → no slopes → None
        lines = [(50.0, 0.0, 50.0, 100.0)]
        assert average_line(lines, roi_height=100) is None

    def test_returns_four_tuple(self):
        lines = [(0.0, 0.0, 100.0, 50.0)]
        result = average_line(lines, roi_height=100)
        assert result is not None
        assert len(result) == 4


# ── warp_segments ─────────────────────────────────────────────────────────────

class TestWarpSegments:
    def test_empty_returns_empty(self):
        H = _default_H()
        result = warp_segments([], H)
        assert result == []

    def test_single_segment_returns_one_tuple(self):
        H = _default_H(200, 100)
        lines = [(10.0, 10.0, 90.0, 80.0)]
        result = warp_segments(lines, H)
        assert len(result) == 1
        assert len(result[0]) == 4

    def test_start_and_end_distinct(self):
        """Warped segment endpoints should be different from each other."""
        H = _default_H(200, 100)
        lines = [(10.0, 10.0, 190.0, 90.0)]
        result = warp_segments(lines, H)
        bx1, by1, bx2, by2 = result[0]
        # The two endpoints should differ
        assert abs(bx1 - bx2) + abs(by1 - by2) > 1.0

    def test_batch_preserves_count(self):
        H = _default_H(200, 100)
        lines = [(0.0, 0.0, 10.0, 10.0), (100.0, 0.0, 90.0, 50.0)]
        result = warp_segments(lines, H)
        assert len(result) == 2


# ── process_frame ─────────────────────────────────────────────────────────────

class TestProcessFrame:
    def _lane_image(self, h: int = 480, w: int = 640) -> np.ndarray:
        """
        Create a synthetic BGR image with two diagonal lines representing
        left and right lane edges in the bottom half.
        """
        img = np.zeros((h, w, 3), dtype=np.uint8)
        roi_top = h // 2
        # Left edge: from bottom-left area upward to the right (negative slope in image coords)
        cv2.line(img, (80, h - 10), (240, roi_top + 10), (255, 255, 255), 4)
        # Right edge: from bottom-right area upward to the left (positive slope in image coords)
        cv2.line(img, (w - 80, h - 10), (w - 240, roi_top + 10), (255, 255, 255), 4)
        return img

    def test_returns_named_tuple(self):
        bgr = _solid_bgr(120, 240)
        cfg = PathEdgeConfig()
        result = process_frame(bgr, cfg)
        assert isinstance(result, PathEdgesResult)

    def test_roi_top_correct(self):
        bgr = np.zeros((200, 400, 3), dtype=np.uint8)
        cfg = PathEdgeConfig(roi_frac=0.5)
        result = process_frame(bgr, cfg)
        assert result.roi_top == 100

    def test_uniform_image_no_lines(self):
        bgr = _solid_bgr(200, 400, color=(80, 80, 80))
        cfg = PathEdgeConfig()
        result = process_frame(bgr, cfg)
        assert result.lines == []
        assert result.left_edge is None
        assert result.right_edge is None
        assert result.left_lines == []
        assert result.right_lines == []

    def test_homography_matrix_shape(self):
        bgr = _solid_bgr(200, 400)
        result = process_frame(bgr, PathEdgeConfig())
        assert result.H.shape == (3, 3)

    def test_birdseye_segments_same_count(self):
        """birdseye_lines and lines must have the same number of segments."""
        bgr = self._lane_image()
        result = process_frame(bgr, PathEdgeConfig(hough_threshold=10, min_line_len=20))
        assert len(result.birdseye_lines) == len(result.lines)

    def test_lane_image_detects_edges(self):
        """Synthetic lane image should detect at least one of left/right edge."""
        bgr = self._lane_image()
        cfg = PathEdgeConfig(
            roi_frac=0.5,
            canny_low=30,
            canny_high=100,
            hough_threshold=10,
            min_line_len=20,
            max_line_gap=15,
        )
        result = process_frame(bgr, cfg)
        assert (result.left_edge is not None) or (result.right_edge is not None)

    def test_segments_px_flat_array_length(self):
        """segments_px-equivalent length must be 4 × line_count."""
        bgr = self._lane_image()
        cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20)
        result = process_frame(bgr, cfg)
        assert len(result.lines) * 4 == sum(4 for _ in result.lines)

    def test_left_right_lines_subset_of_all_lines(self):
        """left_lines + right_lines must be a subset of all lines."""
        bgr = self._lane_image()
        cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20)
        result = process_frame(bgr, cfg)
        all_set = set(result.lines)
        for seg in result.left_lines:
            assert seg in all_set
        for seg in result.right_lines:
            assert seg in all_set
