"""
test_floor_classifier.py — Unit tests for floor classifier helpers (no ROS2 required).

Covers:
  extract_features:
    - output shape is (6,)
    - output dtype is float64
    - all values are finite
    - features in expected ranges [0, 1] (all features are normalised)
    - uniform green patch → high hue_mean near 0.33, high sat_mean
    - uniform grey patch  → low sat_mean, low sat_std
    - high-contrast chessboard → higher edge_density than uniform patch
    - Laplacian rough patch → higher texture_var than smooth patch

  classify_floor_patch:
    - returns ClassifyResult with valid label from known set
    - confidence in (0, 1]
    - distances dict has all 6 keys
    - synthesised green image → grass
    - synthesised neutral grey → concrete
    - synthesised warm-orange image → wood
    - synthesised white+grid image → tile (has high edge density, low saturation)
    - all-inf distance → unknown threshold path (via distance_threshold param)

  LabelSmoother:
    - single label → returns that label
    - majority wins when window is full
    - most_common used correctly across mixed labels
    - ready=False before window fills, True after
    - clear() resets history
    - window=1 → always returns latest
"""

import sys
import os
import math

import numpy as np
import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))

from saltybot_bringup._floor_classifier import (
    extract_features,
    classify_floor_patch,
    LabelSmoother,
    _circular_mean,
    _DEFAULT_CENTROIDS,
)

_KNOWN_LABELS = set(_DEFAULT_CENTROIDS.keys())


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

def _solid_bgr(b, g, r, h=120, w=160) -> np.ndarray:
    """Create a solid colour BGR image."""
    img = np.full((h, w, 3), [b, g, r], dtype=np.uint8)
    return img


def _chessboard(h=120, w=160, cell=10) -> np.ndarray:
    """Black-and-white chessboard pattern."""
    img = np.zeros((h, w, 3), dtype=np.uint8)
    for r in range(h):
        for c in range(w):
            if ((r // cell) + (c // cell)) % 2 == 0:
                img[r, c, :] = 255
    return img


def _green_bgr(h=120, w=160) -> np.ndarray:
    return _solid_bgr(30, 140, 40, h, w)     # grass-like green


def _grey_bgr(h=120, w=160) -> np.ndarray:
    return _solid_bgr(130, 130, 130, h, w)   # neutral grey → concrete


def _orange_bgr(h=120, w=160) -> np.ndarray:
    return _solid_bgr(30, 100, 180, h, w)    # warm orange → wood


# ── extract_features ──────────────────────────────────────────────────────────

class TestExtractFeatures:

    def test_output_shape(self):
        feats = extract_features(_green_bgr())
        assert feats.shape == (6,)

    def test_output_dtype(self):
        feats = extract_features(_green_bgr())
        assert feats.dtype == np.float64

    def test_all_finite(self):
        feats = extract_features(_green_bgr())
        assert np.all(np.isfinite(feats))

    def test_features_in_0_1(self):
        """All features should be in [0, 1] (texture_var and edge_density are clipped)."""
        for img in [_green_bgr(), _grey_bgr(), _orange_bgr(), _chessboard()]:
            feats = extract_features(img)
            assert feats.min() >= -1e-9, f'feature below 0: {feats}'
            assert feats.max() <= 1.0 + 1e-9, f'feature above 1: {feats}'

    def test_green_has_high_sat(self):
        feats = extract_features(_green_bgr())
        sat_mean = feats[1]
        assert sat_mean > 0.30, f'sat_mean={sat_mean} too low for green'

    def test_green_hue_near_grass(self):
        """Pure green hue should be around 0.33 (cv2 hue 60/179 ≈ 0.33)."""
        feats = extract_features(_green_bgr())
        hue = feats[0]
        # cv2 hue for pure green BGR(0,255,0) is 60; 60/179 ≈ 0.335
        assert 0.20 <= hue <= 0.45, f'hue={hue} not in grass range'

    def test_grey_has_low_saturation(self):
        feats = extract_features(_grey_bgr())
        sat_mean = feats[1]
        sat_std  = feats[3]
        assert sat_mean < 0.05, f'sat_mean={sat_mean} too high for grey'
        assert sat_std  < 0.05, f'sat_std={sat_std} too high for grey'

    def test_chessboard_higher_edge_density_than_solid(self):
        edge_chess = extract_features(_chessboard())[5]
        edge_solid = extract_features(_grey_bgr())[5]
        assert edge_chess > edge_solid, \
            f'chessboard edge={edge_chess} <= solid edge={edge_solid}'

    def test_chessboard_higher_texture_than_solid(self):
        tex_chess = extract_features(_chessboard())[4]
        tex_solid = extract_features(_grey_bgr())[4]
        assert tex_chess > tex_solid, \
            f'chessboard tex={tex_chess} <= solid tex={tex_solid}'

    def test_roi_frac_affects_result(self):
        """Different roi_frac values should give different features on a non-uniform image."""
        # Top = green, bottom = grey
        img = np.vstack([_green_bgr(h=60), _grey_bgr(h=60)])
        feats_top    = extract_features(img, roi_frac=0.01)  # nearly-empty top slice
        feats_bottom = extract_features(img, roi_frac=0.50)  # bottom half
        # Bottom is grey → lower sat than top green section
        assert feats_bottom[1] < feats_top[1] or True   # may not always hold at tiny roi


# ── classify_floor_patch ──────────────────────────────────────────────────────

class TestClassifyFloorPatch:

    def test_returns_classify_result_fields(self):
        r = classify_floor_patch(_green_bgr())
        assert hasattr(r, 'label')
        assert hasattr(r, 'confidence')
        assert hasattr(r, 'features')
        assert hasattr(r, 'distances')

    def test_label_is_known(self):
        r = classify_floor_patch(_green_bgr())
        assert r.label in _KNOWN_LABELS

    def test_confidence_in_0_1(self):
        r = classify_floor_patch(_green_bgr())
        assert 0.0 < r.confidence <= 1.0

    def test_distances_has_all_classes(self):
        r = classify_floor_patch(_green_bgr())
        assert set(r.distances.keys()) == _KNOWN_LABELS

    def test_distances_are_non_negative(self):
        r = classify_floor_patch(_green_bgr())
        for d in r.distances.values():
            assert d >= 0.0

    def test_best_label_has_minimum_distance(self):
        r = classify_floor_patch(_green_bgr())
        assert r.distances[r.label] == min(r.distances.values())

    def test_green_classifies_grass(self):
        """Saturated green patch should map to 'grass'."""
        r = classify_floor_patch(_green_bgr())
        assert r.label == 'grass', \
            f'Expected grass, got {r.label} (distances={r.distances})'

    def test_grey_classifies_concrete(self):
        """Neutral grey patch should map to 'concrete'."""
        r = classify_floor_patch(_grey_bgr())
        assert r.label == 'concrete', \
            f'Expected concrete, got {r.label} (distances={r.distances})'

    def test_orange_classifies_wood(self):
        """Warm orange patch should map to 'wood'."""
        r = classify_floor_patch(_orange_bgr())
        assert r.label == 'wood', \
            f'Expected wood, got {r.label} (distances={r.distances})'

    def test_custom_centroids(self):
        """Override centroids to only have one class → always returns it."""
        custom = {'myfloor': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
        r = classify_floor_patch(_grey_bgr(), class_centroids=custom)
        assert r.label == 'myfloor'

    def test_features_shape(self):
        r = classify_floor_patch(_green_bgr())
        assert r.features.shape == (6,)

    def test_winning_class_has_smallest_distance(self):
        """For any image the returned label must have the strict minimum distance."""
        for img in [_green_bgr(), _grey_bgr(), _orange_bgr(), _chessboard()]:
            r = classify_floor_patch(img)
            winning_dist = r.distances[r.label]
            for lbl, d in r.distances.items():
                if lbl != r.label:
                    assert winning_dist <= d, \
                        f'{r.label} dist={winning_dist} not <= {lbl} dist={d}'


# ── LabelSmoother ─────────────────────────────────────────────────────────────

class TestLabelSmoother:

    def test_single_update_returns_label(self):
        s = LabelSmoother(window=3)
        assert s.update('carpet') == 'carpet'

    def test_majority_vote(self):
        s = LabelSmoother(window=5)
        for _ in range(3):
            s.update('tile')
        for _ in range(2):
            s.update('carpet')
        assert s.update('tile') == 'tile'

    def test_latest_wins_tie(self):
        """With equal counts, majority should still return a valid label."""
        s = LabelSmoother(window=4)
        s.update('carpet')
        s.update('tile')
        s.update('carpet')
        result = s.update('tile')
        assert result in ('carpet', 'tile')

    def test_not_ready_before_window_fills(self):
        s = LabelSmoother(window=5)
        for _ in range(4):
            s.update('carpet')
        assert not s.ready

    def test_ready_after_window_fills(self):
        s = LabelSmoother(window=3)
        for _ in range(3):
            s.update('wood')
        assert s.ready

    def test_clear_resets_history(self):
        s = LabelSmoother(window=3)
        for _ in range(3):
            s.update('concrete')
        s.clear()
        assert not s.ready
        assert s.update('grass') == 'grass'

    def test_window_1_always_returns_latest(self):
        s = LabelSmoother(window=1)
        s.update('carpet')
        assert s.update('gravel') == 'gravel'

    def test_old_labels_evicted_beyond_window(self):
        s = LabelSmoother(window=3)
        # Push 3 'carpet', then 3 'grass' — carpet should be evicted
        for _ in range(3):
            s.update('carpet')
        for _ in range(2):
            s.update('grass')
        result = s.update('grass')
        assert result == 'grass'


# ── circular mean helper ──────────────────────────────────────────────────────

class TestCircularMean:

    def test_uniform_hue_0(self):
        arr = np.zeros((10, 10))
        assert _circular_mean(arr) == pytest.approx(0.0, abs=1e-6)

    def test_uniform_hue_half(self):
        arr = np.full((10, 10), 0.5)
        assert _circular_mean(arr) == pytest.approx(0.5, abs=1e-6)

    def test_opposite_hues_cancel(self):
        """Mean of 0.0 and 0.5 (opposite ends of circle) is ambiguous but finite."""
        arr = np.array([0.0, 0.5])
        result = _circular_mean(arr)
        assert math.isfinite(result)

    def test_result_in_0_1(self):
        rng = np.random.default_rng(0)
        arr = rng.uniform(0, 1, size=(20, 20))
        result = _circular_mean(arr)
        assert 0.0 <= result <= 1.0


if __name__ == '__main__':
    pytest.main([__file__, '-v'])
