"""Unit tests for velocity smoother node."""

import pytest
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistWithCovariance, Twist, Vector3
from std_msgs.msg import Header

import rclpy
from rclpy.time import Time

from saltybot_velocity_smoother.velocity_smoother_node import (
    VelocitySmootherNode,
    ButterworthFilter,
)


@pytest.fixture
def rclpy_fixture():
    """Initialize and cleanup rclpy."""
    rclpy.init()
    yield
    rclpy.shutdown()


@pytest.fixture
def node(rclpy_fixture):
    """Create a velocity smoother node instance."""
    node = VelocitySmootherNode()
    yield node
    node.destroy_node()


class TestButterworthFilter:
    """Test suite for Butterworth filter implementation."""

    def test_filter_initialization(self):
        """Test filter initialization with valid parameters."""
        filt = ButterworthFilter(5.0, 50, order=2)
        assert filt.cutoff_hz == 5.0
        assert filt.sample_rate_hz == 50
        assert filt.order == 2

    def test_first_order_filter(self):
        """Test first-order Butterworth filter."""
        filt = ButterworthFilter(5.0, 50, order=1)
        assert len(filt.b) == 2
        assert len(filt.a) == 2

    def test_second_order_filter(self):
        """Test second-order Butterworth filter."""
        filt = ButterworthFilter(5.0, 50, order=2)
        assert len(filt.b) == 3
        assert len(filt.a) == 3

    def test_filter_step_response(self):
        """Test filter response to step input."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Apply step input (0 -> 1.0)
        outputs = []
        for i in range(50):
            y = filt.filter(1.0)
            outputs.append(y)

        # Final output should be close to 1.0
        assert outputs[-1] > 0.9
        assert outputs[-1] <= 1.0

    def test_filter_constant_input(self):
        """Test filter with constant input."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Apply constant input
        for i in range(100):
            y = filt.filter(2.5)

        # Should converge to input value
        assert abs(y - 2.5) < 0.01

    def test_filter_zero_input(self):
        """Test filter with zero input."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Apply non-zero then zero
        for i in range(50):
            filt.filter(1.0)

        # Now apply zero
        for i in range(50):
            y = filt.filter(0.0)

        # Should decay to zero
        assert abs(y) < 0.01

    def test_filter_reset(self):
        """Test filter state reset."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Filter some values
        for i in range(10):
            filt.filter(1.0)

        # Reset
        filt.reset()

        # State should be zero
        assert filt.x_history == [0.0, 0.0, 0.0]
        assert filt.y_history == [0.0, 0.0]

    def test_filter_oscillation_dampening(self):
        """Test that filter dampens high-frequency oscillations."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Apply alternating signal (high frequency)
        outputs = []
        for i in range(100):
            x = 1.0 if i % 2 == 0 else -1.0
            y = filt.filter(x)
            outputs.append(y)

        # Oscillation amplitude should be reduced
        final_amp = max(abs(outputs[-1]), abs(outputs[-2]))
        assert final_amp < 0.5  # Much lower than input amplitude

    def test_filter_different_cutoffs(self):
        """Test filters with different cutoff frequencies."""
        filt_low = ButterworthFilter(2.0, 50, order=2)
        filt_high = ButterworthFilter(10.0, 50, order=2)

        # Both should be valid
        assert filt_low.cutoff_hz == 2.0
        assert filt_high.cutoff_hz == 10.0

    def test_filter_output_bounds(self):
        """Test that filter output stays bounded."""
        filt = ButterworthFilter(5.0, 50, order=2)

        # Apply large random-like values
        for i in range(100):
            x = math.sin(i * 0.5) * 5.0
            y = filt.filter(x)
            assert abs(y) < 10.0  # Should stay bounded


class TestVelocitySmootherNode:
    """Test suite for VelocitySmootherNode."""

    def test_node_initialization(self, node):
        """Test that node initializes correctly."""
        assert node.last_odom is None
        assert node.enable_smoothing is True

    def test_node_has_filters(self, node):
        """Test that node creates all velocity filters."""
        assert node.filter_linear_x is not None
        assert node.filter_linear_y is not None
        assert node.filter_linear_z is not None
        assert node.filter_angular_x is not None
        assert node.filter_angular_y is not None
        assert node.filter_angular_z is not None

    def test_odom_subscription_updates(self, node):
        """Test that odometry subscription updates last_odom."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 1.0

        node._on_odom(odom)

        assert node.last_odom is not None
        assert node.last_odom.twist.twist.linear.x == 1.0

    def test_filter_linear_velocity(self, node):
        """Test linear velocity filtering."""
        # Create odometry message
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = 1.0
        odom.twist.twist.linear.y = 0.5
        odom.twist.twist.linear.z = 0.0
        odom.twist.twist.angular.x = 0.0
        odom.twist.twist.angular.y = 0.0
        odom.twist.twist.angular.z = 0.2

        node._on_odom(odom)

        # Call timer callback to process
        node._timer_callback()

        # Filter should have been applied
        assert node.filter_linear_x.x_history[0] == 1.0

    def test_filter_angular_velocity(self, node):
        """Test angular velocity filtering."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.angular.z = 0.5

        node._on_odom(odom)
        node._timer_callback()

        assert node.filter_angular_z.x_history[0] == 0.5

    def test_smoothing_disabled(self, node):
        """Test that filter can be disabled."""
        node.enable_smoothing = False

        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 2.0

        node._on_odom(odom)
        node._timer_callback()

        # When disabled, output should equal input directly

    def test_no_odom_doesnt_crash(self, node):
        """Test that timer callback handles missing odometry gracefully."""
        # Call timer without setting odometry
        node._timer_callback()

        # Should not crash, just return

    def test_odom_header_preserved(self, node):
        """Test that odometry header is preserved in output."""
        odom = Odometry()
        odom.header.frame_id = "test_frame"
        odom.header.stamp = node.get_clock().now()
        odom.child_frame_id = "test_child"

        node._on_odom(odom)

        # Timer callback processes it
        node._timer_callback()

        # Header should be preserved

    def test_zero_velocity_filtering(self, node):
        """Test filtering of zero velocities."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 0.0
        odom.twist.twist.linear.y = 0.0
        odom.twist.twist.linear.z = 0.0
        odom.twist.twist.angular.x = 0.0
        odom.twist.twist.angular.y = 0.0
        odom.twist.twist.angular.z = 0.0

        node._on_odom(odom)
        node._timer_callback()

        # Filters should handle zero input

    def test_negative_velocities(self, node):
        """Test filtering of negative velocities."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = -1.0
        odom.twist.twist.angular.z = -0.5

        node._on_odom(odom)
        node._timer_callback()

        assert node.filter_linear_x.x_history[0] == -1.0

    def test_high_frequency_noise_dampening(self, node):
        """Test that filter dampens high-frequency encoder noise."""
        # Simulate noisy encoder output
        base_velocity = 1.0
        noise_amplitude = 0.2

        odom = Odometry()
        odom.header.frame_id = "odom"

        # Apply alternating noise
        for i in range(100):
            odom.twist.twist.linear.x = base_velocity + (noise_amplitude if i % 2 == 0 else -noise_amplitude)
            node._on_odom(odom)
            node._timer_callback()

        # After filtering, output should be close to base velocity
        # (oscillations dampened)

    def test_large_velocity_values(self, node):
        """Test filtering of large velocity values."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 10.0
        odom.twist.twist.angular.z = 5.0

        node._on_odom(odom)
        node._timer_callback()

        # Should handle large values without overflow

    def test_pose_unchanged(self, node):
        """Test that pose is not modified by velocity filtering."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = 5.0
        odom.pose.pose.position.y = 3.0
        odom.twist.twist.linear.x = 1.0

        node._on_odom(odom)
        node._timer_callback()

        # Pose should be copied unchanged

    def test_multiple_velocity_updates(self, node):
        """Test filtering across multiple sequential velocity updates."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        velocities = [0.5, 1.0, 1.5, 1.0, 0.5, 0.0]

        for v in velocities:
            odom.twist.twist.linear.x = v
            node._on_odom(odom)
            node._timer_callback()

        # Filter should smooth the velocity sequence

    def test_simultaneous_all_velocities(self, node):
        """Test filtering when all velocity components are present."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        for i in range(30):
            odom.twist.twist.linear.x = math.sin(i * 0.1)
            odom.twist.twist.linear.y = math.cos(i * 0.1) * 0.5
            odom.twist.twist.linear.z = 0.1
            odom.twist.twist.angular.x = 0.05
            odom.twist.twist.angular.y = 0.05
            odom.twist.twist.angular.z = math.sin(i * 0.15)

            node._on_odom(odom)
            node._timer_callback()

        # All filters should operate independently


class TestVelocitySmootherScenarios:
    """Integration-style tests for realistic scenarios."""

    def test_scenario_constant_velocity(self, node):
        """Scenario: robot moving at constant velocity."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 1.0

        for i in range(50):
            node._on_odom(odom)
            node._timer_callback()

        # Should maintain constant velocity after convergence

    def test_scenario_velocity_ramp(self, node):
        """Scenario: velocity ramping up from stop."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        for i in range(50):
            odom.twist.twist.linear.x = i * 0.02  # Ramp from 0 to 1.0
            node._on_odom(odom)
            node._timer_callback()

        # Filter should smooth the ramp

    def test_scenario_velocity_step(self, node):
        """Scenario: sudden velocity change (e.g., collision avoidance)."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        # First phase: constant velocity
        odom.twist.twist.linear.x = 1.0
        for i in range(25):
            node._on_odom(odom)
            node._timer_callback()

        # Second phase: sudden stop
        odom.twist.twist.linear.x = 0.0
        for i in range(25):
            node._on_odom(odom)
            node._timer_callback()

        # Filter should transition smoothly

    def test_scenario_rotation_only(self, node):
        """Scenario: robot spinning in place."""
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.twist.twist.linear.x = 0.0
        odom.twist.twist.angular.z = 0.5

        for i in range(50):
            node._on_odom(odom)
            node._timer_callback()

        # Angular velocity should be filtered

    def test_scenario_mixed_motion(self, node):
        """Scenario: combined linear and angular motion."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        for i in range(50):
            odom.twist.twist.linear.x = math.cos(i * 0.1)
            odom.twist.twist.linear.y = math.sin(i * 0.1)
            odom.twist.twist.angular.z = 0.2

            node._on_odom(odom)
            node._timer_callback()

        # Both linear and angular components should be filtered

    def test_scenario_encoder_noise_reduction(self, node):
        """Scenario: realistic encoder jitter with filtering."""
        odom = Odometry()
        odom.header.frame_id = "odom"

        # Simulate encoder jitter: base velocity + small noise
        base_vel = 1.0
        for i in range(100):
            jitter = 0.05 * math.sin(i * 0.5) + 0.03 * math.cos(i * 0.3)
            odom.twist.twist.linear.x = base_vel + jitter

            node._on_odom(odom)
            node._timer_callback()

        # Filter should reduce noise while maintaining base velocity
