""" Titre : Formula Allcode
Module ecrit par Pascal HASSENFORDER
version : V1.10
Date 13/09/2022
"""

from serial import *

import itertools
import winreg


def ouvre_port(COM):
    """Se connecte en Bluetooth au robot par émulation d'un port

    Argument :
            entier : numéro du port série
    Retour :
            adresse du port série ouvert à 115200 bauds"""

    if COM=="":
        sys.exit()

    try :
        port_serie=Serial(port=COM, baudrate=115200,parity=PARITY_NONE, stopbits=STOPBITS_ONE,\
                            bytesize=EIGHTBITS,timeout=1, writeTimeout=1)
        if port_serie.isOpen():
                print(port_serie.readline().decode('UTF-8'))
                return False,port_serie
    except :
        print(COM,'n\'est pas un port série valide')
        return True,False


def enumerate_serial_ports():
    path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
    key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
    ports = []
    for i in itertools.count():
        try:
            val = winreg.EnumValue(key, i)
            ports.append(str(val[1])+" : "+str(val[0])+'\n')
        except EnvironmentError:
            break
    ports.sort()
    message=''
    for ligne in ports:
##        if "BthModem0" in ligne:
##            ouvre_port(ligne[0:4])
##        else:
##            print("Erreur de connexion")
        message=message+ligne
    return(message+'\nIndiquer le numéro du sort série à utiliser :')

ports=enumerate_serial_ports()
# Ouverture du port série RS-232 du PC branché au système


def vider_buffer():
    while port_serie.inWaiting()>0:
        message = port_serie.readline()
        print("buffert = ",message.decode())


def attendre_reponse():
    w=port_serie.inWaiting()
    while w==0:
        w=port_serie.inWaiting()
    return

def EncoderReset():
    """Resets the motor encoder counters
    """
    port_serie.write(bytearray([0, 14]))
    return;


def EncoderRead(index):
    """Reads one of the motor encoder counters
    Approx 0.328296mm of travel per encoder unit.

    Args:
    index: value of encoder (0=Left, 1=Right)

    Returns:
    int: Value of the encoder counter

    """
    port_serie.write(bytearray([0, 15, index]))
    ret = port_serie.read(2)
    r = ret[1]<<8 | ret[0]
    return(r);

def gauche(angle):
    """virage à gauche

    Argument :
            angle : entier qui détermine l'angle de rotation en degres"""

    message='Left {0}\r\n'.format(angle)
    port_serie.write(message.encode())
    attendre_reponse()
    message = port_serie.readline()
    return

def droite(angle):

    """virage à droite

    Argument :
            angle : entier qui détermine l'angle de rotation en degres"""

    message='Right {0}\r\n'.format(angle)
    port_serie.write(message.encode())
    w=port_serie.inWaiting()
    attendre_reponse()
    message = port_serie.readline()

def avance(distance):
    """Faire avancer le robot tout droit

    Argument :
         distance : entier qui détermine la distance en milimètres
         compris entre 1 et 1000 mm"""
    vider_buffer()
    message='Forwards {0}\r\n'.format(distance)
    port_serie.write(message.encode())
    attendre_reponse()
    message = port_serie.readline()

def recule(distance):
    """Faire reculer le robot tout droit

    Argument :
         distance : entier qui détermine la distance en milimètres
         compris entre 1 et 1000 mm"""
    vider_buffer()
    message='Backwards {0}\r\n'.format(distance)
    port_serie.write(message.encode())
    attendre_reponse()
    message = port_serie.readline()

def distance_IR(capteur):
    """Lit et retourne la distance détectée par un capteur

    Argument :
         capteur : entier qui sélectionne le capteur :
                0 : gauche
                1 : avant gauche
                2 : avant centre
                3 : avant droit
                4 : droit
                5 : arrière droit
                6 : arrière centre
                7 : arrière gauche

    Retourne :
        un entier compris entre 0 et 4096 :
                   0 : le capteur ne détecte aucun obstacle
                4096 : le capteur se trouve contre obstacle"""
    vider_buffer()
    message='ReadIR {0}\r\n'.format(capteur)
    port_serie.write(message.encode())
    attendre_reponse()
    message = port_serie.readline()
##    while message.decode().strip()=='':
##        message='ReadIR {0}\r\n'.format(capteur)
##        port_serie.write(message.encode())
##        attendre_reponse()
##        message = port_serie.readline()
##        print('rien')

    return int(message.decode().strip())

def moteurs(gauche,droit):
    """Commander les moteurs gauche et droit :

    Arguments :
            gauche : puissance du moteur gauche
            droit  : puissance du moteur droit

    Valeurs :
            entier compris entre -100 à 100
            les valeurs négatives font reculer le robot
            les valeurs positives font avancer le robot"""

    message='SetMotors {0} {1}\r\n'.format(gauche,droit)
    port_serie.write(message.encode())


def leds(octet):

    """"Allume les leds vertes en formant un octet

    Argument :
        octet : valeur comprise entre 0 et 255
                ou en formant un mot binaire ex : 0b10010101"""

    message='LEDWrite {0}\r\n'.format(octet)
    port_serie.write(message.encode())

def allumer_led(numero):
    """Allumer une seule led

    Arguments :
            entier compris entre 1 et 8"""
    if 0<numero<=8:
        message='LedOn {0}\r\n'.format(numero-1)
        port_serie.write(message.encode())
    else:
        print('ERREUR ! allumer_led(x) x doit être compris entre 1 et 8')

def eteindre_led(numero):
    """éteindre une seule led

    Arguments :
            entier compris entre 1 et 8"""

    if 0<numero<=8:
        message='LedOff {0}\r\n'.format(numero-1)
        port_serie.write(message.encode())
    else:
        print('ERREUR ! eteindre_led(x) x doit être compris entre 1 et 8')

def jouer(note,duree):
    """Jouer une note durant un temps déterminé

    Arguments:
            note : fréquence de la note, valeur comprise entre 1 et 10000 Hz
            duree : durée de la note, valeur comprise entre 1 et 10000 ms"""

    message='PlayNote {0} {1}\r\n'.format(note,duree)
    port_serie.write(message.encode())


def etat_BP(cote):

    """Lit et retourne l'état du bouton poussoir gauche ou droit

    Arguments :
            cote : une chaîne de caractères
                - "gauche" : retourne l'état du bouton poussoir gauche
                - "droit" : retourne l'état du bouton poussoir droit
    Retourne :
            Un entier   0 : BP non activé
                        1 : BP activé"""

    message=""
    if cote=='gauche':
        message='ReadSwitch 0\r\n'
    if cote=='droit':
        message='ReadSwitch 1\r\n'
    if message=='':
        print('erreur de syntaxe, exemple : etat_BP("gauche")')
    else:
        port_serie.write(message.encode())
        message = port_serie.readline()
        return int(message.decode())


##    message='LedOff {0}\r\n'.format(numero)
##    port_serie.write(message.encode())


def capteur_ligne(cote):

    """Lit et retourne l'état du capteur de ligne gauche ou droit

    Arguments :
            cote : une chaîne de caractères
                - "gauche" : retourne l'état du capteur de ligne gauche
                - "droit" : retourne l'état du capteur de ligne droit

    Retourne :
            Une entier compris entre 0 et 4096
                0 : noir
                4096 : blanc """


    message=""
    if cote=='gauche':
        message='ReadLine 0\r\n'
    if cote=='droit':
        message='ReadLine 1\r\n'
    if message=='':
        print('erreur de syntaxe, exemple : capteur_ligne("gauche")')
    else:
        port_serie.write(message.encode())
        message = port_serie.readline()
        return int(message.decode())

def affiche_texte(x,y,texte):
    """Affiche un texte sur l'écran LCD

    Arguments :
            x : abscisse du texte, entier compris entre 0 et 127
            y : ordonnée du texte, entier compris entre 0 et 31
            texte : chaine de caractères du texte à afficher"""

    message='LcdPrint {0} {1} {2}\r\n'.format(x,y,texte)
    port_serie.write(message.encode())

def affiche_nombre(x,y,nombre):
    """Affiche un nombre entier sur l'écran LCD

    Arguments :
            x : abscisse du texte, entier compris entre 0 et 127
            y : ordonnée du texte, entier compris entre 0 et 31
            nombre : chaine de caractères du texte à afficher"""

    message='LcdNumber {0} {1} {2}\r\n'.format(x,y,nombre)
    port_serie.write(message.encode())

def efface_Lcd():
    message='LCDClear\r\n'
    port_serie.write(message.encode())

def retroeclairage(valeur):
    """détermine la luminosité du rétro éclairage de l'afficheur LCD

        Paramètre : entier compris entre 0 et 100"""

    message='LCDBacklight {0}\r\n'.format(int(valeur))
    port_serie.write(message.encode())


def capteur_lumiere():
    """Retourne un entier compris entre 0 et 4095 représentant
    la valeur du capteur de luminosité

        Paramètre : aucun"""

    message='ReadLight\r\n'
    port_serie.write(message.encode())
    message = port_serie.readline()
    return int(message.decode())

def capteur_micro():
    """Retourne un entier compris entre 0 et 4095 représentant
    la valeur du micro

        Paramètre : aucun"""

    message='ReadMic\r\n'
    port_serie.write(message.encode())
    message = port_serie.readline()
    return int(message.decode())


def accelerometre(axe):
    """Affiche un nombre entier sur l'écran LCD

    Arguments :
            x : abscisse du texte, entier compris entre 0 et 127
            y : ordonnée du texte, entier compris entre 0 et 31
            nombre : chaine de caractères du texte à afficher"""
    if axe=='x' or axe=='y' and axe=='z':
        if axe=="x": canal=0
        elif axe=="y": canal=1
        else : canal=2
        message='ReadAxis {0}\r\n'.format(nbr)
        port_serie.write(message.encode())
        message = port_serie.readline()
        return int(message.decode())
    else:
        print('erreur de syntaxe : ecrire accelerometre("x") par exemple')

###################################################
# Vérification de la connexion avec le port série #
###################################################
port_serie=''
def connecter():
    """Début de votre programme"""
    global port_serie
    boucle = True

    while boucle:
        port=input(ports)
        boucle, port_serie=ouvre_port('com'+port)


def deconnecter():
    """Fin du programme"""
    port_serie.close()

def attendre(t):
    """attend x secondes avant de continuer le programme.
       Exemple : attendre(5) pour 5s d'attente"""
    time.sleep(t)


if __name__ == '__main__':
    '''Test moteur + capteurs'''
    connecter()
    gauche(90)
    avance(50)
    recule(50)
    droite(90)
    moteurs(50,50)
    while distance_IR(2)<200:
        moteurs(50,50)
    moteurs(0,0)

    Consigne = 50
    vitesse = 50
    moteurs(vitesse,vitesse)
    print ("\ndistances")
    mvt=0
    mg=distance_IR(0)
    while mvt<6:
        while (mg>50 and distance_IR(2)<200):
            mgn=int(1015.6*mg**-0.518)
            moteurs(vitesse,mgn)
            mg=distance_IR(0)
            #moteurs(vitesse,vitesse+ecart)
        moteurs(0,0)
        print ('IR0 =',mg,'IR2 =',distance_IR(2))
        if distance_IR(2)<50:
            avance(80)
            gauche(90)
            avance(100)
            mg=distance_IR(0)

        else:
            droite(90)
            mg=distance_IR(0)

        mvt+=1

    deconnecter()



# FIN DU SCRIPT