'''
Example program for the EV3 Gyro sensor
Measure and show angle and angular speed on display.
'''
import utime

from lib.robot_consts import Button, Sensor
from ev3_sensor_ll import SensorNotReadyError, SensorMismatchError
from .display_exception import display_incorrect_sensor, display_sensor_not_connected

mode_str = (("Angle"), ("Angle and speed"), ("Tilt angle"), ("Tilt speed"))
N_MODES = len(mode_str)

def ev3_gyro_run(robot, chosen_sensor_port):
    robot.init_sensor(sensor_type=Sensor.EV3_GYRO, port=chosen_sensor_port)
    
    robot.display.fill(0)
    robot.display.centered_text(f"EV3 Gyro S{chosen_sensor_port+1}", 0, 1)
    robot.display.centered_text("Connecting...", 16, 1)
    robot.display.show()

    try:
        robot.sensors.gyro[chosen_sensor_port].angle()
    except SensorNotReadyError as e:
        print(f"EV3 Gyro S{chosen_sensor_port+1} sensor not connected")
        display_sensor_not_connected(robot)
        return
    except SensorMismatchError as e:
        print(f"EV3 Gyro S{chosen_sensor_port+1} incorrect sensor connected")
        display_incorrect_sensor(robot)
        return

    while True:
        robot.display.fill(0)
        robot.display.centered_text(f"EV3 Gyro S{chosen_sensor_port+1}", 0, 1)
        robot.display.text(mode_str[mode], 0, 10, 1)
        try:
            if mode == 0:
                angle = robot.sensors.gyro[chosen_sensor_port].angle()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
            elif mode == 1:
                angle, speed = robot.sensors.gyro[chosen_sensor_port].angle_and_speed()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
                robot.display.text(f"Speed: {speed}", 0, 24, 1)
            elif mode == 2:
                angle = robot.sensors.gyro[chosen_sensor_port].tilt_angle()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
            elif mode == 3:
                speed = robot.sensors.gyro[chosen_sensor_port].tilt_speed()
                robot.display.text(f"Speed: {speed}°", 0, 16, 1)
        except:
            print(f"EV3 Gyro S{chosen_sensor_port+1} sensor not connected")
            display_sensor_not_connected(robot)
            return
        robot.display.text('<', 0, 54, 1)
        robot.display.show()
        
        pressed = robot.buttons.pressed()
        # Exit program if left cube button is pressed
        if pressed[Button.LEFT]:
            break
        if not debounce:
            if pressed[Button.DOWN]:
                robot.sensors.gyro[chosen_sensor_port].reset_angle(0)
                mode += 1
                mode %= N_MODES
                debounce = True
                print("Mode:", mode_str[mode])
            elif pressed[Button.UP]:
                robot.sensors.gyro[chosen_sensor_port].reset_angle(0)
                mode -= 1
                mode %= N_MODES
                debounce = True
                print("Mode:", mode_str[mode])
            
        if not pressed[Button.DOWN] and not pressed[Button.UP]:
            debounce = False
        utime.sleep(0.1)