#!/usr/bin/env pybricks-micropython

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch

import struct

# Declare motors 
left_motor = Motor(Port.B)
right_motor = Motor(Port.D)
#steer_motor = Motor(Port.A)
left = 0
right = 0

# Dichiarazione device
device = EV3Brick()
device.speaker.set_volume(100, "_all_")
device.speaker.set_speech_options(language="it", speed=175, pitch=50)
device.speaker.say("Ciao Matteo. Mi sto preparando")

# Dichiarazione sensore ultrasuoni
#ultrasuoni = UltrasonicSensor(Port.S3)


# Auto center steering wheels.
#steer_motor.run_until_stalled(250)
#steer_motor.reset_angle(90)
#steer_motor.run_target(300,0)


# A helper function for converting stick values (0 - 255)
# to more usable numbers (-100 - 100)
def scale(val, src, dst):
    """
    Scale the given value from the scale of src to the scale of dst.
 
    val: float or int
    src: tuple
    dst: tuple
 
    example: print(scale(99, (0.0, 99.0), (-1.0, +1.0)))
    """
    return (float(val-src[0]) / (src[1]-src[0])) * (dst[1]-dst[0])+dst[0]


# Open the Gamepad event file:
# /dev/input/event3 is for PS3 gamepad
# /dev/input/event4 is for PS4 gamepad
# look at contents of /proc/bus/input/devices if either one of them doesn't work.
# use 'cat /proc/bus/input/devices' and look for the event file.
infile_path = "/dev/input/event4"

# open file in binary mode
in_file = open(infile_path, "rb")

# Read from the file
# long int, long int, unsigned short, unsigned short, unsigned int
FORMAT = 'llHHI'    
EVENT_SIZE = struct.calcsize(FORMAT)
event = in_file.read(EVENT_SIZE)

device.speaker.say("Tutto fatto, adesso sono pronto")

while event:
    (tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event)
    print("Event")
    
    #if ev_type == 1: # A button was pressed or released.
        #if code == 310 and value == 0: #L1
        #    steer_motor.reset_angle(steer_motor.angle()-5)
        #if code == 311 and value == 1: #R1
        #    left = scale(value, (0,255), (100,-100))
        #    right = scale(value, (0,255), (100,-100))
        
        #if code == 312 and value == 1: #L2
        #    left = scale(value, (0,255), (100,-100))
        #    right = scale(value, (0,255), (100,-100))
        #elif code == 312 and value == 0: #L2
        #    left = scale(value, (0,255), (0,0))
        #    right = scale(value, (0,255), (0,0))
        #elif code == 313 and value == 1: #R2
        #    left = -scale(value, (0,255), (100,-100))
        #    right = -scale(value, (0,255), (100,-100))
        #    #device.speaker.beep(frequency=500, duration=-1)
        #elif code == 313 and value == 0: #R2 rilasciato
        #    left = -scale(value, (0,255), (0,0))
        #    right = -scale(value, (0,255), (0,0))
        #    #device.speaker.beep(frequency=500, duration=0)
            
    if ev_type == 3: # Stick was moved
        if code == 3: #right stick horizontal?
            if value > 0:
                left = -scale(value, (0,255), (100,-100))
                right = -scale(value, (0,255), (0,0))
            else:
                right = -scale(value, (0,255), (100,-100))
                left = -scale(value, (0,255), (0,0))
            #left = scale(value, (0,255), (40, -40))
        if code == 1: # left stick vertical
            left = -scale(value, (0,255), (100,-100))
            right = -scale(value, (0,255), (100,-100))
        
    # Set motor voltages. 
    left_motor.dc(left)
    right_motor.dc(right)

    # Track the steering angle
    #steer_motor.track_target(left)

    # Finally, read another event
    event = in_file.read(EVENT_SIZE)

in_file.close()