from Thing import Thing
from LinearAlgebra.Spherical import Spherical
from LinearAlgebra.Direction import Direction
from LinearAlgebra.Angle import Angle

class DifferentialDrive(Thing):
    """! A thing which can move itself using a differential drive system
    """
    def __init__(self, participant = None):
        """! Create a new differential drive
        """
        super().__init__(participant)

        ## The radius of a wheel in meters
        self.wheel_radius = 1.0
        ## The distance between the wheels in meters
        self.wheel_separation = 1.0
        
        self.wheel_left: Thing = None
        self.wheel_right: Thing = None

    def SetDriveDimensions(self, wheel_diameter, wheel_separation):
        if wheel_diameter < 0:
            wheel_diameter = -wheel_diameter
        self.wheel_radius = wheel_diameter / 2
        
        if wheel_separation < 0:
            wheel_separation = -wheel_separation
        self.wheel_separation = wheel_separation
        
        self.SetMotors(self.wheel_left, self.wheel_right)

    def SetMotors(self, wheel_left, wheel_right):
        distance: float = self.wheel_separation / 2

        self.wheel_left = wheel_left
        if self.wheel_left is not None:
            self.wheel_left.SetPosition(Spherical(distance, Direction.Left))

        self.wheel_right = wheel_right
        if self.wheel_right is not None:
            self.wheel_right.SetPosition(Spherical(distance, Direction.Right))

    def SetWheelVelocity(self, speed_left, speed_right):
        """! Directly specify the speeds of the motors
        @params speed_left The speed of the left wheel in degrees per second. Positive moves the robot in the forward direction.
        @params speed_right The speed of the right wheel in degrees per second. Positive moves the robot in the forward direction.
        """
        if self.wheel_left is not None:
            self.wheel_left.SetAngularVelocity(Spherical(speed_left, Direction.Left))
        
        if self.wheel_right is not None:
            self.wheel_right.SetAngularVelocity(Spherical(speed_right, Direction.Right))
        
        speed = self.wheel_radius * (speed_left + speed_right) / 2 * Angle.Deg2Rad
        self.SetLinearVelocity(Spherical(speed, Direction.forward))
        steer = self.wheel_radius * (speed_left - speed_right) / self.wheel_separation
        self.SetAngularVelocity(Spherical(steer, Direction.up))

    def Update(self, currentTimeMs, recursive = True):
        """!
        @copydoc Thing::Update
        """
        # This assumes forward velocity only...
        linear_velocity: Spherical = self.linear_velocity.distance

        angular_velocity: Spherical = self.angular_velocity
        angular_speed: float = angular_velocity.distance * Angle.Deg2Rad # in radians/sec
        # Determine the rotation direction
        if angular_velocity.direction.horizontal < 0:
            angular_speed = -angular_speed
        
        if self.wheel_left is None or self.wheel_right is None:
            return
        
        speed_left: float = 0 if self.wheel_left is None else \
            (linear_velocity + angular_speed * self.wheel_left.position.distance) / self.wheel_radius * Angle.Rad2Deg
        speed_right: float = 0 if self.wheel_right is None else \
            (linear_velocity - angular_speed * self.wheel_right.position.distance) / self.wheel_radius * Angle.Rad2Deg

        self.SetWheelVelocity(speed_left, speed_right)

        super().Update(currentTimeMs)