/* I have tried to make this code run on both the commercial macsyma,
   and the FAIF maxima.  This version of the code replaces the initial
   condition value and derivative value at t=0 with k0 and k1 because it's
   easier.
   To solve the optimal control problem, the solution is 0 at t=T,
   so use odematriccati_bc with the solution from odematriccati.
*/   
if version='version then (
   if get('odematsys, 'version) = 'false then load('odematsys),
   if get('diag, 'version) = 'false then load('diag),
   if get('cholesky_decomp_symb,'version) = 'false then load('cholesky_decomp_symb),
   alias(odematsysA, odematsys))
   else (
   alias(odematsysA, odematsys_ic),
   /* batch("e:\\array_symbols.mac"),
   array_symbols_init(),*/
   odematsys_ic(a,b,[c]):=block([L:array_symbols_unarray([a,b]),used,e],
      e:first(L),
      used:second(L),
      a:first(e),
      b:second(e),
      if length(c) > 0 then array_symbols_atvalue1(c,used,t,0),
      odematsys(a,b)));

odematriccati(Q,B,J,vars):=block([H,A,G,S,L,X,Xsol,W,Y,
      indepvar,nrows,ncols],
   indepvar:first(listofvars(vars)),
   H:cholesky_decomp_symb(Q),
   G:transpose(H).J.H,
   A:invert(H).(B.H-diff(H,indepvar)),
   S:transpose(A)-A,
   print("J",J,"H",H,"A",A, "S",S,"G",G),
   nrows:length(vars),
   ncols:length(vars[1]),
   L:block([auxeqn,auxvars],
      auxvars:genmatrix(lambda([i,k],L[i,k](indepvar)),nrows,ncols),
      odematsysA(
         genmatrix(lambda([i,k],'diff(L[i,k](indepvar),indepvar)),nrows,ncols)
            -(auxvars.S/2),
         auxvars,
         ident(nrows))),
   L:rhs(first(L)),
   print("constant",((L).(G+transpose(A).A+(S.S/4)
            +((transpose(diff(A,indepvar))+diff(A,indepvar))/2)).transpose(L))),
   Xsol:block([jaceqn,jacvars,meqn],
      jacvars:genmatrix(lambda([i,k],X[i,k](indepvar)),nrows,ncols),
      meqn:genmatrix(lambda([i,k],'diff(X[i,k](indepvar),indepvar,2)),
		     nrows,ncols)
            -((jacvars.L).(G+transpose(A).A+(S.S/4)
            +((transpose(diff(A,indepvar))+diff(A,indepvar))/2)).transpose(L)),
   odematsysA(meqn,jacvars,
      genmatrix(lambda([i,k],K0[i,k]),nrows,ncols),
      genmatrix(lambda([i,k],K1[i,k]),nrows,ncols))),
   Xsol:rhs(first(Xsol)),
   W:(transpose(L).invert(Xsol).diff(Xsol,indepvar).L)-((A+transpose(A))/2),
   Y:invert(transpose(H)).W.H.invert(H)
);
/* display2d:false; */
trace(odematsys);
Y:odematriccati(matrix([1,0],[0,1]),
   matrix([1,0],[0,1]),ident(2),genmatrix(lambda([i,j],Y[i,j](t)),2,2));
solve(subst([k0[1,1]=1,k0[1,2]=1,k0[2,1]=1,k0[2,2]=1],apply(append,map(append,args(Y)))),[ k1[1,1],k1[2,2],k1[1,2],k1[2,1]]);
/*

   if length(initconds)=1 then block([initpoint,Xi,xdot],
      initpoint:initconds[1],
      Xi:at(diff(Xsol,indepvar),indepvar=initpoint)-
         invert(transpose(L)).((A+transpose(A))/2).invert(L),
      xdot:genmatrix(
         lambda([i,k],at('diff(X[i,k](indepvar),indepvar),indepvar=0)=0),
         nrows,ncols),
      Xsol:lratsubst( apply( append,args(xdot)), Xsol),
      Xi:lratsubst( apply( append,args(xdot)), Xi),
      Xi:algsys(apply(append,args(Xi)),
         apply(append,args(
            genmatrix(lambda([i,k],at(X[i,k](indepvar),indepvar=0)),
               nrows,ncols)))),
      Xsol:lratsubst(first(Xi),Xsol),
      print("Xsol ",Xsol)
   ),
*/