# Importations des librairies et définition des fonctions

from PymataExpressDef import *
from ConnectToArduino import *
import time
from math import *

async def Get_Dt(data):
    global Dt
    Dt = data[1]*56
    
def Set_Sonar_Pins(board, trigger_pin, echo_pin):
    loop.run_until_complete(board.set_pin_mode_sonar(trigger_pin, echo_pin, Get_Dt))
    
def Sonar_Read(board, trigger_pin):
    loop.run_until_complete(board.sonar_read(trigger_pin))


# Déclaration des constantes et variables

TRIGGER_PIN = 8 
ECHO_PIN = 9
PinButton = 7
PinTone = 3
PinLed = 4

Distance=0
DistanceMesure = 0
DistanceAlarme = 10
Dt=0
Temp = 20
VitTheoUS = 20.05*(sqrt(Temp+273.15))

ValButton=0
OldValButton=0
State = 0
OldState = 0


# Connexion à l'Arduino
             
PortComArduino = SelectPortCOM()
board = OpenPortCom(PortComArduino)


# Connexion à l'Arduino réussie - Déclaration des entrées et sorties

Set_Sonar_Pins(board, TRIGGER_PIN, ECHO_PIN)
Set_DigitalInput_Pin(board, PinButton)
Set_Tone_Pin(board, PinTone)
Set_DigitalOutput_Pin(board, PinLed)
    
print("Connexion à l'arduino établie - Appuyez sur Ctrl-C pour quitter.\n")


# Boucle principale du programme

print("Appuyez sur le bouton poussoir pour mesurer la distance entre le capteur et l'obstacle.\n")

while True:
    try:
        ValButton = Digital_Read(board, PinButton)
        time.sleep(0.01)
        if ValButton == 1 and OldValButton==0:
            State = 1 - State
        OldValButton = ValButton
        if State==1:
            if OldState == 0:
                print("Mesure de la distance en cours.\n")
                print("Distance (cm):")
                OldState=1                   
            Sonar_Read(board,TRIGGER_PIN)
            Distance= int(Dt/2 * VitTheoUS/10000)
            if DistanceMesure != Distance:
                print(Distance)
                DistanceMesure = Distance
            if Distance < DistanceAlarme:
                Tone(board, PinTone, 440, 0)
                Digital_Write(board, PinLed, 1)
                time.sleep(0.1)
                No_Tone(board, PinTone)
                Digital_Write(board, PinLed, 0)   
                time.sleep(0.1)
            else:
                No_Tone(board, PinTone)
                Digital_Write(board, PinLed, 0)
            time.sleep(.01)
        else:
            if OldState == 1:
                print("\nFin des mesures.\n")
                OldState = 0
            No_Tone(board, PinTone)
            Digital_Write(board, PinLed, 0)
            
    except KeyboardInterrupt:
        No_Tone(board, PinTone)
        Digital_Write(board, PinLed, 0)
        Arduino_Exit(board)
        sys.exit(0)
