from IKImage import Pos2D, ImageManager, fenetre, canvas
import tkinter as Tk
import time
import random
import math

class Serpent3DDL:
    # Le serpent avec 3 segments et 3 degres de liberte
    def __init__(self):
        self.manager = ImageManager() # le manager d'images
        self.manager.ajouterImage('herbe') # fond en herbe (800x600 en 0,0)

        self.manager.ajouterImage('serpentCorps167-1') # corps du serpent uniquement (167x49)
        self.manager.setInitialPositionImage('serpentCorps167-1',50,520)
        self.manager.setInitialRotationCenterPositionImage('serpentCorps167-1',0,24)
        self.manager.setInitialNextRotationCenterPositionImage('serpentCorps167-1',167,24)
        self.setAngleCorps1(0)

        self.manager.ajouterImage('serpentCorps167-2') # corps du serpent uniquement (167x49)
        self.manager.setInitialPositionImage('serpentCorps167-2',217,520)
        self.manager.setInitialRotationCenterPositionImage('serpentCorps167-2',0,24)
        self.manager.setInitialNextRotationCenterPositionImage('serpentCorps167-2',167,24)
        self.manager.setParentImage('serpentCorps167-2','serpentCorps167-1')
        self.setAngleCorps2(0)

        self.manager.ajouterImage('serpentCorps166Tete') # corps+Corps3 du serpent (166x49)
        self.manager.setInitialPositionImage('serpentCorps166Tete',384,520)
        self.manager.setInitialRotationCenterPositionImage('serpentCorps166Tete',0,24)
        self.manager.setInitialNextRotationCenterPositionImage('serpentCorps166Tete',166,24)
        self.manager.setParentImage('serpentCorps166Tete','serpentCorps167-2')
        self.setAngleCorps3(0)

        self.manager.ajouterImage('pomme') # pomme (50x50)
        self.manager.setInitialPositionImage('pomme',350,200) # position correspondant a une infinité de solutions pour les angles (ex. 20/61/-45 ou 40/-22/58)

        # Interface utilisateur pour le serpent 3DDL
        self.sliderAngleCorps1 = Tk.Scale(fenetre,from_=-180,to=180,orient=Tk.HORIZONTAL,command=self.changeAngleSliderCorps1)
        self.sliderAngleCorps1.place(x=800,y=0,width=200,height=50)
        self.sliderAngleCorps1.set(0)
        self.sliderAngleCorps2 = Tk.Scale(fenetre,from_=-180,to=180,orient=Tk.HORIZONTAL,command=self.changeAngleSliderCorps2)
        self.sliderAngleCorps2.place(x=800,y=50,width=200,height=50)
        self.sliderAngleCorps2.set(0)
        self.sliderAngleCorps3 = Tk.Scale(fenetre,from_=-180,to=180,orient=Tk.HORIZONTAL,command=self.changeAngleSliderCorps3)
        self.sliderAngleCorps3.place(x=800,y=100,width=200,height=50)
        self.sliderAngleCorps3.set(0)
        fenetre.bind("<ButtonRelease-1>",self.clickPositionPomme)
        self.buttonRechercheAleatoire = Tk.Button(fenetre,text="Recherche aleatoire",command=self.cherche_aleatoire)
        self.buttonRechercheAleatoire.place(x=800,y=150,width=200,height=50)
        self.buttonRecherche_CCD = Tk.Button(fenetre,text="Calcul CCD",command=self.cherche_CCD)
        self.buttonRecherche_CCD.place(x=800,y=200,width=200,height=50)
        self.buttonRecherche_Gradient = Tk.Button(fenetre,text="Calcul Descente Gradient",command=self.cherche_Gradient)
        self.buttonRecherche_Gradient.place(x=800,y=250,width=200,height=50)
        self.buttonRecherche_Jacobien = Tk.Button(fenetre,text="Calcul Jacobien",command=self.cherche_Jacobien)
        self.buttonRecherche_Jacobien.place(x=800,y=300,width=200,height=50)
        self.buttonRecherche_Fabrik = Tk.Button(fenetre,text="Calcul Fabrik",command=self.cherche_Fabrik)
        self.buttonRecherche_Fabrik.place(x=800,y=350,width=200,height=50)
        self.buttonAnimation = Tk.Button(fenetre,text="Animation",command=self.animation)
        self.buttonAnimation.place(x=800,y=400,width=200,height=50)
        self.textStatut = Tk.StringVar()
        self.labelStatut = Tk.Label(fenetre,textvariable=self.textStatut)
        self.labelStatut.place(x=800,y=550,width=200,height=50)
        self.textStatut.set("Cliquer sur un bouton de recherche")
        self.debutRecherche = time.time()

    def setAngleCorps1(self,angle):
        # donne un angle au corps 1 du serpent
        self.manager.setLocalOrientationImage('serpentCorps167-1',angle)
        self.manager.update()

    def setAngleCorps2(self,angle):
        # donne un angle au corps 2 du serpent
        self.manager.setLocalOrientationImage('serpentCorps167-2',angle)
        self.manager.update()

    def setAngleCorps3(self,angle):
        # donne un angle au corps 3/la tête du serpent
        self.manager.setLocalOrientationImage('serpentCorps166Tete',angle)
        self.manager.update()

    def update(self):
        # met a jour l'affichage et teste si la pomme est atteinte
        self.manager.update()
        if self.mangePomme():
            self.textStatut.set("Trouve en "+str(round(time.time() - self.debutRecherche,3))+" secondes")

    def mangePomme(self):
        # determine si la bouche est proche de la pomme
        posPomme = self.manager.getCenterPositionImage('pomme')
        posBouche = self.manager.getGlobalNextCenterPositionImage('serpentCorps166Tete')
        return posBouche.distance(posPomme) < 10

    def close(self):
        # fermeture des ressources du serpent
        self.manager.close()

    def changeAngleSliderCorps1(self,value):
        # change l'angle du corps 1 du serpent suite a un changement de placement du slider
        self.setAngleCorps1(int(value))
        self.update()

    def changeAngleSliderCorps2(self,value):
        # change l'angle du corps 2 du serpent suite a un changement de placement du slider
        self.setAngleCorps2(int(value))
        self.update()

    def changeAngleSliderCorps3(self,value):
        # change l'angle du corps 3 + tete du serpent suite a un changement de placement du slider
        self.setAngleCorps3(int(value))
        self.update()

    def clickPositionPomme(self,event):
        # change la position de la pomme suite a un clic
        posxPommeFenetre = event.x_root-fenetre.winfo_rootx()
        posyPommeFenetre = event.y_root-fenetre.winfo_rooty()
        if posxPommeFenetre < 775:
            self.manager.setCenterPositionImage('pomme',posxPommeFenetre,posyPommeFenetre)
            self.update()

    def cherche_aleatoire(self):
        # cherche les angles du serpent de maniere aleatoire
        self.debutRecherche = time.time()
        iteration = 0

        # TODO : Donner une valeur assez grande a iteration_max
        iteration_max = 1

        while not self.mangePomme() and iteration < iteration_max:
            iteration += 1
            self.textStatut.set("En recherche aleatoire..."+str(iteration))

            # TODO : tirer aleatoirement trois valeurs pour les variables angleCorps1, angleCorps2 et angleCorps3
            angleCorps1 = 0
            angleCorps2 = 0
            angleCorps3 = 0

            self.setAngleCorps1(angleCorps1)
            self.setAngleCorps2(angleCorps2)
            self.setAngleCorps3(angleCorps3)
            self.sliderAngleCorps1.set(angleCorps1)
            self.sliderAngleCorps2.set(angleCorps2)
            self.sliderAngleCorps3.set(angleCorps3)
            self.update()
            fenetre.update()

        if iteration==iteration_max:
            self.textStatut.set("Nombre d'iterations max atteint")

    def cherche_CCD(self):
        # cherche en utilisant le principe de Cyclic Coordinate Descent
        self.debutRecherche = time.time()
        iteration = 0

        # TODO : Donner une valeur assez grande a iteration_max
        iteration_max = 1

        while not self.mangePomme() and iteration < iteration_max:
            iteration += 1
            self.textStatut.set("En recherche CCD... "+str(iteration))
            # recuperation des positions utiles
            posCentreRotCorps1 = self.manager.getGlobalCenterPositionImage('serpentCorps167-1')
            posCentreRotCorps2 = self.manager.getGlobalCenterPositionImage('serpentCorps167-2')
            posCentreRotCorps3 = self.manager.getGlobalCenterPositionImage('serpentCorps166Tete')
            PosBouche = self.manager.getGlobalNextCenterPositionImage('serpentCorps166Tete')
            posPomme = self.manager.getCenterPositionImage('pomme')

            # TODO (version corps1 puis corps2 puis corps3)
            # TODO : calcul vecteur C1T et C1P
            # TODO : calcul de l'angle oriente entre les deux vecteurs (fonction angleOrienteVecteur)
            # TODO : calcul de l'angle de corps 1
            angleCorps1 = 0

            # mise a jour de l'angle et affichage
            self.setAngleCorps1(angleCorps1)
            self.sliderAngleCorps1.set(angleCorps1)
            self.update()
            fenetre.update()

            # TODO : calcul vecteur C2T et C2P
            # TODO : calcul de l'angle oriente entre les deux vecteurs
            # TODO : calcul de l'angle de corps 2
            angleCorps2 = 0

            # mise a jour de l'angle et affichage
            self.setAngleCorps2(angleCorps2)
            self.sliderAngleCorps2.set(angleCorps2)
            self.update()
            fenetre.update()

            # TODO : calcul vecteur TT et TP
            # TODO : calcul de l'angle oriente entre les deux vecteurs
            # TODO : calcul de l'angle de corps 3
            angleCorps3 = 0

            # mise a jour de l'angle et affichage
            self.setAngleCorps3(angleCorps3)
            self.sliderAngleCorps3.set(angleCorps3)
            self.update()
            fenetre.update()

        if iteration==iteration_max:
            self.textStatut.set("Nombre d'iterations max atteint")

    def cherche_Gradient(self):
        # cherche en utilisant le principe de la descente de gradient
        self.debutRecherche = time.time()
        iteration = 0

        # TODO : Donner une valeur assez grande a iteration_max
        iteration_max = 1

        # TODO : Donner des valeurs a delta_angle et beta
        delta_angle = 1
        beta = 0.1

        while not self.mangePomme() and iteration < iteration_max:
            iteration += 1
            self.textStatut.set("En recherche Descente Gradient... "+str(iteration))
            # recuperation des positions utiles
            posCentreRotCorps1 = self.manager.getGlobalCenterPositionImage('serpentCorps167-1')
            posCentreRotCorps2 = self.manager.getGlobalCenterPositionImage('serpentCorps167-2')
            posCentreRotCorps3 = self.manager.getGlobalCenterPositionImage('serpentCorps166Tete')
            PosBouche = self.manager.getGlobalNextCenterPositionImage('serpentCorps166Tete')
            posPomme = self.manager.getCenterPositionImage('pomme')
            # calcul de l'erreur de la pose courante
            erreur_courante = posPomme.distance(PosBouche)

            # Calcul des gradients
            # Corps 1
            # TODO : recuperation de l'angle de C1
            # TODO : calcul de la variation de cet angle par delta_angle
            # TODO : mise a jour de l'angle dans le manager sans affichage : appels setAngleCorps1 et update
            # TODO : recuperation de la nouvelle position de la bouche
            # TODO : calcul de l'erreur (distance entre bouche et pomme)
            # TODO : calcul du gradient pour C1
            # TODO : retour en arriere par application de l'angle avant changement : appels setAngleCorps1 et update

            # Corps 2
            # TODO : faire de meme pour l'angle de C2 => gradient pour C2

            # Corps 3 - Tête
            # TODO : faire de meme pour l'angle de C3/la tête => gradient pour C3

            # Mise à jour des angles opposée aux gradients
            # TODO : calculs des angles angleC1, angleC2 et angleC3 grace a leur gradient (et beta)
            angleC1 = 0
            angleC2 = 0
            angleC3 = 0

            # Mise à jour et affichage
            self.setAngleCorps1(angleC1)
            self.sliderAngleCorps1.set(angleC1)
            self.setAngleCorps2(angleC2)
            self.sliderAngleCorps2.set(angleC2)
            self.setAngleCorps3(angleC3)
            self.sliderAngleCorps3.set(angleC3)
            self.update()
            fenetre.update()

        if iteration==iteration_max:
            self.textStatut.set("Nombre d'iterations max atteint")

    def cherche_Jacobien(self):
        # cherche en utilisant le principe du Jacobien
        self.debutRecherche = time.time()
        iteration = 0

        # TODO : Donner une valeur assez grande a iteration_max
        iteration_max = 1

        # TODO : Donner une valeur a gamma
        gamma = 0.0001

        while not self.mangePomme() and iteration < iteration_max:
            iteration += 1
            self.textStatut.set("En recherche Jacobien... "+str(iteration))
            # recuperation des positions utiles
            posCentreRotCorps1 = self.manager.getGlobalCenterPositionImage('serpentCorps167-1')
            posCentreRotCorps2 = self.manager.getGlobalCenterPositionImage('serpentCorps167-2')
            posCentreRotCorps3 = self.manager.getGlobalCenterPositionImage('serpentCorps166Tete')
            PosBouche = self.manager.getGlobalNextCenterPositionImage('serpentCorps166Tete')
            posPomme = self.manager.getCenterPositionImage('pomme')
            BouchePomme = posPomme - PosBouche

            # Corps 1
            # TODO : calcul du vecteur P1
            # TODO : calcul du deplacement du corps 3/la tete par une rotation en P1 (fonction produitVectorielZ)
            # TODO : calcul du deplacement du corps 3/la tête par la rotation P1 vers la pomme (fonction produitScalaire)
            # TODO : calcul de l'angle (avec gamma)
            angleC1 = 0

            # Corps 2
            # TODO : faire de meme pour le deuxieme centre de rotation
            angleC2 = 0

            # Corps 3
            # TODO : faire de meme pour le troisieme centre de rotation
            angleC3 = 0

            # Mise à jour et affichage
            self.setAngleCorps1(angleC1)
            self.sliderAngleCorps1.set(angleC1)
            self.setAngleCorps2(angleC2)
            self.sliderAngleCorps2.set(angleC2)
            self.setAngleCorps3(angleC3)
            self.sliderAngleCorps3.set(angleC3)
            self.update()
            fenetre.update()

        if iteration==iteration_max:
            self.textStatut.set("Nombre d'iterations max atteint")

    def cherche_Fabrik(self):
        # cherche en utilisant le principe de Fabrik
        self.debutRecherche = time.time()
        iteration = 0

        # TODO : Donner une valeur assez grande a iteration_max
        iteration_max = 1

        while not self.mangePomme() and iteration < iteration_max:
            iteration += 1
            self.textStatut.set("En recherche Fabrik... "+str(iteration))
            # recuperation des positions utiles
            posCentreRotCorps1 = self.manager.getGlobalCenterPositionImage('serpentCorps167-1')
            posCentreRotCorps2 = self.manager.getGlobalCenterPositionImage('serpentCorps167-2')
            posCentreRotCorps3 = self.manager.getGlobalCenterPositionImage('serpentCorps166Tete')
            PosBouche = self.manager.getGlobalNextCenterPositionImage('serpentCorps166Tete')
            posPomme = self.manager.getCenterPositionImage('pomme')

            # Pomme vers Corps 1
            # NOTE : Ne pas oublier de bien prendre les valeurs mises à jour quand nécessaire.
            # TODO : calcul du vecteur Pomme P3
            # TODO : calcul de P3 par deplacement de pomme (fonction translation) | Ex: posP3prime.translation(vecDirection,166)
            # TODO : calcul du vecteur P3 P2
            # TODO : calcul de P2 par deplacement de P3

            # Corps 1 vers Pomme
            # TODO : calcul du vecteur P1 P2
            # TODO : calcul de P2 par deplacement de P1
            # TODO : calcul du vecteur P2 P3
            # TODO : calcul de P3 par deplacement de P2
            # TODO : calcul du vecteur P3 Pomme
            # TODO : calcul de la bouche par deplacement de P3

            # Calcul des angles angleC1, angleC2 et angleC3 grace aux positions des centres de rotation
            # création du vecteur x de taille (1,0)
            axex = Pos2D(1,0)
            # TODO : calcul du nouveau vecteur P1 P2 avec les nouvelles valeurs
            # TODO : calcul de l'angle C1 grace aux positions du centre de rotation (fonction angleOrienteVecteur)
            angleC1 = 0
            # TODO : Faire la même chose pour angle C2 et angle C3
            angleC2 = 0
            angleC3 = 0

            # Passer les angles en degrés
            angleC1 = math.degrees(angleC1)
            angleC2 = math.degrees(angleC2)
            angleC3 = math.degrees(angleC3)

            # Mise à jour et affichage
            self.setAngleCorps1(angleC1)
            self.sliderAngleCorps1.set(angleC1)
            self.setAngleCorps2(angleC2)
            self.sliderAngleCorps2.set(angleC2)
            self.setAngleCorps3(angleC3)
            self.sliderAngleCorps3.set(angleC3)
            self.update()
            fenetre.update()

        if iteration==iteration_max:
            self.textStatut.set("Nombre d'iterations max atteint")

    def animation(self):
        i = 0
        while i < math.pi / 2:
            # deplace la pomme et demande au serpent de la suivre
            posPommex = 50+math.cos(i)*480
            posPommey = 544-math.sin(i)*480
            deltax = math.sin(i*8)*20
            deltay = -math.sin(i*8)*20
            self.manager.setCenterPositionImage('pomme',posPommex+deltax,posPommey+deltay)

            # calcul l'angle
            # TODO : choisir la methode de resolution pour l'animation en (de)commentant les lignes suivantes
            self.cherche_CCD()
            #self.cherche_Gradient()
            #self.cherche_Jacobien()
            #self.cherche_Fabrik()

            # mise a jour affichage
            self.update()
            fenetre.update()
            i += 0.01

s = Serpent3DDL()
fenetre.update()
fenetre.mainloop()
s.close()
