function [sys, x0, str, ts] = msdafc_edy(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


% 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 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;  %the controller
  plant_model=params.plant_model;  %the model of the plant
  sampTime=params.sampTime;  %the sampling time of the system
  Q=params.Q;  %the cost matrix for the outputs
  R=params.R;  %the cost matrix for the inputs

  %Have to look whether the initial controller is given under the form of
  %the name of a mat file
  if ischar(controller), 
    load(controller);
    controller=model;
  end

  %Have to look whether the plant model is given under the form of the name
  %of a mat file
  if ischar(plant_model), 
    load(plant_model);
    plant_model=model;
  end

  if isfield(params,'set_points'), %controlled inputs
    set_points = params.set_points;
  else
    set_points = 1:get(plant_model,'n_out');
  end
  set_points_dim=size(set_points, 2); %number of controlled inputs
  
  
  % Compute for the controller and the model the length of the time horizon
  % needed to calculate the control action and the predicted output. This
  % depends on the regressor of the model and the regressor of the
  % controller
  max_thc= get(controller,'horiz_length');
  max_thm= get(plant_model,'horiz_length');

  %memo is the biggest of these values and represents the number of previous
  %input output vectors that have to be kept into memory
  memo=max(max_thc,max_thm); 

  % 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  = memo*(get(controller,'n_in')+get(controller,'n_out'));	
  sizes.NumOutputs     = get(controller,'n_out');
  sizes.NumInputs      = get(controller,'n_in');
  sizes.DirFeedthrough = 1;
  sizes.NumSampleTimes = 1;	

  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 -------->|           |  |
  %   |                ------------              -----------   |
  %   |                                                        | 
  %    --------------------------------------------------------
  %
  %
  %the status matrix of the plant has the following structure
  %
  %    | plant_outputs | plant_inputs(pert_us and cont_us) |
  % 

  %'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

  %x0=[zeros(memo, get(controller,'n_out')+set_points_dim) ...
  %initial_state(ones(memo, 1), :)];
  x0=[zeros(memo, get(controller,'n_out')+get(controller,'n_in'))]; 
  x0=x0(:);  %transform in a column all the data
  str = [];
  ts  = [sampTime 0];  
  
  % Parameters that are not used during the initialisation phase could be
  % needed later and are passed here
  p=params;	
  p.controller=controller;
  p.plant_model=plant_model;
  p.memo=memo;
  
  
  if ~isfield(p,'horizon'), 
    p.horizon = 5; 
  end
  
  if ~isfield(p,'pert_us'), 
    p.pert_us = []; 
  end	
  
  if ~isfield(p,'set_points'),
    p.set_points = 1:get(plant_model,'n_out');
  end
  
  if ~isfield(p,'delayed'), 
    p.delayed = 1; 
  end	 

  % stuff for debuggin
  p.optimparams=[];
  p.predictions=[];
  p.current_y=[];
  p.current_u=[];
  p.current_x=[];
  p.activations=[];
  
  execute; % Prepare the values of the persistent variables of execute

% 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. 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 get(p.controller,'n_out')+get(p.controller,'n_in')]);
  x(1:end-1, :)=x(2:end, :);
  x(end, :)=[u y'];
  sys=x(:);

	p.current_x=cat(3,p.current_x,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
% Heart of the s-function. It is called twice a time step:
% Once to compute the new state and once to compute the output
% It uses persistent variables in order to only do the computations
% once.
%=======================================================================

function [y, p]=execute(t, x, u, p);
%t is the timestep
%x is a matrix [ys_t-n us_t-n;...;ys_t-1 us_t-1;

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
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

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%					
  %%	PART I: RECOVERY OF THE SYSTEM VARIABLES			
  %%				
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  				
  plant_model = p.plant_model;
  controller = p.controller;
  Q = p.Q;
  R = p.R;
  etha = p.etha;
  memo = p.memo;
  horiz=p.horizon;
  pert_us = p.pert_us;
  set_points = p.set_points;
  set_points_dim=size(set_points, 2); %number of controlled inputs
  delayed=p.delayed;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%					
  %%	PART II: DEFINITION OF AUXILIARY VARIABLES			
  %%				
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  %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_n_out=get(controller, 'n_out');
  cntrl_n_in=get(controller, 'n_in');
  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 inputs and
  %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.Derivatives older than memo time steps are not saved.
  % it structure follows:
  %                     --------------------------
  %                   /                          /|
  %                  /                          / |
  %                 /                          /  |
  %                 --------------------------    |
  %                |        |        |        |   |
  %                |        |        |        |   | 
  %                |        |        |        |   | 
  %  memo 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.
  dxdw=zeros(memo, cntrl_n_out+cntrl_n_in, number_parameters);
  
  %derivative_table stores the final value of every dy/dp and du/dp in
  %order to be able to compute the adaptation law. the stucture is the
  %same as 'dxdw'
  derivative_table=zeros(horiz+1, plant_n_out+cntrl_n_out, ...
			 number_parameters);
  
  %prepare the array to save all the val;ues of the ys and the us which
  %will be used during the adaptation
  saved_us=zeros(horiz+1,cntrl_n_out);
  saved_ys=zeros(horiz+1,plant_n_out);
  
  %save the current values of the state it will be used during the adaptation
  %#####
  %#####I do not know if this is correct for the predictor version of the algorithm
  %#####
  current_x=x;
      
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%					
  %%	PART III-a: ADAPTATION (delayed)
  %%				
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
  if delayed == 1,
    
    %keyboard;
   
    %build the current regressor for the current output of the controller using its state
    %matrix. In this way it calculates the regressor for the controller at
    %time 't-1': u(k-1)=Fuzzy(psi(k-1),w(k-1))
    %remember that in the controller there is direct feedtrought of its
    %inputs.
    cntrl_regress=build_regressors(controller,x,1);
    
    %prepare the matrix for the derivatives of the fuzzy controller respect
    %to the regressor the last column is the independent term of the linears
    dfuzzydpsi=zeros(cntrl_n_out,size(cntrl_regress{1},2));
    
    %computes the jacobian of the fuzzy system respect to the inputs
    for j=1:cntrl_n_out,
      dfuzzydpsi(j,:) = interp_model(controller, cntrl_regress{j},j);
    end
    
    %now calculate the dpsi/dw
    dpsidw=build_regressors(controller,dxdw,1);
    
    %calculates the jacobian of the controller respect to the parameters.
    %It calculates the du(k-1)/dw
    for j=1:cntrl_n_out,
      dudw=jacob_params(controller,cntrl_regress,j)+...  %dfuzzydw
	   (dpsidw{j}*dfuzzydpsi(j,:)')';
    end
    %save it in the 'dxdw' and the 'derivative_table'
    dxdw(end,us_indexes_in_x,:)= reshape(dudw,1,size(us_indexes_in_x,2),number_parameters);
    derivative_table(1,plant_n_out+1:end,:)=dudw;
    
    %build the status matrix for the plant: its structure follows
    %
    %      |plant_outputs | plant_inputs(pert_us and cont_us)|
    % 
    
    %creates a vector of the controlled inputs of the plant
    contr_us = setdiff(1:plant_n_in, pert_us); 
    
    %sets the outputs of the plant
    plant_x = [x(:,cntrl_n_out+set_points_dim+(1:plant_n_out)) ...
	       zeros(memo,plant_n_in)];  
    %sets the pert_us of the plant
    plant_x(:, plant_n_out + pert_us) = x(:,cntrl_n_out+ ...
					  set_points_dim+plant_n_out+1:end); 
    %sets the contr_us of the plant
    plant_x(:, plant_n_out +contr_us) = x(:,1:cntrl_n_out);
    
    %do exactly the same for the 'dxdw' array
    dplant_xdw = [dxdw(:,cntrl_n_out+set_points_dim+(1:plant_n_out),:) ...
		  zeros(memo,plant_n_in,number_parameters)];  
    dplant_xdw(:, plant_n_out + pert_us,:) = dxdw(:,cntrl_n_out+ ...
						set_points_dim+plant_n_out+1:end,:); 
    dplant_xdw(:, plant_n_out +contr_us,:) = dxdw(:,1:cntrl_n_out,:);
    
    %now shift up all the status matrix of the plant since we have to
    %calculate the regressor for y(k)=F[phi(k-1)], and there is a delay
    %between the inputs and the outputs of the system. 
    plant_x(1:end-1,:)=plant_x(2:end,:);
    plant_x(end,:)=zeros(1,plant_n_out+plant_n_in);
    dplant_xdw(1:end-1,:,:)=dplant_xdw(2:end,:,:);
    dplant_xdw(end,:,:)=zeros(1,plant_n_out+plant_n_in,number_parameters);
    
    %calculate the regressors of the plant phi(k-1).The last line of 'plant_x'
    %is not used in the computation of the regressor, but it is necessary
    %since there is the input-output delay.
    plant_r = build_regressors(plant_model,plant_x,1);   
    
    %prepare the matrix for the derivative
    %the last column is the independent term of the linears
    dydphi=zeros(plant_n_out,size(plant_r{1},2));
    
    %computes the jacobian respect to the inputs
    for j=1:plant_n_out,
      dydphi(j,:) = interp_model(plant_model,plant_r{j},j);
    end
    
    %now calculate the dy/dw
    dphidw=build_regressors(plant_model,dplant_xdw,1);
    
    for j=1:plant_n_out,
      dydw=(dphidw{j}*dydphi(j,:)')';
    end

    %save it in the 'dxdw' and the 'derivative_table'
    dxdw(1:end-1,:,:)=dxdw(2:end,:,:);
    dxdw(end,:,:)=zeros(1,cntrl_n_out+cntrl_n_in,number_parameters);
    dxdw(end,ys_indexes_in_x,:)= dydw;  %###this is wrong for sure
    derivative_table(1,1:plant_n_out,:)=dydw;
    
    %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
    saved_us(1,:)=x(end,us_indexes_in_x);
    x(1:end-1,:)=x(2:end,:);
    x(end,:)=[zeros(1,cntrl_n_out) u'];
    saved_ys(1,:)=x(end,ys_indexes_in_x);
    
    %build the new regressors
    contr_reg = build_regressors(controller,x,1);
    %calculate the output of the controller
    for output=1:cntrl_n_out,
      y(output)=eval(controller, contr_reg, output);
    end
    
    %add it to the state vector x
    x(end,:)=[y' u'];
       
    %now we go in the forward loop with the multiple step head simulation
    %it is the same code as before except for the last part when the
    %update the the x vector is done 
    for i=1:horiz,
      %build the current regressor for the current output of the controller using its state
      %matrix. 
      cntrl_regress=build_regressors(controller,x,1);
      
      %prepare the matrix for the derivatives of the fuzzy controller respoect
      %to the regressor the last column is the independent term of the linears
      dfuzzydpsi=zeros(cntrl_n_out,size(cntrl_regress{1},2));
      
      %computes the fuzzy interpolation of the local models, producing 
      %the resulting linear 
      for j=1:cntrl_n_out,
	dfuzzydpsi(j,:) = interp_model(controller, cntrl_regress{j},j);
      end
    
      %now calculate the dpsi/dw
      dpsidw=build_regressors(controller,dxdw,1);
      
      %calculates the jacobian of the controller respect to the
      %parameters.
      for j=1:cntrl_n_out,
	dudw=jacob_params(controller,cntrl_regress,j)+...  %dfuzzydw
	     (dpsidw{j}*dfuzzydpsi(j,:)')';
      end
            
      %save it in the 'dxdw' and the 'derivative_table'
      dxdw(end,us_indexes_in_x,:)= reshape(dudw,1,size(us_indexes_in_x,2),number_parameters);
      derivative_table(i+1,plant_n_out+1:end,:)=dudw;
    
      %build the status matrix for the plant: 
      contr_us = setdiff(1:plant_n_in, pert_us); 
      plant_x = [x(:,cntrl_n_out+set_points_dim+(1:plant_n_out)) ...
		 zeros(memo,plant_n_in)];  
      plant_x(:, plant_n_out + pert_us) = x(:,cntrl_n_out+ ...
					    set_points_dim+plant_n_out+1:end); 
      plant_x(:, plant_n_out +contr_us) = x(:,1:cntrl_n_out);
      
      %do exactly the same for the 'dxdw' array
      dplant_xdw = [dxdw(:,cntrl_n_out+set_points_dim+(1:plant_n_out),:) ...
		    zeros(memo,plant_n_in,number_parameters)];  
      dplant_xdw(:, plant_n_out + pert_us,:) = dxdw(:,cntrl_n_out+ ...
						  set_points_dim+plant_n_out+1:end,:); 
      dplant_xdw(:, plant_n_out +contr_us,:) = dxdw(:,1:cntrl_n_out,:);
      
      %now shift up all the status matrix of the plant 
      plant_x(1:end-1,:)=plant_x(2:end,:);
      plant_x(end,:)=zeros(1,plant_n_out+plant_n_in);
      dplant_xdw(1:end-1,:,:)=dplant_xdw(2:end,:,:);
      dplant_xdw(end,:,:)=zeros(1,plant_n_out+plant_n_in,number_parameters);
      
      %calculate the regressors of the plant.The last line of 'plant_x'
      %is not used in the computation of the regressor, but it is necessary
      %since there is the input-output delay.
      plant_r = build_regressors(plant_model,plant_x,1);
      
      %prepare the matrix for the derivatives
      %the last column is the independent term of the linears
      dydphi=zeros(plant_n_out,size(plant_r{1},2));
      
      %computes the fuzzy interpolation of the local models, producing 
      %the resulting linear 
      for j=1:plant_n_out,
	dydphi(j,:) = interp_model(plant_model,plant_r{j},j);
      end
      
      
      %now calculate the dy/dw
      dphidw=build_regressors(plant_model,dplant_xdw,1);
      for j=1:plant_n_out,
      	dydw=(dphidw{j}*dydphi(j,:)')';
      end
      
      %save it in the 'dxdw' and the 'derivative_table'
      dxdw(1:end-1,:,:)=dxdw(2:end,:,:);
      dxdw(end,:,:)=zeros(1,cntrl_n_out+cntrl_n_in,number_parameters);
      dxdw(end,ys_indexes_in_x,:)= dydw;  %###this is wrong for sure
      derivative_table(i+1,1:plant_n_out,:)=dydw;
      
      %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
      saved_us(i+1,:)=x(end,us_indexes_in_x);
      x(1:end-1,:)=x(2:end,:);
      
      %calculate the future output of the plant
      for output=1:cntrl_n_out,
	plant_y(output)=eval(plant_model,plant_r,output);
      end
      
      %keyboard
      x(end,:)=[zeros(1,cntrl_n_out) u(1:set_points_dim)' plant_y'];  
      saved_ys(i+1,:)=x(end,ys_indexes_in_x);
     
      %build the new regressors
      contr_reg = build_regressors(controller,x,1);
      
      %calculate the output of the controller
      for output=1:cntrl_n_out,
	y(output)=eval(controller,contr_reg,output);
      end
    
      %add it to the state vector x
      x(end,1:cntrl_n_out)=y';
    end %for i=0:horiz
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%					
    %%	PART IVa: UPDATE OF THE PARAMETERS	
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


    
    %error between the desired and the actual output of the plant from the
    %input vector of the system.
    error = u(set_points_dim+1:2*set_points_dim)-u(1:set_points_dim);
    
    % First step
    deltas=error * Q(:,:,1) * squeeze(derivative_table(1,set_points,:))+ ...
          (saved_us(1,:)-current_x(end-1,us_indexes_in_x)) * ...
          R(:,:,i+1) * squeeze(derivative_table(1,plant_n_out+1:end,:));
	  % saved_us(1,:) * R(:,:,i+1) * squeeze(derivative_table(1,1:cntrl_n_out,:));
		    
    %Define set_points that can be substracted from the us
    long_set_points = zeros(1,plant_n_out);
    long_set_points(1,set_points) = u(1:set_points_dim)';

    % Next steps
    for i=1:horiz,
      deltas = deltas + (saved_ys(i+1,:)-long_set_points) * ...
	       Q(:,:,i+1) * squeeze(derivative_table(i+1,set_points,:)) +...
              (saved_us(i+1,:)-saved_us(i,:)) * R(:,:,i+1) ...
              * squeeze(derivative_table(i+1,plant_n_out+1:end,:)-derivative_table(i,plant_n_out+1:end,:));    
              %+saved_us(i+1,:) * R(:,:,i+1) * squeeze(derivative_table(i+1,1:cntrl_n_out,:));
    end		
	
    controller=	set(controller,'params',current_parameters - etha*deltas(:)');
    
    %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
    current_x(1:end-1,:)=current_x(2:end,:);
    current_x(end,:)=[zeros(1,cntrl_n_out) u'];  
    
    %build the new regressors
    r = build_regressors(controller,current_x,1);
    
    %calculate the output of the controller
    for output=1:cntrl_n_out,
      y(output)=eval(controller,r,output);
    end   
    
    
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%					
  %%	PART III-b: ADAPTATION (with predictor)
  %%				
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
  else
    
    %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'];
    %keyboard
    
    for i=1:horiz,
      %build the current regressor for the current output of the controller using its state
      %matrix. 
      cntrl_regress=build_regressors(controller,x,1);
      
     %calculate the dpsi/dw
     dpsidw=build_regressors(controller,dxdw,1);      

     %calculates the jacobian of the controller respect to the
     %parameters.
     dfuzzydw=jacob_params(controller,cntrl_regress,[1:cntrl_n_out]);
     
     for j=1:cntrl_n_out,
	%computes the fuzzy interpolation of the local models, producing 
	%the resulting linear for output j
	dfuzzydpsi = interp_model(controller, cntrl_regress{j},j);
 
	%calculates the jacobian of the output respect the parameters 
	dudw(j,:)=dfuzzydw(j,:)+(dpsidw{j}*dfuzzydpsi')';
      end
            
      %save it in the 'dxdw' and the 'derivative_table'
      dxdw(end,us_indexes_in_x,:)= reshape(dudw,1,size(us_indexes_in_x,2),number_parameters);
      derivative_table(i,plant_n_out+1:end,:)=dudw;
    
      %now it is necessary to evaluate the output of the controller which
      %needs to be added to the state matrix
      for output=1:cntrl_n_out,
	controller_y(output)=eval(controller,cntrl_regress{output},output);
      end
    
      %update the state matrix
      x(end,1:cntrl_n_out)=controller_y;
      saved_us(i,:)=controller_y;
    
      %build the status matrix for the plant: 
      contr_us = setdiff(1:plant_n_in, pert_us); 
      plant_x = [x(:,cntrl_n_out+set_points_dim+(1:plant_n_out)) ...
		 zeros(memo,plant_n_in)];  
      plant_x(:, plant_n_out + pert_us) = x(:,cntrl_n_out+ ...
					    set_points_dim+plant_n_out+1:end); 
      plant_x(:, plant_n_out +contr_us) = x(:,1:cntrl_n_out);
      
      %do exactly the same for the 'dxdw' array
      dplant_xdw = [dxdw(:,cntrl_n_out+set_points_dim+(1:plant_n_out),:) ...
		    zeros(memo,plant_n_in,number_parameters)];  
      dplant_xdw(:, plant_n_out + pert_us,:) = dxdw(:,cntrl_n_out+ ...
						  set_points_dim+plant_n_out+1:end,:); 
      dplant_xdw(:, plant_n_out +contr_us,:) = dxdw(:,1:cntrl_n_out,:);
      
      %now shift up all the status matrix of the plant 
      plant_x(1:end-1,:)=plant_x(2:end,:);
      plant_x(end,:)=zeros(1,plant_n_out+plant_n_in);
      dplant_xdw(1:end-1,:,:)=dplant_xdw(2:end,:,:);
      dplant_xdw(end,:,:)=zeros(1,plant_n_out+plant_n_in,number_parameters);
      
      %calculate the regressors of the plant.The last line of 'plant_x'
      %is not used in the computation of the regressor, but it is necessary
      %since there is the input-output delay.
      plant_r = build_regressors(plant_model,plant_x,1);
      %keyboard
      %prepare the matrix for the derivatives
      %the last column is the independent term of the linears
      dydphi=zeros(plant_n_out,size(plant_r{1},2));
      
      dphidw=build_regressors(plant_model,dplant_xdw,1);
      %computes the fuzzy interpolation of the local models, producing 
      %the resulting linear 
      for j=1:plant_n_out,
	dydphi(j,:) = interp_model(plant_model,plant_r{j},j);
      end
      %dydphi=[0 0 0;0 0 0;0 0 0;-0.6 -0.25 -0.25;-0.6 5.75 5.75]';
      
      %now calculate the dy/dw
      %dphidw=build_regressors(plant_model,dplant_xdw,1);
      for j=1:plant_n_out,
	dydw(j,:)=(dphidw{j}*dydphi(j,:)')';
      end
      
      %save it in the 'dxdw' and the 'derivative_table'
      dxdw(1:end-1,:,:)=dxdw(2:end,:,:);
      dxdw(end,:,:)=zeros(1,cntrl_n_out+cntrl_n_in,number_parameters);
      dxdw(end,ys_indexes_in_x,:)= dydw;  %###this is wrong for sure
      derivative_table(i,1:plant_n_out,:)=dydw;
      
      %calculate the future output of the plant
      for output=1:plant_n_out,
	plant_y(output)=eval(plant_model,plant_r(output),output);
      end
    
      %keyboard
      x(1:end-1,:)=x(2:end,:);
      x(end,:)=[zeros(1,cntrl_n_out) u(1:set_points_dim)' ...
		plant_y u(end-size(pert_us,2)+1:end)'];  
      saved_ys(i,:)=x(end,ys_indexes_in_x);
     
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%					
    %%	PART IVb: UPDATE OF THE PARAMETERS	
    %%				
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
    %keyboard
    
    saved_ys=[zeros(1,size(saved_ys,2));saved_ys];
    saved_ys=saved_ys(:,set_points);
    saved_us=[ current_x(end,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(1,set_points) = u(1:set_points_dim,1)';
    
    
    for i=1:horiz,
      deltas = deltas + (saved_ys(i+1,:)-long_set_points) * ...
	       Q(:,:,i) * squeeze(derivative_table(i+1,set_points,:)) +...
	       (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,:));    
               %+saved_us(i+1,:) * R(:,:,i+1) * squeeze(derivative_table(i+1,1:cntrl_n_out,:));
    end		
	
%    if t<500,
%      learning=etha;
%    else
%      learning=500*etha/t;
%    end  
      
    controller=	set(controller,'params',current_parameters - etha*deltas(:)');     

    current_x(1:end-1,:)=current_x(2:end,:);
    current_x(end,:)=[zeros(1,cntrl_n_out) u'];
    cntrl_regress=build_regressors(controller,current_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);
    end 
    
  end
  
  lasty(:,:,1)=y;
  p.controller=controller;
  p.optimparams=[p.optimparams;get(controller,'params')];
  %memb1=membership(controller{1},cntrl_regress{1});
  %memb2=membership(controller{2},cntrl_regress{2});
  %p.activations=[p.activations; memb1./sum(memb1) memb2./sum(memb2)];
  curr_y=u(1+set_points_dim:end,1)';
  p.current_y=[p.current_y; curr_y(set_points)];
  p.current_u=[p.current_u;y];
  saved_ys(1,:)= curr_y(set_points);
  p.predictions=[p.predictions;reshape(saved_ys(1:end-1,:)',[1,size(saved_ys(1:end-1,:)')])];
end   
    
