import numpy as np
import math

from typing import Literal
from numpy.typing import NDArray

from spatz.simulations.csv_source import CSVSource


def T1(angle):
    # return Rotation.from_euler('X', angle, degrees=False).as_matrix()
    return np.array([
        [1, 0, 0],
        [0, math.cos(angle), math.sin(angle)],
        [0, -math.sin(angle), math.cos(angle)],
    ])


def T2(angle):
    # return Rotation.from_euler('Y', angle, degrees=False).as_matrix()
    return np.array([
        [math.cos(angle), 0, -math.sin(angle)],
        [0, 1, 0],
        [math.sin(angle), 0, math.cos(angle)]
    ])


def T3(angle):
    # return Rotation.from_euler('Z', angle, degrees=False).as_matrix()
    return np.array([
        [math.cos(angle), math.sin(angle), 0],
        [-math.sin(angle), math.cos(angle), 0],
        [0, 0, 1]
    ])


class ASTOSSource(CSVSource):
    def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
        super().__init__(path, 'time', interpolation)

    def get_position(self) -> NDArray:
        return self.fetch_values(['x', 'y', 'z'])

    def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
        vel = self.fetch_values(['vx', 'vy', 'vz'])

        if frame == 'local':
            return self.global_to_local() @ vel
        
        return vel

    def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
        acc = self.fetch_values(['ax', 'ay', 'az'])

        if frame == 'local':
            return self.global_to_local() @ acc
        
        return acc

    def get_attitude(self) -> NDArray:
        raise NotImplementedError()

    def local_to_global(self) -> NDArray:
        return self.global_to_local().T
    
    def global_to_local(self) -> NDArray:
        # ASTOS local to body
        rots = self.fetch_values(['pitch_l', 'yaw_l', 'roll_l'])
        pitch_l, yaw_l, roll_l = rots[0], rots[1], rots[2]
        local_to_body = T1(roll_l) @ T2(pitch_l - math.pi/2) @ T1(-yaw_l)

        # ASTOS global to launch rail.
        init_long = self.fetch_init_value('longitude')
        init_lat = self.fetch_init_value('latitude')
        global_to_launch_rail = T2(-math.pi/2 - init_lat) @ T3(init_long)

        # ASTOS global to local.
        decl = self.fetch_value('declination')
        long = self.fetch_value('longitude')
        t0 = self.get_start_time()
        omega_E = (2*math.pi) / (24*60*60)
        global_to_local = T2(-decl) @ T3(long + omega_E * t0)

        # ASTOS local to launch rail inverted
        local_to_launch_rail_inv = global_to_launch_rail @ global_to_local

        return local_to_body @ local_to_launch_rail_inv
    
    def get_angular_velocity(self) -> NDArray:
        return self.fetch_values(['OMEGA_X', 'OMEGA_Y', 'OMEGA_Z'])
    
    def get_static_pressure(self) -> float:
        return self.fetch_value('pressure')
    
    def get_longitude(self) -> float:
        return self.fetch_value('longitude')
    
    def get_latitude(self) -> float:
        return self.fetch_value('latitude')
    
    def get_altitude(self) -> float:
        return self.fetch_value('altitude')