# -*- coding: utf-8 -*-
"""
Created on Mon Apr 03 22:32:10 2017

@author: PC Alexandre.Ribault
"""
import socket
import time

def Capteur_IR1():
    global monSocket_Emission     
    
    flag=0 
    while (flag==0):
        data, addr = monSocket_Reception.recvfrom(2048)
        if Conversion_en_INT(data,1)>=1 and ord(data[0])==1  : #si le capteur 1 du robotino voit un mur
            flag=1 # on sort de la boucle en mettant le drapeau à 1
            monSocket_Emission.sendto(ConversionUdp(0,0,0,0,0,0,1,0,0), (UDP_IP, UDP_PORT_EMISSION))  # on en envoie MESSAGE à robotino (ici le frein)
    
    MESSAGE=ConversionUdp(1,0,0,0,0,0,0,0,0)#On desactive le frein, on supprime les commandes déplacement #on demande le passage à l'étape 2
    monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION))
    time.sleep(0.05)
    
def Angle_optimum():
   global monSocket_Emission
   global monSocket_Reception
   Distance_Angle=120*[0]# Création d'un tableau permettant de stocker jusqu'a 120 informations : la valeur renvoyé par le capteur 1
   flag=0
   flag2=0
   i=0
   angle=0
    
   while(i<=119) and (flag2==0): # on réalise jusqu'à 120 test : de 0 à 199 * 3 = 360 *, soit un tour complet du robotino
       MESSAGE=ConversionUdp(0,0,0,angle,2,1,0,0,0)
       monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION)) # On demande un déplacement de 3 degrées
       time.sleep(0.05)
       MESSAGE=ConversionUdp(0,0,0,angle,2,0,0,0,0) # On demande un déplacement de 3 degrées. LA consigne prend sur un front descendant de l'entrée 4
       monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION))
       # rotation en cours
       data='nullllllllll'
       while flag ==0 :
           if (ord(data[0])==1) and (Conversion_en_INT(data,0)==1): # si la sortie 0 du message 1 est égale à 1("orientation atteinte")
               flag=1
               Distance_Angle[i]=Conversion_en_INT(data,1) # on enregistre dans le tableau la valeur lue par le capteur 1
               print  "distance i: " , Distance_Angle[i] # on affiche dans la console la valeur lue parle capteur 1
               angle+=3 # on incrémentente la consigne
               if ((Distance_Angle[i]-Distance_Angle[i-1])<=0): # on compare la valeur lue à l'instant à celle lue le tour d'avant. Si la différence est négative :  la position est moins bonne. 
                   flag2=1
                   i-=2 # On devrait faire -1 , mais comme je le réincrémente juste apres , je soustrait 2
               i+=1
           else:
               data, addr = monSocket_Reception.recvfrom(2048)
       print "angle: ", angle
       flag=0
       
   return i

def ConversionUdp(ID_Message,int0,int1,int2,int3,int4,int5,int6,int7):
    TAB_BYTE_HEX =[ '0', '1', '2', '3', '4', '5', '6','7','8', '9', 'a', 'b', 'c', 'd', 'e','f' ]		
    CH_int=[0,0,0,0,0,0,0,0]
    somme=36
    tampon_Byte0=0
    tampon_Byte1=0
    tampon_Byte2=0
    tampon_Byte3=0
    tampon2=""
    chaine2=''
    if ID_Message==0:
        chaine2="002400"
    if  ID_Message==1:
        chaine2="012400"
        somme+=1
        
    Valeur_entrees2=""
    

    CH_int=[int0,int1,int2,int3,int4,int5,int6,int7]
    i=0
    while(i<8) :
        if (CH_int[i]<0):
            CH_int[i]=(2147483647+ CH_int[i] +1)
            CH_int[i]=(2147483647+ CH_int[i] +1)		
    			
        tampon_Byte3=(CH_int[i]/(256*256*256))
        tampon2+=TAB_BYTE_HEX[(tampon_Byte3/16)]
        somme+=tampon_Byte3
        			
        CH_int[i]=CH_int[i]%(256*256*256);
        tampon_Byte2=(CH_int[i]/(256*256))
        tampon2+=TAB_BYTE_HEX[tampon_Byte2/16]
        tampon2+=TAB_BYTE_HEX[tampon_Byte2%16]
        somme+=tampon_Byte2
        			
        CH_int[i]=CH_int[i]%(256*256);
        tampon_Byte1=(CH_int[i]/(256))
        tampon2+=TAB_BYTE_HEX[tampon_Byte1/16]
        tampon2+=TAB_BYTE_HEX[tampon_Byte1%16]
        somme+=tampon_Byte1
    			
        CH_int[i]=CH_int[i]%256
        tampon_Byte0=(CH_int[i])
        tampon2+=TAB_BYTE_HEX[tampon_Byte0/16]
        tampon2+=TAB_BYTE_HEX[tampon_Byte0%16]
        somme+=tampon_Byte0
            
        tampon_chaine2=  TAB_BYTE_HEX[tampon_Byte0/16]+TAB_BYTE_HEX[tampon_Byte0%16]+ TAB_BYTE_HEX[tampon_Byte1/16]+TAB_BYTE_HEX[tampon_Byte1%16]+ TAB_BYTE_HEX[tampon_Byte2/16]+TAB_BYTE_HEX[tampon_Byte2%16]+ TAB_BYTE_HEX[tampon_Byte3/16]+TAB_BYTE_HEX[tampon_Byte3%16];
        Valeur_entrees2+=tampon_chaine2
        i=i+1
        
    tampon_Byte0= (255-(somme%256))
    chaine2+=TAB_BYTE_HEX[(tampon_Byte0/16)]+TAB_BYTE_HEX[(tampon_Byte0%16)]+Valeur_entrees2
    return chaine2.decode("hex")

def Conversion_en_INTPart2(String_a_Massacrer):
    Int_encours =0
    Char_Tampon3=String_a_Massacrer[0]
    Char_Tampon2=String_a_Massacrer[1]
    Char_Tampon1=String_a_Massacrer[2]
    Char_Tampon0=String_a_Massacrer[3]
    if ord(Char_Tampon0)>128:
        Int_encours=-((256*256*256*256) -(ord(Char_Tampon3)+(256*ord(Char_Tampon2)) +(256*256*ord(Char_Tampon1)+ (256*256*256*ord(Char_Tampon0)))))
    else:
        Int_encours=ord(Char_Tampon3)+(256*ord(Char_Tampon2)) +(256*256*ord(Char_Tampon1)+ (256*256*256*ord(Char_Tampon0)))
    #print "Int en cours: ", Int_encours    
    return Int_encours
    
def Conversion_en_INT(String_a_Massacrer,int_a_retourner):  
    Octet_Depart=4+4*int_a_retourner
    String_a_Massacrer = String_a_Massacrer[Octet_Depart:len(String_a_Massacrer)]
    return Conversion_en_INTPart2(String_a_Massacrer)


"""____________________________________________________________________________"""



"""________________________________PROGRAMME___________________________________"""


"""____________________________________________________________________________"""
# Chargemment des valeurs du serveur

UDP_IP='localhost'
UDP_PORT_EMISSION=9182
UDP_PORT_RECEPETION= 9180

print "passage au premier deplacement"
#Premier deplacement _ ok

monSocket_Emission = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
monSocket_Reception = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

MESSAGE=ConversionUdp(0,-300,0,0,0,0,0,0,0) # on demande au robot de se déplacer sur l'axe X ( à reculons)
monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION)) # On envoie le message contenant le message
time.sleep(0.05) # On bloque durant un temps très court le programma

monSocket_Reception.bind((UDP_IP, UDP_PORT_RECEPETION)) # On configure la recepetion des message émit par le robotino 
flag=0

print "prepaparation du frein"

while (flag==0):
    data, addr = monSocket_Reception.recvfrom(2048)  
    print Conversion_en_INT(data,0) 
    if Conversion_en_INT(data,0)>=1 and ord(data[0])==1 : #si un capteur voit un obstacle
        flag=1
        monSocket_Emission.sendto(ConversionUdp(0,0,0,0,0,0,1,0,0), (UDP_IP, UDP_PORT_EMISSION)) # On freine
    else :
        print "rien"

print "Element du décors répéré" # Robotino a trouvé un mur mais on ne sait pas ou par rapport au robotino (Nord Sud Est Ouest , etc ? )

#On reinitialise les valeurs
monSocket_Emission.sendto(ConversionUdp(0,0,0,0,0,0,0,0,0), (UDP_IP, UDP_PORT_EMISSION))
time.sleep(0.05) # Le robot est à l'arrêt


# On demande un deplacement angulaire
monSocket_Emission.sendto(ConversionUdp(0,0,0,50,0,0,0,0,0), (UDP_IP, UDP_PORT_EMISSION)) # Le robotino tourne sur lui même
tentative=0
# On attend un rponse du capteur IR1 après rotation
Capteur_IR1()

#on demande le passage à l'étape 2, à l'aide de l'entrée 3
monSocket_Emission.sendto(ConversionUdp(0,0,0,0,2,0,0,0,0), (UDP_IP, UDP_PORT_EMISSION))

print "passage a l'etape 2"

#On établi une nouvelle position "zero"
monSocket_Emission.sendto(ConversionUdp(0,0,0,0,2,0,1,0,0), (UDP_IP, UDP_PORT_EMISSION))
time.sleep(0.05)

i=Angle_optimum()


Distance_au_mur= Conversion_en_INT(data,1) # Indication pour debuggage : inutile dans le programme
#print "on est a la fin"

print "on a passé la recherche d'angle optimum"        

MESSAGE=ConversionUdp(0,0,0,(i)*3,2,1,0,0,0) # On prend la position ideale
monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION)) 
time.sleep(0.05) 
MESSAGE=ConversionUdp(0,0,0,(i)*3,2,0,0,0,0)# Front descendant pour la position ideale
monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION))
data='nullllllllll'
print "distance au mur : ", Distance_au_mur

while flag ==0 :
        if (ord(data[0])==1) and (Conversion_en_INT(data,0)==1): # on lit la sortie 0 du message 1 :  Si on est dans la position de l'angle idéal
            MESSAGE=ConversionUdp(0,0,0,0,2,0,1,0,0)# Permet d'établir une nouvelle position "zero"
            monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION)) # On envoie la consigne pour être en position de référence 0
            time.sleep(0.05)
            flag=2
        else:
            data, addr = monSocket_Reception.recvfrom(2048) # Si on n'a pas la valeur souhaitée on relis le message (1 ou 0)
print "on est en positiotn ideale, et c'est la position de reference"

# On va suivre un un mur. Pour ça on enregistre "la distance" entre le mur et Robotino qui nous sert de point de référence
# Tant que l'on devie pas trop du mur (tolérence à définir), on continue d'avancer sur la droite.
# Si on dévie, on corrige l'orientation du robot.

MESSAGE=ConversionUdp(0,0,0,0,1,0,0,0,0) # On retourne à l'étape 1
monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION))  
MESSAGE=ConversionUdp(0,0,-100,0,1,0,0,0,0) # On  donne une consigne  vitesse vers la droite du robot
monSocket_Emission.sendto(MESSAGE, (UDP_IP, UDP_PORT_EMISSION)) 

""" Le robot va à droite comme je le veux """
""" Il faut encore lui demander de controler en permanence la distance """

data='nullllllllll'

while flag ==0 :
        if (ord(data[0])==1) and (Conversion_en_INT(data,0)==1): # Si le message d'ID 1 
            
            time.sleep(0.05)
            flag=2
        else:
            data, addr = monSocket_Reception.recvfrom(2048)
print "on est en positiotn ideale, et c'est la position de reference"

#On retourne à l'étape Step1
monSocket_Emission.sendto(ConversionUdp(0,0,0,0,0,1,0,0,0), (UDP_IP, UDP_PORT_EMISSION))
time.sleep(0.05)

#On reinitiliase toute les valeurs
monSocket_Emission.sendto(ConversionUdp(0,0,0,0,0,0,0,0,0), (UDP_IP, UDP_PORT_EMISSION))
time.sleep(0.05)

#On se déplace vers la droite
monSocket_Emission.sendto(ConversionUdp(0,0,-200,0,0,0,0,0,0), (UDP_IP, UDP_PORT_EMISSION))
time.sleep(0.05)

monSocket_Emission.close()
monSocket_Reception.close()
