import matplotlib.pyplot as plt
import math as mp

d4 = 20 
l4 = 350
xA = 260
yB = 100
l3 = 280
l2 = 280
prec= 10**(-12)

def fct(teta2,teta4):
    teta4_rd=teta4*mp.pi/180.
    teta2_rd=teta2*mp.pi/180.
    DCx0=l2*mp.sin(teta2_rd)-xA-l4*mp.sin(teta4_rd)+d4*mp.cos(teta4_rd)
    DCy0=-l2*mp.cos(teta2_rd)+yB+l4*mp.cos(teta4_rd)+d4*mp.sin(teta4_rd)
    costeta3=DCy0/l3
    sinteta3=-DCx0/l3
    return 1.-costeta3**2-sinteta3**2

from scipy.optimize import fsolve
def solve(teta2,teta4):
    sol = fsolve(fct,x0=teta2,args=(teta4))[0]
    return sol

L_teta4=[ -x for x in range(91)]


L_teta2_solve=[solve(-100,teta4)  for teta4 in L_teta4]

    
def dichotomie2(f,teta4,xmin,xmax,prec): # renvoie teta2 connaissant teta4
    if f(xmin,teta4)*f(xmax,teta4)>0:
        print('probleme')
        return
    xmilieu=(xmin+xmax)/2.
    k=0
    while abs(f(xmilieu,teta4) )>prec and k<n:
        xmilieu=(xmin+xmax)/2.
        if f(xmin,teta4)*f(xmilieu,teta4)<0:
            xmax=xmilieu
            k=k+1
        else:
            xmin=xmilieu
            k=k+1
    return xmilieu

def dichotomie4(f,teta2,xmin,xmax,n): # renvoie teta4 connaissant teta2
    if f(teta2,xmin)*f(teta2,xmax)>0:
        print('probleme')
        return
    xmilieu=(xmin+xmax)/2.
    k=0
    while abs(f(teta2,xmilieu) )>10**(-12) and k<n:
        xmilieu=(xmin+xmax)/2.
        if f(teta2,xmin)*f(teta2,xmilieu)<0:
            xmax=xmilieu
            k=k+1
        else:
            xmin=xmilieu
            k=k+1
    return xmilieu

#print(dichotomie(mp.sin, -0.1,0.3,200))

# Etude géométrique
    
L_teta4=[ -x for x in range(91)]
L_teta2_dicho=[dichotomie2(fct,x,10,-130,prec) for x in L_teta4]
    
fctmin=[fct(x,10) for x in L_teta4]
fctmax=[fct(x,-130) for x in L_teta4]


plt.close('all')
plt.figure("recherche de l'intervalle pour teta2")
plt.plot(L_teta4,fctmin,L_teta4,fctmax)
plt.show()

plt.figure('Loi entrée sortie')
plt.plot(L_teta4,L_teta2_dicho, label='dicho')
plt.plot(L_teta4,L_teta2_solve, label='solve')
plt.xlabel('angle teta4 en degré')
plt.ylabel('angle teta2 en degré')
plt.grid()
plt.legend()
plt.show()

'''
# Etude cinematique
teta2min=L_teta2[-1]
teta2max=L_teta2[0]
nbpoints=100
omega21=1 # 1 deg/seconde
duree_mvt=(teta2max-teta2min)/omega21
delta_T=duree_mvt/nbpoints
L_teta2=[teta2min+omega21*x*delta_T for x in range(nbpoints+1)]

def derivee(Y,delta_T):
    Yp=[None]*len(Y)
    Yp[0]=(Y[1]-Y[0])/delta_T
    Yp[-1]=(Y[-1]-Y[-2])/delta_T
    for x in range(1,len(Y)-1):
        Yp[x]=(Y[x+1]-Y[x-1])/delta_T/2.
    return Yp

L_teta4=[dichotomie4(fct,x,-100,10,200) for x in L_teta2]
L_omega4=derivee(L_teta4,delta_T)
L_temps=[x*delta_T for x in range(nbpoints+1)]
plt.figure("Omega vantail en deg/s")
plt.plot(L_temps,L_omega4)
plt.show()

# Etude dynamique
# Rapport omega21 sur omega 41 en fonction du temps t
L_rap_omega=[None]*len(L_omega4)
for k in range(len(L_omega4)):
    L_rap_omega[k]=L_teta4[k],omega21/L_omega4[k]
    
    
def rapport_omega(teta4,L_rap_omega):
    indice_trouve=False
    k=-1
    k_indice=len(L_rap_omega)-1 # si on cherche pour teta4 superieur au max
    while k<(len(L_rap_omega)-2) and not(indice_trouve):
        k=k+1
        if L_rap_omega[k+1][0]>=teta4 and L_rap_omega[k][0]<=teta4:
            indice_trouve=True
            k_indice=k
    return L_rap_omega[k_indice][1]
    
def tetapp4(teta4,L_rap_omega,Cmot):
    #Cmot positif phase d acceleration sinon de deceleration
    J4=10 # en kg.m2
    return Cmot*rapport_omega(teta4,L_rap_omega)/J4 # en rad/seconde
    
def euler(a,h,f0,fp0,tetamax,Cmot):
    # a temps initial
    # h pas de temps de calcul
    # fo valeur initiale de f
    # fp0 valeur initiale de la dérivée de f
    # tetamax valeur maximale de teta
    t = a
    f = f0
    fp = fp0
    T = [a]
    Fon = [f0] # en degre
    Fonp = [fp0] # en degre /seconde
    k=0
    while f<tetamax:
        t = t+h
        f = f + h*Fonp[k]
        fp = fp + h*tetapp4(f,L_rap_omega,Cmot)*180./mp.pi
        T.append(t)
        Fon.append(f)
        Fonp.append(fp)
        k=k+1
    return T,Fon,Fonp
    
C2=0.3
Cmot=C2
L2_T,L2_teta4,L2_omega4=euler(0,0.01,-90.,0,0.,Cmot)
plt.figure("Etude dynamique : sans limitation de vitesse teta vantail en deg")
plt.plot(L2_T,L2_teta4)
plt.show()


# Limitation de la vitesse de rotation du bras moteurW2max
def euler_wmax(a,h,f0,fp0,tetamax,Cmot,w2max):
    # a temps initial
    # h pas de temps de calcul
    # fo valeur initiale de f
    # fp0 valeur initiale de la dérivée de f
    # tetamax valeur maximale de teta
    t = a
    f = f0
    fp = fp0
    T = [a]
    Fon = [f0] # en degre
    Fonp = [fp0] # en degre /seconde
    L_omega2=[fp0]
    k=0
    teta2=teta2min
    omega2=fp0
    while f<tetamax and omega2<=w2max:
        t = t+h
        f = f + h*Fonp[k]
        fp = fp + h*tetapp4(f,L_rap_omega,Cmot)*180./mp.pi
        T.append(t)
        Fon.append(f)
        Fonp.append(fp)
        k=k+1
        teta2=teta2+omega2*h
        omega2=fp*rapport_omega(f,L_rap_omega) # omega4 * G(etat4)
        L_omega2.append(omega2)
    print('Teta 4', f, 'omega2 ',omega2)
    #Ensuite on poursuite a vitesse constante w2max
    while f<tetamax:
        t=t+h
        teta2=teta2+w2max*h
        omega4=w2max/rapport_omega(f,L_rap_omega)
        f=f+h*omega4
        fp=omega4
        T.append(t)
        Fon.append(f)
        Fonp.append(fp)
        L_omega2.append(omega2)
    
    return T,Fon,Fonp,L_omega2

C2=0.3
Cmot=C2
w2max=12 # en deg/s
L3_T,L3_teta4,L3_omega4,L3_omega2=euler_wmax(0,0.01,-90.,0,0.,Cmot,w2max)
plt.figure("Etude dynamique : avec limitation de vitesse : w4 et w2 en deg/s")
plt.plot(L3_T,L3_omega4,L3_T,L3_omega2)
plt.show()
plt.figure("Etude dynamique : avec limitation de vitesse  : teta vantail en deg")
plt.plot(L3_T,L3_teta4)
plt.show()

    
#     
# print('-90.',tetapp4(-90.,L_rap_omega,L_teta4,L_temps))
# print('-45.',tetapp4(-45.,L_rap_omega,L_teta4,L_temps))
# print('0.',tetapp4(0.,L_rap_omega,L_teta4,L_temps))            
#     
'''