function  [out1,controller_params] = RAPTOR_controllers(varargin)
% RAPTOR_controllers
%   Module to call feedback controllers in RAPTOR
%
%   usage:
%   1) Define function handles to different controller modules
%        params.controllers = RAPTOR_controllers({@Ip_controller,@my_favorite_controller})
%
%   2) Specify input signal (U) to controller mapping
%         params.controllers.mapping = {{out1,in1},{out2,in2},...}
%             where outX is the array of actuators commanded from controller X
%                and inX is the array of actuators commanded from controller X
%        e.g:
%        params.controllers.mapping = {{1,1},{[2 3],2}}
%                 Ip_controller: 1->1
%                 my_favorite_controller: 2->[2,3]
%
%   3) Generate RAPTOR model
%        
%   4) Run RAPTOR_predictive with this params structure
%
%  See RAPTOR_controller_demo for details

% use open-loop controller by default
controller_params = 'Direct feedthrough. Use RAPTOR_controllers to define feedback';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input processing

if nargin == 0
    out1 = controller_params;
    return %empty call, probably to get default structures
elseif nargin==1
    % get params for controller function specified in function handle
    function_handles = varargin{1};

    % run functions with empty input to get controller parameters
    clear controller_params
    for ifun = 1:numel(function_handles)
        fhandle = function_handles{ifun};
        % check fhandle
        if ~isa(fhandle,'function_handle'); errmsg = [fhandle,' is not a valid function handle']; error(errmsg); end
        
        % assign funcion handle name and parameters
        controller_params.(char(fhandle)).fhandle = fhandle; % set function handle for this module
        controller_params.(char(fhandle)).params =  fhandle(); % get default parameters for this module
    end
    controller_params.mapping = 'must be defined';

    out1 = controller_params;
    return    
elseif nargin==7
    [x,g,v,Uin,it,model,params] = deal(varargin{:});
    controller_params = model.controllers;
    %controller_params = local_procinput( ); done locally inside each module
else
    error('invalid number of arguments');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Do the actual work

if ischar(controller_params)
    % no controllers defined, direct feedthrough for all channels
    uout = Uin(:,it);
else
    
    if ischar(controller_params.mapping)
        error('must define controller input to actuator mapping first');
    end
    % check absurdities in mapping here! [TODO]
    
    fieldname = fieldnames(controller_params);
    fieldname = fieldname(~strcmp(fieldname,'mapping'));
    
    geop = geometry_profiles(g,0*g,model);
    stap = state_profiles(x,0*x,g,0*g,v,0*g,model);
    
    for ifield=1:numel(fieldname)
        module = fieldname{ifield};
        iout= controller_params.mapping{ifield}{1}; % outputs for this controller
        iin = controller_params.mapping{ifield}{2}; % inputs for this controller
        uout(iout,:) = controller_params.(module).fhandle(stap,geop,Uin(iin,:),it,model,params); % call controller module
    end
end
out1 = uout;

return