function [sys, x0, str, ts] = simple_ctrl(t, x, u, flag, params)
% t is the current time step
% x is the current state (for the controllers). It holds the past inputs
% and outputs of the block necessary to the computation of the adaptation rule.
% u is the input of the block (e.g. the output of the plant)
% params are the parameters passed to the controller
%  
% the structure params must have the following fields:
% params.x0 - matrix for initializing the state of the controller
%
% the controller must be connected with the inputs in the following way
% [ y_desired, y, non_controllable_input ]  
%
%                    ------------               
%      mode ------->|            |            
%                   |            |             -----------  
%       yd -------->|            |            |           |
%                   |            |     u      |           |
%  y -------------->| controller |----------->|    plant  |-- y
%   |               |            | 	      |           |  |
%   |    v -------->|            | v -------->|           |  |
%   |                ------------              -----------   |
%   |                                                        | 
%    --------------------------------------------------------
%
% see further down for the specifications of the state vector 'x'
    
  
% This block cannot be duplicated !!!!!
%
% it uses a persistent variable that would be destroyed if there was two copies
% of this block.
%
persistent p

switch flag, 	% flag given by simulink to specify what it wants to be returned

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0, 
    [sys, x0, str, ts, p] = mdlInitializeSizes(params);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,                                                
   [sys, p] = mdlUpdate(t, x, u, p); 

  %%%%%%%%%%
  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                                
     [sys, p] = mdlOutputs(t, x, u, p);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case {1,4,9},                                                
    [sys] = []; % do nothing
    
  %%%%%%%%%%%%%
  % debug     %
  %%%%%%%%%%%%%
  case 10,                                                
    [sys] = p;  

  otherwise
    error(['unhandled flag = ', num2str(flag)]);
end


%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================

function [sys, x0, str, ts, p] = mdlInitializeSizes(params);

  % Get the controller, the model of the plant, the sampling period, the length
  % of the control horizon, the cost matrices Q and R the initial state x0 and
  % the number of set points of the system (setpoints for the controller, and
  % therefore output what need to be controlled)

  controller=params.controller;
  plant_model=params.plant_model;

  temp1=get(controller,'horiz_length');
  temp2=get(plant_model,'horiz_length');
  params.memo=max(temp1, temp2);
  temp=get(params.controller, 'n_in')+ ...
       get(params.controller, 'n_out');

  % Initialize the simulink stuff
  sizes = simsizes;	% Typical simulink function
  sizes.NumContStates  = 0;	% No continuous state
  
  % The 'state' of the block contains the past values needed to optimise
  sizes.NumDiscStates  = params.memo*temp;
  sizes.NumOutputs     = params.controller.n_out;
  sizes.NumInputs      = params.controller.n_in;
  sizes.DirFeedthrough = 1;
  sizes.NumSampleTimes = 1;
  ts  = [params.sampTime 0];  

  sys = simsizes(sizes);		% Format the initialisation values

  %The state 'x0' of this block is made of the outputs and the inputs of the
  %controller for 'memo' time steps in the past. In order to initialises the
  %state, the assumption that the model was stationary before starting the
  %control is taken.  The structire of the matrix follows:
  %
  % | outputs_contr(u)  |set_points(yd) | outputs_plant(y) | noncontrollable_inputs_plant(v)|
  % |       ...         |   ...   older data   ...         |               ...              |
  % |       ...         |      ...      |      ...         |               ...              |
  % |                             'memo' times                                               |
  % |       ...         |      ...      |      ...         |               ...              |
  % |                   |         newer data               |                                |
  %
  %                    ------------               
  %       yd -------->|            |             -----------
  %                   |            |     u      |           |
  %  y -------------->| controller |----------->|    plant  |-- y
  %   |               |            | 	        |           |  |
  %   |    v -------->|            | v -------->|           |  |
  %   |                ------------              -----------   |
  %   |                                                        | 
  %    --------------------------------------------------------
  %
  %
  % 
  %'x0' initialy contains the output of the plant and the non-controllable
  %inputs at time 0. The controller is supposed to be stationary before starting the
  %control actions

  if ~isfield(params,'x0'),
    x0=[zeros(params.memo, temp)];
  elseif size(params.x0)==[1, temp],
    x0=repmat(params.x0,params.memo,1);
  elseif size(params.x0)==[params.memo, temp],
    x0=params.x0; 
  else
    a=sprintf('bad dimensions of ''x0'': \n ideal x0: (%ix%i)\n',params.memo,temp);
    error(a);
  end
  x0=x0(:);  %transform in a column all the data 
  str=[];
  execute; % Prepare the values of the persistent variables of execute

  p=params;
  p.optimparams=[];

% end mdlInitializeSizes

%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=======================================================================
%
function [sys, p] = mdlUpdate(t, x, y, p)
  
  % note above the y instead of the u in order to keep the same convention
  % The second argument is the state of the block. The third is the input
  % of the block e.g. mode, y_desired, y et v.
  
  [u, p]=execute(t, x, y, p);

  %Shift the state matrix and add the new state
  x=reshape(x, [p.memo p.controller.n_in+p.controller.n_out]);
  
  %update the system
  x(1:end-1, :)=x(2:end, :);
  x(end, :)=[u y'];
  
  sys=x(:);

%end mdlUpdate


%=======================================================================
% mdlOutputs
% Return the output vector for the S-function
%=======================================================================
%
function [sys, p] = mdlOutputs(t, x, u, p)

  [sys, p]=execute(t, x, u, p);

%end mdlOutputs

%=======================================================================
% execute_controller
% Heart of the s-function, adapts the controller and calculates its 
% output
% It uses persistent variables in order to do the computations only once.
%=======================================================================
%
function [y, p]=execute(t, x, u, p);
%  t - timestep
%  x - is the state matrix [ys_t-n us_t-n;...;ys_t-1 us_t-1];
%  u - inputs to the controller [y_desired, y, v];
%  p - parameters of the controller  
  persistent lastx lastu lasty;
  persistent flag;

  %If there is no argument, it means that it is the initialisation step 
  if nargin==0, 
    flag=0;
    lastx=[];
    lastu=[];
    lasty=[];
    return;  %Exit the subroutine
  end %End of the initialisation

  %look if we have already done the computation (except the first time when flag is 0)
  if (isequal(x, lastx(:, :, 1)))&(isequal(u, lastu(:, :, 1)))&(flag), 
    y=lasty(:, :, 1);		%Just need to return what has already
                                %been computed
    flag=0;
  else
    flag=1;
    %Remember the x and u for the next time
    lastx(:, :, 1)=x;		% There is a strange bug if we don't write this that way
    lastu(:, :, 1)=u;		% There is a strange bug if we don't
                                % write this that way

    controller = p.controller;
    cntrl_n_in=get(controller, 'n_in');
    cntrl_n_out=get(controller, 'n_out');

    %Shift the state matrix and add the new state
    x=reshape(x, [p.memo cntrl_n_out+cntrl_n_in]);
    y=zeros(1, cntrl_n_out);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%					
    %%	PART I: RECOVERY OF THE SYSTEM VARIABLES			
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    plant_model = p.plant_model;
    Q = p.Q;
    R = p.R;
    etha = p.etha;
    memo = p.memo;
    horiz=p.horizon;
%    pert_us = p.pert_us;
    pert_us = [];
    set_points = p.set_points;
    set_points_dim=size(set_points, 2); %number of controlled inputs

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%				
    %%	PART II: DEFINITION OF AUXILIARY VARIABLES			
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
    %keyboard 
  
    %Read a few attributes
    plant_n_out=get(plant_model, 'n_out');
    plant_n_in=get(plant_model, 'n_in');
    plant_dynamics=get(plant_model, 'dynamics');
    cntrl_dynamics=get(controller, 'dynamics');
  
    %get the current parameters and their number
    current_parameters=get(controller,'params');
    number_parameters=size(current_parameters, 2);
  
    % Reshape x so that each row corresponds to one time step
    % x contains all the input and outputs of the controller 
    % from time 't-1' up to memo steps in the past. It provides a snapshop
    % of the state of the controller at time 't-1'
    x=reshape(x, [memo cntrl_n_out+cntrl_n_in]);

    %Determine the output and the input part of the state vector
    us_indexes_in_x = 1:cntrl_n_out;
    ys_indexes_in_x = (cntrl_n_out+set_points_dim+1):(cntrl_n_out+set_points_dim+plant_n_out);  
    %the v have been included into the y vector since they are
    %uncontrollable outputs of the plant
  
    %now create the matrix which will store the derivatives of the
    %the outputs of the controller respect to the parameters. The stack has is
    %to put it simple stores the derivatives of the state matrix 'x' respect
    %to the parameters.
    % it structure follows:
    %                     --------------------------
    %                   /                          /|
    %                  /                          / |
    %                 /                          /  |
    %                 --------------------------    |
    %                |        |        |        |   |
    %                |        |        |        |   | 
    %                |        |        |        |   | 
    %  horiz times   |        |        |        |   |
    %                |        |        |        |  /
    %                |  du/dp | dyd/dw |  dy/dp | / number of parameters 
    %                |        |        |        |/
    %                 -------------------------- 
    %at the beginning it is all filled with zeros since past actions are
    %considered constant. Of course the rerivative of the parameters
    %respect to the desired inputs are always eqral to 0.

    %derivative_table stores the final value of every dy/dp and du/dp in
    %order to be able to compute the adaptation law. 
    %since this table is now computed is C it's not necessary anymore
    % to pre-allocate it.
    derivative_table=zeros(horiz, plant_n_out+cntrl_n_out, ...
                     number_parameters);
  
    %prepare the array to save all the values of the ys and the us which
    %will be used during the adaptation
    %since this tables are now computed is C it's not necessary anymore
    %to pre-allocate them.
    saved_us=zeros(horiz,cntrl_n_out);
    saved_ys=zeros(horiz,plant_n_out);
  
    %now update the state vector and add the current input of the
    %controller. The current output is still zero since it has not yet been
    %calculated
    x(1:end-1,:)=x(2:end,:);
    x(end,:)=[zeros(1,cntrl_n_out) u'];  

    %calculate the perturbation on the outputs of the plant
    %this value can be calculated filtering the errors between the predicted
    %value and the actual value of the output.
    %this option is used to minimise steady state error.
    % here, we simply put zeros (there is no implementation of this 
    % fonctionnality in this simple example).
    y_error_on_prediction=zeros(1,plant_n_out);

  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%					
    %%	PART III: ADAPTATION (with predictor)
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
  
%    dbmex on;
    [derivative_table,saved_us,saved_ys]=ICctrl(x',y_error_on_prediction,p);
%    dbmex off;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%					
    %%	PART IV: UPDATE OF THE PARAMETERS	
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   

%   keyboard;

    saved_ys=[zeros(1,size(saved_ys,2));saved_ys];
    saved_ys=saved_ys(:,set_points);
    saved_us=[x(end-1,1:cntrl_n_out,:); saved_us];
    derivative_table=[zeros(1,size(derivative_table,2),number_parameters); derivative_table];
    deltas=0;
  
    %Define set_points that can be substracted from the us
    %long_set_points = zeros(1,plant_n_out);
    long_set_points = u(1:set_points_dim)';
  
    for i=1:horiz,
      deltas = deltas + ...
               reshape((saved_ys(i+1,:)-long_set_points) * ...
		       Q(:,:,i) * squeeze(derivative_table(i+1,set_points,:)),[number_parameters,1]) +...
	       reshape((saved_us(i+1,:)-saved_us(i,:)) * R(:,:,i) ...
		       * squeeze(derivative_table(i+1,plant_n_out+1:end,:)-...
				 derivative_table(i,plant_n_out+1:end,: ...
						  )),[number_parameters,1]);
    end	
    if t>memo*p.sampTime
      controller=set(controller,'params',current_parameters - etha*deltas(:)');     
    else
      fprintf('.');
    end

    cntrl_regress=build_regressors(controller,x,1);
  
    %now that the controller has been adapted calculate the output of the
    %controller using the regressor 
    for output=1:cntrl_n_out,
      y(output)=eval(controller,cntrl_regress{output},output);
    end 

    lasty(:,:,1)=y;  
    p.controller=controller;
    p.optimparams=[p.optimparams;get(controller,'params')];
  end

%end execute_controller
