# 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

Distance = 0
DistanceMesure = 0
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)  

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
            time.sleep(.01)
        else:
            if OldState == 1:
                print("\nFin des mesures.\n")
                OldState = 0
    except KeyboardInterrupt:
        Arduino_Exit(board)
        sys.exit(0)