from machine import Pin,SoftI2C
import time
from hcsr04 import HCSR04           #Driver ultrasuoni
import ssd1306                      #Driver Schermo Oled
from servoMotor import servoMotor   #Driver fatto in casa con amore per il motore servo
from stopLight import StopLight     #Driver fatto in casa con amore per il semaforo

def gate():
    global sensSbarra
    global servo
    servo.open()
    while True:
        print("Aperto")
        if(sensSbarra.distance_cm() < 10):
            print("Ti ho visto")
            while(sensSbarra.distance_cm()<= 10):
                print("Ti vedo")
                time.sleep(1)
            time.sleep(1)
            servo.close()
            print("Chiuso")
            break
        time.sleep(1)

#Inizializzazione del motore
global servo
servo = servoMotor(18)
servo.close()
print("Ho chiuso il gate")

#Inizializzazione led
#led = Pin(12,Pin.OUT)
#led.off()

#Inizializzazione Semaforo
semaforo = StopLight(PinRed = 14,PinYellow= 12, PinGreen= 13)
semaforo.default()

#Inizializzazione Oled
i2c = SoftI2C(sda=Pin(21), scl=Pin(22))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)
oled.fill(0)

#Inizializzazione Ultrasuoni
global sensSbarra
sensSbarra = HCSR04(trigger_pin=4, echo_pin=2)
sensFinecorsa = HCSR04(trigger_pin=19, echo_pin=5)

#Inizializzazione Bottone
btn = Pin(15,Pin.IN, Pin.PULL_DOWN)

while True:
    semaforo.greenLight()
    #Insert MQTT implementation here
    if (btn.value() == 1):
        semaforo.yellowLight()
        gate()
        while True:
            if(sensFinecorsa.distance_cm()<= 10):
                semaforo.redLight()
                #Riduzione numero posti
                #led.on()
                while(sensFinecorsa.distance_cm()<= 10):
                    print("Sei al finecorsa")
                    time.sleep(1)
                break
        semaforo.yellowLight()
        gate()
        #led.off()
        oled.text("Grazie",40,20)
        oled.text("ed Arrivederci",10,30)
        oled.show()
        time.sleep(2)
        
        #Aumento numero posti