import numpy
import matplotlib.pyplot as plt
import matplotlib.animation as animation


from systemeCinematique import *

solide_0 = Solide(0,0,0)
solide_1 = Solide(0.1,0,0,[[-0.1,0],[0.1,0]])
solide_2 = Solide(0.7,0,0,[[-0.5,0],[0.5,0]])
solide_3 = Solide(1.2,0,0,[[-0.1,-0.1],[0.1,-0.1],[0.1,0.1],[-0.1,0.1],[-0.1,-0.1]])


systeme = Systeme()
systeme.ajouter_solide(solide_0)
systeme.ajouter_solide(solide_1)
systeme.ajouter_solide(solide_2)
systeme.ajouter_solide(solide_3)


angle = Angle(1,0)

systeme.ajouter_contrainte(SolideFixe(0,0,0,0))
systeme.ajouter_contrainte(angle)
systeme.ajouter_contrainte(Pivot(0,1,0,0,-0.1,0))
systeme.ajouter_contrainte(Pivot(1,2,0.1,0,-0.5,0))
systeme.ajouter_contrainte(Pivot(2,3,0.5,0,0,0))
systeme.ajouter_contrainte(GlissiereHorizontale(3,0))




fig,ax = plt.subplots()
ax.set_aspect('equal')
ax.axis([-1.5,1.5,-1.5,1.5])
systeme.dessiner(ax)
ax.grid()


def animate(i):
    angle.theta += 0.01
    systeme.calcul(1e-3)
    systeme.redessiner(ax)

anim = animation.FuncAnimation(fig,animate,repeat=True,interval=10)

plt.show()