%
%  This function implments Example A
%  continuous time dynamic optimization with no terminal constraints
%
function dummy
%
  clear
%
%  set some options
%
  options = optimset('MaxFunEvals',1e6,'TolFun',1e-4,'TolX',1e-8,'MaxIter',100);
%
%  set the initial parameters
%
  s0 = [1;1];
  tf = 0.5;
  t0 = 0;
%
%  N is the number of sample points between t0 and tf
%
  N = 200;
%
%  This is the initial guess for the control input. 
%  Comment this out as you refine the number of points
%
%  u = -35*linspace(1,0,N);
%
  times = linspace(t0,tf,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(0,tf,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_a,u',options,s0,times);
%
  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 successful
%
  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_a,times,s0,[],times,u);
%  
   x = s(:,1);
   y = s(:,2);
% 
  figure
  orient tall
  subplot(2,1,1); plot(t,u); grid; ylabel('u(t)');
  subplot(2,1,2); plot(t,x,'-',t,y,'--'); legend('x(t)','y(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_a(u,s0,times);
%
%  s = [x y]'
%
%  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_a,times,s0,[],times,u);
%
  x = s(end,1);
  y = s(end,2);
%
%  compute the final cost
%
  phi = 0.5*(x^2+y^2);
%
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  This is our state description
%
function f = ode_a(t,s,times,ut)
%
%  s = [x y]'
%
  x = s(1); y = s(2); 
%
%  get the u value for this time
%
  u = interp1(times',ut',t);
%
%  s' = f(s,u,t)
%
   f = [t+u ; x];
%
   return;
