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

class DifferentialDrive(Thing):
    def __init__(self, participant = None):
        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 = None
        self.wheel_right = 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):
        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))

    def Update(self, currentTimeMs, recursive = True):
        # This assumes forward velocity only...
        linear_velocity: Spherical = self.GetLinearVelocity().distance

        angular_velocity: Spherical = self.GetAngularVelocity()
        angular_speed: float = angular_velocity.distance * Angle.Deg2Rad # in radians/sec
        # Determine the rotation direction
        if angular_velocity.direction.horizontal.InDegrees() < 0:
            angular_speed = -angular_speed
        
        speed_left: float = (linear_velocity + angular_speed * self.wheel_left.GetPosition().distance) / self.wheel_radius * Angle.Rad2Deg
        speed_right: float = (linear_velocity - angular_speed * self.wheel_right.GetPosition().distance) / self.wheel_radius * Angle.Rad2Deg

        self.SetWheelVelocity(speed_left, speed_right)