# Sensor data retrieval — ROS2 cache + simulation
import math
import random

from .config import log, _ros2_cache, _ros2_lock


def get_ros2_data():
    """Get latest cached ROS2 data from rclpy subscriptions."""
    _EXCLUDE_KEYS = {"camera_frame", "camera_frame_ts", "camera_stream", "camera_stream_fps"}
    with _ros2_lock:
        data = {k: v for k, v in _ros2_cache.items() if k not in _EXCLUDE_KEYS}

    try:
        import psutil
        battery = psutil.sensors_battery()
        data["battery"] = {
            "voltage": 24.0,
            "percentage": battery.percent if battery else 100,
            "charging": battery.power_plugged if battery else False,
        }
    except Exception:
        data.setdefault("battery", {"voltage": 24.0, "percentage": 100, "charging": False})

    try:
        import psutil
        temps = psutil.sensors_temperatures()
        if temps:
            first = list(temps.values())[0]
            data["temperature"] = first[0].current if first else 40.0
        else:
            data["temperature"] = 40.0
    except Exception:
        data.setdefault("temperature", 40.0)

    return data if data.get("imu") or data.get("odometry") else None


def get_sim_data(tick):
    """Generate simulated sensor data."""
    s = tick * 0.1
    return {
        "imu": {
            "roll": math.sin(s) * 0.05,
            "pitch": math.cos(s * 0.7) * 0.03,
            "yaw": (s * 0.2) % (math.pi * 2) - math.pi,
            "accel_x": math.sin(s * 2) * 0.5,
            "accel_y": math.cos(s * 1.5) * 0.3,
            "accel_z": 9.81 + (random.random() - 0.5) * 0.05,
        },
        "odometry": {
            "x": math.sin(s * 0.2) * 2,
            "y": math.cos(s * 0.2) * 2,
            "theta": s * 0.1,
            "linear_vel": abs(math.sin(s * 0.3)) * 0.5,
            "angular_vel": math.sin(s * 0.5) * 0.2,
        },
        "battery": {
            "voltage": 24.0 - tick * 0.001,
            "percentage": max(0, 95 - tick * 0.01),
            "charging": False,
        },
        "temperature": 35 + math.sin(s * 0.1) * 5,
    }
