import pypot
import time
%pylab inline
import cv2
import numpy as np 
import matplotlib.pyplot as plt
import requests
import json
#initialisation de l'ergo junior
from poppy_ergo_jr import PoppyErgoJr
ergo = PoppyErgoJr()

liste1={'nombre':0,'ok':"non"}
presentecam=open("presentecam.json","w")
json.dump(liste1,presentecam,indent=4)
presentecam.close()
balls="0"
approche={'m1':-90,'m2': -70}
prend1={'m1':0}
prend2={'m2':6,'m3':18 ,'m4':5,'m5': 30,'m6':20}
leve={'m1':0,'m2': -90,'m3':112, 'm4':0,'m5':-90,'m6':-5}
vert={'m1':90,'m2': -20,'m3':70, 'm4':3,'m5':-70}
jaune={'m1':30,'m2': -20,'m3':70, 'm4':3,'m5':-70}
orange={'m1':-40,'m2': -20,'m3':70, 'm4':3,'m5':-70}
magenta={'m1':60,'m2': -20,'m3':70, 'm4':3,'m5':-70}
cyan={'m1':-20,'m2': -20,'m3':70, 'm4':3,'m5':-70}
lache={'m6':20}
ergo.compliant = False

while(balls==0):
    content=requests.get("http://orkidee.local:8888/files/color.json")
    data=content.json()
    balls=data['numero']
        
for i in range(1,4):
    #prise de balle sur la rampe
    ergo.m1.goal_position=0
    time.sleep(0.5)
    ergo.goto_position(prend1,2,wait=True)
    ergo.goto_position(prend2,2,wait=True)
    ergo.m6.goal_position=-5
    time.sleep(3)
    ergo.m2.goal_position=-10
    
    #présente la balle à orkidee
    ergo.goto_position(leve,2,wait=True)
    ergo.goto_position(approche,2,wait=True)
    time.sleep(1)
    liste1={'nombre':"?",'ok':"oui"}     
    presentecam=open("presentecam.json","w")
    json.dump(liste1,presentecam,indent=4)
    presentecam.close()
    r="non"
    
    #boucle pour attendre que la couleur soit détectée par orkidee
    while(r=="non"):
        content=requests.get("http://orkidee.local:8888/files/color.json")
        data=content.json()
        r=data['ok']
    
    #lecture de la couleur   
    content=requests.get("http://orkidee.local:8888/files/color.json")
    data=content.json()
    t=data['couleur']
    liste1={'nombre':0,'ok':"non"}
    presentecam=open("presentecam.json","w")
    json.dump(liste1,presentecam,indent=4)
    presentecam.close()
    #balle dans le verre en fonction de sa couleur
    print (t),
    
    if(t=="vert"):        
        ergo.goto_position(vert,2,wait=True)
    if(t=="jaune"):        
        ergo.goto_position(jaune,2,wait=True)
    if(t=="orange"):        
        ergo.goto_position(orange,2,wait=True)
    if(t=="magenta"):        
        ergo.goto_position(magenta,2,wait=True)
    if(t=="cyan"):        
        ergo.goto_position(cyan,2,wait=True)
    ergo.goto_position(lache,2,wait=True)
    time.sleep(3)
    ergo.goto_position(leve,1,wait=True)
    liste1={'nombre':i,'ok':"oui"}
    presentecam=open("presentecam.json","w")
    json.dump(liste1,presentecam,indent=4)
    presentecam.close()
    print(i)