%
%  This function implments Example B
%  continuous time dynamic optimization with no terminal constraints
%
function dummy
%
  clear
%
%  set some options
%
  options = optimset('MaxFunEvals',1e6,'TolFun',1e-4,'TolX',1e-6,'MaxIter',100);
%
%  set the initial parameters
%
  A = [1 -2; 1 0]; B=[0;-2];
  Qf = 1; R = 1; Q = 1;  
  s0 = [0;0;0];
  tf = 1.0;
  t0 = 0;
%
%  N is the number of sample points between t0 and tf
%
  N = 10;
%
%  This is the initial guess for the control input. 
%  Comment this out as you refine the number of points
%
  u = 2*linspace(1,0,N);
%
  times = linspace(t0,tf,N);
  r = ones(1,N);
%
%  interpolate between old values as we refine
%  comment these lines out for your initial estimate
%  uncomment them as you refine
%
%   load old_ut
%   times = linspace(t0,tf,N);
%   r = ones(1,N);
%   u = interp1(old_t,old_u,times');
%   figure; plot(old_t,old_u,'+',times,u,'o'); legend('old','new');
%   title([ 'N = ' num2str(N)]); grid; 
%
%  run the code
%  Note: everything after 'options' is an argument passed to function
%     fminunc_a
%
  [u,fval,exitflag,output] = fminunc(@fminunc_b,u',options,s0,times,A,B,r,Qf,R,Q);
%
  fval
  fprintf(1,'\n');
  fprintf(1,' Exit Flag is %d \n', exitflag);
  fprintf(1,' The number of iterations is %d \n', output.iterations);
  fprintf(1,' The number of function evaluations is %d \n', output.funcCount);
%
%  save the time and control sequence to initialize the next run if this
%  run was succesQful
%
  if (exitflag > 0)
    old_u = u';
    old_t = times';
    save old_ut old_u old_t
  end;
%
%  Now plot the results, first integrate forward in time once more
%
  [t,s] = ode45(@ode_b,times,s0,[],times,u,A,B,r,Qf,R,Q);
%  
   x1 = s(:,1);
   x2 = s(:,2);
% 
  figure
  orient tall
  subplot(3,1,1); plot(t,u); grid; ylabel('u(t)');
  title(['N = ', num2str(N) ', S_f = ' num2str(Qf) ', R = ', num2str(R) ', Q = ' num2str(Q)]);
  subplot(3,1,2); plot(t,r,'--',t,x1,'-'); legend('r(t)','x_1(t)'); ylabel('r(t) and x_1(t)'); grid;
  subplot(3,1,3); plot(t,x2,'-'); ylabel('x_2(t)'); grid; xlabel('Time (sec)');
%
  return;
%  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  This function computes the cost for the Mayer form
%  It will integrate the state function forward in time
%
function phi = fminunc_b(u,s0,times,A,B,r,Qf,R,Q);
%
%  s = [x1 x2  q]'
%
%  Now integrate the state equations forward in time
%  Note: everything after 'options' is an argument passed to function
%  ode_a. Here we are using the default options, so we enter a []
%
   [t,s] = ode45(@ode_b,times,s0,[],times,u,A,B,r,Qf,R,Q);
%
  x1 = s(end,1);
  x2 = s(end,2);
  q = s(end,3);
%
%  compute the final cost
%
  phi = 0.5*Qf*(x1-r(end))^2+q;
%
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  This is our state description
%
function f = ode_b(t,s,times,ut,A,B,r,Qf,R,Q)
%
%  s = [x1 x2 q]'
%
  x1 = s(1); x2 = s(2); q = s(3); 
  x = [x1; x2];
%
%  get the u value for this time
%
  u = interp1(times',ut',t);
  rr = interp1(times',r',t);
%
%  s' = f(s,u,t)
%
   f = [A*x+B*u; 0.5*R*(x1-rr)^2+0.5*Q*u^2];
%
  return;
