# -*- 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

def g(arg):
	return -0.1*arg**3
def dg(arg):
	return -.1*3*arg**2    
def d2g(arg):
	return -.1*6*arg
	
def resol(npt,Phi,xi,k,dt):
	k=k+1
	z=np.linspace(1,npt,npt)*0
	z[0]=Phi[0]
	z[1]=Phi[0]+dt*Phi[1]
	for kk in range(2,npt):
		z[kk]=2*z[kk-1]-z[kk-2]+xi*dt*(z[kk-1]-z[kk-2])-dt**2*k*z[kk-1]
	return z
		
def resolnl(npt,qq,xi,k,ydot,dt):
	k=k+1
	sol1=np.linspace(1,npt,npt)*0
	sol1[0]=qq[0]
	sol1[1]=qq[0]+dt*qq[1]
	for lijl in range(2,npt):
		sol1[lijl]=2*sol1[lijl-1]-sol1[lijl-2]+(xi-dg(ydot[lijl-1]))*(sol1[lijl-1]-sol1[lijl-2])*dt-sol1[lijl-1]*(k+d2g((ydot[lijl]-ydot[lijl-1])/dt))*dt**2
	return sol1	
xi=-.02
k=2.
y0=.4
y1=.01
T=10.
npt=20000
dt=T/npt
b=1.
y=np.linspace(1,npt,npt)*0
ydot=np.linspace(1,npt,npt)*0.
ysanscont=np.linspace(1,npt,npt)*0.
yinter=np.linspace(1,npt,npt)*0.
ydotinter=np.linspace(1,npt,npt)*0.
temps=np.linspace(1,T,npt)-1
p=np.linspace(1,npt,npt)*0
beta=np.linspace(1,npt,npt)*0.
Phi=[0,0]
q=[0,0]

lanbda=np.zeros((2,2))

for i in range(2):
	for k in range(2):
		Phi[k]=0
	Phi[i]=1
	sol1=resol(npt,Phi,xi,k,dt)
	for j in range(2):
		for k in range(2):
			q[k]=0
		q[j]=1
		sol2=resol(npt,q,xi,k,dt)
		s=0.
		for ij in range(1,npt-1):
			s=s+sol1[ij]*sol2[ij]
		s=s+(sol1[0]*sol2[0]+sol1[npt-1]*sol2[npt-1])/2
		s=s*dt*b**2
		lanbda[i,j]=s
l=np.zeros(2)
l[0]=y1+xi*y0
l[1]=-y0
qq=la.solve(lanbda,np.transpose(l))
betamax=100
p[0]=qq[0]
beta[0]=min(betamax,max(-betamax,-b*p[0]))
p[1]=qq[0]+dt*qq[1]
beta[1]=min(betamax,max(-betamax,-b*p[1]))
for ijk in range(2,npt):
	p[ijk]=2*p[ijk-1]-p[ijk-2]+dt*xi*(p[ijk-1]-p[ijk-2])-(k+1)*dt**2*p[ijk-1]
	beta[ijk]=min(betamax,max(-betamax,-b*p[ijk]))
y[0]=y0
y[1]=y0+dt*y1
for ikj in range(2,npt):
	y[ikj]=2*y[ikj-1]-y[ikj-2]-dt*xi*(y[ikj-1]-y[ikj-2])-(k+1)*dt**2*y[ikj-1]+dt**2*b*beta[ikj-1]
plt.figure(1)
plt.subplot(1,2,1)
plt.plot(temps,y)
plt.xlabel('temps')
plt.ylabel('Solution de depart')
plt.title('Initialisation de l algorithme')
plt.subplot(1,2,2)
plt.plot(temps,beta)
plt.xlabel('temps')
plt.ylabel('Controle de depart')
plt.title('Initialisation de l algorithme')
ysanscont[0]=y[0]
ysanscont[1]=y[1]
ydot[0]=y1
ydot[1]=y1
for ikj in range(2,npt):
	y[ikj]=2*y[ikj-1]-y[ikj-2]-dt*xi*(y[ikj-1]-y[ikj-2])-(k+1)*dt**2*y[ikj-1]+dt**2*b*beta[ikj-1]+dt**2*g(ydot[ikj-1])
	ydot[ikj]=(y[ikj]-y[ikj-1])/dt
	ysanscont[ikj]=2*ysanscont[ikj-1]-ysanscont[ikj-2]-dt*xi*(ysanscont[ikj-1]-ysanscont[ikj-2])-(k+1)*dt**2*ysanscont[ikj-1]+dt**2*g((ysanscont[ikj-1]-ysanscont[ikj-2])/dt)
plt.figure(2)
plt.subplot(1,2,1)
plt.plot(temps,y)
plt.xlabel('temps')
plt.ylabel('Solution non lineaire avec controle lineaire')
plt.title('Solution non lineaire avec controle lineaire')
plt.subplot(1,2,2)
plt.plot(temps,ysanscont)
plt.xlabel('temps')
plt.ylabel('Solution non lineaire sans controle')
plt.title('Solution non lineaire sans controle')
itermax=15
y[0]=y0
y[1]=y0+dt*y1
yinter[0]=y0
yinter[1]=y0+dt*y1
ydot[0]=y1
ydot[1]=y1
ydotinter[0]=y1
ydotinter[1]=y1
norm=np.linspace(1,itermax,itermax)*0
for iter in range(itermax):
	p[0]=qq[0]
	beta[0]=-b*p[0]
	p[1]=qq[0]+dt*qq[1]
	beta[1]=-b*p[1]
	for ii in range(2,npt):
		yddot=(ydot[ii-1]-ydot[ii-2])/dt
		p[ii]=2*p[ii-1]-p[ii-2]+dt*(xi-dg(ydot[ii-1]))*(p[ii-1]-p[ii-2])-dt**2*((k+1)+d2g(ydot[ii-1])*yddot)*p[ii-1]
		beta[ii]=-b*p[ii]
		yinter[ii]=2*yinter[ii-1]-yinter[ii-2]-dt*xi*(yinter[ii-1]-yinter[ii-2])-dt**2*((k+1)*yinter[ii-1]-g(ydotinter[ii-1]))+b*beta[ii-1]*dt**2
		ydotinter[ii]=(yinter[ii]-yinter[ii-1])/dt
	qqa=[1,0]
	q1=resolnl(npt,qqa,xi,k,ydotinter,dt)
	qqa=[0,1]
	q2=resolnl(npt,qqa,xi,k,ydotinter,dt)
	s1=0.
	s2=0.
	for ll in range(2,npt-1):
             s1=s1+g(ydotinter[ll])*q1[ll]
             s2=s2+g(ydotinter[ll])*q2[ll]
	s1=s1+(g(ydotinter[0])*q1[0]+g(ydotinter[npt-1])*q1[npt-1])/2
	s2=s2+(g(ydotinter[0])*q2[0]+g(ydotinter[npt-1])*q2[npt-1])/2
	s1=s1*dt
	s2=s2*dt
	l[0]=y1+xi*y0+s1
	l[1]=-y0+s2
	qqp=la.solve(lanbda,np.transpose(l))
	norm[iter]=abs(np.inner(qqp,qqp)-np.inner(qq,qq))
	qq=qqp
	for inew in range(2,npt):
		y[inew]=2*y[inew-1]-y[inew-2]-xi*(y[inew-1]-y[inew-2])*dt+dt**2*(g(ydot[inew-1])+b*beta[inew-1]-(k+1)*y[inew-1])
		ydot[inew]=(y[inew]-y[inew-1])/dt   
nbiter=np.linspace(1,itermax,itermax)
plt.figure(2)
plt.subplot(2,2,1)
plt.plot(temps,y)
plt.xlabel('temps')
plt.ylabel('Solution non lineaire avec controle non lineaire')
plt.title('Solution non lineaire avec controle non lineaire')
plt.subplot(2,2,2)
plt.plot(temps,beta)
plt.xlabel('temps')
plt.ylabel('Controle non lineaire')
plt.title('Controle non lineaire')
plt.subplot(2,2,3)
plt.plot(nbiter,norm)
plt.xlabel('Iterations')
plt.ylabel('Evolution de: norme (Pn+1)-norme(Pn)')
plt.title('Courbe de convergence de l algorithme')
plt.subplot(2,2,4)
plt.plot(y,ydot)
plt.xlabel('y')
plt.ylabel('ydot')
plt.title('Visualisation dans le plan des phases')
plt.show()