import cozmo
import time
from cozmo.util import degrees, distance_mm, speed_mmps


def cozmo_program(robot: cozmo.robot.Robot):
    # robot.say_text("Hi Martina").wait_for_completed()
    # Drive forwards for 150 millimeters at 50 millimeters-per-second.
    # robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()

    # Turn 90 degrees to the left.
    # Note: To turn to the right, just use a negative number.
    # robot.turn_in_place(degrees(90)).wait_for_completed()

    print("fov_x ", robot.camera.config.fov_x)
    print("fov_y ", robot.camera.config.fov_y)
    print("focal_length ", robot.camera.config.focal_length)

    cozmo = CozmoDriver(robot)
    cozmo.updateLift()
    cozmo.drive()

    turn_speed = 30
    forward_speed = 50

    l_wheel_speed = (0 * forward_speed) + (turn_speed * 1)
    r_wheel_speed = (0 * forward_speed) - (turn_speed * 1)
    robot.drive_wheels(l_wheel_speed,r_wheel_speed,l_wheel_speed *4,r_wheel_speed *4)
    time.sleep(9.5)



class CozmoDriver:
    """ Contains all functions that will be used to move cozmo
    """

    def __init__(self, robot: cozmo.robot.Robot):
        self.speed = 50
        self.turnSpeed = 30
        self.robot = robot
        self.initializeDriver()

    def initializeDriver(self):
        """ Set Cozmo in the starting position
        """
        self.updateLift()
        self.updateHead()

    def drive(self):
        """ Start driving
        """
        self.robot.drive_wheel_motors(self.speed, self.speed)

    def driveLeft(self):
        """ Drive to left
        """
        self.robot.drive_wheel_motors(self.speed - 15, self.speed)

    def driveRight(self):
        """ Drive to right
        """
        self.robot.drive_wheel_motors(self.speed, self.speed - 15)

    def updateLift(self):
        """ Set Lift to maximum height
        """
        self.robot.set_lift_height(1).wait_for_completed()


    def updateHead(self):
        """ Set Head to minimum angle
        """
        self.robot.set_head_angle(cozmo.robot.MIN_HEAD_ANGLE).wait_for_completed()

cozmo.run_program(cozmo_program)