import time

# Library imports
from vex import *

class Pierre:
    def __init__(self) -> None:
        self.brain = Brain()
        self.brain_inertial = Inertial()
        self.left_drive_smart = Motor(
            Ports.PORT1,
            GearSetting.RATIO_2_1,
            False)
        self.right_drive_smart = Motor(
            Ports.PORT6,
            GearSetting.RATIO_2_1,
            True)
        self.drivetrain = SmartDrive(
            lm=self.left_drive_smart, 
            rm=self.right_drive_smart, 
            g=self.brain_inertial, 
            wheelTravel=200,
            trackWidth=292,
            wheelBase=127,
            units=DistanceUnits.MM,
            externalGearRatio=2.0,
        )
        # touchled_2 = Touchled(Ports.PORT2)
        # optical_3 = Optical(Ports.PORT3)
        self.color_sensor = ColorSensor(Ports.PORT3)
        # distance_7 = Distance(Ports.PORT7)
        # bumper_8 = Bumper(Ports.PORT8)
        self.velocity = 50
        self.brain.buttonLeft.pressed(self.velocity_change, (-10,))
        self.brain.buttonRight.pressed(self.velocity_change, (10,))
        self.brain.buttonCheck.pressed(self.forward_routine)

    def action_loop(self):
        while True:
            #brain.screen.set_cursor(1, 1)
            #brain.screen.print("Hue " + str(color_3.hue()))
            #brain.screen.set_cursor(2, 1)
            #brain.screen.print("Brt "  + str(color_3.brightness()))
            #brain.screen.set_cursor(3, 1)
            #brain.screen.print("Obj " + str(color_3.is_near_object()))
            time.sleep(.05)
                    
    def calibrate_drivetrain(self):
        # Calibrate the Drivetrain Inertial
        sleep(200, MSEC)
        self.brain.screen.print("Calibrating")
        self.brain.screen.next_row()
        self.brain.screen.print("Inertial")
        self.brain_inertial.calibrate()
        while self.brain_inertial.is_calibrating():
            sleep(25, MSEC)
        self.brain.screen.clear_screen()
        self.brain.screen.set_cursor(1, 1)
        self.brain.screen.print("Pierre is ready.")

    def forward_routine(self) -> None:
        self.brain.screen.set_cursor(1, 1)
        self.brain.screen.print("driving routine")
        self.play_confirm_sound()
        
        self.drivetrain.set_drive_velocity(self.velocity, VelocityUnits.PERCENT)
        self.drivetrain.set_turn_velocity(self.velocity, VelocityUnits.PERCENT)
        for _ in range(4):
            self.brain.screen.clear_line(2)
            self.brain.screen.set_cursor(2, 1)
            self.brain.screen.print("straight")
            self.drivetrain.drive_for(DirectionType.FORWARD, 20.0, DistanceUnits.IN, wait=True)

            self.play_pause_sound()
            self.brain.screen.clear_line(3)
            self.brain.screen.set_cursor(3, 1)
            self.brain.screen.print("turn")
            self.drivetrain.turn_for(
                direction=TurnType.LEFT,
                angle=90,
                wait=True)

        self.brain.screen.set_cursor(2, 1)
        self.brain.screen.print("Complete.")
        self.play_complete_sound()

    def play_confirm_sound(self) -> None:
        self.brain.play_note(1, 1, 100)
        self.brain.play_note(1, 2, 100)
        self.brain.play_note(1, 3, 100)
        self.brain.play_note(1, 6, 100)
        self.brain.play_note(1, 4, 100)

    def play_complete_sound(self) -> None:
        self.brain.play_note(1, 5, 100)
        time.sleep(0.2)
        self.brain.play_note(1, 5, 400)

    def play_pause_sound(self) -> None:
        self.brain.play_note(1, 7, 100)
        time.sleep(0.2)
        self.brain.play_note(1, 6, 200)
        time.sleep(0.2)
        self.brain.play_note(1, 5, 300)
        
    def velocity_change(self, delta: int) -> None:
        self.velocity -= delta
        if self.velocity < 10:
            self.velocity = 10
        elif self.velocity > 100:
            self.velocity = 100
        self.brain.screen.set_cursor(1, 1)
        self.brain.screen.print("Velocity: " + str(self.velocity) + "%")
        

    
def main() -> None:
    # Brain should be defined by default
    pierre = Pierre()
    pierre.calibrate_drivetrain()
    pierre.action_loop()

#drivetrain.drive_for(FORWARD, 10)
#drivetrain.drive_for(REVERSE, 10)
#color_3.set_light(LedStateType.ON)
if __name__ == "__main__":
    main()