import numpy as np
			

def evalPol(a,x):
	k=len(a)-1
	p = a[k] #a_n
	k -= 1
	while k>=0:
		p = a[k]+p*x
		k -= 1
	return p
	
			

a=[1+1j*0,1+1j*0,1+1j*0]
p = evalPol(a,1j)
			

def evalDerivPol(a,x):
	k=len(a)-1
	p = k*a[k] #(n-1)na_n
	k -= 1
	while k>=1:
		p = k*a[k]+p*x
		k -= 1
	return p
	
			

a=[1+1j*0,1+1j*0,1+1j*0]
p = evalDerivPol(a,1j)
			

def evalDeriv2Pol(a,x):
	k=len(a)-1
	p = (k-1)*k*a[k] #na_n
	k -= 1
	while k>=2:
		p = (k-1)*k*a[k]+p*x
		k -= 1
	return p
	
			

a=[1+1j*0,1+1j*0,1+1j*0]
p = evalDeriv2Pol(a,1j)
			

a=[0+0j,0+0j,0+0j,1+0j]
p = evalDeriv2Pol(a,1)
			

def iteration(a,x):
	n = len(a)-1
	p = evalPol(a,x)
	if p==0: return 0
	pp = evalDerivPol(a,x)
	ppp = evalDeriv2Pol(a,x)
	G = pp/p
	H = G*G-ppp/p
	sq = np.sqrt((n-1)*(n*H-G*G))
	z1 = G+sq
	z2 = G-sq
	if np.absolute(z2) > np.absolute(z1):
		z1 = z2
	d = n/z1
	return d
	
def laguerre(a,x,tol,maxiter=100):
	d = iteration(a,x)
	if d==0:
		return x
	x = x-d
	iter = 1
	while np.absolute(d)>tol:
		d = iteration(a,x)
		if d==0:
			return x
		x = x-d
		iter += 1
		if iter > maxiter:
			raise Exception("Non convergence")
	return x
	
	
				

def polynome(x0,x1,x2):
	return np.array([-x0*x1*x2,x0*x2+x0*x1+x1*x2,-(x0+x1+x2),1],dtype=np.complex128)
				

x0 = 1
x1 = 2+3*1j
x2 = 2-3*1j
a = polynome(x0,x1,x2)
try:
	x = laguerre(a,0,1e-4)
except:
	print("Non convergence")
	x = 0
				

def factorisation(a,x0):
	n = len(a)-1
	b = np.zeros(n,dtype=np.complex128)
	k = n-1
	b[k] = a[k+1]
	k -= 1
	while k>=0:
		b[k] = a[k+1]+x0*b[k+1]
		k -= 1
	return b
				 

def racines(a,tol=1e-4):
	n = len(a)-1
	r = []
	for k in range(n-1):
		x = laguerre(a,0,tol)
		r.append(x)
		a = factorisation(a,x)
	r.append(-a[0]/a[1]) # racine d'une polynome de degré 1
	return r
				 

x0 = 1
x1 = 2+3*1j
x2 = 2-3*1j
a = polynome(x0,x1,x2)
r = racines(a)
				 

def racines2(a,tol=1e-4):
	r = racines(a,tol)
	rac = []
	for x in r:
		x = laguerre(a,x,tol)
		rac.append(x)
	return rac
				  

x0 = 1
x1 = 2+3*1j
x2 = 2-3*1j
a = polynome(x0,x1,x2)
r = racines2(a)
				 

x0 = -3
x1 = 1+1j
x2 = 1-1j
a = polynome(x0,x1,x2)
r = racines2(a,tol=1e-6)
				 

x0 = 1
x1 = 1
x2 = 2
a = polynome(x0,x1,x2)
r = racines2(a,tol=1e-6)
				 

x0 = -3
x1 = 1+1j
x2 = 1+2j
a = polynome(x0,x1,x2)
r = racines2(a,tol=1e-6)
				 

from numpy.linalg import solve

def reponseIndicielle(a,t,tol=1e-3):
	r = racines2(a,tol)
	n = len(r)
	A = np.zeros((n,n),dtype=np.complex128)
	for j in range(n):
		for i in range(n):
			A[i][j] = r[j]**i
	B = np.zeros(n)
	B[0] = -1
	K = solve(A,B)
	s = 1
	for i in range(n):
		s += K[i]*np.exp(r[i]*t)
	return np.real(s)
					 

from matplotlib.pyplot import *
T = 1
t = np.linspace(0,20*T,1000)
figure()
for m in [0.5,0.8,0.99,2]:
	a=np.array([1,2*m*T,T**2],dtype=np.complex128)
	s = reponseIndicielle(a,t)
	plot(t,s,label="m=%0.2f"%m)
grid()
legend(loc="lower right")
xlabel("t/T")
ylabel("s")

					  

m=1
a=np.array([1,2*m*T,T**2],dtype=np.complex128)
r = racines(a,1e-6)
					  

def racinesMultiples(r):
	n = len(r)
	rac = []
	mul =[]
	for i in range(n):
		if r[i] in rac:
			j = rac.index(r[i])
			mul[j] += 1
		else:
			rac.append(r[i])
			mul.append(1)
	return rac,mul
			
						

rac,mul = racinesMultiples(r)
						

def reponseIndicielleBis(a,t,tol=1e-3,eps=1e-8):
	r = racines2(a,tol)
	rac,mul = racinesMultiples(r)
	r = []
	for i in range(len(rac)):
		rho = np.absolute(rac[i])
		for k in range(mul[i]):
			r.append(rac[i]+k*eps*rho)
	n = len(r)
	A = np.zeros((n,n),dtype=np.complex128)
	for j in range(n):
		for i in range(n):
			A[i][j] = r[j]**i
	B = np.zeros(n)
	B[0] = -1
	K = solve(A,B)
	s = 1
	for i in range(n):
		s += K[i]*np.exp(r[i]*t)
	return np.real(s)
					

T = 1
t = np.linspace(0,20*T,1000)
figure()
for m in [0.5,0.8,1,2]:
	a=np.array([1,2*m*T,T**2],dtype=np.complex128)
	s = reponseIndicielleBis(a,t,eps=1e-3)
	plot(t,s,label="m=%0.2f"%m)
grid()
legend(loc="lower right")
xlabel("t/T")
ylabel("s")

					  

x0 = -1+1j
x1 = -1+1j
x2 = -1+1j
a = polynome(x0,x1,x2)
r = racines2(a)
					  

figure()
t = np.linspace(0,10,1000)
s = reponseIndicielleBis(a,t,eps=1e-3)
plot(t,s)
grid()
xlabel("t")
ylabel("s")
					  

figure()
t = np.linspace(0,10,1000)
s = reponseIndicielleBis(a,t,eps=1e-8)
plot(t,s)
grid()
xlabel("t")
ylabel("s")