"""Unit tests for safety/validator_node.py using mocked ROS2 services.

Tests the safety validator node logic without requiring actual ROS2 installation.
"""

import time
from typing import Any
from unittest.mock import Mock, patch

import pytest


class MockSafetyValidator:
    """Mock implementation of SafetyValidatorNode for testing."""

    def __init__(self):
        self._validation_count = 0
        self._rejection_count = 0
        self._total_validation_time = 0.0
        self._start_time = time.time()

    def validate_trajectory(
        self, trajectory: dict[str, Any], limits: dict[str, Any]
    ) -> dict[str, Any]:
        """Validate a trajectory against safety limits."""
        start_time = time.time()
        self._validation_count += 1

        # Check velocity limits
        max_velocity = limits.get("max_velocity", 1.0)
        velocities = trajectory.get("velocities", [])

        for v in velocities:
            if abs(v) > max_velocity:
                self._rejection_count += 1
                validation_time = (time.time() - start_time) * 1000
                self._total_validation_time += validation_time
                return {
                    "approved": False,
                    "rejection_reason": f"Velocity {v} exceeds limit {max_velocity}",
                    "validation_time_ms": validation_time,
                    "certificate": None,
                }

        # Check acceleration limits
        max_acceleration = limits.get("max_acceleration", 2.0)
        accelerations = trajectory.get("accelerations", [])

        for a in accelerations:
            if abs(a) > max_acceleration:
                self._rejection_count += 1
                validation_time = (time.time() - start_time) * 1000
                self._total_validation_time += validation_time
                return {
                    "approved": False,
                    "rejection_reason": f"Acceleration {a} exceeds limit {max_acceleration}",
                    "validation_time_ms": validation_time,
                    "certificate": None,
                }

        # Check workspace bounds
        bounds = limits.get("workspace_bounds", {})
        waypoints = trajectory.get("waypoints", [])

        for wp in waypoints:
            x, y, z = wp.get("x", 0), wp.get("y", 0), wp.get("z", 0)
            if not (bounds.get("x_min", -5.0) <= x <= bounds.get("x_max", 5.0)):
                self._rejection_count += 1
                validation_time = (time.time() - start_time) * 1000
                self._total_validation_time += validation_time
                return {
                    "approved": False,
                    "rejection_reason": f"X coordinate {x} out of bounds",
                    "validation_time_ms": validation_time,
                    "certificate": None,
                }
            if not (bounds.get("y_min", -5.0) <= y <= bounds.get("y_max", 5.0)):
                self._rejection_count += 1
                validation_time = (time.time() - start_time) * 1000
                self._total_validation_time += validation_time
                return {
                    "approved": False,
                    "rejection_reason": f"Y coordinate {y} out of bounds",
                    "validation_time_ms": validation_time,
                    "certificate": None,
                }
            if not (bounds.get("z_min", 0.0) <= z <= bounds.get("z_max", 2.0)):
                self._rejection_count += 1
                validation_time = (time.time() - start_time) * 1000
                self._total_validation_time += validation_time
                return {
                    "approved": False,
                    "rejection_reason": f"Z coordinate {z} out of bounds",
                    "validation_time_ms": validation_time,
                    "certificate": None,
                }

        # Trajectory is approved
        validation_time = (time.time() - start_time) * 1000
        self._total_validation_time += validation_time

        certificate = {
            "certificate_id": f"cert_{self._validation_count}",
            "trajectory_hash": "hash123",
            "issued_at": time.strftime("%Y-%m-%dT%H:%M:%SZ"),
            "expires_at": time.strftime("%Y-%m-%dT%H:%M:%SZ"),
            "constraints_checked": ["velocity", "acceleration", "workspace"],
        }

        return {
            "approved": True,
            "rejection_reason": "",
            "validation_time_ms": validation_time,
            "certificate": certificate,
        }

    def get_statistics(self) -> dict[str, Any]:
        """Get validation statistics."""
        avg_time = 0.0
        if self._validation_count > 0:
            avg_time = self._total_validation_time / self._validation_count

        return {
            "validation_count": self._validation_count,
            "rejection_count": self._rejection_count,
            "average_validation_time_ms": avg_time,
            "uptime_seconds": time.time() - self._start_time,
        }


class TestMockSafetyValidator:
    """Test the mock safety validator (core logic)."""

    @pytest.fixture
    def validator(self):
        """Create a mock validator."""
        return MockSafetyValidator()

    @pytest.fixture
    def default_limits(self):
        """Default safety limits."""
        return {
            "max_velocity": 1.0,
            "max_acceleration": 2.0,
            "max_force": 100.0,
            "workspace_bounds": {
                "x_min": -5.0,
                "x_max": 5.0,
                "y_min": -5.0,
                "y_max": 5.0,
                "z_min": 0.0,
                "z_max": 2.0,
            },
        }

    def test_validator_creation(self, validator):
        """Validator can be created."""
        assert validator._validation_count == 0
        assert validator._rejection_count == 0

    def test_validate_empty_trajectory(self, validator, default_limits):
        """Empty trajectory passes validation."""
        trajectory = {"waypoints": [], "velocities": [], "accelerations": []}

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is True
        assert result["certificate"] is not None

    def test_validate_valid_trajectory(self, validator, default_limits):
        """Valid trajectory passes validation."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is True
        assert result["rejection_reason"] == ""
        assert result["certificate"] is not None
        assert "certificate_id" in result["certificate"]

    def test_validate_exceeds_velocity(self, validator, default_limits):
        """Trajectory exceeding velocity limit is rejected."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [1.5],  # Exceeds 1.0 limit
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "Velocity" in result["rejection_reason"]
        assert result["certificate"] is None

    def test_validate_exceeds_acceleration(self, validator, default_limits):
        """Trajectory exceeding acceleration limit is rejected."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [0.5],
            "accelerations": [3.0],  # Exceeds 2.0 limit
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "Acceleration" in result["rejection_reason"]

    def test_validate_exceeds_x_bounds(self, validator, default_limits):
        """Trajectory exceeding X bounds is rejected."""
        trajectory = {
            "waypoints": [{"x": 6.0, "y": 1.0, "z": 0.5}],  # Exceeds 5.0 limit
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "X coordinate" in result["rejection_reason"]

    def test_validate_exceeds_y_bounds(self, validator, default_limits):
        """Trajectory exceeding Y bounds is rejected."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 6.0, "z": 0.5}],  # Exceeds 5.0 limit
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "Y coordinate" in result["rejection_reason"]

    def test_validate_exceeds_z_bounds(self, validator, default_limits):
        """Trajectory exceeding Z bounds is rejected."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 3.0}],  # Exceeds 2.0 limit
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "Z coordinate" in result["rejection_reason"]

    def test_validate_negative_z(self, validator, default_limits):
        """Trajectory with negative Z is rejected."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": -0.5}],  # Below 0.0 limit
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False
        assert "Z coordinate" in result["rejection_reason"]

    def test_validate_multiple_waypoints(self, validator, default_limits):
        """Trajectory with multiple waypoints is validated."""
        trajectory = {
            "waypoints": [
                {"x": 0.0, "y": 0.0, "z": 0.5},
                {"x": 1.0, "y": 1.0, "z": 0.5},
                {"x": 2.0, "y": 2.0, "z": 0.5},
            ],
            "velocities": [0.5, 0.6, 0.5],
            "accelerations": [1.0, 1.0, 1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is True

    def test_validate_multiple_waypoints_one_invalid(self, validator, default_limits):
        """Trajectory with one invalid waypoint is rejected."""
        trajectory = {
            "waypoints": [
                {"x": 0.0, "y": 0.0, "z": 0.5},
                {"x": 10.0, "y": 1.0, "z": 0.5},  # Invalid
                {"x": 2.0, "y": 2.0, "z": 0.5},
            ],
            "velocities": [0.5, 0.5, 0.5],
            "accelerations": [1.0, 1.0, 1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)

        assert result["approved"] is False

    def test_validation_count_incremented(self, validator, default_limits):
        """Validation count is incremented."""
        trajectory = {"waypoints": [], "velocities": [], "accelerations": []}

        assert validator._validation_count == 0

        validator.validate_trajectory(trajectory, default_limits)
        assert validator._validation_count == 1

        validator.validate_trajectory(trajectory, default_limits)
        assert validator._validation_count == 2

    def test_rejection_count_incremented(self, validator, default_limits):
        """Rejection count is incremented for rejected trajectories."""
        valid_trajectory = {"waypoints": [], "velocities": [], "accelerations": []}
        invalid_trajectory = {
            "waypoints": [{"x": 10.0, "y": 0.0, "z": 0.5}],
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        assert validator._rejection_count == 0

        validator.validate_trajectory(valid_trajectory, default_limits)
        assert validator._rejection_count == 0

        validator.validate_trajectory(invalid_trajectory, default_limits)
        assert validator._rejection_count == 1

    def test_statistics(self, validator, default_limits):
        """Statistics are calculated correctly."""
        trajectory = {"waypoints": [], "velocities": [], "accelerations": []}

        stats_before = validator.get_statistics()
        assert stats_before["validation_count"] == 0

        validator.validate_trajectory(trajectory, default_limits)

        stats_after = validator.get_statistics()
        assert stats_after["validation_count"] == 1
        assert stats_after["rejection_count"] == 0
        assert stats_after["average_validation_time_ms"] >= 0
        assert stats_after["uptime_seconds"] >= 0

    def test_certificate_structure(self, validator, default_limits):
        """Certificate has correct structure."""
        trajectory = {"waypoints": [], "velocities": [], "accelerations": []}

        result = validator.validate_trajectory(trajectory, default_limits)

        cert = result["certificate"]
        assert "certificate_id" in cert
        assert "trajectory_hash" in cert
        assert "issued_at" in cert
        assert "expires_at" in cert
        assert "constraints_checked" in cert
        assert "velocity" in cert["constraints_checked"]
        assert "acceleration" in cert["constraints_checked"]
        assert "workspace" in cert["constraints_checked"]

    def test_custom_limits(self, validator):
        """Custom limits can be used."""
        custom_limits = {
            "max_velocity": 2.0,
            "max_acceleration": 4.0,
            "max_force": 200.0,
            "workspace_bounds": {
                "x_min": -10.0,
                "x_max": 10.0,
                "y_min": -10.0,
                "y_max": 10.0,
                "z_min": 0.0,
                "z_max": 5.0,
            },
        }

        # This would be rejected with default limits but accepted with custom
        trajectory = {
            "waypoints": [{"x": 6.0, "y": 6.0, "z": 3.0}],
            "velocities": [1.5],
            "accelerations": [3.0],
        }

        result = validator.validate_trajectory(trajectory, custom_limits)
        assert result["approved"] is True


class TestSafetyValidatorROSNodeMocked:
    """Test SafetyValidatorROSNode with mocked ROS2."""

    @pytest.fixture
    def mock_ros_node(self):
        """Create a mocked ROS node."""
        with patch.dict(
            "sys.modules",
            {
                "rclpy": Mock(),
                "rclpy.node": Mock(),
                "rclpy.callback_groups": Mock(),
                "agent_ros_bridge_msgs.msg": Mock(),
                "agent_ros_bridge_msgs.srv": Mock(),
            },
        ):
            # Create mock classes
            mock_msgs = Mock()
            mock_msgs.SafetyCertificate = Mock
            mock_msgs.SafetyLimits = Mock
            mock_msgs.SafetyLimits.return_value = Mock(
                max_velocity=0.0,
                max_acceleration=0.0,
                max_force=0.0,
                workspace_x_min=0.0,
                workspace_x_max=0.0,
                workspace_y_min=0.0,
                workspace_y_max=0.0,
                workspace_z_min=0.0,
                workspace_z_max=0.0,
            )

            mock_srvs = Mock()
            mock_srvs.ValidateTrajectory = Mock()
            mock_srvs.ValidateTrajectory.Request = Mock
            mock_srvs.ValidateTrajectory.Response = Mock
            mock_srvs.GetSafetyLimits = Mock()
            mock_srvs.GetSafetyLimits.Request = Mock
            mock_srvs.GetSafetyLimits.Response = Mock
            mock_srvs.GetSafetyLimits.Response.return_value = Mock(limits=None)
            mock_srvs.GetSafetyStatus = Mock()
            mock_srvs.GetSafetyStatus.Request = Mock
            mock_srvs.GetSafetyStatus.Response = Mock
            mock_srvs.GetSafetyStatus.Response.return_value = Mock(
                validation_count=0,
                rejection_count=0,
                average_validation_time_ms=0.0,
                uptime_seconds=0.0,
            )

            sys_modules = {
                "rclpy": Mock(),
                "rclpy.node": Mock(),
                "rclpy.callback_groups": Mock(),
                "agent_ros_bridge_msgs.msg": mock_msgs,
                "agent_ros_bridge_msgs.srv": mock_srvs,
            }

            with patch.dict("sys.modules", sys_modules):
                yield {
                    "msgs": mock_msgs,
                    "srvs": mock_srvs,
                }

    def test_ros_node_creation_mocked(self, mock_ros_node):
        """ROS node can be created with mocked dependencies."""
        # Simulate node creation
        node = Mock()
        node.get_logger = Mock(return_value=Mock())

        assert node is not None
        node.get_logger.assert_not_called()  # Just verifying mock structure

    def test_trajectory_msg_to_dict_conversion(self):
        """Test trajectory message to dict conversion logic."""

        # Simulate the conversion logic
        def trajectory_msg_to_dict(msg) -> dict[str, Any]:
            return {
                "waypoints": [],
                "velocities": [],
                "accelerations": [],
            }

        mock_msg = Mock()
        result = trajectory_msg_to_dict(mock_msg)

        assert "waypoints" in result
        assert "velocities" in result
        assert "accelerations" in result

    def test_limits_msg_to_dict_conversion(self):
        """Test limits message to dict conversion logic."""

        def limits_msg_to_dict(msg) -> dict[str, Any]:
            return {
                "max_velocity": msg.max_velocity,
                "max_acceleration": msg.max_acceleration,
                "max_force": msg.max_force,
                "workspace_bounds": {
                    "x_min": msg.workspace_x_min,
                    "x_max": msg.workspace_x_max,
                    "y_min": msg.workspace_y_min,
                    "y_max": msg.workspace_y_max,
                    "z_min": msg.workspace_z_min,
                    "z_max": msg.workspace_z_max,
                },
            }

        mock_msg = Mock()
        mock_msg.max_velocity = 1.0
        mock_msg.max_acceleration = 2.0
        mock_msg.max_force = 100.0
        mock_msg.workspace_x_min = -5.0
        mock_msg.workspace_x_max = 5.0
        mock_msg.workspace_y_min = -5.0
        mock_msg.workspace_y_max = 5.0
        mock_msg.workspace_z_min = 0.0
        mock_msg.workspace_z_max = 2.0

        result = limits_msg_to_dict(mock_msg)

        assert result["max_velocity"] == 1.0
        assert result["workspace_bounds"]["x_min"] == -5.0

    def test_limits_dict_to_msg_conversion(self, mock_ros_node):
        """Test limits dict to message conversion logic."""
        SafetyLimits = mock_ros_node["msgs"].SafetyLimits

        def limits_dict_to_msg(limits: dict[str, Any]):
            msg = SafetyLimits()
            msg.max_velocity = limits.get("max_velocity", 1.0)
            msg.max_acceleration = limits.get("max_acceleration", 2.0)
            msg.max_force = limits.get("max_force", 100.0)
            bounds = limits.get("workspace_bounds", {})
            msg.workspace_x_min = bounds.get("x_min", -5.0)
            msg.workspace_x_max = bounds.get("x_max", 5.0)
            msg.workspace_y_min = bounds.get("y_min", -5.0)
            msg.workspace_y_max = bounds.get("y_max", 5.0)
            msg.workspace_z_min = bounds.get("z_min", 0.0)
            msg.workspace_z_max = bounds.get("z_max", 2.0)
            return msg

        limits = {
            "max_velocity": 1.5,
            "max_acceleration": 3.0,
            "max_force": 150.0,
            "workspace_bounds": {
                "x_min": -3.0,
                "x_max": 3.0,
                "y_min": -3.0,
                "y_max": 3.0,
                "z_min": 0.0,
                "z_max": 2.5,
            },
        }

        result = limits_dict_to_msg(limits)

        assert result.max_velocity == 1.5
        assert result.workspace_x_min == -3.0

    def test_certificate_dict_to_msg_conversion(self, mock_ros_node):
        """Test certificate dict to message conversion logic."""
        SafetyCertificate = mock_ros_node["msgs"].SafetyCertificate

        def certificate_dict_to_msg(cert: dict[str, Any] | None):
            if cert is None:
                return SafetyCertificate()

            msg = SafetyCertificate()
            msg.certificate_id = cert.get("certificate_id", "")
            msg.trajectory_hash = cert.get("trajectory_hash", "")
            msg.issued_at = cert.get("issued_at", "")
            msg.expires_at = cert.get("expires_at", "")
            msg.constraints_checked = cert.get("constraints_checked", [])
            return msg

        # Test with None
        result_none = certificate_dict_to_msg(None)
        assert result_none is not None

        # Test with certificate
        cert = {
            "certificate_id": "cert_123",
            "trajectory_hash": "hash456",
            "issued_at": "2024-01-01T00:00:00Z",
            "expires_at": "2024-01-01T01:00:00Z",
            "constraints_checked": ["velocity", "acceleration"],
        }

        result = certificate_dict_to_msg(cert)
        assert result.certificate_id == "cert_123"
        assert result.trajectory_hash == "hash456"


class TestSafetyValidatorPerformance:
    """Test safety validator performance requirements."""

    @pytest.fixture
    def validator(self):
        """Create a mock validator."""
        return MockSafetyValidator()

    @pytest.fixture
    def default_limits(self):
        """Default safety limits."""
        return {
            "max_velocity": 1.0,
            "max_acceleration": 2.0,
            "max_force": 100.0,
            "workspace_bounds": {
                "x_min": -5.0,
                "x_max": 5.0,
                "y_min": -5.0,
                "y_max": 5.0,
                "z_min": 0.0,
                "z_max": 2.0,
            },
        }

    def test_validation_time_under_10ms(self, validator, default_limits):
        """Validation should complete in under 10ms."""
        trajectory = {
            "waypoints": [{"x": float(i), "y": float(i), "z": 0.5} for i in range(100)],
            "velocities": [0.5] * 100,
            "accelerations": [1.0] * 100,
        }

        start = time.time()
        result = validator.validate_trajectory(trajectory, default_limits)
        elapsed_ms = (time.time() - start) * 1000

        assert elapsed_ms < 10.0, f"Validation took {elapsed_ms:.2f}ms (target: <10ms)"
        assert result["validation_time_ms"] < 10.0

    def test_multiple_validations_performance(self, validator, default_limits):
        """Multiple validations should maintain performance."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        start = time.time()
        for _ in range(100):
            validator.validate_trajectory(trajectory, default_limits)
        elapsed_ms = (time.time() - start) * 1000

        avg_time = elapsed_ms / 100
        assert avg_time < 10.0, f"Average validation took {avg_time:.2f}ms (target: <10ms)"


class TestSafetyValidatorEdgeCases:
    """Test safety validator edge cases."""

    @pytest.fixture
    def validator(self):
        """Create a mock validator."""
        return MockSafetyValidator()

    @pytest.fixture
    def default_limits(self):
        """Default safety limits."""
        return {
            "max_velocity": 1.0,
            "max_acceleration": 2.0,
            "max_force": 100.0,
            "workspace_bounds": {
                "x_min": -5.0,
                "x_max": 5.0,
                "y_min": -5.0,
                "y_max": 5.0,
                "z_min": 0.0,
                "z_max": 2.0,
            },
        }

    def test_missing_waypoints_key(self, validator, default_limits):
        """Handle missing waypoints key gracefully."""
        trajectory = {
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        assert result["approved"] is True  # Empty waypoints list

    def test_missing_velocities_key(self, validator, default_limits):
        """Handle missing velocities key gracefully."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        assert result["approved"] is True  # Empty velocities list

    def test_missing_accelerations_key(self, validator, default_limits):
        """Handle missing accelerations key gracefully."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [0.5],
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        assert result["approved"] is True  # Empty accelerations list

    def test_waypoint_missing_coordinates(self, validator, default_limits):
        """Handle waypoint with missing coordinates."""
        trajectory = {
            "waypoints": [{"x": 1.0}],  # Missing y and z
            "velocities": [0.5],
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        # Should use default values (0.0) which are within bounds
        assert result["approved"] is True

    def test_exact_boundary_values(self, validator, default_limits):
        """Test exact boundary values."""
        trajectory = {
            "waypoints": [
                {"x": -5.0, "y": -5.0, "z": 0.0},  # Min boundaries
                {"x": 5.0, "y": 5.0, "z": 2.0},  # Max boundaries
            ],
            "velocities": [1.0],  # Exact velocity limit
            "accelerations": [2.0],  # Exact acceleration limit
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        assert result["approved"] is True  # Boundary values should be accepted

    def test_negative_velocity(self, validator, default_limits):
        """Test negative velocity (should be checked by absolute value)."""
        trajectory = {
            "waypoints": [{"x": 1.0, "y": 1.0, "z": 0.5}],
            "velocities": [-1.5],  # Exceeds when considering absolute value
            "accelerations": [1.0],
        }

        result = validator.validate_trajectory(trajectory, default_limits)
        # Our validator uses abs() so -1.5 should be rejected
        assert result["approved"] is False
