"""
Data conversion functions for Tilt sensors.

Converts raw sensor data to physical units (angles, temperatures).
"""

import numpy as np
import logging
from typing import Tuple

logger = logging.getLogger(__name__)


def convert_tilt_link_hr_data(
    angle_data: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw Tilt Link HR data to physical units (angles in degrees).

    Converts MATLAB conv_grezziTLHR.m function.

    Args:
        angle_data: Raw angle data (ADC counts)
        temperature: Raw temperature data
        calibration_data: Calibration coefficients
            If column 4 == 0: XY gain is common
                Column 1: gain XY
                Column 2: gain temp
                Column 3: offset temp
            Else: separate XY gains
                Column 1: gain X
                Column 2: gain Y
                Column 3: gain temp
                Column 4: offset temp
        n_sensors: Number of sensors

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

    n_timestamps = angle_data.shape[0]
    angle_converted = angle_data.copy()
    temp_converted = temperature.copy()

    # Check if XY gains are common or separate
    if len(calibration_data.shape) == 1 or calibration_data.shape[1] < 4:
        # Simple case: single calibration set
        xy_common = True
        gain_xy = calibration_data[0] if len(calibration_data) > 0 else 1.0
        gain_temp = calibration_data[1] if len(calibration_data) > 1 else 1.0
        offset_temp = calibration_data[2] if len(calibration_data) > 2 else 0.0
    else:
        # Check column 4 (index 3)
        if np.all(calibration_data[:, 3] == 0):
            # XY gains are common
            xy_common = True
            gain_angles = calibration_data[:, 0]  # Common gain for both axes
            gain_temp = calibration_data[:, 1]
            offset_temp = calibration_data[:, 2]
        else:
            # Separate XY gains
            xy_common = False
            gain_x = calibration_data[:, 0]
            gain_y = calibration_data[:, 1]
            gain_temp = calibration_data[:, 2]
            offset_temp = calibration_data[:, 3]

    # Convert angles
    if xy_common:
        # Common gain for X and Y
        for i in range(n_sensors):
            gain = gain_angles[i] if hasattr(gain_angles, '__len__') else gain_xy
            angle_converted[:, i * 2] = angle_data[:, i * 2] * gain  # X
            angle_converted[:, i * 2 + 1] = angle_data[:, i * 2 + 1] * gain  # Y
    else:
        # Separate gains for X and Y
        for i in range(n_sensors):
            angle_converted[:, i * 2] = angle_data[:, i * 2] * gain_x[i]  # X
            angle_converted[:, i * 2 + 1] = angle_data[:, i * 2 + 1] * gain_y[i]  # Y

    # Convert temperatures
    for i in range(n_sensors):
        g_temp = gain_temp[i] if hasattr(gain_temp, '__len__') else gain_temp
        off_temp = offset_temp[i] if hasattr(offset_temp, '__len__') else offset_temp
        temp_converted[:, i] = temperature[:, i] * g_temp + off_temp

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


def convert_tilt_link_data(
    acceleration: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw Tilt Link data to physical units (acceleration in g).

    Similar to RSN conversion but for standard Tilt Link sensors.

    Args:
        acceleration: Raw acceleration data
        temperature: Raw temperature data
        calibration_data: Calibration coefficients for each sensor
        n_sensors: Number of sensors

    Returns:
        Tuple of (converted_acceleration, converted_temperature)
    """
    logger.info(f"Converting Tilt Link data for {n_sensors} sensors")

    n_timestamps = acceleration.shape[0]
    acc_converted = np.zeros_like(acceleration)
    temp_converted = np.zeros_like(temperature)

    for i in range(n_sensors):
        cal = calibration_data[i]

        # Acceleration conversion (typically 2 or 3 axes)
        # Assume biaxial for Tilt Link
        acc_converted[:, i * 2] = cal[0] * acceleration[:, i * 2] + cal[1]  # X
        acc_converted[:, i * 2 + 1] = cal[2] * acceleration[:, i * 2 + 1] + cal[3]  # Y

        # Temperature conversion
        if len(cal) > 4:
            temp_converted[:, i] = cal[4] * temperature[:, i] + cal[5]
        else:
            temp_converted[:, i] = temperature[:, i]

    logger.info("Tilt Link data conversion completed")
    return acc_converted, temp_converted


def convert_biaxial_link_data(
    raw_data: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw Biaxial Link (BL) data to physical units.

    Converts MATLAB conv_grezziBL.m function.

    Args:
        raw_data: Raw sensor data
        temperature: Raw temperature data
        calibration_data: Calibration coefficients
        n_sensors: Number of sensors

    Returns:
        Tuple of (converted_data, converted_temperature)
    """
    logger.info(f"Converting Biaxial Link data for {n_sensors} sensors")

    data_converted = np.zeros_like(raw_data)
    temp_converted = np.zeros_like(temperature)

    for i in range(n_sensors):
        cal = calibration_data[i]

        # Biaxial: 2 axes per sensor
        data_converted[:, i * 2] = cal[0] * raw_data[:, i * 2] + cal[1]
        data_converted[:, i * 2 + 1] = cal[2] * raw_data[:, i * 2 + 1] + cal[3]

        # Temperature
        if len(cal) > 4:
            temp_converted[:, i] = cal[4] * temperature[:, i] + cal[5]
        else:
            temp_converted[:, i] = temperature[:, i]

    logger.info("Biaxial Link data conversion completed")
    return data_converted, temp_converted


def convert_pendulum_link_data(
    raw_data: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw Pendulum Link (PL) data to physical units.

    Args:
        raw_data: Raw sensor data
        temperature: Raw temperature data
        calibration_data: Calibration coefficients
        n_sensors: Number of sensors

    Returns:
        Tuple of (converted_data, converted_temperature)
    """
    logger.info(f"Converting Pendulum Link data for {n_sensors} sensors")

    data_converted = np.zeros_like(raw_data)
    temp_converted = np.zeros_like(temperature)

    for i in range(n_sensors):
        cal = calibration_data[i]

        # Pendulum typically has 2 axes
        data_converted[:, i * 2] = cal[0] * raw_data[:, i * 2] + cal[1]
        data_converted[:, i * 2 + 1] = cal[2] * raw_data[:, i * 2 + 1] + cal[3]

        # Temperature
        if len(cal) > 4:
            temp_converted[:, i] = cal[4] * temperature[:, i] + cal[5]
        else:
            temp_converted[:, i] = temperature[:, i]

    logger.info("Pendulum Link data conversion completed")
    return data_converted, temp_converted


def convert_kessler_link_data(
    raw_data: np.ndarray,
    temperature: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Convert raw Kessler Link (KL/KLHR) data to physical units.

    Converts MATLAB conv_grezziKLHR.m function.

    Args:
        raw_data: Raw sensor data
        temperature: Raw temperature data
        calibration_data: Calibration coefficients
        n_sensors: Number of sensors

    Returns:
        Tuple of (converted_data, converted_temperature)
    """
    logger.info(f"Converting Kessler Link data for {n_sensors} sensors")

    data_converted = np.zeros_like(raw_data)
    temp_converted = np.zeros_like(temperature)

    for i in range(n_sensors):
        cal = calibration_data[i]

        # Kessler biaxial inclinometer
        data_converted[:, i * 2] = cal[0] * raw_data[:, i * 2] + cal[1]
        data_converted[:, i * 2 + 1] = cal[2] * raw_data[:, i * 2 + 1] + cal[3]

        # Temperature
        if len(cal) > 4:
            temp_converted[:, i] = cal[4] * temperature[:, i] + cal[5]
        else:
            temp_converted[:, i] = temperature[:, i]

    logger.info("Kessler Link data conversion completed")
    return data_converted, temp_converted


def convert_thermistor_data(
    raw_data: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> np.ndarray:
    """
    Convert raw thermistor (ThL) data to temperature in Celsius.

    Converts MATLAB conv_grezziThL.m function.

    Args:
        raw_data: Raw ADC values
        calibration_data: Calibration coefficients (gain, offset)
        n_sensors: Number of sensors

    Returns:
        Converted temperature array
    """
    logger.info(f"Converting Thermistor data for {n_sensors} sensors")

    temp_converted = np.zeros_like(raw_data)

    for i in range(n_sensors):
        cal = calibration_data[i]
        # Linear conversion: T = gain * ADC + offset
        temp_converted[:, i] = cal[0] * raw_data[:, i] + cal[1]

    logger.info("Thermistor data conversion completed")
    return temp_converted


def convert_pt100_data(
    raw_data: np.ndarray,
    calibration_data: np.ndarray,
    n_sensors: int
) -> np.ndarray:
    """
    Convert raw PT100 sensor data to temperature in Celsius.

    Converts MATLAB conv_grezziPT100.m function.

    Args:
        raw_data: Raw resistance or ADC values
        calibration_data: Calibration coefficients
        n_sensors: Number of sensors

    Returns:
        Converted temperature array
    """
    logger.info(f"Converting PT100 data for {n_sensors} sensors")

    temp_converted = np.zeros_like(raw_data)

    for i in range(n_sensors):
        cal = calibration_data[i]
        # PT100 typically linear: T = gain * R + offset
        temp_converted[:, i] = cal[0] * raw_data[:, i] + cal[1]

    logger.info("PT100 data conversion completed")
    return temp_converted