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


from systemeCinematique import *

solide_0 = Solide(0,0,0)
a = numpy.pi/4
solide_1 = Solide(0.5*numpy.cos(a),0.5*numpy.sin(a),a,[[-0.5,0],[0.5,0]])
solide_2 = Solide(numpy.cos(a),numpy.sin(a),numpy.pi/2+a,[[-0.1,-0.1],[0.1,-0.1],[0.1,0.1],[-0.1,0.1],[-0.1,-0.1]])
solide_3 = Solide(numpy.cos(a),numpy.sin(a),numpy.pi/2+a,[[-1,0],[2,0]])
solide_4 = Solide(2*numpy.cos(a),0,0)

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


angle = Angle(1,a)

systeme.ajouter_contrainte(SolideFixe(0,0,0,0))
systeme.ajouter_contrainte(Pivot(0,1,0,0,-0.5,0))
systeme.ajouter_contrainte(angle)
systeme.ajouter_contrainte(Pivot(1,2,0.5,0,0,0))
systeme.ajouter_contrainte(Glissiere(2,3,0,0,0,1,1,0,0))
systeme.ajouter_contrainte(Pivot(3,4,-1,0,0,0))
systeme.ajouter_contrainte(SolideFixe(4,2*numpy.cos(a),0,0))


fig,ax = plt.subplots()
ax.set_aspect('equal')
ax.axis([-2,2,-2,2])
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()