# -*- coding: utf-8 -*-
"""
Created on Wed Jan  1 14:30:33 2015
ok
@author: joseorellana
"""

# import basic modules
import numpy as np
import matplotlib.pyplot as plt
from numpy import linalg as la
import math


def cm0(alpha1):
    return -.3*alpha1+.001
    
def cmg(alpha1):
    return -.1*alpha1
    
def cxg(beta0):
    return .2*beta0
    
def cz0(alpha1):
    return 10.*alpha1-.05*alpha1**2  
    
def czg(alpha1):
    return 1.*alpha1-.01*alpha1**2  
         
def dcm0(alpha1):
    return -.3
    
def dcmg(alpha1):
    return -.1
        
def dcz0(alpha1):
    return 10.-.1*alpha1**2

    
def dczg(alpha1):
    return 1.-.02*alpha1 

def soluedo(RS,NPTs,dt,phi0s):
	zs=np.linspace(1,2,NPT)*0.
	alphas=np.linspace(1,2,NPT)*0.
	zs[0]=phi0s[0]
	zs[1]=zs[0]+dt*phi0s[1]
	alphas[0]=phi0s[2]
	alphas[1]=alphas[0]+dt*phi0s[3]
	X1=np.transpose([zs[0],alphas[0]])
	X2=np.transpose([zs[1],alphas[1]])
	for i in range(2,NPTs):
		X3=2*X2-X1-dt**2*np.dot(RS,X2)
		zs[i]=X3[0]
		alphas[i]=X3[1]
		X1=X2
		X2=X3
	return zs,alphas 
	
	
	
	
	



def calculgramraideur(NPT,dt,z0M,dz0M,alpha0M,dalpha0M,xeqM,Rad,B0,F0,a0):
	GRAMM=np.zeros((4,4),float)
	phi0=0.*np.linspace(1,4,4)
	deltaphi0=0.*np.linspace(1,4,4)
	Lsec=0.*np.linspace(1,2,4)
	for k1 in range(4):
		for i1 in range(4):
			if i1 == k1:
				phi0[i1]=1
			else:
				phi0[i1]=0
		P1,P2=soluedo(Rad,NPT,dt,phi0)
		P1dotT=(P1[NPT-1]-P1[NPT-2])/dt		
		P2dotT=(P2[NPT-1]-P2[NPT-2])/dt
		Lsec[k1]=dz0M*phi0[0]+dalpha0M*phi0[2]-z0M*phi0[1]-alpha0M*phi0[3]+xeqM[0]*P1dotT+xeqM[1]*P2dotT
		squatro=0
		for jkl in range(NPT):
			squatro=squatro+F0[0]*P1[jkl]+F0[1]*P2[jkl]	
		Lsec[k1]=Lsec[k1]+squatro*dt
		for k2 in range(k1,4):
			for i2 in range(4):
				if i2 == k2 :
					deltaphi0[i2]=1
				else:
					deltaphi0[i2]=0
			Q1,Q2=soluedo(Rad,NPT,dt,deltaphi0)
			sbis=0.
			for jj in range(NPT):
				sbis=sbis+(B0[0]*P1[jj]+B0[1]*P2[jj])*(B0[0]*Q1[jj]+B0[1]*Q2[jj]) 
			GRAMM[k1,k2]=sbis*dt
			GRAMM[k2,k1]=GRAMM[k1,k2]
	GRAMM=GRAMM/a0
	PHISOL=la.solve(GRAMM,np.transpose(Lsec))
	return PHISOL
    
    
    
def calculetatadj(NPT,dt,PHIHUMc,Rad):
	zz=np.linspace(1,2,NPT)*0.
	alphaa=np.linspace(1,2,NPT)*0.
	zz[0]=PHIHUMc[0]
	zz[1]=zz[0]+dt*PHIHUMc[1]
	alphaa[0]=PHIHUMc[2]
	alphaa[1]=alphaa[0]+dt*PHIHUMc[3]
	X1=np.transpose([zz[0],alphaa[0]])
	X2=np.transpose([zz[1],alphaa[1]])
	for i in range(2,NPT):
		X3=2*X2-X1-dt**2*np.dot(Rad,X2)
		zz[i]=X3[0]
		alphaa[i]=X3[1]
		X1=X2
		X2=X3
	return zz,alphaa

m=1.
J0=1.
k=10.
c=7.
a=-.02
V=1.
z0=.0
dz0=0.
alpha0=0.05
dalpha0=1.
beta0=.1
ro=1.2
S=1.
L=1.
a0=1.
NPT=6000
T=3.
dt=T/NPT
temp=np.linspace(1,T+1,NPT)-1
z=np.linspace(1,2,NPT)*0.
alpha=np.linspace(1,2,NPT)*0.

M=[[m, a*math.cos(alpha0)],[a*math.cos(alpha0),J0]]

K=[[k ,-ro*S*V**2*dcz0(alpha0)/2],[0., c-ro*S*V**2*L*dcm0(alpha0)/2]]

R=la.solve(M,K)



Rad=la.solve(M,np.transpose(K))


F0=[ro*S*V**2*cz0(alpha0)/2,ro*S*L*V**2*cm0(alpha0)/2]
B0=[ro*S*V**2*dczg(beta0)/2,ro*S*L*V**2*dcmg(beta0)/2]
xeq=la.solve(K,np.transpose(F0))
F=la.solve(M,np.transpose(F0))
B=la.solve(M,np.transpose(B0))
beta=np.linspace(1,2,NPT)*0
condinit= np.dot(M,[z0,alpha0])
vitesseinit=np.dot(M,[dz0,dalpha0])
xeqM=np.dot(M,xeq)


z0M=condinit[0]
alpha0M=condinit[1]
dz0M=vitesseinit[0]
dalpha0M=vitesseinit[1]
PHIHUM=calculgramraideur(NPT,dt,z0M,dz0M,alpha0M,dalpha0M,xeqM,Rad,B0,F0,a0)
P1,P2=calculetatadj(NPT,dt,PHIHUM,Rad)

for kk in range(NPT):
	beta[kk]=-(B0[0]*P1[kk]+B0[1]*P2[kk])/a0
       
plt.figure(1) 
plt.subplot(1,3,1)
plt.plot(temp,beta)
plt.xlabel ('temps')
plt.ylabel(' Controle de raideur')
plt.title('Loi de controle  portant uniquement sur la raideur',fontsize=10)

z[0]=z0;
z[1]=z0+dt*dz0
alpha[0]=alpha0
alpha[1]=alpha[0]+dt*dalpha0
X1=np.transpose([z[0],alpha[0]])
X2=np.transpose([z[1],alpha[1]])

for i in range(2,NPT):
	X3=2*X2-X1+(dt**2)*(F+B*beta[i-1]-np.dot(R,X2))
	z[i]=X3[0]
	alpha[i]=X3[1]
	X1=X2
	X2=X3

plt.subplot(1,3,2)
plt.plot(temp,z,temp,alpha)
plt.xlabel ('temps')
plt.ylabel(' En bleu z et en vert alpha')
plt.title(' Mouvements avec le controle de raideur',fontsize=10)

X1=np.transpose([z[NPT-2],alpha[NPT-2]])
X2=np.transpose([z[NPT-1],alpha[NPT-1]])
zp=np.linspace(1,2,2*NPT)
alphap=np.linspace(1,2,2*NPT)
tempo=np.linspace(1,2*T+1,2*NPT)-1


for i in range(NPT):
	zp[i]=z[i]
	alphap[i]=alpha[i]


for i in range(NPT):
	X3=2*X2-X1+(dt**2)*(F-np.dot(R,X2))
	zp[i+NPT]=X3[0]
	alphap[i+NPT]=X3[1]
	X1=X2
	X2=X3

plt.subplot(1,3,3)
plt.plot(tempo,zp,tempo,alphap)
plt.xlabel ('temps')
plt.ylabel(' En bleu z et en vert alpha')
plt.title('Mouvements post-controle portant sur la raideur',fontsize=10)
plt.show()