# Copyright INRIA
if SearchText(`Release 3`,interface(version))<>0 then
	matadd:=add;
elif SearchText(`Release 4`,interface(version))<>0 then
	1;
else
  ERROR(`Unknown Release`)
fi;

read(`euler.map`);


#----------------------------------------------------------------------------
# AND NOW THE N-LINK PNDULUM
#-----------------------------------------------------------------------------

#----------------------------------------------------------------------------
# TeX Notations for my problem ( x= `x` is in fact useless ut just help
#				to remind the state names )
#-----------------------------------------------------------------------------

addnotations( { x = `x`  , th = `\\theta`, y =`y` , t = `\\eta`} );

#----------------------------------------------------------------------------
# Lagrangian for My problem : the N-link pendulum 
# l[i] : length of links  r[i]:=l[i]/2;
#---------------------------------------------------------------------------

# number of link in the pendulum 
n:=10;

# [x,y,theta] is of size three
mm:=3: 

LLi:=proc(i)
	(1/2)*m[i]*( ( xd[i-1]- r[i] *sin(th[i])*thd[i])**2 + 
	( yd[i-1]+ r[i]*cos(th[i])*thd[i])**2 ) + 1/2*J[i]*(thd[i])**2
	-m[i]*g*(y[i-1]+r[i]*sin(th[i])):
end:

# The point zero is fixed 

x[0]:=0:xd[0]:=0:
y[0]:=0:yd[0]:=0:

L:=sum( LLi(j),j=1..n):

sorties(`systeme.tex`,`Lagrangian`,L):

# Lagrangian variables :
q := [ seq (op([x[i],y[i],th[i]]),i=1..n)]:
qd := [ seq (op([xd[i],yd[i],thd[i]]),i=1..n)]:
qdd:= [ seq (op([xdd[i],ydd[i],thdd[i]]),i=1..n)]:

# Lhs of Euler equations 

EL:=euler_equations(L,q,qd,qdd):
EE:=map((i)->rhs(i),EL):
sortiesM(`systeme.tex`,`Lhs of Euler Equations`,EL):

#-----------------------------------------------------
# Rewritting the Euler equations to have a canonical form 
# for TeX output
#           ..         .          .
# El= ME(q)  q + CE(q) q^2 + RE(q,q)
# x^2 means transpose( x[1]^2 ,....,x[n]^2)
# Computation of ME,CE,RE 
#-----------------------------------------------------

XX:=CEuler(EE,q,qd,qdd):

# simplifying notations for output

ME1:=XX[1]:
ME1:=subs(seq(m[k]*r[k]*cos(th[k])=mrc[k],k=1..n),
	seq(m[k]*r[k]*sin(th[k])=mrs[k],k=1..n),eval(ME1)):

sortiesI(`systeme.tex`,`$mrc_k=m_k r_k \\cos(\\theta_k)$`);
sortiesI(`systeme.tex`,`$mrs_k=m_k r_k \\sin(\\theta_k)$`);
sorties(`systeme.tex`,`$M(q)\\ddot{q}+C(q)\\dot{q}^2 +R(q,\\dot{q})$,M:`,
	eval(ME1)):
sorties(`systeme.tex`,`C~:`,XX[2]):
sorties(`systeme.tex`,`R~:`,XX[3]):
#------------------------------------------------
# Constraints on the N-link Pendulum 
#------------------------------------------------
ncont:=2*n;
cont:=[ seq(op([ x[i]-x[i-1] -2*r[i]*cos(th[i]),
		y[i]-y[i-1] -2*r[i]*sin(th[i])]),i=1..n)]:

sortiesM(`systeme.tex`,`Constraints `,cont);

# time derivative of constraints;

cont1:=map ( (exp)->( time_diff(exp,q,qd,qdd)),cont):

# Derivatives of constraints are of type Aprim qd = 0 

Aprim:=genmatrix(cont1,qd):

sorties(`systeme.tex`,`Time derivative of constraints : $A'(q)\\dot{q}$`,Aprim);

#---------
# Total Energy Ec + Ep 
#---------

EEi:=proc(i)
(1/2)*m[i]*( ( xd[i-1]- r[i] *sin(th[i])*thd[i])**2 + 
	( yd[i-1]+ r[i]*cos(th[i])*thd[i])**2 ) + 1/2*J[i]*(thd[i])**2
	+m[i]*g*(y[i-1]+r[i]*sin(th[i])):
end:

ET:=sum( EEi(j),j=1..n):
Scont1:=solve({op(cont1)},{ seq (op([xd[i],yd[i]]),i=1..n)}):
Scont2:=solve({op(cont)},{ seq (op([x[i],y[i]]),i=1..n)}):
ET:=subs(Scont1,ET):
ET:=subs(Scont2,ET):

sorties(`systeme.tex`,`N-pendulum energy~:`,ET);

#----------------------------------------------
# Computing SS:=S(q);
# S(q) will solve Aprim S(q) = 0 
# in The Euler equations  Equ= A(q)' lambda + u 
# the term A(q)lambda can be eliminated 
# if we left-multiply euler equations by S(q)'
#----------------------------------------------

SS:=linsolve(Aprim,matrix(ncont,1,0)):

#------------------------------------------------------------------------
# The constraints are now dotq=S(q)eta 
# can be used to see that eta[i]=thd[i] ( eta[i] is the maple variable ti )
# but the indices can be mixed and linsolve doesn't 
# always return the same result 
# We have to check the correspondance between t.sig(i)=thd[i]
# and to change SS to have a good corespondance 
#-------------------------------------------------------------------------

permut:={seq(SS[mm*i,1]=t_s[i],i=1..n)}:
SS:=subs(permut,eval(SS)):
permut:={seq(t_s[i]=t[i],i=1..n)}:
SS:=subs(permut,eval(SS)):

S:= genmatrix(convert(convert(SS,vector),list),[seq( t[i],i=1..n)]):
sorties(`systeme.tex`,`$\\dot{q}=S(q)\\eta$ Kernel of $A(q)'$~:`,S):

#-----------------------------------------------------
# this multiplication eliminates the term A(q) lambda 
# in the Euler equations 
#-----------------------------------------------------

E1:=multiply(transpose(S),EE):

# sortiesM(`systeme.tex`,`$S(q)^T E$`,E1);

#-----------------------------------------------------
# since Aprim(q) dotq=0 
# .
# q= S(q) eta ; here  eta = [t1,t2]
#                                 ..
# we use this equation to compute q
# Warning : t1 and t2 are time dependent
#-----------------------------------------------------

qt  := [ seq (t[i]  ,i=1..n)]:
qtd := [ seq (td[i] ,i=1..n)]:
qtdd:= [ seq (tdd[i],i=1..n)]:

qqdd:=map((x,y,z,t)-> time_diff(x,y,z,t),eval(SS),
	[op(q),op(qt)],[op(qd),op(qtd)],[op(qdd),op(qtdd)]):

#-----------------------------------------------------
#       ..                       .
# using  q= d/dt [ S(q) eta] and q= S(q) eta 
# we can subsitute these expressions in E1
#-----------------------------------------------------

E2:=subs(seq(qdd[i]=qqdd[i,1],i=1..nops(qdd)),eval(E1)):
E3:=subs(seq(qd[i]=SS[i,1],i=1..nops(qd)),eval(E2)):

#-----------------------------------------------------
# The global system is now 
#             .
#  E3 = 0 and q= S(q) eta 
#-----------------------------------------------------

E3:=map((x)-> simplify(x),E3):

sortiesM(`systeme.tex`,
	`$S(q)^T E$ simplified with $\\dot{q}=S(q)\\eta $`,E3);

#------------------------------------------------------------------
# Trying to find canonical representation 
# for the simplified euler equations
#            .
# El= ME(q)  t + CE(q) t^2 + RE
# we use CEuler with a little trick in the parameter call qt,qt,qtd
#------------------------------------------------------------------

XX1:=CEuler(E3,qt,qt,qtd):

MM3:=map((i)->factor(combine(i,trig)),XX1[1]):
CC3:=map((i)->factor(combine(i,trig)),XX1[2]):
RR3:=map((i)->factor(combine(i,trig)),XX1[3]):

sorties(`systeme.tex`,`a cononical form $M(q)\\dot{t}+C(q)t^2 +R$,M:`,MM3);
sorties(`systeme.tex`,`C:`,CC3);
sorties(`systeme.tex`,`R:`,RR3);

sortiesI(`systeme.tex`,`Since $M,C,R$ only depends on $\\theta$ `);
sortiesI(`systeme.tex`,`,we can forget $x_i,y_i$ in the dynamical system `);
sortiesI(`systeme.tex`,` and just keep the $\\dot{\theta}=\\eta$ in the	constraints`);

#-----------------------------------------------------
# FORTRAN GENERATION 
#-----------------------------------------------------

#-----------------------------------------------------
# First routine npend(neq,t,th,ydot)
#                     .
# we have MM3(theta) eta + CC3(theta)*eta^2 +RR3(theta) = 0 
#   .
# theta = eta
#                           .
# ==> th=[ theta,eta] ydot= th
#-----------------------------------------------------


flist:=[subroutinem,`npend`,[`neq`,`t`,`th`,`ydot`],
           [
            [ parameterf,[`n=`.n]],
            [ declaref,doubleprecision,[`t,th(2*n),ydot(2*n),r(n),j(n),m(n)`]],
            [ declaref,doubleprecision,[`me3s(n,n),cc3s(n,n),const(n,1)`]],
            [ declaref,doubleprecision,[`w(3*n),rcond	`]],
            [ declaref,`implicit doubleprecision`,[`(t)`] ],
            [ declaref,integer,[`i,k,neq,ierr`]],
            [ declaref,`data g`,[`/ 9.81/`]],
            [ declaref,`data r`,[`/ n*1.0/`] ],
            [ declaref,`data m`,[`/ n*1.0/`] ],
            [ declaref,`data j`,[`/ n*0.3/`] ],
            [ matrixm,`me3s`,MM3 ] ,
            [ matrixm,`cc3s`,CC3 ] ,
            [ matrixm,`const`,RR3 ] ,
	    [ dom , `i ` ,1,`n `,1,[ equalf,`ydot(i)`,`th(i+n)`]],
	    [ dom , `i ` ,1,`n `,1,[
		[ dom , `k ` ,1,`n `,1,
			[[ equalf,`Const(i,1)`,
				` Const(i,1)+CC3S(i,k)*(th(k+n)**2)`]]],
		[ equalf,` Const(i,1)`,`-Const(i,1)`]]],
	    [commentf,` we must solve  M z =const `],
	    [commentf,` which gives ydot((n+1)..2*n) `],
	    [ callf , `dlslv`,[`me3s,n,n,Const,n,1,w, rcond,ierr,1`]],
	    [ if_then_m,ierr<>0,[
		[writef,6,ff_w,[]],
		[formatf,ff_w,[`'Matrice mal conditionnee'`]]]],
	    [ dom , `i ` ,1,`n `,1,[ equalf,`ydot(n+i)`,`const(i,1)`]],
	    [returnf]]]:


Gener(`npend.f`,flist):

# New ener.f
ET:=matrix(1,1,[ET]):

flist:=[subroutinem,`ener`,[`th`,`e`],
           [
            [ parameterf,[`n=`.n]],
            [ declaref,doubleprecision,[`th(2*n),thd(n),et(1,1),r(n),j(n),m(n)`]],
            [ declaref,`implicit doubleprecision`,[`(t)`] ],
            [ declaref,integer,[`i `]],
            [ declaref,`data g`,[`/ 9.81/`]],
            [ declaref,`data r`,[`/ n*1.0/`] ],
            [ declaref,`data m`,[`/ n*1.0/`] ],
            [ declaref,`data j`,[`/ n*0.3/`] ],
	    [ dom , `i ` ,1,`n `,1,[ equalf,`thd(i)`,`th(i+n)`]],
            [ matrixm,`et`,ET ] ,
	    [ equalf,`e`,`et(1,1)`],
	    [returnf]]]:

Gener(`ener.f`,flist):

# New np.f 

flist:=[subroutinem,`np`,[`i `],
           [
            [ declaref,integer,[`i `]],
	    [ equalf,`i `,n],
	    [returnf]]]:

Gener(`np.f`,flist):