"""
Data elaboration functions for Tilt sensors.

Processes tilt sensor data to calculate displacements and rotations.
"""

import numpy as np
import logging
from typing import Tuple, Optional
from pathlib import Path
from ..common.database import DatabaseConnection
from ..common.validators import approximate_values
from .geometry import arot_hr, asse_a_hr, asse_b_hr

logger = logging.getLogger(__name__)


def elaborate_tilt_link_hr_data(
    conn: DatabaseConnection,
    control_unit_id: str,
    chain: str,
    n_sensors: int,
    angle_data: np.ndarray,
    temp_max: float,
    temp_min: float,
    temperature: np.ndarray,
    node_list: list,
    timestamps: np.ndarray,
    is_new_zero: bool,
    n_data_avg: int,
    n_data_despike: int,
    error_flags: np.ndarray,
    initial_date: str,
    installation_angles: np.ndarray,
    sensor_lengths: np.ndarray
) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """
    Elaborate Tilt Link HR data to calculate displacements.

    Converts MATLAB elaboration for TLHR sensors.

    Args:
        conn: Database connection
        control_unit_id: Control unit identifier
        chain: Chain identifier
        n_sensors: Number of sensors
        angle_data: Angle data array (degrees)
        temp_max: Maximum valid temperature
        temp_min: Minimum valid temperature
        temperature: Temperature array
        node_list: List of node IDs
        timestamps: Timestamp array
        is_new_zero: Whether this is a new zero point
        n_data_avg: Number of data for averaging
        n_data_despike: Number of data for despiking
        error_flags: Error flags array
        initial_date: Initial processing date
        installation_angles: Installation angle for each sensor (degrees)
        sensor_lengths: Length/spacing for each sensor (meters)

    Returns:
        Tuple of (X_global, Y_global, Z_global, X_local, Y_local, Z_local, temperature)
    """
    logger.info("Starting Tilt Link HR elaboration")

    # Handle new zero point
    if is_new_zero:
        n_skip = max(n_data_avg, n_data_despike)
        ini = round(n_skip / 2) + 1
        if n_skip % 2 == 0:
            ini += 1

        angle_data = angle_data[ini:, :]
        temperature = temperature[ini:, :]
        timestamps = timestamps[ini:]
        error_flags = error_flags[ini:, :]

    n_timestamps = len(timestamps)

    # Temperature validation
    n_corrections_temp = 0
    for b in range(temperature.shape[1]):
        for a in range(temperature.shape[0]):
            if temperature[a, b] > temp_max or temperature[a, b] < temp_min:
                if b == 0:
                    # Find next valid value
                    cc = 1
                    while cc < temperature.shape[1]:
                        if temp_min <= temperature[a, cc] <= temp_max:
                            temperature[a, b] = temperature[a, cc]
                            break
                        cc += 1
                else:
                    temperature[a, b] = temperature[a, b-1]
                n_corrections_temp += 1

    if n_corrections_temp > 0:
        logger.info(f"{n_corrections_temp} temperature corrections applied")

    # Calculate displacements for each sensor
    # Global coordinates (absolute)
    X_global = np.zeros((n_timestamps, n_sensors))
    Y_global = np.zeros((n_timestamps, n_sensors))
    Z_global = np.zeros((n_timestamps, n_sensors))

    # Local coordinates (relative to installation)
    X_local = np.zeros((n_timestamps, n_sensors))
    Y_local = np.zeros((n_timestamps, n_sensors))
    Z_local = np.zeros((n_timestamps, n_sensors))

    # Extract angle arrays (reshape for geometry functions)
    ax = np.zeros((n_sensors, n_timestamps))
    ay = np.zeros((n_sensors, n_timestamps))

    for i in range(n_sensors):
        ax[i, :] = angle_data[:, i * 2]
        ay[i, :] = angle_data[:, i * 2 + 1]

    # Calculate displacements using geometric transformations
    for t in range(n_timestamps):
        for i in range(n_sensors):
            # Installation angle for this sensor
            install_angle = installation_angles[i] if i < len(installation_angles) else 0.0

            # Sensor length/spacing
            spe_tl = sensor_lengths[i] if i < len(sensor_lengths) else 1.0

            # Calculate displacement components
            n_disp, e_disp, z_disp = arot_hr(
                ax, ay, install_angle,
                np.array([spe_tl]),  # Wrap in array for compatibility
                i, t
            )

            # Store in global coordinates
            X_global[t, i] = n_disp
            Y_global[t, i] = e_disp
            Z_global[t, i] = z_disp

            # Local coordinates (simplified - could add rotation matrix)
            X_local[t, i] = n_disp
            Y_local[t, i] = e_disp
            Z_local[t, i] = z_disp

    # Calculate horizontal shift
    H_shift_global = np.sqrt(X_global**2 + Y_global**2)
    H_shift_local = np.sqrt(X_local**2 + Y_local**2)

    # Calculate azimuth (direction of movement)
    Azimuth = np.degrees(np.arctan2(Y_global, X_global))

    # Apply approximation (round to specified decimal places)
    X_global, Y_global, Z_global, X_local, Y_local, Z_local, temperature = \
        approximate_values(X_global, Y_global, Z_global, X_local, Y_local, Z_local, temperature, decimals=6)

    # Calculate differentials (relative to first reading or reference)
    X_global, Y_global, Z_global = calculate_tilt_differentials(
        control_unit_id, chain, X_global, Y_global, Z_global, is_new_zero, "TLHR"
    )

    logger.info("Tilt Link HR elaboration completed successfully")

    return X_global, Y_global, Z_global, X_local, Y_local, Z_local, temperature


def calculate_tilt_differentials(
    control_unit_id: str,
    chain: str,
    x_data: np.ndarray,
    y_data: np.ndarray,
    z_data: np.ndarray,
    is_new_zero: bool,
    sensor_type: str
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Calculate differential values relative to reference.

    Args:
        control_unit_id: Control unit identifier
        chain: Chain identifier
        x_data: X displacement data
        y_data: Y displacement data
        z_data: Z displacement data
        is_new_zero: Whether this is first processing
        sensor_type: Sensor type identifier

    Returns:
        Tuple of differential x, y, z
    """
    ref_file_x = Path(f"{control_unit_id}-{chain}-{sensor_type}-RifX.csv")
    ref_file_y = Path(f"{control_unit_id}-{chain}-{sensor_type}-RifY.csv")
    ref_file_z = Path(f"{control_unit_id}-{chain}-{sensor_type}-RifZ.csv")

    if not is_new_zero:
        # First processing - save reference and calculate diff
        np.savetxt(ref_file_x, x_data[0:1, :], delimiter=',')
        np.savetxt(ref_file_y, y_data[0:1, :], delimiter=',')
        np.savetxt(ref_file_z, z_data[0:1, :], delimiter=',')

        x_diff = x_data - x_data[0, :]
        y_diff = y_data - y_data[0, :]
        z_diff = z_data - z_data[0, :]
    else:
        # Load reference and calculate diff
        try:
            ref_x = np.loadtxt(ref_file_x, delimiter=',')
            ref_y = np.loadtxt(ref_file_y, delimiter=',')
            ref_z = np.loadtxt(ref_file_z, delimiter=',')

            x_diff = x_data - ref_x
            y_diff = y_data - ref_y
            z_diff = z_data - ref_z
        except FileNotFoundError:
            logger.warning("Reference files not found, using first value as reference")
            x_diff = x_data - x_data[0, :]
            y_diff = y_data - y_data[0, :]
            z_diff = z_data - z_data[0, :]

    return x_diff, y_diff, z_diff


def elaborate_biaxial_link_data(
    data: np.ndarray,
    temperature: np.ndarray,
    n_sensors: int,
    installation_angles: np.ndarray,
    sensor_lengths: np.ndarray,
    temp_max: float,
    temp_min: float
) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """
    Elaborate Biaxial Link data.

    Args:
        data: Sensor data array (acceleration or angles)
        temperature: Temperature array
        n_sensors: Number of sensors
        installation_angles: Installation angles
        sensor_lengths: Sensor lengths
        temp_max: Maximum valid temperature
        temp_min: Minimum valid temperature

    Returns:
        Tuple of (X_disp, Y_disp, Z_disp, temperature)
    """
    logger.info(f"Elaborating Biaxial Link data for {n_sensors} sensors")

    n_timestamps = data.shape[0]

    # Validate temperature
    for i in range(temperature.shape[1]):
        invalid_mask = (temperature[:, i] < temp_min) | (temperature[:, i] > temp_max)
        if np.any(invalid_mask):
            # Forward fill valid values
            valid_indices = np.where(~invalid_mask)[0]
            if len(valid_indices) > 0:
                temperature[invalid_mask, i] = np.interp(
                    np.where(invalid_mask)[0],
                    valid_indices,
                    temperature[valid_indices, i]
                )

    # Calculate displacements
    X_disp = np.zeros((n_timestamps, n_sensors))
    Y_disp = np.zeros((n_timestamps, n_sensors))
    Z_disp = np.zeros((n_timestamps, n_sensors))

    for i in range(n_sensors):
        # Extract axes for this sensor
        ax = data[:, i * 2]
        ay = data[:, i * 2 + 1]

        angle = installation_angles[i] if i < len(installation_angles) else 0.0
        length = sensor_lengths[i] if i < len(sensor_lengths) else 1.0

        # Calculate displacement for each timestamp
        for t in range(n_timestamps):
            # Use geometry functions
            n_a, e_a, z_a = asse_a_hr(
                np.array([[ax[t]]]), angle,
                np.array([length]), 0, 0
            )
            n_b, e_b, z_b = asse_b_hr(
                np.array([[ay[t]]]), angle,
                np.array([length]), 0, 0
            )

            X_disp[t, i] = n_a + n_b
            Y_disp[t, i] = e_a + e_b
            Z_disp[t, i] = z_a + z_b

    logger.info("Biaxial Link elaboration completed")
    return X_disp, Y_disp, Z_disp, temperature


def calculate_velocity_acceleration(
    displacement: np.ndarray,
    timestamps: np.ndarray
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Calculate velocity and acceleration from displacement data.

    Args:
        displacement: Displacement array (timestamps x sensors)
        timestamps: Timestamp array

    Returns:
        Tuple of (velocity, acceleration)
    """
    n_timestamps, n_sensors = displacement.shape

    # Calculate time differences (convert to seconds if needed)
    dt = np.diff(timestamps)
    dt = np.concatenate([[dt[0]], dt])  # Prepend first dt

    # Velocity: dDisplacement/dt
    velocity = np.zeros_like(displacement)
    velocity[1:, :] = np.diff(displacement, axis=0) / dt[1:, np.newaxis]
    velocity[0, :] = velocity[1, :]  # Forward fill first value

    # Acceleration: dVelocity/dt
    acceleration = np.zeros_like(displacement)
    acceleration[1:, :] = np.diff(velocity, axis=0) / dt[1:, np.newaxis]
    acceleration[0, :] = acceleration[1, :]

    return velocity, acceleration


def approximate_tilt_values(
    *arrays: np.ndarray,
    decimals_pos: int = 6,
    decimals_angle: int = 1,
    decimals_temp: int = 1
) -> Tuple[np.ndarray, ...]:
    """
    Approximate tilt values to specified decimal places.

    Converts MATLAB approx_TLHR.m function.

    Args:
        arrays: Variable number of arrays to approximate
        decimals_pos: Decimal places for positions (micrometers precision)
        decimals_angle: Decimal places for angles
        decimals_temp: Decimal places for temperature

    Returns:
        Tuple of approximated arrays
    """
    # First arrays are typically positions (X, Y, Z) - use high precision
    # Last array is typically temperature - use lower precision
    result = []

    for i, arr in enumerate(arrays):
        if i < len(arrays) - 1:
            # Position data
            result.append(np.round(arr, decimals_pos))
        else:
            # Temperature data
            result.append(np.round(arr, decimals_temp))

    return tuple(result)
