# -*- 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 cmfnaca(xi):
	return 1.5*xi 
 
def cmsnaca(xi):
    return 1.5*xi

def czfnaca(xi):
    return 7.*xi 
        
def czsnaca(xi):
    return 7.*xi
    
def dcmfnaca(xi):
    return 1.5
    
def dcmsnaca(xi):
    return 1.5

def dczfnaca(xi):
    return 7.
        
def dczsnaca(xi):
    return 7.
    
def resoladj(npt,dt,TCM,TKM,COND):
	sol=np.zeros((2,npt),float)
	sol[0,0]=COND[0]
	sol[1,0]=COND[1]
	sol[0,1]=COND[0]+dt*COND[2]
	sol[1,1]=COND[1]+dt*COND[3]
	for kt in range(2,npt):
		for ii in range(2):
			sum=0
			for jj in range(2):
				sum=sum-dt*TCM[ii,jj]*(sol[jj,kt-1]-sol[jj,kt-2])+dt**2*TKM[ii,jj]*sol[jj,kt-1]
			sol[ii,kt]=2*sol[ii,kt-1]-sol[ii,kt-2]-sum
	return sol

roe=1000.# masse volumique de l'eau
df=2.#longueur du foil
ds=2. #longueur du safran
Sf=.2# surace du foil oppose a  l'ecoulement
Ss=.1# surface du safran 
#
L=1.
h=5.# distance entre le foil et le safran
raddeg=math.pi/180# coefficient pour transformer des degrés en radians
m=1000. #poids du bateau
J0=10000.# inertie du bateau en tangage autour de O
a=.2# distance du centre de gravité au point O
#============
npv=300#% nombre de points de calcul pour la vitesse
umin=0.# vitesse minimale
umax=20.# vitesse maximale
du=(umax-umin)/npv # pas en vitesscomplexe
mur1=np.linspace(umin,umax,npv)# stockage des parties réelles des valeurs propres
mur2=np.linspace(umin,umax,npv)# stockage des parties réelles des valeurs propres
mur3=np.linspace(umin,umax,npv)# stockage des parties réelles des valeurs propres
mur4=np.linspace(umin,umax,npv)#stockage des parties réelles des valeurs propres
mui1=np.linspace(umin,umax,npv)# stockage des parties imaginaires des valeurs propres
mui2=np.linspace(umin,umax,npv)# stockage des parties imaginaires des valeurs propres
mui3=np.linspace(umin,umax,npv)# stockage des parties imaginaires des valeurs propres
mui4=np.linspace(umin,umax,npv)# stockage des parties imaginaires des valeurs propres
vit=np.linspace(umin,umax,npv)
M=[[m,-a*m],[-a*m,J0]]#Matrice des inerties
M1=la.inv(M)# Inverse de la matrice des inerties
#========
# angles de braquage safran beta et foil alpha
#========
beta=raddeg*3 #braquage du safran
alpha=raddeg*4 # braquage du foil (initial)
#=======
#Donnees controle
#=======
a0=1.
b0=1. # coefficient dans le critere de controle
#=====
for i in range(npv):  #boucle en vitesse pour construire les matrice C et K et calculer leur valeurs propres
    u=umin+i*du
    c11=-(roe*u/2)*(Ss*dczsnaca(beta)+Sf*dczfnaca(alpha))
    c12=(roe*u/2)*(Ss*(2.*ds*czsnaca(beta)+h*dczsnaca(beta))+Sf*(2*df*math.cos(alpha)*czfnaca(alpha)-df*math.sin(alpha)*dczsnaca(alpha)))
    c21=-(roe*L*u/2)*(Ss*dcmsnaca(beta)+Sf*dcmfnaca(alpha))+(roe*u/2)*(Ss*h*dczsnaca(beta)-Sf*df*math.sin(alpha)*dczfnaca(alpha))
    c22=(roe*L*u*Ss/2)*(2*ds*cmsnaca(beta)+h*dcmsnaca(beta))+(roe*Sf*df*u*L/2)*(-math.sin(alpha)*dcmfnaca(alpha)+2*math.cos(alpha)*cmfnaca(alpha))
    c22=c22-(roe*Ss*h*u/2)*(2*ds*czsnaca(beta)+h*dczsnaca(beta))+(roe*u*Sf*df**2/2)*(math.sin(2*alpha)*czfnaca(alpha)-math.sin(alpha)**2*dczfnaca(alpha))
    k11=100000.#% raideur artificielle correspondant au fait que la voilure est flexible et que les foils ne peuvent (doivent) pas sortir de l'eau
    k12=-(roe*u**2/2)*(Ss*dczsnaca(beta)+Sf*dczfnaca(alpha))
    k21=0.
    k22=(roe*L*u**2/2)*(-Ss*dcmsnaca(beta)-Sf*dcmfnaca(alpha))+(roe*u**2/2)*(-Sf*df*math.sin(alpha)*dczfnaca(alpha)+Ss*h*dczsnaca(beta))
    k22=k22-((roe*u**2)/2)*(Sf*df*math.cos(alpha)*czfnaca(alpha)+Ss*ds*czsnaca(beta))
    C=np.array([[c11,c12],[c21,c22]])
    K=np.array([[k11,k12],[k21,k22]])
    #K=np.array([[1,0],[.0,2]])
    
    M2=np.bmat([[np.identity(2),np.zeros((2,2))],[np.zeros((2,2)),M]])
    K2=np.bmat([[np.zeros((2,2)),np.identity(2)],[K,-1j*C]])
    
  
    #===========
    #Matrice du systeme dynamique
    #===========
 
# Resolution dynamique
#===========
T=10.#temps sur lequel on calcule le controle
npt=1500# nombre de pas de temps
dt=T/npt #pas de temps
temps=(np.linspace(1,npt,npt)-1)*dt
z= np.linspace(1,npt,npt).reshape(-1,1)*0
dz=np.linspace(1,npt,npt)*0
gamma= np.linspace(1,npt,npt).reshape(-1,1)*0
dgamma=np.linspace(1,npt,npt)*0
#=======
X=np.bmat([z,gamma])
Xdot=np.linspace(1,npt,npt).reshape(-1,1)*0
delta=np.linspace(1,npt,npt)*0
deltadot=np.linspace(1,npt,npt)*0
#==============
# conditions initiales
#==============
z0=0.
dz0=0.
gamma0=-2.0*raddeg
dgamma0=0*raddeg
X[0,0]=z0   
X[0,1]=gamma0
X[1,0]=z0+dt*dz0
X[1,1]=gamma0+dt*dgamma0
#============
#============

CM=np.dot(M1,C)
KM=np.dot(M1,K)
#vppk=eig(KM);
#E=[C[0,1]+.01,C[1,1]+.01]
yy=(roe*Sf*df*u*L/2)*(-math.sin(alpha)*dcmfnaca(alpha)+2*math.cos(alpha)*cmfnaca(alpha))
yy=yy+(roe*Sf*df*df*u/2)*(math.sin(2*alpha)*czfnaca(alpha)-math.sin(alpha)**2*dczfnaca(alpha))
E=[(roe*u/2)*(Sf)*(2*df*math.cos(alpha)*czfnaca(alpha)-df*math.sin(alpha)*dczsnaca(alpha))+.01,yy+.01]
Ec=np.dot(M1,np.transpose(E))
xx=(roe*L*u**2/2)*(Sf*dcmfnaca(alpha))+(roe*u**2/2)*(Sf*df*math.sin(alpha)*dczfnaca(alpha))
xx=xx+((roe*u**2)/2)*(Sf*df*math.cos(alpha)*czfnaca(alpha))
B=[(roe*u**2/2)*(Sf*dczfnaca(alpha))+.1,xx+.1]
#B=[-K[0,1]+.1,-K[1,1]+.1]
Bc=np.dot(M1,np.transpose(B))
#==========
#=======
# Calcul du controle exact
#=======
#==
lsec=np.linspace(1,4,4)*0
Sol=np.linspace(1,4,4)*0
Gramm=np.zeros((4,4),float)
P1=np.zeros((2,npt),float)
DP1=np.zeros((2,npt),float)
Q1=np.zeros((2,npt),float)
DQ1=np.zeros((2,npt),float)
aux=np.linspace(1,npt,npt)*0
CMt=np.transpose(CM)
KMt=np.transpose(KM)
#===========
coef=np.sqrt(a0*b0)
r=np.sqrt(a0/b0)

for i1 in range(4):
   Phi=np.linspace(1,4,4)*0
   Phi[i1]=1
   P1=resoladj(npt,dt,CMt,KMt,Phi)
   for ikl in range(2):
       DP1[ikl,0]=0
       for ider in range(1,npt):
           DP1[ikl,ider]=(P1[ikl,ider]-P1[ikl,ider-1])/dt       
   for ip0 in range(npt):
       sum1=0
       for ip in range(1,ip0+1):
           f=Ec[0]*DP1[1,ip]-Bc[0]*P1[0,ip]+Ec[1]*DP1[1,ip]-Bc[1]*P1[1,ip]
           sum1=sum1+dt*f*math.sinh(r*(ip0-ip)*dt)
       aux[ip0]=sum1        
   for it in range(npt):
        delta[it]=((math.sinh(r*dt*(it+1))/math.sinh(r*dt*(npt)))*aux[npt-1]-aux[it])/coef     
   for i2 in range(4):
	   deltaPhi=np.linspace(1,4,4)*0
	   deltaPhi[i2]=1
	   Q1=resoladj(npt,dt,CMt,KMt,deltaPhi)
	   for ikl in range(2):
		   DQ1[ikl,0]=0
		   for ider in range(1,npt):
			   DQ1[ikl,ider]=(Q1[ikl,ider]-Q1[ikl,ider-1])/dt
	   sum2=0.
	   for kt in range(npt):
			g=Ec[0]*DQ1[0,kt]-Bc[0]*Q1[0,kt]+Ec[1]*DQ1[1,kt]-Bc[1]*Q1[1,kt]
			sum2=sum2+dt*delta[kt]*g
	   Gramm[i1,i2]=sum2
vp=la.eigvals(Gramm)
lsec[0]=dz0-CM[0,0]*z0-CM[0,1]*gamma0
lsec[1]=dgamma0-CM[1,0]*z0-CM[1,1]*gamma0
lsec[2]=-z0
lsec[3]=-gamma0
#lasolution=la.solve(Gramm,np.transpose(lsec))
P1=resoladj(npt,dt,CMt,KMt,lsec)

for i1 in range(2):
    DP1[i1,0]=(P1[i1,1]-P1[i1,0])/dt
    for it in range(1,npt):
        DP1[i1,it]=(P1[i1,it]-P1[i1,it-1])/dt
        
for ip0 in range(npt):
    sum1=0
    for ip in range(ip0+1):
            f=Ec[0]*DP1[0,ip]-Bc[0]*P1[0,ip]+Ec[1]*DP1[1,ip]-Bc[1]*P1[1,ip]
            sum1=sum1+dt*f*math.sinh(r*(ip0-ip)*dt)
    aux[ip0]=sum1
deltamax=3*raddeg
for it in range(npt):
    delta[it]=min(deltamax,max(-deltamax,((math.sinh(r*dt*(it+1))/math.sinh(r*dt*npt))*aux[npt-1]-aux[it])/coef))/r
#print 'delta', delta
for it in range(1,npt-1):   
    deltadot[it]=(delta[it+1]-delta[it-1])/(2*dt)
    
deltadot[0]=deltadot[1]
deltadot[npt-1]=deltadot[npt-2]

for ii in range(2,npt):
    for jj in range(2):
        sum=0
        for k in range(2):
            sum=sum-dt*CM[jj,k]*(X[ii-1,k]-X[ii-2,k])+dt**2*KM[jj,k]*X[ii-1,k]
        X[ii,jj]=2*X[ii-1,jj]-X[ii-2,jj]+dt**2*(Bc[jj]*delta[ii-1]+Ec[jj]*deltadot[ii-1])-sum
        z[ii]=X[ii,0]
        gamma[ii]=X[ii,1]
z[0]=z0
z[1]=z0+dt*dz0
gamma[0]=gamma0
gamma[1]=gamma0+dt*dgamma0
        
plt.figure(2)
plt.subplot(3,1,1)
plt.plot(temps,z)
#plt.xlabel('Time')
plt.ylabel('Heaving')
plt.title('Heaving with exact control V(m/s)='+str(umax))
plt.subplot(3,1,2)
plt.plot(temps,gamma)
plt.title('Pitching with exact control V(m/s)='+str(umax))
#plt.xlabel('Time')
plt.ylabel('Pitching')
plt.subplot(3,1,3)
plt.plot(temps,delta)
plt.xlabel('Time')
plt.ylabel('Control')
plt.title('Exact Control V(m/s)='+str(umax))


#=====
#   methode otusa
#======
# Resolution dynamique
#===========
temps=(np.linspace(1,npt,npt)-1)*dt
z= np.linspace(1,npt,npt).reshape(-1,1)*0
dz=np.linspace(1,npt,npt)*0
gamma= np.linspace(1,npt,npt).reshape(-1,1)*0
dgamma=np.linspace(1,npt,npt)*0
#=======
X=np.bmat([z,gamma])
Xdot=np.linspace(1,npt,npt).reshape(-1,1)*0
delta=np.linspace(1,npt,npt)*0
deltadot=np.linspace(1,npt,npt)*0
#==============
# conditions initiales
#==============
z0=0.
dz0=0.
gamma0=-2.*raddeg
dgamma0=0*raddeg
deltamax=2.*raddeg
X[0,0]=z0   
X[0,1]=gamma0
X[1,0]=z0+dt*dz0
X[1,1]=gamma0+dt*dgamma0
#============
CM=np.dot(M1,C)
KM=np.dot(M1,K)
vppk=la.eigvals(KM)
E=[C[0,1],C[1,1]]
Ec=np.dot(M1,np.transpose(E))
B=[-K[0,1],-K[1,1]]
Bc=np.dot(M1,np.transpose(B))

#======
for ii in range(2,npt):
    if ii>= 500:
        deltamax=1.3*raddeg
    if ii>= 1000:
        deltamax=.65*raddeg
    if ii>= 2000:
        deltamax=.4*raddeg
    if ii>=3000:
        deltamax=.1*raddeg
    for jj in range(2):
        sum=0
        for k in range(2):
            sum=sum-dt*CM[jj,k]*(X[ii-1,k]-X[ii-2,k])+dt**2*KM[jj,k]*X[ii-1,k]
        X[ii,jj]=2*X[ii-1,jj]-X[ii-2,jj]+dt**2*Bc[jj]*deltadot[ii-1]-sum
        if (jj==1):
			if(abs(X[ii,1]-X[ii-1,1])<=.000001):
				deltadot[ii]=0
			else:
				Xdot[ii]=np.sign(X[ii,1]-X[ii-1,1])
				deltadot[ii]=-deltamax*Xdot[ii]
        
    
plt.figure(3)
for k in range(npt):
    z[k]=X[k,0]
    gamma[k]=X[k,1]
plt.subplot(3,1,1)
plt.plot(temps,z)
#plt.xlabel ('Time')
plt.ylabel('Heaving')
plt.title('Heaving V(m/s)='+str(umax))
plt.subplot(3,1,2)
plt.plot(temps,gamma)
plt.title ('Pitching V(m/s)='+str(umax))
#plt.xlabel('Time')
plt.ylabel('Pitching')
plt.subplot(3,1,3)
plt.plot(temps,deltadot)
plt.xlabel('Time')
plt.ylabel('Control')
plt.title('Control OTUSA type using the steering box V(m/s)='+str(umax))
plt.show()