"""
Data conversion functions for RSN sensors.

Converts raw sensor data to physical units using calibration.
"""

import numpy as np
import logging
from typing import Tuple

logger = logging.getLogger(__name__)


def convert_rsn_data(
    n_sensors: int,
    acceleration: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    mems_type: int
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Convert raw RSN Link data to physical units.

    Converts MATLAB conv_grezziRSN.m function.

    Args:
        n_sensors: Number of sensors
        acceleration: Raw acceleration data (timestamps x axes*sensors)
        temperature: Raw temperature data
        calibration_data: Calibration coefficients for each sensor
        mems_type: Type of MEMS sensor (1, 2, etc.)

    Returns:
        Tuple of (converted_acceleration, acceleration_magnitude, converted_temperature)
    """
    logger.info(f"Converting RSN data for {n_sensors} sensors, MEMS type {mems_type}")

    n_timestamps = acceleration.shape[0]

    if mems_type == 2:
        # Freescale MEMS - 2 axes per sensor
        n_axes = 2
        acc_converted = np.zeros((n_timestamps, n_sensors * n_axes))
        acc_magnitude = np.zeros((n_timestamps, n_sensors))

        for i in range(n_sensors):
            # Get calibration for this sensor
            cal = calibration_data[i]
            # Axes indices
            ax_idx = i * n_axes
            ay_idx = i * n_axes + 1

            # Apply calibration: physical = gain * raw + offset
            acc_converted[:, ax_idx] = cal[0] * acceleration[:, ax_idx] + cal[1]
            acc_converted[:, ay_idx] = cal[2] * acceleration[:, ay_idx] + cal[3]

            # Calculate magnitude
            acc_magnitude[:, i] = np.sqrt(
                acc_converted[:, ax_idx]**2 +
                acc_converted[:, ay_idx]**2
            )

    elif mems_type == 1:
        # 3-axis MEMS
        n_axes = 3
        acc_converted = np.zeros((n_timestamps, n_sensors * n_axes))
        acc_magnitude = np.zeros((n_timestamps, n_sensors))

        for i in range(n_sensors):
            # Get calibration for this sensor
            cal = calibration_data[i]
            # Axes indices
            ax_idx = i * n_axes
            ay_idx = i * n_axes + 1
            az_idx = i * n_axes + 2

            # Apply calibration
            acc_converted[:, ax_idx] = cal[0] * acceleration[:, ax_idx] + cal[1]
            acc_converted[:, ay_idx] = cal[2] * acceleration[:, ay_idx] + cal[3]
            acc_converted[:, az_idx] = cal[4] * acceleration[:, az_idx] + cal[5]

            # Calculate magnitude
            acc_magnitude[:, i] = np.sqrt(
                acc_converted[:, ax_idx]**2 +
                acc_converted[:, ay_idx]**2 +
                acc_converted[:, az_idx]**2
            )
    else:
        raise ValueError(f"Unsupported MEMS type: {mems_type}")

    # Convert temperature
    temp_converted = np.zeros_like(temperature)
    for i in range(n_sensors):
        # Temperature calibration (typically linear)
        if len(calibration_data[i]) > n_axes * 2:
            temp_cal = calibration_data[i][n_axes * 2:n_axes * 2 + 2]
            temp_converted[:, i] = temp_cal[0] * temperature[:, i] + temp_cal[1]
        else:
            # No calibration, use raw values
            temp_converted[:, i] = temperature[:, i]

    logger.info("RSN data conversion completed")
    return acc_converted, acc_magnitude, temp_converted


def convert_rsn_hr_data(
    n_sensors: int,
    angle_data: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw RSN Link HR data to physical units.

    Converts MATLAB conv_grezziRSNHR.m function.

    Args:
        n_sensors: Number of sensors
        angle_data: Raw angle data
        temperature: Raw temperature data
        calibration_data: Calibration coefficients

    Returns:
        Tuple of (converted_angles, converted_temperature)
    """
    logger.info(f"Converting RSN HR data for {n_sensors} sensors")

    n_timestamps = angle_data.shape[0]
    angle_converted = np.zeros((n_timestamps, n_sensors * 2))

    for i in range(n_sensors):
        # Get calibration for this sensor
        cal = calibration_data[i]
        # Angle indices (X and Y)
        ax_idx = i * 2
        ay_idx = i * 2 + 1

        # Apply calibration
        angle_converted[:, ax_idx] = cal[0] * angle_data[:, ax_idx] + cal[1]
        angle_converted[:, ay_idx] = cal[2] * angle_data[:, ay_idx] + cal[3]

    # Convert temperature
    temp_converted = temperature.copy()
    for i in range(n_sensors):
        if len(calibration_data[i]) > 4:
            temp_cal = calibration_data[i][4:6]
            temp_converted[:, i] = temp_cal[0] * temperature[:, i] + temp_cal[1]

    logger.info("RSN HR data conversion completed")
    return angle_converted, temp_converted


def convert_load_link_data(
    adc_data: np.ndarray,
    calibration_data: np.ndarray,
    node_list: list
) -> np.ndarray:
    """
    Convert raw Load Link ADC data to physical units (force/load).

    Converts MATLAB conv_grezziLL.m function.

    Args:
        adc_data: Raw ADC values
        calibration_data: Calibration coefficients for each sensor
        node_list: List of node IDs

    Returns:
        Converted load data in physical units
    """
    logger.info(f"Converting Load Link data for {len(node_list)} sensors")

    n_timestamps, n_sensors = adc_data.shape
    load_converted = np.zeros((n_timestamps, n_sensors))

    for i in range(n_sensors):
        cal = calibration_data[i]
        # Typically: Load = gain * ADC + offset
        load_converted[:, i] = cal[0] * adc_data[:, i] + cal[1]

    logger.info("Load Link data conversion completed")
    return load_converted
