"""
Optimus Research Center - Laboratory 2: Robotics & Machine Perception Simulation
Simulation-only environment for perception, Kalman filtering, and collision-free path planning.
SAFETY LOCK: Physical hardware actuation is disabled and architecturally locked.
"""

import os
import numpy as np
from typing import Dict, Any, List, Tuple, Optional


class HardwareActuationLockedError(RuntimeError):
    """Raised if any physical robot actuation is attempted."""
    pass


class RoboticsSimulationEngine:
    """Provides simulated perception, Kalman filtering, and kinematic path generation."""

    PHYSICAL_HARDWARE_CONNECTED = False
    HARDWARE_CONTROL_SAFETY_LOCK = True

    @classmethod
    def assert_simulation_only(
        cls,
        allow_physical_actuation: bool = False,
        hardware_interface: Optional[str] = None
    ) -> Dict[str, Any]:
        env_override = os.environ.get("ALLOW_PHYSICAL_ACTUATION", "false").strip().lower() in {"1", "true", "yes"}
        if (
            cls.PHYSICAL_HARDWARE_CONNECTED
            or not cls.HARDWARE_CONTROL_SAFETY_LOCK
            or allow_physical_actuation
            or bool(hardware_interface)
            or env_override
        ):
            raise HardwareActuationLockedError(
                "SAFETY VIOLATION: Physical robotics hardware actuation is strictly locked. "
                "RoboticsSimulationEngine operates exclusively in mathematical 2D/kinematic simulation mode."
            )
        return {
            "simulation_only": True,
            "physical_hardware_connected": False,
            "hardware_control_safety_lock": True
        }

    @classmethod
    def simulate_kalman_filter_tracking(
        cls,
        true_positions: np.ndarray,
        measurement_noise_std: float = 0.5,
        process_noise_std: float = 0.05,
        dt: float = 0.1
    ) -> Dict[str, Any]:
        """
        Kinematic Kalman Filter tracking [position, velocity] state on noisy simulated sensor stream.
        Accurately reconstructs position and velocity from noisy position observations.
        """
        cls.assert_simulation_only()
        rng = np.random.RandomState(42)
        n = len(true_positions)
        noisy_measurements = true_positions + rng.normal(0, measurement_noise_std, size=n)

        # State vector: [position, velocity]
        x_est = np.array([noisy_measurements[0], 0.0])
        # State covariance
        P_est = np.eye(2) * 1.0

        # State transition matrix
        F = np.array([[1.0, dt],
                      [0.0, 1.0]])

        # Measurement matrix (we observe position only)
        H = np.array([[1.0, 0.0]])

        # Covariances
        Q = np.array([[0.25 * (dt**4), 0.5 * (dt**3)],
                      [0.5 * (dt**3), dt**2]]) * (process_noise_std ** 2)
        R = np.array([[measurement_noise_std ** 2]])

        estimated_positions = []

        for z in noisy_measurements:
            # 1. Prediction step
            x_pred = F @ x_est
            P_pred = F @ P_est @ F.T + Q

            # 2. Measurement update
            y_res = z - (H @ x_pred)[0]
            S = H @ P_pred @ H.T + R
            K = (P_pred @ H.T) / S[0, 0]

            x_est = x_pred + K.ravel() * y_res
            P_est = (np.eye(2) - K @ H) @ P_pred

            estimated_positions.append(float(x_est[0]))

        estimated_positions = np.array(estimated_positions)
        noisy_rmse = float(np.sqrt(np.mean((noisy_measurements - true_positions) ** 2)))
        filtered_rmse = float(np.sqrt(np.mean((estimated_positions - true_positions) ** 2)))
        noise_reduction_pct = round(((noisy_rmse - filtered_rmse) / noisy_rmse) * 100, 2)

        return {
            "simulation_module": "RoboticsSimulation_KalmanFilter",
            "physical_actuators_active": False,
            "samples_processed": n,
            "raw_measurement_rmse": round(noisy_rmse, 4),
            "kalman_filtered_rmse": round(filtered_rmse, 4),
            "noise_reduction_percent": noise_reduction_pct,
            "sample_trajectory": {
                "ground_truth": true_positions[:10].tolist(),
                "noisy_sensor": np.round(noisy_measurements[:10], 3).tolist(),
                "filtered_estimate": np.round(estimated_positions[:10], 3).tolist()
            }
        }

    @classmethod
    def simulate_grid_navigation(
        cls,
        grid_size: Tuple[int, int] = (10, 10),
        obstacle_density: float = 0.2,
        random_seed: int = 42
    ) -> Dict[str, Any]:
        """Simulates 2D robotic mobile navigation in synthetic obstacle grid."""
        cls.assert_simulation_only()
        rng = np.random.RandomState(random_seed)
        rows, cols = grid_size
        grid = (rng.rand(rows, cols) < obstacle_density).astype(int).tolist()
        grid[0][0] = 0
        grid[rows - 1][cols - 1] = 0

        from app.lab_algorithms.algorithms.search_planning import AStarSearch
        res = AStarSearch.search(grid, start=(0, 0), goal=(rows - 1, cols - 1))

        return {
            "simulation_module": "RoboticsSimulation_GridNavigation",
            "physical_actuators_active": False,
            "grid_dimensions": grid_size,
            "obstacle_density": obstacle_density,
            "path_found": res["path_found"],
            "path_length": res.get("path_length", 0),
            "nodes_explored": res["nodes_explored"],
            "waypoints": res.get("path", [])[:15]
        }