function [f,J,D] = tapas_ceode_fx_cmc(x, u, P, M, xtau)
% [f,J,D] = tapas_ceode_fx_cmc(x, u, P, M, xtau)
%
% Computation of the dynamical (state) equation for a neural mass model
% (canonical microcircuit). Compatibile with continuous extension for ODE
% methods. Adapted from spm_fx_cmc.m (original function license below).
% Function output signature kept the same to ensure compatibility.
%
%
% INPUT
%   x           mat             vector of current state activity
%   u           mat             vector of current driving input
%   P           struct          parameter structure
%   M           struct          model specification
%   xtau        mat             vector of delayed state activity
%
% OUTPUT
%   f           mat             matrix of dx/dt
%   J           mat             Jacobian of the system df/dx. Fixed to 0
%                               since irrelevant for this integration
%                               scheme.
%   D           mat             matrix of delays. Empty since irrelevant
%                               for this integration scheme.

% -------------------------------------------------------------------------
%
% Author: Dario Schöbi
% Created: 2020-08-10
% Copyright (C) 2020 TNU, Institute for Biomedical Engineering, University of Zurich and ETH Zurich.
%
% This file is part of the TAPAS ceode Toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------
%
% ORIGINAL FUNCTION LICENSE
%
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging
%
% Karl Friston
% $Id: spm_fx_cmc.m 6720 2016-02-15 21:06:55Z karl $
%--------------------------------------------------------------------------

% get dimensions and configure state variables
%--------------------------------------------------------------------------
if nargin < 5
    xtau = x;
end

% get dimensions and configure state variables
%--------------------------------------------------------------------------
x  = spm_unvec(x,M.x);            % neuronal states
xtau = spm_unvec(xtau, M.x);
n  = size(x,1);                   % number of sources

% [default] fixed parameters
%--------------------------------------------------------------------------
E  = [1 1/2 1 1/2]*200;           % extrinsic (forward and backward)
G  = [4 4 8 4 4 2 4 4 2 1]*200;   % intrinsic connections
T  = [2 2 16 28];                 % synaptic time constants
R  = 2/3;                         % slope of sigmoid activation function
B = 0;                            % bias or background (sigmoid)

% [specified] fixed parameters
%--------------------------------------------------------------------------
if isfield(M,'pF')
    try, E = M.pF.E; end
    try, G = M.pF.G; end
    try, T = M.pF.T; end
    try, R = M.pF.R; end
end

% Extrinsic connections
%--------------------------------------------------------------------------
% ss = spiny stellate
% sp = superficial pyramidal
% dp = deep pyramidal
% ii = inhibitory interneurons
%--------------------------------------------------------------------------
if n > 1
    A{1} = exp(P.A{1})*E(1);      % forward  connections (sp -> ss)
    A{2} = exp(P.A{2})*E(2);      % forward  connections (sp -> dp)
    A{3} = exp(P.A{3})*E(3);      % backward connections (dp -> sp)
    A{4} = exp(P.A{4})*E(4);      % backward connections (dp -> ii)
else
    A    = {0,0,0,0};
end

% detect and reduce the strength of reciprocal (lateral) connections
%--------------------------------------------------------------------------
for i = 1:length(A)
    L    = (A{i} > exp(-8)) & (A{i}' > exp(-8));
    A{i} = A{i}./(1 + 4*L);
end

% input connections
%--------------------------------------------------------------------------
C    = exp(P.C);

% pre-synaptic inputs: s(V)
%--------------------------------------------------------------------------
R    = R.*exp(P.S);              % gain of activation function
F    = 1./(1 + exp(-R*xtau + B));   % firing rate
S    = F - 1/(1 + exp(B));       % deviation from baseline firing

% input
%==========================================================================
if isfield(M,'u')

    % endogenous input
    %----------------------------------------------------------------------
    U = u(:)*512;

else
    % exogenous input
    %----------------------------------------------------------------------
    U = C*u(:)*32;

end

% time constants and intrinsic connections
%==========================================================================
T    = ones(n,1)*T/1000;
G    = ones(n,1)*G;

% extrinsic connections
%--------------------------------------------------------------------------
% forward  (i)   2  sp -> ss (+ve)
% forward  (ii)  1  sp -> dp (+ve)
% backward (i)   2  dp -> sp (-ve)
% backward (ii)  1  dp -> ii (-ve)
%--------------------------------------------------------------------------
% free parameters on time constants and intrinsic connections
%--------------------------------------------------------------------------
% G(:,1)  ss -> ss (-ve self)  4
% G(:,2)  sp -> ss (-ve rec )  4
% G(:,3)  ii -> ss (-ve rec )  4
% G(:,4)  ii -> ii (-ve self)  4
% G(:,5)  ss -> ii (+ve rec )  4
% G(:,6)  dp -> ii (+ve rec )  2
% G(:,7)  sp -> sp (-ve self)  4
% G(:,8)  ss -> sp (+ve rec )  4
% G(:,9)  ii -> dp (-ve rec )  2
% G(:,10) dp -> dp (-ve self)  1
%--------------------------------------------------------------------------
% Neuronal states (deviations from baseline firing)
%--------------------------------------------------------------------------
%   S(:,1) - voltage     (spiny stellate cells)
%   S(:,2) - conductance (spiny stellate cells)
%   S(:,3) - voltage     (superficial pyramidal cells)
%   S(:,4) - conductance (superficial pyramidal cells)
%   S(:,5) - current     (inhibitory interneurons)
%   S(:,6) - conductance (inhibitory interneurons)
%   S(:,7) - voltage     (deep pyramidal cells)
%   S(:,8) - conductance (deep pyramidal cells)
%--------------------------------------------------------------------------
j     = [1 2 3 4];
for i = 1:size(P.T,2)
    T(:,j(i)) = T(:,j(i)).*exp(P.T(:,i));
end
j     = [7 2 3 4];
for i = 1:size(P.G,2)
    G(:,j(i)) = G(:,j(i)).*exp(P.G(:,i));
end

% Modulatory effects of dp depolarisation on intrinsic connection j(1)
%--------------------------------------------------------------------------
if isfield(P,'M')
    G(:,j(1)) = G(:,j(1)).*exp(-P.M*32*S(:,7));
end

% Motion of states: f(x)
%--------------------------------------------------------------------------

% Conductance
%==========================================================================

% Granular layer (excitatory interneurons): spiny stellate: Hidden causes
%--------------------------------------------------------------------------
u      =   A{1}*S(:,3) + U;
u      = - G(:,1).*S(:,1) - G(:,3).*S(:,5) - G(:,2).*S(:,3) + u;
f(:,2) =  (u - 2*x(:,2) - x(:,1)./T(:,1))./T(:,1);

% Supra-granular layer (superficial pyramidal cells): Hidden causes - error
%--------------------------------------------------------------------------
u      = - A{3}*S(:,7);
u      =   G(:,8).*S(:,1) - G(:,7).*S(:,3) + u;
f(:,4) =  (u - 2*x(:,4) - x(:,3)./T(:,2))./T(:,2);

% Supra-granular layer (inhibitory interneurons): Hidden states - error
%--------------------------------------------------------------------------
u      = - A{4}*S(:,7);
u      =   G(:,5).*S(:,1) + G(:,6).*S(:,7) - G(:,4).*S(:,5) + u;
f(:,6) =  (u - 2*x(:,6) - x(:,5)./T(:,3))./T(:,3);

% Infra-granular layer (deep pyramidal cells): Hidden states
%--------------------------------------------------------------------------
u      =   A{2}*S(:,3);
u      = - G(:,10).*S(:,7) - G(:,9).*S(:,5) + u;
f(:,8) =  (u - 2*x(:,8) - x(:,7)./T(:,4))./T(:,4);

% Voltage
%==========================================================================
f(:,1) = x(:,2);
f(:,3) = x(:,4);
f(:,5) = x(:,6);
f(:,7) = x(:,8);

if nargout < 2; return, end

% Jacobian and delay matrix (will be properly computed in
% tnudcm_compute_xtau)
%==========================================================================
J  = 0;
D  = [];


function [result] = VBA_susceptibility(posterior,out,ind)
% scores the susceptibility of u->y relationships w.r.t. model parameters
% [result] = VBA_susceptibility(posterior,out,ind)
% IN:
%   - out/posterior: the output structures of the VBA inversion
%   - ind: a structure containing the following fields:
%       .u  : indices of inputs of interests
%       .phi: indices of observation parameters, whose importance in the
%       u->y relationship one wants to score
%       .theta: idem, for evolution parameters.
%       .y : indices of observations of interest
% OUT:
%   - susceptibility: nuXnphi and nuxntheta relative susceptibility
%   matrices (w.r.t. observation/evolution parameters), where nu and
%   nphi/ntehta are the number of considered inputs and parameters, respectively.
%   - susceptibility: nuXntphi qnd nuxntheta relative susceptibility matrices
%   (w.r.t. observation/evolution  parameters).

% ========================================================================================
% check parameters
% ========================================================================================

dim = out.dim;

% ________________________________________________________________________________________
% check if models has inputs

if dim.u == 0
    disp('*** Susceptibility analysis: the system does not receive any input!')
    result.susceptibility = struct('phi',[],'theta',[]);
    result.specificity    = struct('phi',[],'theta',[]);
    return
end

% ________________________________________________________________________________________
% set default parameters of interest

if ~exist('ind'), ind = struct; end

% check if is DCM
try, isDCM=out.options.inF.fullDCM; catch, isDCM=0; end

% complete option strucutre
if isDCM
    % keep only DCM connections
   idxConnect = 1:(out.options.inF.indself-1);
   % idxConnect = setdiff(idxConnect,out.options.inF.indC); % trivial effects (or maybe not)
   if isfield(out.options.inF,'indhself')
      % behaviour DCM
      idxResp = [out.options.sources(2:end).out] ;
   else
      % normal DCM
      idxResp = 1:dim.p;
   end
   ind = VBA_check_struct(ind,        ...
        'u'     , 1:dim.u       , ...
        'phi'   , []            , ...
        'theta' , idxConnect    , ...
        'y'     , idxResp         ...
    );
else
    % all params
    ind = VBA_check_struct(ind,       ...
        'u'     , 1:dim.u       , ...
        'phi'   , 1:dim.n_phi   , ...
        'theta' , 1:dim.n_theta , ...
        'y'     , 1:dim.p         ...
    );
end

% get rid of fixed parameters
fixedTheta = find(diag(out.options.priors.SigmaTheta)==0);
ind.theta = setdiff(ind.theta,fixedTheta);
fixedPhi = find(diag(out.options.priors.SigmaPhi)==0);
ind.phi = setdiff(ind.phi,fixedPhi);

% get rid of session param
if isfield(out.options, 'multisession')
    ind.u = setdiff(ind.u,dim.u);
end
% ========================================================================================
% shortcuts
% ========================================================================================

%options = out.options;
%f_fname = options.f_fname;
%g_fname = options.g_fname;
%y = out.y;

% ========================================================================================
% initialization
% ========================================================================================

% ========================================================================================
% Mutilating the models and deriving the output distortion
% ========================================================================================

% ________________________________________________________________________________________
% 1 - switching off inputs
vu = zeros(length(ind.u),1);
parfor iu=1:length(ind.u)
    mutilated_input_idx = ind.u(iu);
    vu(iu) = mutilation_score(posterior,out,ind,'u',mutilated_input_idx);
end

% dealing with parameters
for paramType = {'phi','theta'}
    paramType = paramType{1} ;

    if ~isempty(ind.(paramType)) > 0

        % compute mutilation score
        vparam  = zeros(1,numel(ind.(paramType)));
        % ________________________________________________________________________________
        % 2 - switching off system parameters
        parfor ip=1:length(ind.(paramType))
            mutilated_param_idx = ind.(paramType)(ip);
            vparam(ip) = mutilation_score(posterior,out,ind,paramType,mutilated_param_idx);
        end

        % _______________________________________________________________________________
        % 3 - bilateral mutilations
        vuparam = zeros(numel(ind.u),numel(ind.(paramType)));
        for iu=1:length(ind.u)
           parfor ip=1:length(ind.(paramType))
               mutilated_param_idx = ind.(paramType)(ip);
               mutilated_input_idx = ind.u(iu);
               vuparam_iu(ip) = mutilation_score(posterior,out,ind, ...
               paramType,mutilated_param_idx,'u',mutilated_input_idx);
           end
           vuparam(iu,:) = vuparam_iu ;
        end

        % ________________________________________________________________________________
        % compute susceptibility and specificity
        dvuparam = vuparam;
        dvu      = repmat(vu,1,length(ind.(paramType)));
        dvparam  = repmat(vparam,length(ind.u),1);

        % relative susceptibility
        susceptibility.raw.(paramType)  = 1 + (dvparam - dvuparam)      ;
        susceptibility.norm.(paramType) = 1 + (dvparam - dvuparam)./dvu ;
       % relative specificity
        specificity.raw.(paramType)  = 1 + (dvu - dvuparam)      ;
        specificity.norm.(paramType) = 1 + (dvu - dvuparam)./dvparam ;

        % interaction scores
        inter =  ((dvparam+dvu)-dvuparam) ;
        interaction.(paramType) = inter;
        interaction_norm.(paramType)  =  inter./min(dvu,dvparam);
        interaction_normU.(paramType) =  inter./dvu;
        interaction_normP.(paramType) =  inter./dvparam ;
        ii = max(inter,0) ;
        interaction_normC.(paramType) =  ii./ repmat(sum(ii),size(ii,1),1) ;

    else
        susceptibility.(paramType) = [];
        specificity.(paramType) = [];
        interaction.(paramType) = [];
        interaction_norm.(paramType) = [];
        interaction_normP.(paramType) = [];
        interaction_normU.(paramType) = [];
        interaction_normC.(paramType) = [];
    end

end

result.ind = ind ;
result.susceptibility = susceptibility;
result.specificity    = specificity;
result.interaction.raw     = interaction;
result.interaction.norm    = interaction_norm;
result.interaction.normP   = interaction_normP;
result.interaction.normU   = interaction_normU;
result.interaction.normC   = interaction_normC;

end

% ========================================================================================
% Additional functions
% ========================================================================================

% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% compute the loss of explained variance of a mutilated model
function v = mutilation_score(posterior,out,ind,varargin)

    % default is no mutilation
    mutilation = VBA_check_struct(struct,'u',0,'phi',0,'theta',0);

    % set mutilation from args
    for iArg = 1:2:numel(varargin)
        mutilation.(varargin{iArg}) = varargin{iArg+1} ;
    end

    % compute mutilated model trajectory
    [muy] = laplace_mutilated(posterior,out,mutilation);

    % compute score
    y = out.y ;
    vfull = score(y,out.suffStat.gx,out.options,ind) ;
    v = vfull - score(y,muy,out.options,ind) ;
end

% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% compute the loss of explained variance of a trajectory
function v = score(y,muy,options,ind)

    % select observation of interest
    y      =   y(ind.y,:);
    muy    = muy(ind.y,:);
    isYout = options.isYout(ind.y,:);

    % get rid of unused data
    y(isYout==1)   = [] ;
    muy(isYout==1) = [] ;

    % compute explained variance % ### TODO: multiline data
    dy = VBA_vec(y) - VBA_vec(muy);
    v =  1 - mean(dy.^2)./var(y) ;
end

% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% compute the trajectory moments for a mutilated model
function [muy] = laplace_mutilated(posterior,out,mutilation)

    mutilation = VBA_check_struct(mutilation,'u',0,'phi',0,'theta',0);

    mutilation_ratio = 0;
    % define input
    u = out.u;
    if all(mutilation.u > 0)
        u(mutilation.u,:) = mutilation_ratio*u(mutilation.u,:);
    end

    % define parameters
    opt = out.options;
    opt.priors = posterior;

    if mutilation.theta > 0
        opt.priors.muTheta(mutilation.theta)      = mutilation_ratio*opt.priors.muTheta(mutilation.theta);
        opt.priors.SigmaTheta(mutilation.theta,:) = 0;
        opt.priors.SigmaTheta(:,mutilation.theta) = 0;
    end

    if mutilation.phi > 0
        opt.priors.muPhi(mutilation.phi)      = mutilation_ratio*opt.priors.muPhi(mutilation.phi);
        opt.priors.SigmaPhi(mutilation.phi,:) = 0;
        opt.priors.SigmaPhi(:,mutilation.phi) = 0;
    end

   % compute trajectory moments
   [muy] = VBA_getLaplace(u,out.options.f_fname,out.options.g_fname,out.dim,opt,0,'skip');
   muy = reshape(muy,size(out.y));
end


function [Rob,Sen,Lmk,Obs] = correctKnownLmks(Rob, Sen, Raw, Lmk, Obs, Opt)

% CORRECTKNOWNLMKS  Correct known landmarks.
%   [Rob,Sen,Lmk,Obs] = correctKnownLmks(Rob, Sen, Raw, Lmk, Obs, Opt)
%   performs EKF corrections to a selection of observations, updating Map,
%   Rob, Sen, Lmk and Obs structures.
%
%   Input/output structures:
%       Rob:  the robot
%       Sen:  the sensor
%       Raw:  the raw data issued from Sen
%       Lmk:  the set of landmarks
%       Obs:  the set of observations for the sensor Sen
%       Opt:  the algorithm options
%
%   The selection of the landmarks to observe is based on active-search
%   procedures and individual compatibility tests to reject outliers.
%
%   This function takes care of:
%       1. Selection of observations to correct
%       2. Feature matching
%       3. EKF correction
%       4. Correction of landmark parameters out of the EKF
%       5. Landmark re-parametrization
%       6. Landmark deletion in case of corruption
%
%   The algorithm is configurable through a number of options in structure
%   Opt.correct. Edit USERDATA to access and modify these options.
%
%   See also PROJECTLMK, PROJEUCPNTINTOPINHOLEONROB, SMARTDELETELMK.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% steps in this function
% 0- update Rob and Sen info from Map
% 1- project all landmarks
% 2- select landmarks to correct. For each one:
% 3- do feature matching. If feature found:
% 4- compute innovation.
% 5- perform consistency test. If it is OK:
% 6- do correction
% 7- eventually reparametrize landmark
% 8- eventually delete corrupted landmarks
% 9- force covariance symmetry

global Map

% 0. UPDATE ROB AND SEN INFO FROM MAP
Rob = map2rob(Rob);
Sen = map2sen(Sen);

% 1. PROJECT ALL LMKS - get all expectations
for lmk = find([Lmk.used])

    Obs(lmk) = projectLmk(Rob,Sen,Lmk(lmk),Obs(lmk),Opt);

end  % --- all landmarks are now projected.

vis = [Obs.vis];

if any(vis) % Consider only visible observations

    % 2. SELECT LMKS TO CORRECT
    [lmksToObs,lmksToSkip] = selectLmksToObserve(Obs(vis),Opt.correct.nUpdates);

    % lmks to skip, update Obs info
    [Obs(lmksToSkip).measured] = deal(false);
    [Obs(lmksToSkip).matched]  = deal(false);
    [Obs(lmksToSkip).updated]  = deal(false);

    for lmk = lmksToObs % for each landmark to correct

        % Update Lmk search counter
        Lmk(lmk).nSearch = Lmk(lmk).nSearch + 1;

        % 3. TRY TO MATCH FEATURE
        Obs(lmk) = matchFeature(Sen,Raw,Obs(lmk));

        if Obs(lmk).matched

            % Update Lmk matched counter
            Lmk(lmk).nMatch = Lmk(lmk).nMatch + 1;

            % 4. COMPUTE INNOVATIONS
            Rob = map2rob(Rob);
            Sen = map2sen(Sen);

            if Opt.correct.reprojectLmks
                % re-project landmark for improved Jacobians
                Obs(lmk) = projectLmk(Rob,Sen,Lmk(lmk),Obs(lmk),Opt);
            end

            Obs(lmk) = observationInnovation(Obs(lmk),Opt.correct.lines.innType);

            % 5. CHECK CONSISTENCE
            if Obs(lmk).inn.MD2 < Opt.correct.MD2th

                % Update Lmk inlier counter
                Lmk(lmk).nInlier = Lmk(lmk).nInlier + 1;

                % 6. LANDMARK CORRECTION AND 7. REPARAMETRIZATION
                % fully correct landmark - EKF, reparam. and off-filter
                [Rob,Sen,Lmk(lmk),Obs(lmk)] = correctLmk(...
                    Rob,      ...
                    Sen,      ...
                    Lmk(lmk), ...
                    Obs(lmk), ...
                    Opt          );

            else % obs is inconsistent - do not update

                Obs(lmk).updated = false;

            end % if consistent

        end % if matched

    end % for lmk = lmkList

    % 8. LANDMARK DELETION -- loop all visible
    for lmk = find([Obs.vis])

        [Lmk(lmk),Obs(lmk)] = smartDeleteLmk(Lmk(lmk),Obs(lmk));

    end

    % 9. FORCE COVARIANCE SYMMETRY
    Map.P(Map.used,Map.used) = (Map.P(Map.used,Map.used) + Map.P(Map.used,Map.used)')/2;

end % if any(vis)

end % function

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function mpc = case14
%CASE14    Power flow data for IEEE 14 bus test case.
%   Please see CASEFORMAT for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee14cdf.txt) on 15-Oct-2014 by cdf2matp, rev. 2393
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       https://labs.ece.uw.edu/pstca/
%
%  08/19/93 UW ARCHIVE           100.0  1962 W IEEE 14 Bus Test Case

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1.06	0	0	1	1.06	0.94;
	2	2	21.7	12.7	0	0	1	1.045	-4.98	0	1	1.06	0.94;
	3	2	94.2	19	0	0	1	1.01	-12.72	0	1	1.06	0.94;
	4	1	47.8	-3.9	0	0	1	1.019	-10.33	0	1	1.06	0.94;
	5	1	7.6	1.6	0	0	1	1.02	-8.78	0	1	1.06	0.94;
	6	2	11.2	7.5	0	0	1	1.07	-14.22	0	1	1.06	0.94;
	7	1	0	0	0	0	1	1.062	-13.37	0	1	1.06	0.94;
	8	2	0	0	0	0	1	1.09	-13.36	0	1	1.06	0.94;
	9	1	29.5	16.6	0	19	1	1.056	-14.94	0	1	1.06	0.94;
	10	1	9	5.8	0	0	1	1.051	-15.1	0	1	1.06	0.94;
	11	1	3.5	1.8	0	0	1	1.057	-14.79	0	1	1.06	0.94;
	12	1	6.1	1.6	0	0	1	1.055	-15.07	0	1	1.06	0.94;
	13	1	13.5	5.8	0	0	1	1.05	-15.16	0	1	1.06	0.94;
	14	1	14.9	5	0	0	1	1.036	-16.04	0	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	232.4	-16.9	10	0	1.06	100	1	332.4	0	0	0	0	0	0	0	0	0	0	0	0;
	2	40	42.4	50	-40	1.045	100	1	140	0	0	0	0	0	0	0	0	0	0	0	0;
	3	0	23.4	40	0	1.01	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
	6	0	12.2	24	-6	1.07	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
	8	0	17.4	24	-6	1.09	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	2	0.01938	0.05917	0.0528	0	0	0	0	0	1	-360	360;
	1	5	0.05403	0.22304	0.0492	0	0	0	0	0	1	-360	360;
	2	3	0.04699	0.19797	0.0438	0	0	0	0	0	1	-360	360;
	2	4	0.05811	0.17632	0.034	0	0	0	0	0	1	-360	360;
	2	5	0.05695	0.17388	0.0346	0	0	0	0	0	1	-360	360;
	3	4	0.06701	0.17103	0.0128	0	0	0	0	0	1	-360	360;
	4	5	0.01335	0.04211	0	0	0	0	0	0	1	-360	360;
	4	7	0	0.20912	0	0	0	0	0.978	0	1	-360	360;
	4	9	0	0.55618	0	0	0	0	0.969	0	1	-360	360;
	5	6	0	0.25202	0	0	0	0	0.932	0	1	-360	360;
	6	11	0.09498	0.1989	0	0	0	0	0	0	1	-360	360;
	6	12	0.12291	0.25581	0	0	0	0	0	0	1	-360	360;
	6	13	0.06615	0.13027	0	0	0	0	0	0	1	-360	360;
	7	8	0	0.17615	0	0	0	0	0	0	1	-360	360;
	7	9	0	0.11001	0	0	0	0	0	0	1	-360	360;
	9	10	0.03181	0.0845	0	0	0	0	0	0	1	-360	360;
	9	14	0.12711	0.27038	0	0	0	0	0	0	1	-360	360;
	10	11	0.08205	0.19207	0	0	0	0	0	0	1	-360	360;
	12	13	0.22092	0.19988	0	0	0	0	0	0	1	-360	360;
	13	14	0.17093	0.34802	0	0	0	0	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	3	0.0430292599	20	0;
	2	0	0	3	0.25	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.01	40	0;
];

%% bus names
mpc.bus_name = {
	'Bus 1     HV';
	'Bus 2     HV';
	'Bus 3     HV';
	'Bus 4     HV';
	'Bus 5     HV';
	'Bus 6     LV';
	'Bus 7     ZV';
	'Bus 8     TV';
	'Bus 9     LV';
	'Bus 10    LV';
	'Bus 11    LV';
	'Bus 12    LV';
	'Bus 13    LV';
	'Bus 14    LV';
};

% Warnings from cdf2matp conversion:
%
% ***** check the title format in the first line of the cdf file.
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** MVA limit of branch 1 - 2 not given, set to 0
% ***** MVA limit of branch 1 - 5 not given, set to 0
% ***** MVA limit of branch 2 - 3 not given, set to 0
% ***** MVA limit of branch 2 - 4 not given, set to 0
% ***** MVA limit of branch 2 - 5 not given, set to 0
% ***** MVA limit of branch 3 - 4 not given, set to 0
% ***** MVA limit of branch 4 - 5 not given, set to 0
% ***** MVA limit of branch 4 - 7 not given, set to 0
% ***** MVA limit of branch 4 - 9 not given, set to 0
% ***** MVA limit of branch 5 - 6 not given, set to 0
% ***** MVA limit of branch 6 - 11 not given, set to 0
% ***** MVA limit of branch 6 - 12 not given, set to 0
% ***** MVA limit of branch 6 - 13 not given, set to 0
% ***** MVA limit of branch 7 - 8 not given, set to 0
% ***** MVA limit of branch 7 - 9 not given, set to 0
% ***** MVA limit of branch 9 - 10 not given, set to 0
% ***** MVA limit of branch 9 - 14 not given, set to 0
% ***** MVA limit of branch 10 - 11 not given, set to 0
% ***** MVA limit of branch 12 - 13 not given, set to 0
% ***** MVA limit of branch 13 - 14 not given, set to 0


function [ripples,sd,bad] = FindRipples(filtered,varargin)

%FindRipples - Find hippocampal ripples (100~200Hz oscillations).
%
%  USAGE
%
%    [ripples,stdev,noise] = FindRipples(filtered,<options>)
%
%    Ripples are detected using the normalized squared signal (NSS) by
%    thresholding the baseline, merging neighboring events, thresholding
%    the peaks, and discarding events with excessive duration.
%    Thresholds are computed as multiples of the standard deviation of
%    the NSS. Alternatively, one can use explicit values, typically obtained
%    from a previous call.
%
%    filtered       ripple-band filtered LFP (one channel).
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'thresholds'  thresholds for ripple beginning/end and peak, in multiples
%                   of the stdev (default = [2 5])
%     'durations'   minimum inter-ripple interval, and minimum and maximum
%                   ripple durations, in ms (default = [30 20 100])
%     'baseline'    interval used to compute normalization (default = all)
%     'restrict'    same as 'baseline' (for backwards compatibility)
%     'frequency'   sampling rate (in Hz) (default = 1250Hz)
%     'stdev'       reuse previously computed stdev
%     'show'        plot results (default = 'off')
%     'noise'       noisy ripple-band filtered channel used to exclude ripple-
%                   like noise (events also present on this channel are
%                   discarded)
%    =========================================================================
%
%  OUTPUT
%
%    ripples        for each ripple, [start_t peak_t end_t peakNormalizedPower]
%    stdev          standard deviation of the NSS (can be reused subsequently)
%    noise          ripple-like activity recorded simultaneously on the noise
%                   channel (for debugging info)
%
%  SEE
%
%    See also FilterLFP, RippleStats, SaveRippleEvents, PlotRippleStats.

% Copyright (C) 2004-2011 by Michaël Zugaro, initial algorithm by Hajime Hirase
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Default values
frequency = 1250;
show = 'off';
restrict = [];
sd = [];
lowThresholdFactor = 2; % Ripple envoloppe must exceed lowThresholdFactor*stdev
highThresholdFactor = 5; % Ripple peak must exceed highThresholdFactor*stdev
minInterRippleInterval = 30; % in ms
minRippleDuration = 20; % in ms
maxRippleDuration = 100; % in ms
noise = [];

% Check number of parameters
if nargin < 1 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help FindRipples'' for details).');
end

% Check parameter sizes
if ~isdmatrix(filtered) | size(filtered,2) ~= 2,
	error('Parameter ''filtered'' is not a Nx2 matrix (type ''help FindRipples'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help FindRipples'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'thresholds',
			thresholds = varargin{i+1};
			if ~isdvector(thresholds,'#2','>0'),
				error('Incorrect value for property ''thresholds'' (type ''help FindRipples'' for details).');
			end
			lowThresholdFactor = thresholds(1);
			highThresholdFactor = thresholds(2);
		case 'durations',
			durations = varargin{i+1};
			if ~isdvector(durations,'#2','>0') && ~isdvector(durations,'#3','>0'),
				error('Incorrect value for property ''durations'' (type ''help FindRipples'' for details).');
			end
			if length(durations) == 2,
				minInterRippleInterval = durations(1);
				maxRippleDuration = durations(2);
			else
				minInterRippleInterval = durations(1);
				minRippleDuration = durations(2);
				maxRippleDuration = durations(3);
			end
		case 'frequency',
			frequency = varargin{i+1};
			if ~isdscalar(frequency,'>0'),
				error('Incorrect value for property ''frequency'' (type ''help FindRipples'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring(show,'on','off'),
				error('Incorrect value for property ''show'' (type ''help FindRipples'' for details).');
			end
		case {'baseline','restrict'},
			restrict = varargin{i+1};
			if ~isempty(restrict) & ~isdvector(restrict,'#2','<'),
				error('Incorrect value for property ''restrict'' (type ''help FindRipples'' for details).');
			end
		case 'stdev',
			sd = varargin{i+1};
			if ~isdscalar(sd,'>0'),
				error('Incorrect value for property ''stdev'' (type ''help FindRipples'' for details).');
			end
		case 'noise',
			noise = varargin{i+1};
			if ~isdmatrix(noise) | size(noise,1) ~= size(filtered,1) | size(noise,2) ~= 2,
				error('Incorrect value for property ''noise'' (type ''help FindRipples'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help FindRipples'' for details).']);
	end
end

% Parameters
windowLength = round(frequency/1250*11);

% Square and normalize signal
signal = filtered(:,2);
squaredSignal = signal.^2;
window = ones(windowLength,1)/windowLength;
keep = [];
if ~isempty(restrict),
	keep = filtered(:,1)>=restrict(1)&filtered(:,1)<=restrict(2);
end

[normalizedSquaredSignal,sd] = unity(Filter0(window,sum(squaredSignal,2)),sd,keep);

% Detect ripple periods by thresholding normalized squared signal
thresholded = normalizedSquaredSignal > lowThresholdFactor;
start = find(diff(thresholded)>0);
stop = find(diff(thresholded)<0);
% Exclude last ripple if it is incomplete
if length(stop) == length(start)-1,
	start = start(1:end-1);
end
% Exclude first ripple if it is incomplete
if length(stop)-1 == length(start),
    stop = stop(2:end);
end
% Correct special case when both first and last ripples are incomplete
if start(1) > stop(1),
	stop(1) = [];
	start(end) = [];
end
firstPass = [start,stop];
if isempty(firstPass),
	disp('Detection by thresholding failed');
	return
else
	disp(['After detection by thresholding: ' num2str(length(firstPass)) ' events.']);
end

% Merge ripples if inter-ripple period is too short
minInterRippleSamples = minInterRippleInterval/1000*frequency;
secondPass = [];
ripple = firstPass(1,:);
for i = 2:size(firstPass,1)
	if firstPass(i,1) - ripple(2) < minInterRippleSamples,
		% Merge
		ripple = [ripple(1) firstPass(i,2)];
	else
		secondPass = [secondPass ; ripple];
		ripple = firstPass(i,:);
	end
end
secondPass = [secondPass ; ripple];
if isempty(secondPass),
	disp('Ripple merge failed');
	return
else
	disp(['After ripple merge: ' num2str(length(secondPass)) ' events.']);
end

% Discard ripples with a peak power < highThresholdFactor
thirdPass = [];
peakNormalizedPower = [];
for i = 1:size(secondPass,1)
	[maxValue,maxIndex] = max(normalizedSquaredSignal([secondPass(i,1):secondPass(i,2)]));
	if maxValue > highThresholdFactor,
		thirdPass = [thirdPass ; secondPass(i,:)];
		peakNormalizedPower = [peakNormalizedPower ; maxValue];
	end
end
if isempty(thirdPass),
	disp('Peak thresholding failed.');
	return
else
	disp(['After peak thresholding: ' num2str(length(thirdPass)) ' events.']);
end

% Detect negative peak position for each ripple
peakPosition = zeros(size(thirdPass,1),1);
for i=1:size(thirdPass,1),
	[minValue,minIndex] = min(signal(thirdPass(i,1):thirdPass(i,2)));
	peakPosition(i) = minIndex + thirdPass(i,1) - 1;
end

% Discard ripples that are way too short
time = filtered(:,1);
ripples = [time(thirdPass(:,1)) time(peakPosition) time(thirdPass(:,2)) peakNormalizedPower];
duration = ripples(:,3)-ripples(:,1);
ripples(duration<minRippleDuration/1000,:) = [];
disp(['After min duration test: ' num2str(size(ripples,1)) ' events.']);

% Discard ripples that are way too long
duration = ripples(:,3)-ripples(:,1);
ripples(duration>maxRippleDuration/1000,:) = [];
disp(['After max duration test: ' num2str(size(ripples,1)) ' events.']);

% If a noisy channel was provided, find ripple-like events and exclude them
bad = [];
if ~isempty(noise),
	% Square and pseudo-normalize (divide by signal stdev) noise
	squaredNoise = noise(:,2).^2;
	window = ones(windowLength,1)/windowLength;
	normalizedSquaredNoise = unity(Filter0(window,sum(squaredNoise,2)),sd,[]);
	excluded = logical(zeros(size(ripples,1),1));
	% Exclude ripples when concomittent noise crosses high detection threshold
	previous = 1;
	for i = 1:size(ripples,1),
		j = FindInInterval(noise,[ripples(i,1),ripples(i,3)],previous);
		previous = j(2);
		if any(normalizedSquaredNoise(j(1):j(2))>highThresholdFactor),
			excluded(i) = 1;
		end
	end
	bad = ripples(excluded,:);
	ripples = ripples(~excluded,:);
	disp(['After noise removal: ' num2str(size(ripples,1)) ' events.']);
end

% Optionally, plot results
if strcmp(show,'on'),
	figure;
	if ~isempty(noise),
		MultiPlotXY([time signal],[time squaredSignal],[time normalizedSquaredSignal],[time noise(:,2)],[time squaredNoise],[time normalizedSquaredNoise]);
		nPlots = 6;
		subplot(nPlots,1,3);
 		ylim([0 highThresholdFactor*1.1]);
		subplot(nPlots,1,6);
  		ylim([0 highThresholdFactor*1.1]);
	else
		MultiPlotXY([time signal],[time squaredSignal],[time normalizedSquaredSignal]);
%  		MultiPlotXY(time,signal,time,squaredSignal,time,normalizedSquaredSignal);
		nPlots = 3;
		subplot(nPlots,1,3);
  		ylim([0 highThresholdFactor*1.1]);
	end
	for i = 1:nPlots,
		subplot(nPlots,1,i);
		hold on;
  		yLim = ylim;
		for j=1:size(ripples,1),
			plot([ripples(j,1) ripples(j,1)],yLim,'g-');
			plot([ripples(j,2) ripples(j,2)],yLim,'k-');
			plot([ripples(j,3) ripples(j,3)],yLim,'r-');
			if i == 3,
				plot([ripples(j,1) ripples(j,3)],[ripples(j,4) ripples(j,4)],'k-');
			end
		end
		for j=1:size(bad,1),
			plot([bad(j,1) bad(j,1)],yLim,'k-');
			plot([bad(j,2) bad(j,2)],yLim,'k-');
			plot([bad(j,3) bad(j,3)],yLim,'k-');
			if i == 3,
				plot([bad(j,1) bad(j,3)],[bad(j,4) bad(j,4)],'k-');
			end
		end
		if mod(i,3) == 0,
			plot(xlim,[lowThresholdFactor lowThresholdFactor],'k','linestyle','--');
			plot(xlim,[highThresholdFactor highThresholdFactor],'k-');
		end
	end
end

function y = Filter0(b,x)

if size(x,1) == 1,
	x = x(:);
end

if mod(length(b),2) ~= 1,
	error('filter order should be odd');
end

shift = (length(b)-1)/2;

[y0 z] = filter(b,1,x);

y = [y0(shift+1:end,:) ; z(1:shift,:)];

function [U,stdA] = unity(A,sd,restrict)

if ~isempty(restrict),
	meanA = mean(A(restrict));
	stdA = std(A(restrict));
else
	meanA = mean(A);
	stdA = std(A);
end
if ~isempty(sd),
	stdA = sd;
end

U = (A - meanA)/stdA;


function DEMO_niche_construction
% Demo of niche construction using active inference
%__________________________________________________________________________
%
% The free-energy principle is an attempt to explain the structure of the
% agent and its brain, starting from the fact that an agent exists (Friston
% and Stephan, 2007; Friston et al., 2010). More specifically, it can be
% regarded as a systematic attempt to understand the 'fit' between an
% embodied agent and its niche, where the quantity of free-energy is a
% measure for the 'misfit' or disattunement (Bruineberg and Rietveld, 2014)
% between agent and environment. This paper offers a proof-of-principle
% simulation of niche construction under the free-energy principle. The key
% point of this paper is that the minimum of free-energy is not at a point
% in which the agent is maximally adapted to the statistics of a static
% environment, but can better be conceptualized an attracting manifold
% within the joint  agent-environment state-space as a whole, which the
% system tends toward through mutual interaction. We will provide a general
% introduction to active inference and the free-energy principle. Using
% Markov Decision Processes (MDPs), we then describe a canonical generative
% model and the ensuing update equations that minimize free-energy. We then
% apply these equations to simulations of foraging in an environment; in
% which an agent learns the most efficient path to a pre-specified
% location. In some of those simulations, unbeknownst to the agent, the
% environment changes as a function of the activity of the agent (i.e.
% unintentional niche construction occurs). We will show how, depending on
% the relative inertia of the environment and agent, the joint
% agent-environment system moves to different attracting sets of jointly
% minimized free-energy.
%
% see also: spm_MPD_VB_X.m
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEMO_niche_construction.m 7679 2019-10-24 15:54:07Z spm $

%% Set up and preliminaries
%--------------------------------------------------------------------------
rng('default')

label.factor     = {'where'};
label.modality   = {'what','where'};
label.outcome{1} = {'open','closed'};
MAZE =  [...
    0 0 0 0 0 0 0 0;
    1 1 0 1 1 1 1 0;
    1 1 0 1 1 1 1 0;
    1 1 0 1 1 1 1 0;
    1 1 0 1 1 1 1 0;
    0 0 0 1 1 1 1 0;
    0 1 1 1 1 1 1 0;
    0 0 0 0 0 0 0 0];

END   = sub2ind(size(MAZE),1,1);                  % goal or target location
START = sub2ind(size(MAZE),8,1);

% prior beliefs about initial states: D
%--------------------------------------------------------------------------
D{1}  = zeros(numel(MAZE),1);
Ns    = numel(D{1});

% Capture generative process in form of A: Aaux
%--------------------------------------------------------------------------
Aaux{1} = [1 - MAZE(:), MAZE(:)]';                % what
Aaux{2} = eye(Ns,Ns);                             % where
Ng    = numel(Aaux);
for g = 1:Ng
    No(g)  = size(Aaux{g},1);
end

% controlled transitions: B (up, down, left, right, stay)
%--------------------------------------------------------------------------
u    = [1 0; -1 0; 0 1; 0 -1; 0 0];               % allowable actions
nu   = size(u,1);                                 % number of actions
B{1} = zeros(Ns,Ns,nu);
[n,m] = size(MAZE);
for i = 1:n
    for j = 1:m

        % allowable transitions from state s to state ss
        %------------------------------------------------------------------
        s     = sub2ind([n,m],i,j);
        for k = 1:nu
            try
                ss = sub2ind([n,m],i + u(k,1),j + u(k,2));
                B{1}(ss,s,k) = 1;
            catch
                B{1}(s, s,k) = 1;
            end
        end
    end
end

% priors: (negative cost) C:
%--------------------------------------------------------------------------
for g = 1:Ng
    C{g} = zeros(No(g),1);
end

% allowable policies (1 move): V
%--------------------------------------------------------------------------
V = 1:nu;

% basic MDP structure
%--------------------------------------------------------------------------
mdp.V = V;                      % allowable policies
mdp.B = B;                      % transition probabilities
mdp.C = C;                      % preferred outcomes
mdp.D = D;                      % prior over initial states

mdp.label = label;
mdp.alpha = 8;
mdp.tau   = 8;

%% Simulation 1: Learning over trials
%==========================================================================
% Define concentration parameters for environment:
%--------------------------------------------------------------------------
Acp.open   = [4;0];
Acp.closed = [0;4];

% Define concentration parameters for agent:
%--------------------------------------------------------------------------
acp     = [1/8;1/8];

% Generative process: Generate probabilistic mapping from hidden states to
% outcomes: A and matrix of concentration parameters
%--------------------------------------------------------------------------
[A, As] = spm_gen_process(Aaux, Acp);

% Generative model: Generate probabilistic mapping from hidden states to
% outcomes: a matrix
a{1} = repmat(acp, 1,size(A{1},2));
a{2} = A{2}*128;

% MDP structure
%--------------------------------------------------------------------------
mdp.A  = A;
mdp.As = As;
mdp.a  = a;
mdp    = spm_MDP_check(mdp);
ntime  = 16;    % number of moves per path
ntrial = 4;     % number of paths
mdpt   = mdp;

for i = 1:ntrial
    SDP  = spm_maze_active(mdpt,ntime,START,END);
    mdpt = SDP(end);
    trial(i).MDP = SDP;
end

% show results - behavioural
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1'); clf
spm_print_agentstrials(trial, START, END, a)

%% Simulation 2: sensitivity to concentration parameters
%==========================================================================

% Show typical trial for
%--------------------------------------------------------------------------
acp    = [1/8,1/2,2;1/8,1/2,2];   % Initial concentration parameters of agent (cp1 = cp2)
ntime  = 16;                      % number of timesteps per trial
na     = length(acp);
ntrial = 4;
trial  = [];

for i = 1:na
    mdp.a{1} = repmat(acp(:,i), 1,size(A{1},2));
    mdpt = mdp;
    for j = 1:ntrial
        SDP = spm_maze_active(mdpt,ntime,START,END);
        mdpt = SDP(end);
    end
    trial(i).MDP = SDP;
end
spm_figure('GetWin','Figure 2'); clf
spm_plot_3a(trial, START, END, acp);

%% Simulation 3: sensitivity of concentration parameters agent and environment
%======================================================================
acp   = [1/8,4; 1/8,4];      % Initial concentration parameters of agent (cp1 = cp2)

Acp(1).open   = [1;0];
Acp(1).closed = [0;1];
Acp(2).open   = [16;0];
Acp(2).closed = [0;16];

ntime  = 16;                 % number of moves per path
ntrial = 32;                 % number of paths

% Inital concentration parameters
%--------------------------------------------------------------------------
na    = size(acp,2);
nenv  = size(Acp,2);
agent = [];

for i = 1:na
    mdp.a{1} = repmat(acp(:,i), 1,size(A{1},2));
    for j = 1:nenv
        [A,As] = spm_gen_process(Aaux, Acp(j));
        mdp.A  = A;
        mdp.As = As;
        mdpt   = mdp;
        for k = 1:ntrial
            SDP  = spm_maze_active(mdpt,ntime,START,END);
            mdpt = SDP(end);
            agent(i).env(j).trial(k).MDP = SDP;
        end
    end
end
spm_figure('GetWin','Figure 3'); clf
spm_plot_3b(agent, START, END);

%% Simulation 4: Singular value decomposition
%==========================================================================
spm_figure('GetWin','Figure 4'); clf
nenv  = 2;
ntt   = ntime*ntrial;
c     = linspace(10,ntt/4,ntt);
for i = 1:na
    for j = 1:nenv
        Pag  = [];
        Penv = [];
        for k = 1:ntrial
            SDP = agent(i).env(j).trial(k).MDP;
            for l = 1:ntime

                % get expectations
                %----------------------------------------------------------
                Pa   = SDP(l).a{1};
                Pa   = Pa/diag(sum(Pa));
                Pag  = [Pag, Pa(1,:)'];
                Pa   = SDP(l).As;
                Pa   = Pa/diag(sum(Pa));
                Penv = [Penv, Pa(1,:)'];
            end
        end
        Pag  = Pag  -  Pag(:,end)*ones(1,ntt);
        Penv = Penv - Penv(:,end)*ones(1,ntt);
        X    = [Pag'; Penv'];

        % eigenvariates
        %------------------------------------------------------------------
        subplot(2,2,2*(i-1) + j)
        [U,S] = svd(X);
        U   = U*S;
        V1a = U((1:ntt),1);
        V2a = U((1:ntt),2);
        V1e = U((1:ntt) + ntt,1);
        V2e = U((1:ntt) + ntt,2);

        scatter(V1a, V2a, [], c, 'filled'), hold on
        scatter(V1e, V2e, [], c, 'ro');
        axis([-1 1 -1 1]*2.5), axis square, hold off
        xlabel('First principle component')
        ylabel('Second principle component')
        title('phenotypic trajectories','FontSize',16)
    end
end

% get and show various free energies
%--------------------------------------------------------------------------
for i = 1:na
    for j = 1:nenv
        for k = 1:ntrial
            SDP = agent(i).env(j).trial(k).MDP;
            for l = 1:ntime;
                Gu     = log(spm_softmax(SDP(l).G));
                r(l,k) = mean(SDP(l).rt);
                f(l,k) = mean(SDP(l).H);
                g(l,k) = mean(SDP(l).P'*Gu);
            end
        end
        R(i,j,:) = sum(r);
        F(i,j,:) = sum(f);
        G(i,j,:) = sum(g);
    end
end

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 5'); clf
col = {'k','r';':k',':r'};

for i = 1:na
    for j = 1:nenv
        subplot(2,2,1)
        plot(spm_vec(F(i,j,:)),col{i,j}), hold on
        xlabel('exposure'), ylabel('seconds')
        title('(negative) free energy','FontSize',16)
        axis square, spm_axis tight, %set(gca,'YLim',[-3 1])

        subplot(2,2,2)
        plot(spm_vec(G(i,j,:)),col{i,j}), hold on
        xlabel('exposure'), ylabel('seconds')
        title('expected free energy','FontSize',16)
        axis square, spm_axis tight, %set(gca,'YLim',[-16 2])

        subplot(2,1,2)
        plot(spm_vec(R(i,j,:)),col{i,j}), hold on
        xlabel('exposure'), ylabel('seconds')
        title('reaction time','FontSize',16)
        axis square, spm_axis tight

    end
end

legend({'A:low-E:low','A:low-E:high','A:high-E:low','A:high-E:high'})

%%
return

function MDP = spm_maze_active(mdp,N,START,END)
% FORMAT MDP = spm_maze_search(mdp,N,START,END,alpha,beta,z)
% mdp   - MDP structure
% N     - number of moves (i.e., default 8)
% START - index of intial state (default 1)
% END   - index of target state (default 1)
% MDP   - MDP structure array

% preliminaries
%--------------------------------------------------------------------------
try, N;     catch, N     = 8;   end
try, START; catch, START = 1;   end
try, END;   catch, END   = 1;   end

% initialise concentration parameters: a (if unspecified)
%--------------------------------------------------------------------------
if ~isfield(mdp,'o')
    mdp.o = [];
end
if ~isfield(mdp,'u')
    mdp.u = [];
end
mdp.s    = START;

% Evaluate a sequence moves
%==========================================================================
for i = 1:N

    % Evaluate preferred states (subgoals) on the basis of current beliefs
    %----------------------------------------------------------------------
    mdp.C{2} = spm_maze_cost(mdp,END);

    % proceed with subsequent move
    %----------------------------------------------------------------------
    MDP(i)   = spm_MDP_VB_X(mdp);
    mdp      = MDP(i);
    mdp.s    = mdp.s(:,end);
    mdp.D{1} = MDP(i).X{1}(:,end);
    mdp.o    = [];
    mdp.u    = [];
    mdp.a{1} = MDP(i).a{1};

    % UPDATE: Change A matrix dependent on current position of agent
    %----------------------------------------------------------------------
    pos      = MDP(i).s(:,end);

    % Update environment
    %----------------------------------------------------------------------
    MDP(i).As(1,pos) = MDP(i).As(1,pos) + 1;

    % place in MDP structure
    %----------------------------------------------------------------------
    mdp.As   = MDP(i).As;
    mdp.A{1} = MDP(i).As/diag(sum(MDP(i).As));

end
return

function C = spm_maze_cost(MDP,END)
% Evaluate subgoals using
%==========================================================================
START = MDP.s(1);
if isfield(MDP,'a')
    Q = MDP.a{1};
else
    Q = MDP.A{1};
end
Q   = Q/diag(sum(Q));
Q   = Q(1,:);                                % open states
P   = diag(Q)*any(MDP.B{1},3);               % allowable transitions
ns  = length(Q);                             % number of states
X   = zeros(ns,1);X(START) = 1;              % initial state
Y   = zeros(ns,1);Y(END)   = 1;              % target state

% Preclude transitions to closed states and evaluate graph Laplacian
%--------------------------------------------------------------------------
P   = P - diag(diag(P));
P   = P - diag(sum(P));
P   = expm(P);

% evaluate (negative) cost as a path integral conjunctions
%--------------------------------------------------------------------------
for t = 1:size(MDP.V,1)
    X = P*X;
end
X     = X > exp(-3);                    % Default cut-off is -3
C     = log(X.*(P*Y) + exp(-32));

return

function spm_plot_3a (trial, START, GOAL, cp_ag)
% unpack properties
ntrial = size(trial,2);
ntime  = size(trial(1).MDP,2);
A  = spm_vec(trial(1).MDP(1).A{1}(1,:));
ns = numel(A);
ni = sqrt(ns);

for j = 1:ntrial

    A  = spm_vec(trial(j).MDP(end).A{1}(1,:));
    A  = reshape(A,ni,ni);
    subplot(2,ntrial,j), imagesc(A, [0,1]), axis image
    title(['CP = ' num2str(cp_ag(1,j))], 'fontsize',14)
    hold on, colormap gray

    % Plot trajectory
    %----------------------------------------------------------------------
    for k = 1:ntime
        step    = trial(j).MDP(k).s;
        [l1,m1] = ind2sub([ni,ni],step(1));
        [l2,m2] = ind2sub([ni,ni],step(2));
        plot([m1,m2], [l1,l2],':m');
    end

    [p,q] = ind2sub([ni,ni],START);
    plot(q,p,'.','MarkerSize',32,'Color','g');
    [p,q] = ind2sub([ni,ni],GOAL);
    plot(q,p,'.','MarkerSize',32,'Color','r');
    [p,q] = ind2sub([ni,ni],trial(j).MDP(end).s(2));
    plot(q,p,'.','MarkerSize',32,'Color','b');

    hold off

    % Plot Likelihood matrix
    %----------------------------------------------------------------------
    Q   = trial(j).MDP(end).a{1};
    Q   = Q/diag(sum(Q));
    Q   = Q(1,:);
    a   = reshape(Q(:),ni,ni);
    subplot(2,ntrial,j+3), imagesc(a, [0,1]), axis image
    title('Likelihood','fontsize',14);

end

return

function spm_plot_3b (agent, START, GOAL)

% unpack properties
%--------------------------------------------------------------------------
nag   = size(agent,2);
nenv  = size(agent(1).env,2);
ntime = size(agent(1).env(1).trial(1).MDP,2);
A     = spm_vec(agent(1).env(1).trial(1).MDP(1).A{1}(1,:));
ns    = numel(A);
ni    = sqrt(ns);

for i = 1:nenv
    for j = 1:nag
        A  = spm_vec(agent(j).env(i).trial(end).MDP(end).A{1}(1,:));
        A  = reshape(A,ni,ni);
        subplot(nenv,nag*2,(2*(j-1))+1+(2*nenv*(i-1)))
        imagesc(A, [0,1]), axis image

        hold on
        colormap gray
        % Plot trajectory
        %------------------------------------------------------------------
        for k = 1:ntime
            step    = agent(j).env(i).trial(end).MDP(k).s;
            [l1,m1] = ind2sub([ni,ni],step(1));
            [l2,m2] = ind2sub([ni,ni],step(2));
            plot([m1,m2], [l1,l2],':r');
        end

        [p,q] = ind2sub([ni,ni],START);
        plot(q,p,'.','MarkerSize',32,'Color','g');
        [p,q] = ind2sub([ni,ni],GOAL);
        plot(q,p,'.','MarkerSize',32,'Color','r');
        [p,q] = ind2sub([ni,ni],agent(j).env(i).trial(end).MDP(end).s(2));
        plot(q,p,'.','MarkerSize',32,'Color','b');

        % Plot Likelihood matrix
        %------------------------------------------------------------------
        Q   = agent(j).env(i).trial(end).MDP(end).a{1};
        Q   = Q/diag(sum(Q));
        Q   = Q(1,:);
        a   = reshape(Q(:),ni,ni);
        subplot(nenv,nag*2,(2*(j-1))+2+(2*nenv*(i-1))), imagesc(a, [0,1]), axis image

        hold off

    end
end
return

function [A, As] = spm_gen_process(Aaux, Acp)
% This function generates the observation matrix A of the generative
% process based on the layout of the maze Aaux and the environmental
% concentration parameters Acp
%--------------------------------------------------------------------------
initwh = Acp.open;
initbl = Acp.closed;
As     = initwh*Aaux{1}(1,:)+initbl*Aaux{1}(2,:);
Ap     = repmat(sum(As,1),2,1);

A{1} = As./Ap;    % Expected value of dirichlet is alpha1/(alpha1+alpha2);
A{2} = Aaux{2};

return

function spm_print_agentstrials(trial, START, GOAL,ainit)
%--------------------------------------------------------------------------
ntrial = size(trial,2);
ntime  = size(trial(1).MDP,2);
A  = spm_vec(trial(1).MDP(1).A{1}(1,:));
a  = ainit;
ns = numel(A);
ni = sqrt(ns);

% plot initial maze
%--------------------------------------------------------------------------
A  = spm_vec(trial(1).MDP(1).A{1}(1,:));
A  = reshape(A,ni,ni);
subplot(ntrial+1,2,1), imagesc(A, [0,1]), axis image
title('Initial maze', 'fontsize',14)
hold on
colormap gray
try
    [p,q] = ind2sub([ni,ni],START);
    plot(q,p,'.','MarkerSize',32,'Color','g');
    [p,q] = ind2sub([ni,ni],GOAL);
    plot(q,p,'.','MarkerSize',32,'Color','r');
end
hold off

% plot initial likelihood
%--------------------------------------------------------------------------
Q = a{1};
Q = Q/diag(sum(Q));
Q = Q(1,:);
a = reshape(Q(:),ni,ni);
subplot(ntrial+1,2,2), imagesc(a, [0,1]), axis image
title('Initial Likelihood','fontsize',14);

for j = 1:ntrial
    A = spm_vec(trial(j).MDP(end).A{1}(1,:));
    A = reshape(A,ni,ni);
    subplot(ntrial+1,2,1+(2*j)), imagesc(A, [0,1]), axis image
    title(['Trial ' num2str(j)], 'fontsize',14)
    hold on
    % Plot trajectory
    %----------------------------------------------------------------------
    for k = 1:ntime
        step = trial(j).MDP(k).s;
        [l1,m1]= ind2sub([ni,ni],step(1));
        [l2,m2]= ind2sub([ni,ni],step(2));
        p = plot([m1,m2], [l1,l2],':m');
    end

    % try
    %----------------------------------------------------------------------
    [p,q] = ind2sub([ni,ni],START);
    plot(q,p,'.','MarkerSize',32,'Color','g');
    [p,q] = ind2sub([ni,ni],GOAL);
    plot(q,p,'.','MarkerSize',32,'Color','r');
    [p,q] = ind2sub([ni,ni],trial(j).MDP(end).s(2));
    plot(q,p,'.','MarkerSize',32,'Color','b');

    hold off

    % Plot Likelihood matrix
    %----------------------------------------------------------------------
    Q     = trial(j).MDP(end).a{1};
    Q     = Q/diag(sum(Q));
    Q     = Q(1,:);
    a     = reshape(Q(:),ni,ni);
    subplot(ntrial+1,2,2+(2*j)), imagesc(a, [0,1]), axis image
    title(['Likelihood after trial ' num2str(j)],'fontsize',14);
end

return


function [sr_img_deconv, sr_img, sampled_img, sr_affix] = lmsSR_generateSRusingNUIv4weighted(lr_seq, warped_meshXN, warped_meshYN, upscaling, psf, lucy_iter, weighting_struct)
%
% LMSSR_GENERATESRUSINGNUIV4WEIGHTED Performs a Non-Uniform Interpolation (NUI) Super-Resolution (SR) approach with Spatial Quantization using Cubic Interpolation
%                                    and a Triple Sample Weighting. Supports GS/Y and YUV.
%    [sr_img_deconv, sr_img, sampled_img, sr_affix] = LMSSR_GENERATESRUSINGNUIV4WEIGHTED(lr_seq, warped_meshXN, warped_meshYN, upscaling, psf, lucy_iter, weighting_struct)
%
% Parameters: lr_seq             -   4-D matrix containing a LR YUV444 or GS/Y sequence (height,width,dye,frames)
%             warped_meshXN      -   Warped image mesh for X-coordinates
%             warped_meshYN      -   Warped image mesh for Y-coordinates
%             upscaling          -   1x2 vector containing the upscaling factors for Y- and X-coordinates
%             psf                -   Point spread function
%             lucy_iter          -   Number of iteration for the Lucy-Richardson deconvolution
%             weighting_struct   -   Struct containing the selected weighting mode + corresponding extra information:
%
%                mec_weight_flag   -   MEC Weighting ON/OFF (0: OFF. 1: ON, 2: ON [MECv2], 3: ON [MECv3], 4: ON [MECv4])
%                dis_weight_flag   -   Distance Weighting ON/OFF
%                qps_weight_flag   -   QP Weighting ON/OFF
%
%                mec_confMapN      -   3-D matrix containing confidence information in form of SSD values for each pixel and each auxiliary frame
%                mec_thr_scaler    -   Scalar value responsible for scaling the standard deviation threshold which cuts off SSD values that are too large [default: 2]
%                mec_weight_exp    -   Exponent for intensifying the weighting [default: 2]
%                mec_weighting     -   3-D matrix containing weights for each pixel depending on the quality of the MEC step
%
%                dis_rho           -   Distance Weighting Decay Factor [default: 0.7]
%                dis_scaler        -   Distance Weighting Scaling Factor [default: upscaling*10]
%                dis_weighting     -   3-D matrix containing weights for each pixel depending on the distance to the nearest pixel center
%
%                qps_map           -   3-D matrix containing the quantization parameters for each pixel and each frame including the reference frame
%                qps_rho           -   Quantization Weighting Decay Factor [default: 0.7]
%                qps_weighting     -   3-D matrix containing weights for each pixel depending on the quantization parameters
%
%                alpha             -   MEC Weighting Mixing Factor [default: 1]
%                beta              -   Distance Weighting Mixing Factor [default: 1]
%                gamma             -   Quantization Weighting Mixing Factor [default: 1]
%
% Author: Michel Bätz (LMS)
%
% See also: lmsSR_framework
%

sr_affix = 'sr2weighted000';

if weighting_struct.mec_weight_flag == 1,
    sr_affix(end-2) = '1';
elseif weighting_struct.mec_weight_flag == 2,
    sr_affix(end-2) = '2';
elseif weighting_struct.mec_weight_flag == 3,
    sr_affix(end-2) = '3';
elseif weighting_struct.mec_weight_flag == 4,
    sr_affix(end-2) = '4';
end

if weighting_struct.dis_weight_flag == 1,
    sr_affix(end-1) = '1';
elseif weighting_struct.dis_weight_flag == 2,
    sr_affix(end-1) = '2';
end

if weighting_struct.qps_weight_flag,
    sr_affix(end) = '1';
end

disp(['Weighting-Mode [',sr_affix(end-2:end),'] Selected..']);

alpha = weighting_struct.alpha;
beta  = weighting_struct.beta;
gamma = weighting_struct.gamma;

% Extract information from lr_seq
[lr_height,lr_width,dye,num_imgs] = size(lr_seq);

% Build Original X-Y-Position Grid
[lr_meshX,lr_meshY] = meshgrid(1:lr_width,1:lr_height);

% Build SR X-Y-Position Grid (+Padding)
[sr_meshX,sr_meshY] = meshgrid(1:lr_width*upscaling(2)+2*upscaling(2),1:lr_height*upscaling(1)+2*upscaling(1));

% Computing MEC weights from SSD values
if weighting_struct.mec_weight_flag == 1,
    weighting_struct.mec_weighting = lmsSR_computeWeightsMEC(weighting_struct.mec_confMapN,weighting_struct.mec_thr_scaler,weighting_struct.mec_weight_exp);
elseif weighting_struct.mec_weight_flag == 2,
    weighting_struct.mec_weighting = lmsSR_computeWeightsMECv2(weighting_struct.mec_confMapN,weighting_struct.mec_thr_scaler,weighting_struct.mec_weight_exp);
elseif weighting_struct.mec_weight_flag == 3,
    weighting_struct.mec_weighting = lmsSR_computeWeightsMECv3(weighting_struct.mec_confMapN,weighting_struct.mec_thr_scaler,weighting_struct.mec_weight_exp);
elseif weighting_struct.mec_weight_flag == 4,
    weighting_struct.mec_weighting = lmsSR_computeWeightsMECv4(weighting_struct.mec_confMapN,weighting_struct.mec_thr_scaler*10,weighting_struct.mec_weight_exp); % MAGI-ADD: Scaling with 10 necessary!
else
    weighting_struct.mec_weighting = ones(lr_height,lr_width,num_imgs);
end

% Computing QP weights from quantization parameters
if weighting_struct.qps_weight_flag, % MAGI-TODO: Work-in-Progress
    weighting_struct.qps_weighting = lmsSR_computeWeightsQP(weighting_struct.qps_map,weighting_struct.qps_rho);
else
    weighting_struct.qps_weighting = ones(lr_height,lr_width,num_imgs);
end

% Linearized Meshes (Direct Computation)
merged_meshX = zeros(lr_height,lr_width,num_imgs);
merged_meshY = zeros(lr_height,lr_width,num_imgs);

merged_meshX(:,:,1)     = lr_meshX;
merged_meshX(:,:,2:end) = warped_meshXN;

merged_meshY(:,:,1)     = lr_meshY;
merged_meshY(:,:,2:end) = warped_meshYN;

merged_lin_meshX = merged_meshX(~isnan(merged_meshX));
merged_lin_meshY = merged_meshY(~isnan(merged_meshY));

merged_valsY = lr_seq(:,:,1,:);
merged_lin_valsY = merged_valsY(~isnan(merged_meshX));
if dye == 3,
    merged_valsU = lr_seq(:,:,2,:);
    merged_valsV = lr_seq(:,:,3,:);
    merged_lin_valsU = merged_valsU(~isnan(merged_meshX));
    merged_lin_valsV = merged_valsV(~isnan(merged_meshX));
end

% Removing NaN-Positions of the QP/MEC Weighting Matrix and Linearizing It
merged_mec_weighting = weighting_struct.mec_weighting(~isnan(merged_meshX));
merged_qps_weighting = weighting_struct.qps_weighting(~isnan(merged_meshX));

num_pix = numel(merged_lin_meshX);

disp('... Throwing points into spatially quantized buckets ...');
%timeSR2bucket = tic;

% Averaging all samples inside the HR grid pixel patch
rounded_meshX = round(merged_lin_meshX*upscaling(2)) - upscaling(2) + 1;
rounded_meshY = round(merged_lin_meshY*upscaling(1)) - upscaling(1) + 1;

rounding_errX = abs(merged_lin_meshX - (rounded_meshX + upscaling(2) - 1)/upscaling(2));
rounding_errY = abs(merged_lin_meshY - (rounded_meshY + upscaling(1) - 1)/upscaling(1));

% Memory Cleansing
clear warped_meshXN warped_meshYN lr_seq lr_meshX lr_meshY merged_meshX merged_meshY merged_valsY merged_valsU merged_valsV merged_lin_meshX merged_lin_meshY mec_weighting

% Throwing away all mesh points that lie outside the SR image grid
outlier_vec = (rounded_meshX < 1) | (rounded_meshX > lr_width*upscaling(2)) | (rounded_meshY < 1) | (rounded_meshY > lr_height*upscaling(1));

rounded_meshX = rounded_meshX(~outlier_vec);
rounded_meshY = rounded_meshY(~outlier_vec);

rounding_errX = rounding_errX(~outlier_vec);
rounding_errY = rounding_errY(~outlier_vec);

% Distance Weighting Stuff
if weighting_struct.dis_weight_flag == 1,
    weighting_struct.dis_weighting = weighting_struct.dis_rho.^(sqrt((rounding_errY*weighting_struct.dis_scaler(1)).^2 + (rounding_errX*weighting_struct.dis_scaler(1)).^2));
elseif weighting_struct.dis_weight_flag == 2,
    weighting_struct.dis_weighting = weighting_struct.dis_rho.^(sqrt((rounding_errY*weighting_struct.dis_scaler(1)/2).^2 + (rounding_errX*weighting_struct.dis_scaler(1)/2).^2));
else
    weighting_struct.dis_weighting = ones(size(rounding_errX));
end

merged_lin_valsY = merged_lin_valsY(~outlier_vec);

tmp_sr_imgY = zeros(lr_height*upscaling(1),lr_width*upscaling(2)); % Creating bucketed image
if dye == 3,
    merged_lin_valsU = merged_lin_valsU(~outlier_vec);
    merged_lin_valsV = merged_lin_valsV(~outlier_vec);

    tmp_sr_imgU = zeros(lr_height*upscaling(1),lr_width*upscaling(2)); % Creating bucketed image
    tmp_sr_imgV = zeros(lr_height*upscaling(1),lr_width*upscaling(2)); % Creating bucketed image
end

merged_mec_weighting = merged_mec_weighting(~outlier_vec);
merged_qps_weighting = merged_qps_weighting(~outlier_vec);

counter_matrix = zeros(lr_height*upscaling(1),lr_width*upscaling(2)); % Matrix for counting the number of entries in each bucket
weighting_matrix = zeros(lr_height*upscaling(1),lr_width*upscaling(2)); % Matrix for the sum of MEC-DIS-QPS weights

num_pix = num_pix - sum(outlier_vec);

if dye == 3, % Color case
    for itera = 1:num_pix,
        % Weighted case
        tmp_sr_imgY(rounded_meshY(itera),rounded_meshX(itera))    = tmp_sr_imgY(rounded_meshY(itera),rounded_meshX(itera)) + merged_lin_valsY(itera)*alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);
        tmp_sr_imgU(rounded_meshY(itera),rounded_meshX(itera))    = tmp_sr_imgU(rounded_meshY(itera),rounded_meshX(itera)) + merged_lin_valsU(itera)*alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);
        tmp_sr_imgV(rounded_meshY(itera),rounded_meshX(itera))    = tmp_sr_imgV(rounded_meshY(itera),rounded_meshX(itera)) + merged_lin_valsV(itera)*alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);

        counter_matrix(rounded_meshY(itera),rounded_meshX(itera))   = counter_matrix(rounded_meshY(itera),rounded_meshX(itera)) + 1;
        weighting_matrix(rounded_meshY(itera),rounded_meshX(itera)) = weighting_matrix(rounded_meshY(itera),rounded_meshX(itera)) + alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);
    end

    % Weighted case
    tmp_sr_imgY = tmp_sr_imgY ./ weighting_matrix;
    tmp_sr_imgU = tmp_sr_imgU ./ weighting_matrix;
    tmp_sr_imgV = tmp_sr_imgV ./ weighting_matrix;
else % Grayscale case
    for itera = 1:num_pix,
        % Weighted case
        tmp_sr_imgY(rounded_meshY(itera),rounded_meshX(itera))    = tmp_sr_imgY(rounded_meshY(itera),rounded_meshX(itera)) + merged_lin_valsY(itera)*alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);

        counter_matrix(rounded_meshY(itera),rounded_meshX(itera))   = counter_matrix(rounded_meshY(itera),rounded_meshX(itera)) + 1;
        weighting_matrix(rounded_meshY(itera),rounded_meshX(itera)) = weighting_matrix(rounded_meshY(itera),rounded_meshX(itera)) + alpha*merged_mec_weighting(itera)*beta*weighting_struct.dis_weighting(itera)*gamma*merged_qps_weighting(itera);
    end

    % Weighted case
    tmp_sr_imgY = tmp_sr_imgY ./ weighting_matrix;
end

disp('... Throwing points into spatially quantized buckets: Done.');
%toc(timeSR2bucket);

sampled_img = tmp_sr_imgY;
if dye == 3,
    sampled_img(:,:,2) = tmp_sr_imgU;
    sampled_img(:,:,3) = tmp_sr_imgV;
end

disp('... Interpolating data to the HR grid ...');
%timeSR2interp = tic;

% For Cubic Interpolation a slight Padding is required to avoid incorrectly interpolated values near the image borders
tmp_sr_imgY = padarray(tmp_sr_imgY,upscaling,'symmetric','both');
if dye == 3,
    tmp_sr_imgU = padarray(tmp_sr_imgU,upscaling,'symmetric','both');
    tmp_sr_imgV = padarray(tmp_sr_imgV,upscaling,'symmetric','both');
end

% Bringing the bucketed image into a linearized form for the griddata call
bucket_meshX = sr_meshX(~isnan(tmp_sr_imgY));
bucket_meshY = sr_meshY(~isnan(tmp_sr_imgY));

bucket_valsY  = tmp_sr_imgY(~isnan(tmp_sr_imgY));
if dye == 3,
    bucket_valsU  = tmp_sr_imgU(~isnan(tmp_sr_imgY));
    bucket_valsV  = tmp_sr_imgV(~isnan(tmp_sr_imgY));
end

% SR Reconstruction using Non-Uniform Interpolation (NUI)
% Griddata-Solution (ScatteredInterpolant is called from inside the griddata function)
tmp_Y = griddata(bucket_meshX,bucket_meshY,bucket_valsY,sr_meshX,sr_meshY,'cubic');%,'linear');
tmp_Y(isnan(tmp_Y)) = 0; % Temporary NaN treatment for Y
sr_img(:,:,1) = tmp_Y;
if dye == 3, % YUV444 Input
  tmp_U = griddata(bucket_meshX,bucket_meshY,bucket_valsU,sr_meshX,sr_meshY,'cubic');%,'linear');
  tmp_V = griddata(bucket_meshX,bucket_meshY,bucket_valsV,sr_meshX,sr_meshY,'cubic');%,'linear');
  tmp_U(isnan(tmp_U)) = 127/255; % Temporary NaN treatment for U
  tmp_V(isnan(tmp_V)) = 127/255; % Temporary NaN treatment for V
  sr_img(:,:,2) = tmp_U;
  sr_img(:,:,3) = tmp_V;
end
%sr_affix = [sr_affix,'lin'];

% Cropping the Padding
sr_img = sr_img(1+upscaling(1):end-upscaling(1),1+upscaling(2):end-upscaling(2),:);

disp('... Interpolating data to the HR grid: Done.');
%toc(timeSR2interp);

disp('... Deblurring SR image ...');
%timeSR2deblur = tic;

% Padding Image for the Deblurring Step
pad_size = 16; % This was 5 prior

sr_img_tmp = padarray(sr_img,[pad_size pad_size],'replicate','both');

% SR Deblurring Step (Deconvolution)
sr_img_deconv = deconvlucy(sr_img_tmp,psf,lucy_iter); % Lucy-Richardson deconvolution for deblurring (more iterations improves the result for checkerboard)

% Remove the Pading after the Deblurring Step
sr_img_deconv = sr_img_deconv(1+pad_size:end-pad_size,1+pad_size:end-pad_size,:);

disp('... Deblurring SR image: Done.');
%toc(timeSR2deblur);


function skypos = place (tjd, object, locatn, icoord, star, observ)

% this function computes the apparent direction of a star or solar
% system body at a specified time and in a specified coordinate
% system.  based on kaplan, et al. (1989), astronomical journal 97,
% 1197-1210, with some enhancements from klioner (2003),
% astronomical journal 125, 1580-1597.

%  tjd    = tt julian date for place (in)

%  object = character string identifying object of interest (in)

%           for solar system
%           body,             specify the name using all upper-
%                             case letters ('sun', 'moon',
%                             'jupiter', etc.),
%                             - or -
%                             specify the body id number
%                             in a 4-character string of the
%                             form '=nnn', where nnn is the
%                             body id number
%           for star,         provide a blank string, the word
%                             'star', or any string beginning
%                             with '*'

%  locatn = integer code specifying location of observer (in)

%           set locatn=0 for observer at geocenter
%           set locatn=1 for observer on surface of earth
%           set locatn=2 for observer on near-earth spacecraft

%  icoord = integer code specifying coordinate system of output
%           position (in)

%           set icoord=0 for gcrs (or 'local gcrs')
%           set icoord=1 for true equator and equinox of date
%           set icoord=2 for true equator and cio of date
%           set icoord=3 for astrometric coordinates, i.e.,
%                        without light deflection or aberration

%  star   = array of catalog data for star (in)

%           (not used if solar system body requested)

%           star(1) = icrs right ascension in hours
%           star(2) = icrs declination in degrees
%           star(3) = icrs proper motion in ra in
%                     milliarcseconds/year
%           star(4) = icrs proper motion in dec in
%                     milliarcseconds/year
%           star(5) = parallax in milliarcseconds
%           star(6) = radial velocity in kilometers/second
%           further star array elements are not used here
%           but are reserved for future use

%  observ = array of data specifying location of observer (in)

%           (not used if locatn=0)

%           for locatn = 1,

%           observ(1) = geodetic longitude (wgs-84) of observer
%                       (east +) in degrees
%           observ(2) = geodetic latitude (wgs-84) of observer
%                       (north +) in degrees
%           observ(3) = height of observer above ellipsoid
%                       in meters
%           observ(4) = value of delta-t in seconds
%                       (delta-t=tt-ut1)
%           observ(5) = (not used, reserved for future use)
%           observ(6) = (not used, reserved for future use)

%           for locatn = 2,

%           observ(1) = geocentric x in kilometers
%           observ(2) = geocentric y in kilometers
%           observ(3) = geocentric z in kilometers
%           observ(4) = geocentric x-dot in kilometers/second
%           observ(5) = geocentric y-dot in kilometers/second
%           observ(6) = geocentric z-dot in kilometers/second
%           with respect to true equator and equinox of date

%  skypos = array of output data specifying object's place
%           on the sky at time tjd, with respect to the
%           specified output coordinate system (out)

%           skypos(1) = x, dimensionless      unit vector
%           skypos(2) = y, dimensionless      toward object
%           skypos(3) = z, dimensionless
%           skypos(4) = apparent, topocentric, or astrometric
%                       right ascension in hours
%           skypos(5) = apparent, topocentric, or astrometric
%                       declination in degrees
%           skypos(6) = true (geometric, euclidian) distance
%                       to solar system body in au at time tjd,
%                       or 0.0d0 for star
%           skypos(7) = radial velocity in kilometers/second

%           further skypos array elements are not used here
%           but are reserved for future use

% note 1: values of locatn and icoord for various standard kinds of place:

%   locatn = 0 and icoord = 1 apparent place
%   locatn = 1 and icoord = 1 topocentric place
%   locatn = 0 and icoord = 0 virtual place
%   locatn = 1 and icoord = 0 local place
%   locatn = 0 and icoord = 3 astrometric place
%   locatn = 1 and icoord = 3 topocentric astrometric place

% note 2: arrays star and skypos may be expanded in the future, and
% this can be allowed for in the calling code by dimensioning
% these arrays with 20 and 10 elements, respectively, even though
% elements beyond star(6) and skypos(7) are not now referred to in
% this subroutine.

% note 3: if locatn = 1 and observ(4) = 0.0d0, the value of delta-t will
% be obtained from getdt, which provides the last value of delta-t
% defined by the user via call to setdt.

% note 4: skypos(7), the radial velocity, is the predicted
% radial velocity measure (z) times the speed of light, an
% inherently spectroscopic measure. for a star, it
% includes all effects, such as gravitational red shift,
% contained in the catalog barycentric radial velocity measure,
% which is assumed given in star(6). for a solar system
% body, it applies to a fictitious emitter at the center of the
% observed object, assumed massless (no gravitational red shift),
% and does not in general apply to reflected light.

% ported from NOVAS 3.1

%%%%%%%%%%%%%%%%%%%%%%%

% t0 = tdb julian date of epoch j2000.0 (tt)

t0 = 2451545.0d0;

% get body ids

iearth = idss_novas ('earth');

isun = idss_novas ('sun');

% get c, the speed of light in au/day

c = astcon ('c(au/day)', 1.0d0);

% check on earth as an observed object

if (strcmp(object, '=3') == 1 && locatn ~= 2)

    fprintf ('\n place: will not process earth as observed object except when locatn = 2');

    return

end

% compute tdbjd, the tdb julian date corresponding to ttjd

ttjd = tjd;

tdbjd = tjd;

[x, secdif] = novas_times (tdbjd);

tdbjd = ttjd + secdif / 86400.0d0;

% get position and velocity of the earth wrt barycenter of solar system, in icrs

% fprintf('\n\nearth state vector wrt barycenter\n');

[peb, veb, ierr] = solsys (tdbjd, iearth, 0);

if (ierr ~= 0)

    fprintf ('\nplace: cannot obtain coordinates of earth at jd %16.8f', tjd);

    return

end

% get position and velocity of the sun wrt barycenter of solar system, in icrs

% fprintf('\n\nsun state vector wrt barycenter\n');

[psb, vsb, ierr] = solsys (tdbjd, isun, 0);

if (ierr ~= 0)

    fprintf ('\nplace: cannot obtain coordinates of sun at jd %16.8f', tjd);

    return

end

% get position and velocity of observer

if (locatn == 1 || locatn == 2)

    % for topocentric place, get geocentric position and velocity
    % vectors of observer

    % fprintf('\n\nobserver state vector wrt geocenter\n');

    [pog, vog] = geopos (ttjd, locatn, observ);

    loc = 1;

else

    % for geocentric place, there is nothing to do

    for j = 1:3

        pog(j) = 0.0d0;

        vog(j) = 0.0d0;

    end

    loc = 0;

end

% compute position and velocity of observer wrt barycenter of
% solar system (galilean transformation from gcrs to bcrs)

for j = 1:3

    pob(j) = peb(j) + pog(j);

    vob(j) = veb(j) + vog(j);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find geometric position of observed object
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (strcmp(object, 'star') == 1 || strcmp(object, ' ') == 1 || strcmp(object(1:1), '*'))

    %%%%%%%%%%%%%%%%%%%%%%%%%
    % observed object is star
    %%%%%%%%%%%%%%%%%%%%%%%%%

    idbody = -9999;

    % get position of star updated for its space motion

    [pos1, vel1] = vectrs (star(1), star(2), star(3), star(4), star(5), star(6));

    dt = dlight (pos1, pob);

    pos2 = propmo (t0, pos1, vel1, tdbjd + dt);

    % get position of star wrt observer (corrected for parallax)

    [pos3, tlight] = geocen (pos2, pob);

    dis = 0.0d0;

else

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % observed object is solar system body
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % get id number of body

    if (strcmp(object(1:1), '=') == 1)

        idbody = object(2:length(object));

    else

        idbody = idss_novas (object);

        if (idbody == -9999)

            fprintf ('\nplace: cannot obtain coordinates of object at jd %16.8f', tjd);

            return

        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % get position of body wrt barycenter of solar system
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % fprintf('\n\nobject state vector wrt barycenter\n');

    [pos1, vel1, ierr] = solsys (tdbjd, str2num(idbody), 0);

    if (ierr ~= 0)

        fprintf ('\nplace: cannot obtain coordinates of object at jd %16.8f', tjd);

        return

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % get position of body wrt observer, and true (euclidian) distance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % fprintf('\n\nobject state vector wrt observer\n');

    [pos2, tlight] = geocen (pos1, pob);

    dis = tlight * c;

    % get position of body wrt observer, antedated for light-time

    % fprintf('\n\nobject state vector wrt observer - littim\n');

    [pos3, tlight] = littim (tdbjd, idbody, pob, 0.0);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% apply gravitational deflection of light and aberration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (icoord == 3)

    % these calculations are skipped for astrometric place

    for j = 1:3

        pos5(j) = pos3(j);

    end

else

    % variable loc determines whether earth deflection is included

    if (loc == 1)

        [x, frlimb] = limang (pos3, pog);

        if (frlimb < 0.8d0)

            loc = 0;

        end

    end

    % compute gravitational deflection and aberration

    pos4 = grvdef (tdbjd, loc, pos3, pob);

    pos5 = aberat (pos4, vob, tlight);

    % position vector is now in gcrs

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% transform, if necessary, to output coordinate system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (icoord == 1)

    % transform to equator and equinox of date

    pos6 = frame (pos5, 1);

    pos7 = preces (t0, pos6, tdbjd);

    pos8 = nutate (tdbjd, pos7);

elseif (icoord == 2)

    % transform to equator and cio of date

    % obtain the basis vectors, in the gcrs, of the celestial
    % intermediate system

    kcio = cioloc (tdbjd, rcio);

    [px, py, pz] = ciobas (tdbjd, rcio, kcio);

    % transform position vector to celestial intermediate system

    pos8(1) = px(1) * pos5(1) + px(2) * pos5(2) + px(3) * pos5(3);

    pos8(2) = py(1) * pos5(1) + py(2) * pos5(2) + py(3) * pos5(3);

    pos8(3) = pz(1) * pos5(1) + pz(2) * pos5(2) + pz(3) * pos5(3);

else

    % no transformation -- keep coordinates in gcrs (or icrs for astrometric place)

    for j = 1:3

        pos8(j) = pos5(j);

    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set up star data, if applicable
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (idbody == -9999)

    rvs(1) = star(1);

    rvs(2) = star(2);

    rvs(3) = star(6);

    if (star(5) <= 0.0d0)

        vel1(1) = 0.0d0;

        vel1(2) = 0.0d0;

        vel1(3) = 0.0d0;

    end

else

    rvs(1) = 0.0d0;

    rvs(2) = 0.0d0;

    rvs(3) = 0.0d0;

end

% compute distances: observer-geocenter, observer-sun, object-sun

rvd(1) = sqrt((pob(1) - peb(1))^2 + (pob(2) - peb(2))^2 + (pob(3) - peb(3))^2);

rvd(2) = sqrt((pob(1) - psb(1))^2 + (pob(2) - psb(2))^2 + (pob(3) - psb(3))^2);

rvd(3) = sqrt((pos1(1) - psb(1))^2 + (pos1(2) - psb(2))^2 + (pos1(3) - psb(3))^2);

rv = radvl (pos3, vel1, vob, rvs, rvd);

% finish up

[ra, dec] = angles (pos8);

x = sqrt (pos8(1)^2 + pos8(2)^2 + pos8(3)^2);

for j = 1:3

    skypos(j) = pos8(j) / x;

end

skypos(4) = ra;

skypos(5) = dec;

skypos(6) = dis;

skypos(7) = rv;