"""
test_scan_height_filter.py — Unit tests for scan height filter helpers (no ROS2 required).

Covers:
  - pitch_roll_from_accel: accelerometer → pitch/roll angles
  - filter_scan_by_height: height-based ray filtering
  - AttitudeEstimator: low-pass pitch/roll smoother
"""

import sys
import os
import math

import pytest

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

from saltybot_bringup._scan_height_filter import (
    pitch_roll_from_accel,
    filter_scan_by_height,
    AttitudeEstimator,
)

_G = 9.81  # m/s²


# ── pitch_roll_from_accel ─────────────────────────────────────────────────────

class TestPitchRollFromAccel:

    def test_level_robot_zero_pitch_roll(self):
        # Both z-up (az=-g) and z-down (az=+g) conventions should give 0,0
        for az in (-_G, +_G):
            pitch, roll = pitch_roll_from_accel(0.0, 0.0, az)
            assert pitch == pytest.approx(0.0, abs=1e-6)
            assert roll  == pytest.approx(0.0, abs=1e-6)

    def test_nose_down_positive_pitch(self):
        # Nose down 10°: ax component of gravity = -g·sin(10°)
        angle = math.radians(10.0)
        ax = -_G * math.sin(angle)
        az =  _G * math.cos(angle)    # z-down convention
        pitch, _ = pitch_roll_from_accel(ax, 0.0, az)
        assert pitch == pytest.approx(angle, abs=1e-4)

    def test_roll_right_positive_roll(self):
        # 15° roll (left side up): ay component of gravity = g·sin(15°)
        angle = math.radians(15.0)
        ay = _G * math.sin(angle)
        az = _G * math.cos(angle)     # z-down convention
        _, roll = pitch_roll_from_accel(0.0, ay, az)
        assert roll == pytest.approx(angle, abs=1e-4)

    def test_roll_sign_convention(self):
        # Negative ay (right side up) → negative roll
        _, roll = pitch_roll_from_accel(0.0, -1.0, _G)
        assert roll < 0.0


# ── filter_scan_by_height ─────────────────────────────────────────────────────

class TestFilterScanByHeight:

    def _flat_scan(self, n: int = 360, r: float = 1.0):
        """Uniform range scan, 360°."""
        angle_min = -math.pi
        angle_inc = 2 * math.pi / n
        return [r] * n, angle_min, angle_inc

    def test_level_robot_no_filtering(self):
        """Flat ground, no pitch/roll: all points at lidar_height → kept."""
        ranges, amin, ainc = self._flat_scan(r=2.0)
        out = filter_scan_by_height(
            ranges, amin, ainc,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
            z_min_m=0.05, z_max_m=3.0,
        )
        assert all(o == 2.0 for o in out)

    def test_forward_ray_filtered_when_pitched_down(self):
        """
        Robot pitched nose-down 45°: forward ray (θ=0) at r=0.10m hits ground.
        z = 0.10 + 0.10*cos(0)*sin(45°) = 0.10 + 0.0707 = 0.1707  -- NOT filtered
        Make it shorter so it DOES hit ground:
        r=0.05: z = 0.10 + 0.05*sin(45°) = 0.10 + 0.0354 = 0.135   -- still not
        Actually: ground filter z < 0.05. Let's use pitch = -45° (nose up) so
        forward ray dips below floor:
        pitch = -45°: z = 0.10 + r*cos(0)*sin(-45°) = 0.10 - r*0.707
        For r=0.15: z = 0.10 - 0.106 = -0.006 < 0.05 → filtered
        """
        pitch = -math.radians(45.0)    # nose UP → forward ray height decreases
        ranges = [float('inf')] * 360
        # θ=0 ray: angle_min = -π, index 180 ≈ θ=0
        ranges[180] = 0.15
        angle_inc = 2 * math.pi / 360
        out = filter_scan_by_height(
            ranges, -math.pi, angle_inc,
            lidar_height_m=0.10, pitch_rad=pitch, roll_rad=0.0,
            z_min_m=0.05, z_max_m=3.0,
        )
        assert math.isinf(out[180])

    def test_rear_ray_kept_when_pitched_down(self):
        """
        With pitch=-45° (nose up), rear ray (θ=π) has z = lidar_h + r*cos(π)*sin(pitch)
        = 0.10 + 0.15*(-1)*sin(-45°) = 0.10 + 0.106 = 0.206 → kept
        """
        pitch = -math.radians(45.0)
        ranges = [float('inf')] * 360
        ranges[0] = 0.15   # θ ≈ -π (rear)
        angle_inc = 2 * math.pi / 360
        out = filter_scan_by_height(
            ranges, -math.pi, angle_inc,
            lidar_height_m=0.10, pitch_rad=pitch, roll_rad=0.0,
            z_min_m=0.05, z_max_m=3.0,
        )
        assert not math.isinf(out[0])

    def test_inf_ranges_passed_through(self):
        ranges = [float('inf')] * 10
        out = filter_scan_by_height(
            ranges, 0.0, 0.1,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
        )
        assert all(math.isinf(o) for o in out)

    def test_nan_ranges_passed_through(self):
        ranges = [float('nan')]
        out = filter_scan_by_height(
            ranges, 0.0, 0.1,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
        )
        assert math.isnan(out[0])

    def test_out_of_range_bounds_passed_through(self):
        """Points outside [range_min, range_max] are passed through unchanged."""
        ranges = [0.01, 15.0]    # 0.01 < range_min=0.15; 15.0 > range_max=12.0
        out = filter_scan_by_height(
            ranges, 0.0, 0.1,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
            range_min=0.15, range_max=12.0,
        )
        assert out[0] == pytest.approx(0.01)
        assert out[1] == pytest.approx(15.0)

    def test_z_max_filters_high_points(self):
        """A long-range forward ray on a steep upward pitch exceeds z_max → filtered."""
        pitch = math.radians(80.0)    # strongly nose-down (forward goes up)
        ranges = [float('inf')] * 360
        ranges[180] = 5.0             # θ=0, long range
        angle_inc = 2 * math.pi / 360
        # z = 0.10 + 5.0 * sin(80°) = 0.10 + 4.92 = 5.02 > z_max=2.0 → filtered
        out = filter_scan_by_height(
            ranges, -math.pi, angle_inc,
            lidar_height_m=0.10, pitch_rad=pitch, roll_rad=0.0,
            z_min_m=0.05, z_max_m=2.0,
        )
        assert math.isinf(out[180])

    def test_output_length_equals_input(self):
        ranges = [1.0] * 100
        out = filter_scan_by_height(
            ranges, -math.pi, 2 * math.pi / 100,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
        )
        assert len(out) == 100

    def test_zero_pitch_roll_preserves_valid_points(self):
        """With no tilt, all finite ranges at lidar_height (0.10m) are kept."""
        ranges = [1.0, 2.0, 3.0]
        out = filter_scan_by_height(
            ranges, 0.0, 0.5,
            lidar_height_m=0.10, pitch_rad=0.0, roll_rad=0.0,
            z_min_m=0.05, z_max_m=3.0,
            range_min=0.0, range_max=12.0,
        )
        assert out == ranges


# ── AttitudeEstimator ─────────────────────────────────────────────────────────

class TestAttitudeEstimator:

    def test_first_update_initialises_exactly(self):
        est = AttitudeEstimator(alpha=0.5)
        pitch, roll = est.update(0.0, 0.0, _G)
        assert pitch == pytest.approx(0.0, abs=1e-6)
        assert roll  == pytest.approx(0.0, abs=1e-6)

    def test_converges_to_steady_tilt(self):
        est = AttitudeEstimator(alpha=0.5)
        angle = math.radians(10.0)
        ax = -_G * math.sin(angle)
        az =  _G * math.cos(angle)
        # Run many updates to converge
        for _ in range(50):
            est.update(ax, 0.0, az)
        assert est.pitch == pytest.approx(angle, abs=1e-3)

    def test_properties_accessible_after_update(self):
        est = AttitudeEstimator()
        est.update(0.0, 0.0, -_G)
        assert isinstance(est.pitch, float)
        assert isinstance(est.roll,  float)

    def test_initial_pitch_roll_zero_before_update(self):
        est = AttitudeEstimator()
        assert est.pitch == pytest.approx(0.0)
        assert est.roll  == pytest.approx(0.0)

    def test_alpha_one_tracks_instantly(self):
        est = AttitudeEstimator(alpha=1.0)
        angle = math.radians(20.0)
        ax = -_G * math.sin(angle)
        az =  _G * math.cos(angle)
        est.update(0.0, 0.0, _G)  # initialise level
        est.update(ax, 0.0, az)   # step to tilted angle
        assert est.pitch == pytest.approx(angle, abs=1e-3)


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