%
%  This routine implements a simple dynamic programming algorithm
%    that can be used to match the class examples 
%
%  xmin, xmax = the minimum/maximum state values
%  Nx = the number of state values between (and including) xmin and xmax
%  umin,umax = the minimu/maximum control values
%  Nu = the number of control values between (and including) umin and umax
%  tmin,tmax = the minimum/maximum times
%  Nt = the number of time steps between (and including ) tmin and tmax
%
function dynamic_example_a(xmin,xmax,Nx,umin,umax,Nu,tmin,tmax,Nt);
%
global x
%
%  determine the actual times
%
   time = linspace(tmin,tmax,Nt);
%
%  get the allowed control inputs
%
   u_allowed = linspace(umin,umax,Nu)';
%
%  get the states
%
   x = linspace(xmin,xmax,Nx);
   x = flipud(x');
%
%  initialize the optimal cost to go
%
   J_star = zeros(Nx,Nt);
%
%  initialize the optimal control signal
%
   u_star= zeros(Nx,Nt);
%
%  determine the cost at the final time
%
   J_star(:,Nt) = x.*x;
   
%
%  now go through each time
%
   for kt=Nt-1:-1:1
      %
      %  now go through each state
      %
        for jx=1:Nx
            %
            %  determine the next state for all allowed control inputs
            %
            s = next_state(x(jx),u_allowed,time,kt,xmin,xmax);
            %
            %  determine the cost for each of the allowed control inputs
            %
            J = cost_for_input(J_star,s,u_allowed,time,kt,jx);
            %
            %  determine which input gives the minimum cost to go
            %
            [Jmin,index] = min(J);
            %
            %  update the cost to go and the control signal for this state
            %
            u_star(jx,kt) = u_allowed(index);
            J_star(jx,kt) = Jmin;
        end;
    end;
    disp('   state               J_star ');
    for kk=1:Nx
        disp([x(kk) J_star(kk,1:end)]);
    end;
    
    disp(' ');
    disp('   state               u_star ');
    for kk=1:Nx
        disp([x(kk) u_star(kk,1:end)]);
    end;
 %
 %  At this point we should have the optimal cost to go for each time and
 %  each state
 %
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  
 %  the computation for the next state goes here
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % 
 function s = next_state(x,u,time,kt,xmin,xmax);
 %
 %  compute the next state
 %
   s = x*ones(length(u),1)+u;
 %  
 %  return NaN if the input gives a state out of bounds
 %
   for kk = 1:length(s)
       if ((s(kk) < xmin*1.01) | (s(kk) > xmax*1.01)) s(kk) = NaN; end;
   end;
%  
  return;
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  
 %  the cost for the current input/state goes here
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 function  J = cost_for_input(J_star,s,u_allowed,time,kt,jx);
 global x
 %
   J = zeros(length(u_allowed),1);
   for ku = 1:length(u_allowed)
       if (~isnan(s(ku)))
       %
       %  get the cost based on where we end up
       %
       Jc = interp1(x,J_star(:,kt+1),s(ku),'linear','extrap');
       %
       %  get the cost of this input
       %
       Ju = u_allowed(ku)*u_allowed(ku)*(1/2);
       %
       %  get the total cost to go for this input
       %
       J(ku) = Jc+Ju;
       %
       else
           J(ku) = NaN;
       end;
   end;  
 return;
