function varargout = RAPTOR_EKF_config(varargin)
% KFconfig = RAPTOR_EKF_config;
% [KFmodel,KFparams] = RAPTOR_EKF_config(x0,model,params,KFconfig)

% default parameters

p = struct(...
    'ny',1,... % number of measurements
    'ndx',uint32(0),...      % number of disturbances per state
    'jx_cov',[1e-3,1e-3],... % params for generating jx covariance (state current)
    'jd_cov',0.1*[1e-3,1e-5],... % params for generating jd covariance (disturbance current)
    'px_cov',[1e3,1e3],... % params for generating px covariance (state power)
    'pd_cov',[1e6,1e6],... % params for generating pd covariance (disturbance power)
    'cov_rhopow',2,... % convariance scaling:cove +(sqrt(cov0)-sqrt(cove))*(1-rho^cov_rhopow)
    'cov_usegauss',false,... % use gaussian shape instead of power law
    'cov_gauss_width',0.5,... % width, if gaussian shape is used
    'jx_sm',0.6,...
    'jd_sm',0.3,...
    'px_sm',0.6,...
    'pd_sm',0.3,...
    's0' , 1e-1,...
    'alpha',1,...    % extra factor for weighing past meaurements less (>1)
    'Lgain',1,...       % extra factor multiplying observer gain matrix
    'Ipstop',200e3,.... % Lower bound to stop filter 
    't_Rkdecay',0.1,... % time constant for decay of Rk from first iteration (helps initial convergence)
    'g_Rkdecay',100,... % multiplicative amplitude of decay of Rk from first iteration (helps initial convergence)
    'resmax',1e6,... % residual tolerance to stop filter
    'iterplot',uint32(1),... % plot each step of iterations
    'iterdisp',uint32(1),... % disp of each step of iterations
    'plotmats',false...
    );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
if nargin == 0
    varargout{1} = p;
    return
else
    [x0,RAPTORmodel,RAPTORparams,p] = deal(varargin{:});
    % eventual checks of p done here
end

%% Sizes etc

%% Extended kalman filter (roughly following notation of Anderson 1979, ch 8)
% initialize
% KF.C = C;

KFmodel.nx = numel(x0);
KFmodel.nu = RAPTORmodel.dims.nu;
KFmodel.ny = p.ny;
KFmodel.nd = RAPTORmodel.dims.nd;
KFmodel.nwx = RAPTORmodel.dims.nw;
KFmodel.nz = KFmodel.nx  + KFmodel.nd; % size of augmented state vector

KFmodel.nw = KFmodel.nwx+KFmodel.nd; % process noise term

KFmodel.ix = 1:KFmodel.nx;
KFmodel.id = KFmodel.nx+(1:KFmodel.nd);

KFmodel.dt = mean(diff(RAPTORparams.tgrid)); % Simulink model time step fixed at compile time since source time rate blocks are non-tunable

%% Disturbance model:

% This is how d and w enter into state equation
KFmodel.Bxw =  eye(KFmodel.nx); % df/dwk
KFmodel.Bxd =  eye(KFmodel.nd); % df/ddk

% initial conditions
dk_0 = zeros(KFmodel.nd,1);
psidot0 = RAPTORparams.numerics.Upl0*ones(RAPTORmodel.psi.nsp,1); 
tedot0 = zeros(RAPTORmodel.te.nsp,1);
xdot0 = [psidot0;tedot0]; xdot0(end) = 0;

% append KF parameters to RAPTOR free parameters
%KFparams.rk = p.rk; 
names = fieldnames(p);
for iff = 1:numel(names)
    myfieldname = names{iff};
    KFparams.(myfieldname) = p.(myfieldname);
end
KFparams.dk_0 = dk_0;
KFparams.xk_0 = x0;
KFparams.xdot0 = xdot0;

varargout{1} = KFmodel;
varargout{2} = KFparams;
