#README
#
# ajout de forces sociales
#
# les agents sont guidés par plusieurs forces
#       une force les conduisant a leur objectif commun
#       une force de repulsion vis a vis des voisins
#       la norme de la force de répulsion est en 1/distance (cf methode normeSociale)
#


#objectif a atteindre
class Objectif():
    
    def __init__(self,x,y):
        self.x=x
        self.y=y


#agent avec un objectif
class Agent():

    
    
    def __init__(self,dx,dy,obj):
        self.objectif=obj
        self.x=dx
        self.y=dy

        #par defaut pas de dynamique
        self.vx=20
        self.vy=0
        self.ax=0
        self.ay=0
        self.dt=0.1
        
        #acceleration maximale
        self.aMax=20        

    def evoluer(self):
        self.vx=self.vx+self.ax*self.dt
        self.vy=self.vy+self.ay*self.dt
        self.x=self.x+self.vx*self.dt
        self.y=self.y+self.vy*self.dt


    def afficher(self):
        print("(x,y): "+str(self.x)+","+str(self.y)+"  vx,vy: "+str(self.vx)+","+str(self.vy))


    #retourne la norme de la force de repulsion fonction distance
    def normeSociale(self,distance):
        force=2
        if (distance>100):
            return(0)
        return force*((100)/distance-1)

    # met a jour son acceleration en fonction objectif
    # et des autres agents
    def calculerObj(self,agents):
        from math import sqrt
        
        #vitesse souhaitee (maximisee)
        vx=(self.objectif.x-self.x)
        vy=(self.objectif.y-self.y)
        vMax=50
        vnorme=sqrt(vx*vx+vy*vy)
        if (vnorme>vMax):
            vx=vx*vMax/vnorme
            vy=vy*vMax/vnorme
            
        

        #calcule la commande à donner
        dvx=vx-self.vx
        dvy=vy-self.vy

        #norme avec aMax       
        norme=sqrt(dvx*dvx+dvy*dvy)
        if (norme>self.aMax):
            dvx=dvx*self.aMax/norme
            dvy=dvy*self.aMax/norme
        self.ax=dvx
        self.ay=dvy


        #prise en compte autres agents
        for agent in agents:
            if (agent!=self):
                dx=agent.x-self.x
                dy=agent.y-self.y
                distance=self.distance(agent)
    
                #met a jour la force
                normeForce=self.normeSociale(distance)
                self.ax-=normeForce*dx/distance
                self.ay-=normeForce*dy/distance


    
        
        
            
    def distance(self,O):
        dx=(O.x-self.x)
        dy=(O.y-self.y)
        d2=dx*dx+dy*dy
        from math import sqrt
        return sqrt(d2)


# systeme c'est un agent et un objectif
class Systeme():

    #initialise le systeme avec les coordonnees de l'agent
    def __init__(self,num):
        from random import uniform
        self.num=num
        self.objectif=Objectif(200,200)
        self.agents=[]
        for i in range(num):
            x=uniform(0,500)
            y=uniform(0,700)
            self.agents+=[Agent(x,y,self.objectif)]

    #evoluer le jeu
    def evoluer(self):
        for agent in self.agents:
            agent.calculerObj(self.agents)
            agent.evoluer()
        

    def dessiner(self):
        # First, clear the screen to white. Don't put other drawing commands
        # above this, or they will be erased with this command.
        WHITE = (0xFF, 0xFF, 0xFF)
        RED = (0xFF, 0x00, 0x00)
        BLUE = (0x00, 0xFF, 0x00)
        BLACK = (0x00, 0x00, 0x00)
        screen.fill(WHITE)
        pygame.draw.ellipse(screen,BLUE,(jeu.objectif.x-5,jeu.objectif.y-5,10,10))
        for agent in self.agents:
            pygame.draw.ellipse(screen,RED,(agent.x-5,agent.y-5,10,10))
            pygame.draw.line(screen,BLACK,[agent.x,agent.y],[agent.x+agent.vx,agent.y+agent.vy])
           
import pygame
pygame.init()
size = (700, 500)
screen = pygame.display.set_mode(size)
pygame.display.set_caption("Moteur")


# boolean boucle
done = False
#jeu avec 10 agents
jeu=Systeme(10)

# horloge
clock = pygame.time.Clock()
# -------- boucle principale
while not done:
    
    # --- gestion evenements
    for event in pygame.event.get(): 
        # quitter
        if event.type == pygame.QUIT: 
            done = True

        #si on appui souris: change objectif
        if event.type == pygame.MOUSEBUTTONDOWN:
            jeu.objectif.x=pygame.mouse.get_pos()[0]
            jeu.objectif.y=pygame.mouse.get_pos()[1]
            

            
    # --- evolution
    jeu.evoluer()
    
    # --- dessin
    jeu.dessiner()
        
    # --- mise a jour graphique
    pygame.display.flip()
    # --- attente
    clock.tick(60)

pygame.quit()