import numpy as np
from typing import Self, Any, Iterator
from pathlib import Path
from .core import Double, Vector
from ..frames.frames import Frame, ICRF


# JUST HERE AS REFERENCE FOR FUTURE FRAME IMPLEMENTATION


class GenericState[U: (Double, Vector)]:
    """Base class for state vectors

    Implements operations and basic functionality that is common to all types
    of state vectors and implements the containers that are used internally
    to manipulate their components. The elements q1 to q6 are representations
    of the six components of a state vector as numpy arrays.

    This class is not meant to be instantiated directly, but to be subclassed
    to create specific types of state vectors: cartesian, keplerian, etc.
    """

    def __init__(
        self,
        q1: U,
        q2: U,
        q3: U,
        q4: U,
        q5: U,
        q6: U,
        deg: bool = False,
        wrap: bool = True,
        frame: type[Frame] = ICRF,
        _components: dict[str, str] | None = None,
        _angles: dict[str, tuple[float, float]] | None = None,
    ) -> None:

        # Specify components and angles
        if _components is not None:
            self.components = _components
        else:
            self.components = {
                "q1": "q1",
                "q2": "q2",
                "q3": "q3",
                "q4": "q4",
                "q5": "q5",
                "q6": "q6",
            }
        if _angles is not None:
            self.angles = _angles
        else:
            self.angles: dict[str, tuple[float, float]] = {}
        self.has_angles = len(self.angles) > 0

        # Check input types
        if isinstance(q1, Double):
            input = np.array([q1, q2, q3, q4, q5, q6], dtype=np.float64)
            assert len(input.shape) == 1
            assert input.size == 6
        elif isinstance(q1, np.ndarray) and isinstance(q1[0], Double):
            try:
                input = np.array([q1, q2, q3, q4, q5, q6], dtype=np.float64)
                assert len(input.shape) == 2
                assert input.size / 6 == input.shape[1]
            except ValueError or AssertionError:
                raise ValueError(
                    "Components of state vector cannot have different sizes"
                )
        else:
            raise TypeError(
                "Components of state vector must be floating point numbers"
                " or numpy arrays"
            )

        # Initialize frame and state components
        self.frame = frame
        self.q1: Any = np.array([q1], dtype=np.float64).ravel()
        self.q2: Any = np.array([q2], dtype=np.float64).ravel()
        self.q3: Any = np.array([q3], dtype=np.float64).ravel()
        self.q4: Any = np.array([q4], dtype=np.float64).ravel()
        self.q5: Any = np.array([q5], dtype=np.float64).ravel()
        self.q6: Any = np.array([q6], dtype=np.float64).ravel()

        # Convert angles to radians and wrap them if necessary
        if not self.has_angles:
            return None
        if deg:
            for angle in self.angles:
                self.__dict__[angle] = np.deg2rad(self.__dict__[angle])
        if wrap:
            for angle in self.angles:
                low, high = self.angles[angle]
                # self.__dict__[angle] = self._wrap(self.__dict__[angle], low, high)
                self.__dict__[angle] %= high

        return None

    @property
    def size(self) -> int:
        return self.q1.size

    @property
    def scalar(self) -> bool:
        return self.q1.size == 1

    def __getattr__(self, name: str) -> U:

        if name in self.components:
            qi = self.__dict__[self.components[name]]
            return qi[0] if self.scalar else qi

        if name[-4:] == "_deg" and name[:-4] in self.angles:
            qi = np.rad2deg(self.__dict__[self.components[name[:-4]]])
            return qi[0] if self.scalar else qi

        raise AttributeError(
            f"{name} is not a property of {self.__class__.__name__}"
            f" in {self.frame.__name__}"
        )

    def __getitem__(self, index: int | slice) -> Self:
        return type(self)(
            self.q1[index],
            self.q2[index],
            self.q3[index],
            self.q4[index],
            self.q5[index],
            self.q6[index],
        )

    def __iter__(self) -> Iterator[Self]:
        return iter([self.__getitem__(idx) for idx in range(self.size)])

    def __repr__(self) -> str:

        out = f"{self.__class__.__name__} in {self.frame.__name__}\n"
        out += "-" * len(out) + "\n"
        for component in self.components:
            out += f"{component}: {getattr(self, component)}\n"
        return out

    def __add__(self, other: Self | Double | int) -> Self:
        if isinstance(other, Double) or isinstance(other, int):
            other = float(other)
            out = type(self)(
                self.q1 + other,
                self.q2 + other,
                self.q3 + other,
                self.q4 + other,
                self.q5 + other,
                self.q6 + other,
            )
        elif isinstance(other, self.__class__):
            out = type(self)(
                self.q1 + other.q1,
                self.q2 + other.q2,
                self.q3 + other.q3,
                self.q4 + other.q4,
                self.q5 + other.q5,
                self.q6 + other.q6,
            )
        else:
            raise TypeError(
                "Addition is only supported for state vectors and scalars",
            )

        return out

    def __sub__(self, other: Self | Double | int) -> Self:
        if isinstance(other, Double) or isinstance(other, int):
            other = float(other)
            out = type(self)(
                self.q1 - other,
                self.q2 - other,
                self.q3 - other,
                self.q4 - other,
                self.q5 - other,
                self.q6 - other,
            )
        elif isinstance(other, self.__class__):
            out = type(self)(
                self.q1 - other.q1,
                self.q2 - other.q2,
                self.q3 - other.q3,
                self.q4 - other.q4,
                self.q5 - other.q5,
                self.q6 - other.q6,
            )
        else:
            raise TypeError(
                "Subtraction is only supported for state vectors and scalars"
            )

        return out

    def __mul__(self, other: Self | Double | int) -> Self:
        if isinstance(other, Double) or isinstance(other, int):
            other = float(other)
            out = type(self)(
                self.q1 * other,
                self.q2 * other,
                self.q3 * other,
                self.q4 * other,
                self.q5 * other,
                self.q6 * other,
            )
        elif isinstance(other, self.__class__):
            # CHECK: Do I really want to support this?
            out = type(self)(
                self.q1 * other.q1,
                self.q2 * other.q2,
                self.q3 * other.q3,
                self.q4 * other.q4,
                self.q5 * other.q5,
                self.q6 * other.q6,
            )
        else:
            raise TypeError(
                "Multiplication is only supported for state vectors and scalars"
            )

        return out

    def __eq__(self, other) -> bool:
        if isinstance(other, Double) or isinstance(other, int):
            other = float(other)
            return (
                self.q1 == other
                and self.q2 == other
                and self.q3 == other
                and self.q4 == other
                and self.q5 == other
                and self.q6 == other
            )
        elif isinstance(other, self.__class__):
            return (
                np.allclose(self.q1, other.q1, rtol=1e-17)
                and np.allclose(self.q2, other.q2, rtol=1e-17)
                and np.allclose(self.q3, other.q3, rtol=1e-17)
                and np.allclose(self.q4, other.q4, rtol=1e-17)
                and np.allclose(self.q5, other.q5, rtol=1e-17)
                and np.allclose(self.q6, other.q6, rtol=1e-17)
            )
        else:
            raise TypeError(
                "Can only check for equality against state vectors and scalars"
            )

    def append(self, other: Self | Double | int) -> Self:
        if isinstance(other, Double) or isinstance(other, int):
            other = float(other)
            return type(self)(
                np.append(self.q1, other),
                np.append(self.q2, other),
                np.append(self.q3, other),
                np.append(self.q4, other),
                np.append(self.q5, other),
                np.append(self.q6, other),
            )
        elif isinstance(other, self.__class__):
            return type(self)(
                np.append(self.q1, other.q1),
                np.append(self.q2, other.q2),
                np.append(self.q3, other.q3),
                np.append(self.q4, other.q4),
                np.append(self.q5, other.q5),
                np.append(self.q6, other.q6),
            )
        else:
            raise TypeError("Append is only supported for state vectors and scalars")

    def asarray(self) -> Vector:
        return np.array(
            [self.q1, self.q2, self.q3, self.q4, self.q5, self.q6], dtype=np.float64
        )

    def split(self, idx_list: list[int]) -> list[Self]:
        """Split time series of state vectors into intervals

        :param idx_list: List of indices to split the state vector
        """

        print("WARNING: Splitting state vectors is not properly tested\n")
        low = 0
        out = []
        for high in idx_list:
            out.append(self[low:high])
            low = high
        return out

    def save(self, path: str | Path) -> None:
        """Save state vector to Numpy binary file

        :param path: Path to generated file
        """
        np.save(path, self.asarray())
        return None

    @classmethod
    def load(cls, path: str | Path):
        """Load state vector from Numpy binary file

        :param path: Path to file to load
        """
        data = np.load(path)
        return cls(*data)

    @staticmethod
    def _wrap(angle: Double, low: Double = 0.0, high: Double = 2.0 * np.pi) -> Double:
        """Wrap angle to a given interval

        :param angle: Angle to wrap
        :param low: Lower limit of the interval
        :param high: Upper limit of the interval
        """
        # TODO: This should work, but test it
        angle -= low
        angle = angle % (high - low)
        angle += low
        return angle

    def transform(self, frame: type[Frame]) -> Self:
        """Transform state to another reference frame"""
        q1, q2, q3 = self.frame.transform(self.q1, self.q2, self.q3, frame)
        q4, q5, q6 = self.frame.transform(self.q4, self.q5, self.q6, frame)
        out: Any = type(self)(q1, q2, q3, q4, q5, q6, frame=frame)
        return out


class CartesianState[U: (Double, Vector)](GenericState):
    """Cartesian state vector

    Components might be floating point numbers (single state) or numpy arrays
    of them (time series of states).

    Parameters
    -----------
    x, y, z: Double | Vector
        Components of the position vector [m]
    dx, dy, dz: Double | Vector
        Components of the velocity vector [m/s]
    frame: Frame
        Reference frame in which the state vector is defined
    """

    def __init__(
        self, x: U, y: U, z: U, dx: U, dy: U, dz: U, frame: type[Frame] = ICRF
    ) -> None:

        _components = {
            "x": "q1",
            "y": "q2",
            "z": "q3",
            "dx": "q4",
            "dy": "q5",
            "dz": "q6",
        }
        super().__init__(x, y, z, dx, dy, dz, frame=frame, _components=_components)
        return None

    @property
    def r_vec(self) -> Vector:
        return np.array([self.x, self.y, self.z])

    @property
    def v_vec(self) -> Vector:
        return np.array([self.dx, self.dy, self.dz])

    @property
    def r_mag(self) -> U:
        return np.linalg.norm(self.r_vec, axis=0)

    @property
    def v_mag(self) -> U:
        return np.linalg.norm(self.v_vec, axis=0)

    def to_keplerian(self, mu: Double) -> "KeplerianState":
        """Conversion to keplerian state vector

        Algorithm: Practical astrodynamics slides

        :param mu: Gravitational parameter of central body [m^3/s^2]
        """
        # TODO: Ensure that loss of accuracy in np.sum is not relevant
        # TODO: Only allow conversion if frame is valid

        # Semi-major axis
        a = 1.0 / ((2.0 / self.r_mag) - (self.v_mag * self.v_mag / mu))

        # Angular momentum
        h_vec = np.cross(self.r_vec, self.v_vec, axis=0)
        h = np.linalg.norm(h_vec, axis=0)

        # Eccentricity
        e_vec = (np.cross(self.v_vec, h_vec, axis=0) / mu) - (self.r_vec / self.r_mag)
        e = np.linalg.norm(e_vec, axis=0)
        e_uvec = e_vec / e

        # Inclination
        inc = np.arccos(h_vec[2] / h)

        # N vector
        _z_vec = np.array(
            [np.zeros_like(self.x), np.zeros_like(self.y), np.ones_like(self.z)]
        )
        N_vec = np.cross(_z_vec, h_vec, axis=0)
        Nxy = np.sqrt(N_vec[0] * N_vec[0] + N_vec[1] * N_vec[1])
        N_uvec = N_vec / Nxy

        # RAAN
        raan = np.arctan2(N_vec[1] / Nxy, N_vec[0] / Nxy)

        # Argument of periapsis
        sign_aop_condition = np.sum(np.cross(N_uvec, e_vec, axis=0) * h_vec, axis=0) > 0
        sign_aop = 2 * sign_aop_condition - 1
        aop = sign_aop * np.arccos(np.sum(e_uvec * N_uvec, axis=0))

        # True anomaly
        sign_ta_condition = (
            np.sum(np.cross(e_vec, self.r_vec, axis=0) * h_vec, axis=0) > 0
        )
        sign_ta = 2 * sign_ta_condition - 1
        ta = sign_ta * np.arccos(np.sum(self.r_vec * e_uvec / self.r_mag, axis=0))

        return KeplerianState(a, e, inc, raan, aop, ta, deg=False)

    # def transform(self, frame: type[Frame]) -> Self:
    #     """Transform cartesian state to another reference frame"""
    #     x, y, z = self.frame.transform(self.x, self.y, self.z, frame)
    #     dx, dy, dz = self.frame.transform(self.dx, self.dy, self.dz, frame)
    #     out: Any = CartesianState(x, y, z, dx, dy, dz, frame=frame)
    #     return out

    def to_spherical(self) -> "SphericalState":
        """Conversion from cartesian to spherical state"""
        rho, theta, phi = self.frame.to_spherical(self.q1, self.q2, self.q3)
        drho, dtheta, dphi = self.frame.to_spherical(self.q4, self.q5, self.q6)
        out: Any = SphericalState(rho, theta, phi, drho, dtheta, dphi, frame=self.frame)
        return out


class CartesianStateDerivative[U: (Double, Vector)](GenericState):
    """Derivative of cartesian state

    Derivative of a single state of a time series of states of a body in a
    three-dimensional space in terms of cartesian coordinates. Each component
    can be a floating point number (single state) or a numpy array of them
    (time series of states).

    :param dx: X component of the velocity vector [m/s]
    :param dy: Y component of the velocity vector [m/s]
    :param dz: Z component of the velocity vector [m/s]
    :param ddx: X component of the acceleration vector [m/s^2]
    :param ddy: Y component of the acceleration vector [m/s^2]
    :param ddz: Z component of the acceleration vector [m/s^2]
    """

    def __init__(self, dx: U, dy: U, dz: U, ddx: U, ddy: U, ddz: U) -> None:
        super().__init__(dx, dy, dz, ddx, ddy, ddz)
        return None

    @property
    def dx(self) -> U:
        return self.q1[0] if self.scalar else self.q1

    @property
    def dy(self) -> U:
        return self.q2[0] if self.scalar else self.q2

    @property
    def dz(self) -> U:
        return self.q3[0] if self.scalar else self.q3

    @property
    def ddx(self) -> U:
        return self.q4[0] if self.scalar else self.q4

    @property
    def ddy(self) -> U:
        return self.q5[0] if self.scalar else self.q5

    @property
    def ddz(self) -> U:
        return self.q6[0] if self.scalar else self.q6

    @property
    def v_vec(self) -> Vector:
        return np.array([self.dx, self.dy, self.dz])

    @property
    def a_vec(self) -> Vector:
        return np.array([self.ddx, self.ddy, self.ddz])

    @property
    def v_mag(self) -> U:
        return np.linalg.norm(self.v_vec, axis=0)

    @property
    def dv_mag(self) -> U:
        return np.linalg.norm(self.v_vec, axis=0)

    def times_dt(self, dt: Double) -> CartesianState:
        """Change in cartesian state over interval dt

        Returns the change of a cartesian state with this object as derivative
        over a time interval dt. The main use case are numerical integrators,
        in which the state is updated by multiplying its derivative by the
        time step.

        :param dt: Time step
        :return: Change in cartesian elements over interval dt
        """

        return CartesianState(
            self.dx * dt,
            self.dy * dt,
            self.dz * dt,
            self.ddx * dt,
            self.ddy * dt,
            self.ddz * dt,
        )

    def add_acceleration(self, other: Self) -> None:
        """Update the acceleration of the cartesian state derivative

        Update the acceleration of this object by adding the components of the
        acceleration vector of other cartesian state derivative. The use case is
        to add up all the accelerations acting on a body during orbit propagation.
        Since each acceleration is defined by a cartesian state derivative,
        adding them up directly would also update the velocity, which is incorrect.

        :param other: Cartesian state derivative with acceleration to add
        """

        # q4, q5 and q6 are the components of a general state vector that
        # represent the acceleration in CartesianStateDerivative
        self.q4 += other.q4
        self.q5 += other.q5
        self.q6 += other.q6

    def __repr__(self) -> str:
        return (
            "Cartesian state derivative\n"
            "---------------------------------\n"
            f"dx: {self.dx}\n"
            f"dy: {self.dy}\n"
            f"dz: {self.dz}\n"
            f"ddx: {self.ddx}\n"
            f"ddy: {self.ddy}\n"
            f"ddz: {self.ddz}"
        )


class KeplerianState[U: (Double, Vector)](GenericState):
    """Keplerian state vector

    Single state of a time series of states of a body in a three-dimensional
    space in terms of classical keplerian elements. Each component of the state
    vector can be a floating point number (single state) or a numpy array of them
    (time series of states).

    Angles might be given in degrees or radians, but are always converted to
    radians during initialization. For each angle, the method KeplerianState.angle
    will return the value in radians, while KeplerianState.angle_deg will return
    it in degrees.

    Angles can be wrapped if specified by the wrap parameter, which is activated
    by default. Wrapping will limit inclinations to the range [0, pi) and the
    rest of the angles to [0, 2pi).

    :param a: Semi-major axis [m]
    :param e: Eccentricity
    :param i: Inclination [rad]
    :param raan: Right ascension of the ascending node [rad]
    :param aop: Argument of periapsis [rad]
    :param ta: True anomaly [rad]
    :param deg: Angles are given in degrees (True) or radians (False)
    :param wrap: Wrap angles (True) or not (False)
    """

    def __init__(
        self,
        a: U,
        e: U,
        i: U,
        raan: U,
        aop: U,
        ta: U,
        deg: bool = False,
        wrap: bool = True,
        frame: type[Frame] = ICRF,
    ) -> None:
        components = {
            "a": "q1",
            "e": "q2",
            "i": "q3",
            "raan": "q4",
            "aop": "q5",
            "ta": "q6",
        }
        angles = {
            "q3": (0.0, np.pi),
            "q4": (0.0, 2.0 * np.pi),
            "q5": (0.0, 2.0 * np.pi),
            "q6": (0.0, 2.0 * np.pi),
        }
        super().__init__(
            a,
            e,
            i,
            raan,
            aop,
            ta,
            deg=deg,
            wrap=wrap,
            frame=frame,
            _components=components,
            _angles=angles,
        )

        return None

    @property
    def E(self) -> U:
        """Eccentric anomaly"""
        out: Any = 2.0 * np.arctan2(
            np.sqrt(1.0 - self.e) * np.sin(0.5 * self.ta),
            np.sqrt(1.0 + self.e) * np.cos(0.5 * self.ta),
        )
        return out

    @property
    def M(self) -> U:
        """Mean anomaly"""
        out: Any = self.E - self.e * np.sin(self.E)
        return out

    def period(self, mu: Double) -> U:
        """Orbital period"""
        out: Any = 2.0 * np.pi * np.sqrt(self.a * self.a * self.a / mu)
        return out

    def __sub__(self, other: object) -> Self:
        print(
            "WARNING: Keplerian state subtraction is not properly tested\n"
            "It might produce unexpected results"
        )

        if not isinstance(other, KeplerianState):
            raise TypeError("Subtraction is only supported for keplerian state vectors")

        di = np.unwrap(self.q3, period=np.pi) - np.unwrap(other.q3, period=np.pi)
        draan = np.unwrap(self.q4, period=2.0 * np.pi) - np.unwrap(
            other.q4, period=2.0 * np.pi
        )
        daop = np.unwrap(self.q5, period=2.0 * np.pi) - np.unwrap(
            other.q5, period=2.0 * np.pi
        )
        dta = np.unwrap(self.q6, period=2.0 * np.pi) - np.unwrap(
            other.q6, period=2.0 * np.pi
        )

        return type(self)(
            self.q1 - other.q1,
            self.q2 - other.q2,
            di,
            draan,
            daop,
            dta,
            frame=self.frame,
        )

    def to_cartesian(self, mu: Double) -> "CartesianState":
        """Conversion to cartesian state vector

        Algorithm: Practical astrodynamics slides

        :param mu: Gravitational parameter of central body [m^3/s^2]
        """

        # Auxiliary trigonometric relations
        cos_Omega = np.cos(self.raan)
        sin_Omega = np.sin(self.raan)
        cos_omega = np.cos(self.aop)
        sin_omega = np.sin(self.aop)
        cos_i = np.cos(self.i)
        sin_i = np.sin(self.i)
        cos_theta = np.cos(self.ta)
        sin_theta = np.sin(self.ta)

        l1 = cos_Omega * cos_omega - sin_Omega * sin_omega * cos_i
        l2 = -cos_Omega * sin_omega - sin_Omega * cos_omega * cos_i
        m1 = sin_Omega * cos_omega + cos_Omega * sin_omega * cos_i
        m2 = -sin_Omega * sin_omega + cos_Omega * cos_omega * cos_i
        n1 = sin_omega * sin_i
        n2 = cos_omega * sin_i

        # Orbital radius
        r = self.a * (1.0 - self.e * self.e) / (1.0 + self.e * cos_theta)

        # Position in orbital plane
        xi = r * cos_theta
        eta = r * sin_theta

        # Position in 3D space
        x = l1 * xi + l2 * eta
        y = m1 * xi + m2 * eta
        z = n1 * xi + n2 * eta

        # Angular momentum
        H = np.sqrt(mu * self.a * (1.0 - self.e * self.e))

        # Velocity
        common = mu / H
        e_cos_theta = self.e + cos_theta

        dx = common * (l2 * e_cos_theta - l1 * sin_theta)
        dy = common * (m2 * e_cos_theta - m1 * sin_theta)
        dz = common * (n2 * e_cos_theta - n1 * sin_theta)

        return CartesianState(x, y, z, dx, dy, dz, frame=self.frame)


class KeplerianStateDerivative[U: (Double, Vector)](GenericState):
    """Derivative of keplerian state

    Derivative of a single state of a time series of states of a body in a
    three-dimensional space in terms of classical keplerian elements. Each
    component can be a floating point number (single state) or a numpy array
    of them (time series of states).

    Angular velocities might be given in degrees or radians per second, but are
    always converted to radians per second during initialization. For each angular
    velocity, the method KeplerianStateDerivative.angvel will return the value in
    radians per second, while KeplerianStateDerivative.angvel_deg will return it
    in degrees per second.

    :param da: Rate of change of semi-major axis [m/s]
    :param de: Rate of change of eccentricity [1/s]
    :param di: Rate of change of inclination [rad/s]
    :param draan: Rate of change of right ascension of the ascending node [rad/s]
    :param daop: Rate of change of argument of periapsis [rad/s]
    :param dta: Rate of change of true anomaly [rad/s]
    :param deg: Angular velocities are given in degrees per second (True) or
        radians  per second (False)
    """

    def __init__(
        self,
        da: U,
        de: U,
        di: U,
        draan: U,
        daop: U,
        dta: U,
        deg: bool = True,
    ) -> None:
        super().__init__(da, de, di, draan, daop, dta)

        if not deg:
            self.q3 = np.rad2deg(self.q3)
            self.q4 = np.rad2deg(self.q4)
            self.q5 = np.rad2deg(self.q5)
            self.q6 = np.rad2deg(self.q6)

        return None

    @property
    def da(self) -> U:
        return self.q1[0] if self.scalar else self.q1

    @property
    def de(self) -> U:
        return self.q2[0] if self.scalar else self.q2

    @property
    def di(self) -> U:
        return self.q3[0] if self.scalar else self.q3

    @property
    def draan(self) -> U:
        return self.q4[0] if self.scalar else self.q4

    @property
    def daop(self) -> U:
        return self.q5[0] if self.scalar else self.q5

    @property
    def dta(self) -> U:
        return self.q6[0] if self.scalar else self.q6

    def times_dt(self, dt: Double) -> KeplerianState:
        """Change in keplerian state over interval dt

        Returns the change of a keplerian state with this object as derivative
        over a time interval dt. The main use case are numerical integrators,
        in which the state is updated by multiplying its derivative by the
        time step.

        :param dt: Time step
        :return: Change in keplerian elements over interval dt
        """

        return KeplerianState(
            self.da * dt,
            self.de * dt,
            self.di * dt,
            self.draan * dt,
            self.daop * dt,
            self.dta * dt,
        )

    def __repr__(self) -> str:
        return (
            "Keplerian state derivative\n"
            "---------------------------------\n"
            f"da: {self.da}\n"
            f"de: {self.de}\n"
            f"di: {self.di}\n"
            f"dRAAN: {self.draan}\n"
            f"dAoP: {self.daop}\n"
            f"dTA: {self.dta}"
        )


class SphericalState[U: (Double, Vector)](GenericState):

    def __init__(
        self,
        rho: U,
        theta: U,
        phi: U,
        drho: U,
        dtheta: U,
        dphi: U,
        deg: bool = False,
        wrap: bool = True,
        frame: type[Frame] = ICRF,
    ) -> None:

        components = frame.variables
        angles = frame.angles

        super().__init__(
            rho,
            theta,
            phi,
            drho,
            dtheta,
            dphi,
            deg=deg,
            wrap=wrap,
            frame=frame,
            _components=components,
            _angles=angles,
        )

        return None

    def __add__(self, other: object) -> Self:
        raise NotImplementedError(
            "Addition is still not supported for spherical state vectors"
        )

    def __sub__(self, other: object) -> Self:
        raise NotImplementedError(
            "Subtraction is still not supported for spherical state vectors"
        )

    def transform(self, frame: type[Frame]) -> Self:
        """Transform spherical state to another reference frame"""
        cstate = self.to_cartesian()
        q1, q2, q3 = self.frame.transform(cstate.q1, cstate.q2, cstate.q3, frame)
        q4, q5, q6 = self.frame.transform(cstate.q4, cstate.q5, cstate.q6, frame)
        out: Any = CartesianState(q1, q2, q3, q4, q5, q6, frame=frame).to_spherical()
        return out

    def to_cartesian(self) -> "CartesianState":
        """Conversion from spherical to cartesian state"""
        x, y, z = self.frame.to_cartesian(self.q1, self.q2, self.q3)
        dx, dy, dz = self.frame.to_cartesian(self.q4, self.q5, self.q6)
        out: Any = CartesianState(x, y, z, dx, dy, dz, frame=self.frame)
        return out

    def to_keplerian(self, mu: Double) -> "KeplerianState":
        """Conversion from spherical to keplerian state"""
        return self.to_cartesian().to_keplerian(mu)


class CartesianPosition[U: (Double, Vector)](GenericState):
    """Three dimensional position vector in cartesian coordinates

    Components might be floating point numbers (single vector) or numpy arrays
    of them (time series of vectors).

    :param x: X coordinate [m]
    :param y: Y coordinate [m]
    :param z: Z coordinate [m]
    """

    def __init__(self, x: U, y: U, z: U, *_, frame: type[Frame] = ICRF) -> None:

        components = {"x": "q1", "y": "q2", "z": "q3"}
        null = 0.0 * x
        super().__init__(x, y, z, null, null, null, frame=frame, _components=components)

        return None

    def asarray(self) -> Vector:
        return np.array([self.x, self.y, self.z])

    @property
    def mag(self) -> U:
        return np.linalg.norm(self.asarray(), axis=0)

    def to_spherical(self) -> "SphericalPosition":
        rho, theta, phi = self.frame.to_spherical(self.x, self.y, self.z)
        out: Any = SphericalPosition(rho, theta, phi, frame=self.frame)
        return out


class CartesianVelocity[U: (Double, Vector)](GenericState):
    """Three dimensional velocity vector in cartesian coordinates

    Components might be floating point numbers (single vector) or numpy arrays
    of them (time series of vectors).

    :param dx: X component of the velocity vector [m/s]
    :param dy: Y component of the velocity vector [m/s]
    :param dz: Z component of the velocity vector [m/s]
    """

    def __init__(self, dx: U, dy: U, dz: U, *_, frame: type[Frame] = ICRF) -> None:

        components = {"dx": "q1", "dy": "q2", "dz": "q3"}
        null = 0.0 * dx
        super().__init__(
            dx, dy, dz, null, null, null, frame=frame, _components=components
        )
        return None

    def asarray(self) -> Vector:
        return np.array([self.dx, self.dy, self.dz])

    @property
    def mag(self) -> U:
        return np.linalg.norm(self.asarray(), axis=0)


class SphericalPosition[U: (Double, Vector)](GenericState):
    """Three dimensional position vector in spherical coordinates

    Components might be floating point numbers (single vector) or numpy arrays
    of them (time series of vectors).

    Angles can be passed in radians (default) or degrees (deg=True), but they
    are always converted to radians during initialization. For each angle, the
    method SphericalPosition.angle will return the value in radians, while
    SphericalPosition.angle_deg will return it in degrees.

    :param range: Radial distance [m]
    :param theta: Horizontal angular distance [rad / deg]
    :param phi: Vertical angular distance [rad / deg]
    :param deg: Angles are given in degrees (True) or radians (False)
    :param wrap: Wrap angles (True) or not (False)
    """

    def __init__(
        self,
        rho: U,
        theta: U,
        phi: U,
        deg: bool = False,
        wrap: bool = True,
        frame: type[Frame] = ICRF,
    ) -> None:

        _keys = list(frame.variables.keys())
        components = {_keys[0]: "q1", _keys[1]: "q2", _keys[2]: "q3"}

        null = 0.0 * rho
        super().__init__(
            rho,
            theta,
            phi,
            null,
            null,
            null,
            frame=frame,
            deg=deg,
            wrap=wrap,
            _components=components,
            _angles=frame.angles,
        )

        return None

    def asarray(self) -> Vector:
        return np.array([self.__getattr__(var) for var in self.frame.variables])

    def to_cartesian(self) -> "CartesianPosition":
        x, y, z = self.frame.to_cartesian(self.q1, self.q2, self.q3)
        out: Any = CartesianPosition(x, y, z, frame=self.frame)
        return out

    def transform(self, frame: type[Frame]) -> Self:
        """Transform spherical position to another reference frame"""
        out: Any = self.to_cartesian().transform(frame).to_spherical()
        return out
