//////////////////////////////////////////////////////////////////////// 
//    STOCHASTIC ADDITIVE DYNAMIC PROGRAMMING EQUATION                //
//////////////////////////////////////////////////////////////////////// 

clear;
exec damdata.sce;

// ---------------------------------------------------- 
// DATA 
// ---------------------------------------------------- 

// res represents the quality of the discretization grid for the controls,
// the higher res is, the better the grid is.
res=1;

states=linspace(0,volume_max,res*volume_max+1); 
controls=linspace(0,control_max,res*control_max+1); 

cardinal_states=size(states,'c');
cardinal_controls=size(controls,'c');
cardinal_uncertainty=size(uncertainty,'c');
//cardinal_spilled=size(spilled,'c');

 
final_cost_vector=final_cost(length(states));
zero_final_cost_vector=zeros(1,length(states));


// ---------------------------------------------------- 
//  MACROS 
// ---------------------------------------------------- 


function [VALUE]=SDP(FINAL_COST)
  
  VALUE=zeros(horizon,length(states));  // Bellman Function
  VALUE(horizon,:)= FINAL_COST ; // row vector 
  // loop backward in time 
  for t=T($:-1:1); 
    for k=1:cardinal_states
      s=states(k);
      locmin=zeros(1,cardinal_uncertainty);// Bellman value per uncertainty
      for w=1:cardinal_uncertainty // inflows index
	a=uncertainty(w);// inflows
	loc=zeros(1,cardinal_controls); // Value for a given control
	for j=1:cardinal_controls // control index
            u=controls(j); // control
	    d= max (s - u + a - volume_max,0); // spilled water
	    penalty=1/%eps*(b2m(dynamics(s,u,d,a)>volume_max)+b2m(dynamics(s,u,d,a)<volume_min)); //penalty when the constraints are not respected
	    dyn=max(volume_min,min(volume_max,dynamics(s,u,d,a))); // effective state
	    loc(j) = penalty+instant_cost(t,s,u,d,a)+VALUE(t+1,round(res*dyn+1));
	end
	locmin(1,w)=min(loc); 
      end
      VALUE(t,k)=locmin*proba(:,t);// expectation
    end
  end
endfunction


// optimal policy relying on V
function [u_opt,d]=sdp_policy_V(V,t,s,a)
  opt =  1/%eps // optimal value
  u_opt = 0 // optimal control
  for j=1:cardinal_controls // Loop on control
    u = controls(j); // control tested
    d= max (s - u + a - volume_max,0); // spilled water
    penalty=1/%eps*(b2m(dynamics(s,u,d,a)>volume_max)+b2m(dynamics(s,u,d,a)<volume_min));// stock constraint
    dyn=max(volume_min,min(volume_max,dynamics(s,u,d,a))); // effective state
    loc =penalty + instant_cost(t,s,u,d,a)+V(t+1,round(dyn/res +1));
    if (loc < opt) then
        u_opt = u;
        opt = loc ; 
    end
  end
  d = max (s - u_opt + a - volume_max,0);
endfunction
// -----------------------------------
V = SDP(zero_final_cost_vector);
function [u_opt,d]=sdp_policy(t,s,a)
  [u_opt,d]=sdp_policy_V(V,t,s,a)
endfunction