import math

from .Direction import *
from .Vector import *

class Polar:
    """! A polar 2D vector
    """

    def __init__(self, distance: float, angle: Angle):
        """! Create a new polar vector
        @param distance The length of the vector
        @param angle The direction of the angle
        """
        self.distance: float = distance
        ## The length of the vector
        self.direction: Angle = angle
        ## The direction of the vector

        ## Normalizing such that distance >= 0
        if self.distance < 0:
            self.distance = -self.distance
            self.direction = self.direction.Inverse()
        elif self.distance == 0:
            self.direction = Angle.zero

    def Degrees(distance: float, degrees: float):
        """! Create a polar vector without using the Angle type. All given
        angles are in degrees
        @param distance The distance in meters
        @param horizontal The angle in degrees
        @return The polar vector
        """
        angle: Angle = Angle.Degrees(degrees)
        r: Polar = Polar(distance, angle)
        return r

    def Radians(distance: float, radians: float):
        """! Create polar vector without using the Angle type. All given
        angles are in radians
        @param distance The distance in meters
        @param horizontal The horizontal angle in radians
        @param vertical The vertical angle in radians
        @return The polar vector
        """
        angle: Angle = Angle.Radians(radians)
        r: Polar = Polar(distance, angle)
        return r

    @staticmethod
    def FromVector2(v: Vector2):
        """! Create a polar coordinate from a Vector2 coordinate
        @param v The vector coordinate
        @return The polar coordinate
        """
        distance = v.Magnitude()
        if distance == 0:
            return Polar(0, Angle.zero)
        
        angle = Angle.Radians(math.atan2(v.right, v.up))
        return Polar(distance, angle)

    def ToVector2(self) -> Vector2:
        """! Convert the polar coordinate to a Vector2 coordinate
        @return The vector coordinate
        """
        horizontalRad = self.direction.InRadians()

        cosHorizontal = math.cos(horizontalRad)
        sinHorizontal = math.sin(horizontalRad)

        right = self.distance * sinHorizontal
        up = self.distance * cosHorizontal
        return Vector2(right, up)

    def __eq__(self, other) -> bool:
        """! Check if this vector is equal to the given vector
        @param v The vector to check against
        @return true if it is identical to the given vector
        @note This uses float comparison to check equality which may have strange
        effects. Equality on floats should be avoided.
        """
        return (
            self.distance == other.distance and
            self.direction == other.direction
        )    
    def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
        return (
            math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
            self.direction.isclose(other.direction, rel_tol, abs_tol)
        )

        
    def Magnitude(self) -> float:
        return math.fabs(self.distance)

    def Normalized(self):
        if self.distance == 0:
            return Polar(0, self.direction)
        
        return Polar(1, self.direction)

    def __neg__(self):
        """! Negate the vector
        @return The negated vector
        This will negate the direction. Distance will stay the same.
        """
        return Polar(self.distance, self.direction.Inverse())

    def __sub__(self, other):
        """! Subtract a polar vector from this vector
        @param other The vector to subtract
        @return The result of the subtraction
        """
        # v1 = self.ToVector2()
        # v2 = other.ToVector2()
        # r = v1 - v2
        # return Polar.FromVector2(r)
        r = self + (-other)
        return r

    def __add__(self, other):
        """! Add a polar vector to this vector
        @param other The vector to add
        @return The result of the addition
        """
        # v1 = self.ToVector2()
        # v2 = other.ToVector2()
        # r = v1 - v2
        # return Polar.FromVector2(r)

        if other.distance == 0:
            return Polar(self.distance, self.direction)
        if self.distance == 0:
            return other

        deltaAngle: float = (other.direction - self.direction).InDegrees();
        if deltaAngle < 0:
            rotation = 180 + deltaAngle
        else:
            rotation = 180 - deltaAngle

        if rotation == 180 and other.distance > 0:
            # angle is too small, take this angle and add the distances
            return Polar(self.distance + other.distance, self.direction)

        newDistance: float = Angle.CosineRuleSide(other.distance, self.distance, Angle.Degrees(rotation))

        angle: float = Angle.CosineRuleAngle(newDistance, self.distance, other.distance).InDegrees()
        if deltaAngle < 0:
            new_angle: float = self.direction.InDegrees() - angle
        else:
            new_angle: float = self.direction.InDegrees() + angle
        new_angle_a: Angle = Angle.Degrees(new_angle)
        vector = Polar(newDistance, new_angle_a)
        return vector

    def __mul__(self, factor):
        """! Scale the vector uniformly up
        @param factor The scaling factor
        @return The scaled vector
        @remark This operation will scale the distance of the vector. The angle
        will be unaffected.
        """
        return Polar(
            self.distance * factor,
            self.direction
        )

    def __truediv__(self, factor):
        """! Scale the vector uniformly down
        @param factor The scaling factor
        @return The scaled vector
        @remark This operation will scale the distance of the vector. The angle
        will be unaffected.
        """
        return Polar(
            self.distance / factor,
            self.direction
        )

    @staticmethod
    def Distance(v1, v2) -> float:
        """! Calculate the distance between two spherical coordinates
        @param v1 The first coordinate
        @param v2 The second coordinate
        @return The distance between the coordinates in meters
        """
        v1 = v1.ToVector2()
        v2 = v2.ToVector2()
        distance: float = Vector2.Distance(v1, v2)
        return distance

    @staticmethod
    def Angle(v1, v2) -> Angle:
        """! Calculate the unsigned angle between two spherical vectors
        @param v1 The first vector
        @param v2 The second vector
        @return The unsigned angle between the vectors [0..179.9999999..., -180]
        @remark the strange range is caused by the 2s complement signed values
        which has range [minvalue..maxvalue). This is a hardware limitation,
        not something we can change.
        """
        angle: Angle = Angle.Abs(v1.direction - v2.direction)
        return angle
    
    @staticmethod
    def SignedAngle(v1, v2) -> Angle:
        """! Calculate the unsigned angle between two spherical vectors
        @param v1 The first vector
        @param v2 The second vector
        @return The unsigned angle between the vectors [0..179.9999999..., -180]
        @remark the strange range is caused by the 2s complement signed values
        which has range [minvalue..maxvalue). This is a hardware limitation,
        not something we can change.
        """
        angle: Angle = v2.direction - v1.direction
        return angle

    def Lerp(v1, v2, f: float):
        """! Lerp (linear interpolation) between two vectors
        @param v1 The starting vector
        @param v2 The ending vector
        @param f The interpolation distance
        @return The lerped vector
        @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
        1 matches vector *v2*. Value -1 is vector *v1* minus the difference
        between *v1* and *v2* etc.
        """
        return v1 + (v2 - v1) * f
        # return v1 * (1 - f) + v2 * f

Polar.zero = Polar(0, Angle.zero)

class Spherical(Polar):
    """! A spherical 3D vector
    """

    def __init__(self, distance: float, direction: Direction):
        """! Create a new spherical vector
        @param distance The length of the vector
        @param direction The direction of the vector
        """
        self.distance: float = distance
        ## The length of the vector
        self.direction: Direction = direction
        ## The direction of the vector
        
        ## Normalizing such that distance >= 0
        if self.distance < 0:
            self.distance = -self.distance
            self.direction = -self.direction
        elif self.distance == 0:
            self.direction = Direction.zero

    def Degrees(distance: float, horizontal: float, vertical: float):
        """! Create sperical vector without using the Direction type. All given
        angles are in degrees
        @param distance The distance in meters
        @param horizontal The horizontal angle in degrees
        @param vertical The vertical angle in degrees
        @return The spherical vector
        """
        direction: Direction = Direction.Degrees(horizontal, vertical)
        r: Spherical = Spherical(distance, direction)
        return r

    def Radians(distance: float, horizontal: float, vertical: float):
        """! Create sperical vector without using the Direction type. All given
        angles are in radians
        @param distance The distance in meters
        @param horizontal The horizontal angle in radians
        @param vertical The vertical angle in radians
        @return The spherical vector
        """
        direction: Direction = Direction.Radians(horizontal, vertical)
        r: Spherical = Spherical(distance, direction)
        return r

    @staticmethod 
    def FromVector3(v: Vector3):
        """! Create a Spherical coordinate from a Vector3 coordinate
        @param v The vector coordinate
        @return The spherical coordinate
        """
        distance = v.Magnitude()
        if distance == 0:
            return Spherical(0, Angle(), Angle())
        
        verticalAngle = Angle.Radians((math.pi / 2 - math.acos(v.up / distance)))
        horizontalAngle = Angle.Radians(math.atan2(v.right, v.forward))
        return Spherical(distance, Direction(horizontalAngle, verticalAngle))
    
    def ToVector3(self) -> Vector3:
        """! Convert the spherical coordinate to a Vector3 coordinate
        @return The vector coordinate
        """
        verticalRad = (math.pi / 2) - self.direction.vertical.InRadians()
        horizontalRad = self.direction.horizontal.InRadians()

        cosVertical = math.cos(verticalRad)
        sinVertical = math.sin(verticalRad)
        cosHorizontal = math.cos(horizontalRad)
        sinHorizontal = math.sin(horizontalRad)

        right = self.distance * sinVertical * sinHorizontal
        up = self.distance * cosVertical
        forward = self.distance * sinVertical * cosHorizontal
        return Vector3(right, up, forward)

    def __eq__(self, other) -> bool:
        """! Check if this vector is equal to the given vector
        @param v The vector to check against
        @return true if it is identical to the given vector
        @note This uses float comparison to check equality which may have strange
        effects. Equality on floats should be avoided.
        """
        return (
            self.distance == other.distance and
            self.direction == other.direction
        )
    def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
        return (
            math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
            self.direction.isclose(other.direction, rel_tol, abs_tol)
        )

    def Normalized(self) -> float:
        if self.distance == 0:
            return Spherical(0, self.direction)
        
        return Spherical(1, self.direction)        

    def __neg__(self):
        """! Negate the vector
        @return The negated vector
        This will negate the direction. Distance will stay the same.
        """
        return Spherical(self.distance, self.direction.Inverse())
    
    def __sub__(self, other):
        """! Subtract a spherical vector from this vector
        @param other The vector to subtract
        @return The result of the subtraction
        """
        v1 = self.ToVector3()
        v2 = other.ToVector3()
        r = v1 - v2
        return Spherical.FromVector3(r)

    def __add__(self, other):
        """! Add a spherical vector to this vector
        @param other The vector to add
        @return The result of the addition
        """
        v1 = self.ToVector3()
        v2 = other.ToVector3()
        r = v1 + v2
        return Spherical.FromVector3(r)
    
    def __mul__(self, factor):
        """! Scale the vector uniformly up
        @param factor The scaling factor
        @return The scaled vector
        @remark This operation will scale the distance of the vector. The angle
        will be unaffected.
        """
        return Spherical(
            self.distance * factor,
            self.direction
        )

    def __truediv__(self, factor):
        """! Scale the vector uniformly down
        @param factor The scaling factor
        @return The scaled vector
        @remark This operation will scale the distance of the vector. The angle
        will be unaffected.
        """
        return Spherical(
            self.distance / factor,
            self.direction
        )

    @staticmethod
    def Distance(v1, v2) -> float:
        """! Calculate the distance between two spherical coordinates
        @param s1 The first coordinate
        @param s2 The second coordinate
        @return The distance between the coordinates in meters
        """
        v1: Vector3 = v1.ToVector3()
        v2: Vector3 = v2.ToVector3()
        distance: float = Vector3.Distance(v1, v2)
        return distance

    @staticmethod
    def Angle(s1, s2) -> Angle:
        """! Calculate the unsigned angle between two spherical vectors
        @param s1 The first vector
        @param s2 The second vector
        @return The unsigned angle between the vectors [0..179.9999999..., -180]
        @remark the strange range is caused by the 2s complement signed values
        which has range [minvalue..maxvalue). This is a hardware limitation,
        not something we can change.
        """
        v1: Vector3 = s1.ToVector3()
        v2: Vector3 = s2.ToVector3()
        angle: Angle = Vector3.Angle(v1, v2)
        return angle
    
    def SignedAngle(s1, s2, axis) -> Angle:
        """! Calculate the signed angle between two spherical vectors
        @param s1 The first vector
        @param s2 The second vector
        @param axis The axis around which the angle is calculated
        @return The signed angle between the vectors
        """
        v1 = s1.ToVector3()
        v2 = s2.ToVector3()
        v_axis = axis.ToVector3()
        angle = Vector3.SignedAngle(v1, v2, v_axis)
        return angle
    
    @staticmethod
    def Rotate(s, horizontal: Angle, vertical: Angle):
        """! Rotate a spherical vector
        @param s The vector to rotate
        @param horizontal The horizontal rotation angle in local space
        @param vertical The vertical rotation angle in local space
        @return The rotated vector
        """
        direction: Direction = Direction(
            s.direction.horizontal + horizontal,
            s.direction.vertical + vertical
        )
        r = Spherical(s.distance, direction)
        return r

    def __repr__(self):
        return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"

Spherical.zero = Spherical(0, Direction.zero)