function g = svargplvmLogLikeGradients(model)

% SVARGPLVMLOGLIKEGRADIENTS Compute the gradients for the variational shared GPLVM.
% FORMAT
% DESC returns the gradients of the log likelihood with respect to the
% parameters of the GP-LVM model and with respect to the latent
% positions of the GP-LVM model.
% ARG model : the FGPLVM structure containing the parameters and
% the latent positions.
% RETURN g : the gradients of the latent positions (or the back
% constraint's parameters) and the parameters of the GP-LVM model.
%
% FORMAT
% DESC returns the gradients of the log likelihood with respect to the
% parameters of the GP-LVM model and with respect to the latent
% positions of the GP-LVM model in seperate matrices.
% ARG model : the FGPLVM structure containing the parameters and
% the latent positions.
% RETURN gX : the gradients of the latent positions (or the back
% constraint's parameters).
% RETURN gParam : gradients of the parameters of the GP-LVM model.
%
% COPYRIGHT : Andreas C. Damianou, 2011

% SEEALSO : svargplvmLogLikelihood, svargplvmCreate, modelLogLikeGradients

% VARGPLVM

try
    pool_open = matlabpool('size')>0;
catch e
    pool_open = 0;
end

if pool_open && (isfield(model,'parallel') && model.parallel)
    g=gPar(model);
else
    % Functions g1 and g2 should be equivalent for the static case, but
    % g1 might (?) be faster.
    if ~isfield(model, 'dynamics') || isempty(model.dynamics)
        g=g1(model);
        %g = g2(model);
    else
        if isfield(model.dynamics, 'seq') & ~isempty(model.dynamics.seq)
            g = gDyn(model); % Slower but memory efficient.
        else
            %g = gDyn(model); % Slower but memory efficient.
            g = gDynFast(model); % Faster, but memory consuming
        end
    end
end

%g = g1(model);
%g = g2(model);
end

%%%%%!!!!!! NOTE:
% g1 and g2 should be equivalent for the non-dynamics case. gPar is the
% equivalent of g1 for parallel computations. gDyn and gDynFast are
% suitable when there are dynamics, with the first focusing on memory
% efficiency and the second on speed.

function g = gPar(model)

% THE FOLLOWING WORKS ONLY WHEN THERE ARE NO DYNAMICS... %___ TODO TEMP
% Shared params
if isfield(model, 'dynamics') && isempty(model.dynamics) % TEMP
    error('The gradients for the dynamics case are not implemented correctly yet!'); % TEMP
else % ....
    gVarmeansKL = - model.vardist.means(:)';
    gVarcovsKL = 0.5 - 0.5*model.vardist.covars(:)';
end
model = svargplvmPropagateField(model, 'onlyLikelihood', 1);

%fprintf('# Derivs for KL is (should be zero): ');%%%TEMP

% Private params
% g = [[sum_m(gVar_only_likelihood)+gVar_onlyKL] g_1 g_2 ...]
% where sum_m is the sum over all models and g_m is the gradients for the
% non-shared parameters for model m
gShared = [gVarmeansKL gVarcovsKL];
modelTemp=model.comp;
parfor i=1:model.numModels
    gAll{i} = vargplvmLogLikeGradients(modelTemp{i});
end
g=[];
for i=1:model.numModels
    g_i = gAll{i};
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics) % TEMP (doesn't work correctly for dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams); % TEMP (doesn't work correctly for dynamics)
        g_i = g_i((model.dynamics.nParams+1):end); % TEMP (doesn't work correctly for dynamics)
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g];

end

function g = g1(model)

% THE FOLLOWING WORKS ONLY WHEN THERE ARE NO DYNAMICS... %___ TODO TEMP
% Shared params
if isfield(model, 'dynamics') && isempty(model.dynamics) % TEMP !!!! TODO (~isempty?)
    error('The gradients for the dynamics case are not implemented correctly yet!'); % TEMP
else % ....
    gVarmeansKL = - model.vardist.means(:)';
    gVarcovsKL = 0.5 - 0.5*model.vardist.covars(:)';
end
model = svargplvmPropagateField(model, 'onlyLikelihood', 1);

%fprintf('# Derivs for KL is (should be zero): ');%%%TEMP

% Private params
% g = [[sum_m(gVar_only_likelihood)+gVar_onlyKL] g_1 g_2 ...]
% where sum_m is the sum over all models and g_m is the gradients for the
% non-shared parameters for model m
g = [];
gShared = [gVarmeansKL gVarcovsKL];

for i=1:model.numModels
    g_i = vargplvmLogLikeGradients(model.comp{i});
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics) % !! (doesn't work correctly for dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams); % !! (doesn't work correctly for dynamics)
        g_i = g_i((model.dynamics.nParams+1):end); % TEMP (doesn't work correctly for dynamics)
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g];
end
% TEMP = vargplvmLogLikeGradients(model.comp{1}); %%%TEMP
% sum(sum(abs(gShared - TEMP(1:160)))) %%%TEMP
% g = [g 0*g_i]; %%%%%%%%%%% TEMP
%g = [-vargplvmLogLikeGradients(model.comp{1}) 0*g_i];

% NOT TESTED FOR THE DYNAMICS CASE
function g = g2(model)
g_1 = vargplvmLogLikeGradients(model.comp{1});
gShared = g_1(1:model.vardist.nParams);
g_1 = g_1(model.vardist.nParams+1:end);
model = svargplvmPropagateField(model, 'onlyLikelihood', 1, true);
g=[];
for i=2:model.numModels
    g_i = vargplvmLogLikeGradients(model.comp{i});
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams);
        g_i = g_i((model.dynamics.nParams+1):end);
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g_1 g];
end

function g = gDynFast(modelAll)

gPrivAll = [];
gSharedCoeff = 0;
dynModel = modelAll.dynamics;
gSharedVar = zeros(1,dynModel.vardist.nParams);

for m=1:modelAll.numModels
    % This is similar to vargplvmLogLikeGradients but for every model

    model = modelAll.comp{m}; % current model

    % Include calculations for the KL term only once, for the first model.
    if m == 1
        includeKL = 1;
    else
        includeKL = 0;
    end

    % Likelihood terms (coefficients)
    [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model);

    if isfield(model, 'learnInducing')
        learnInducing = model.learnInducing;
    else
        learnInducing = true;
    end

    % Get (in three steps because the formula has three terms) the gradients of
    % the likelihood part w.r.t the data kernel parameters, variational means
    % and covariances (original ones). From the field model.vardist, only
    % vardist.means and vardist.covars and vardist.lantentDimension are used.
    [gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1', learnInducing);
    [gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2, learnInducing);
    [gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);
    gKern3 = kernGradient(model.kern, model.X_u, gK_uu);

    % At this point, gKern gVarmeansLik and gVarcovsLik have the derivatives for the
    % likelihood part. Sum all of them to obtain the final result.
    gKern = gKern0 + gKern1 + gKern2 + gKern3;
    gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

    if strcmp(model.kern.type, 'rbfardjit')
        % different derivatives for the variance, which is super-numerically stable for
        % this particular kernel
        if model.learnSigmaf == 1
            gKern(1) = 0.5*model.d*( - model.k+ sum(sum(model.invLat.*model.invLat))/model.beta - model.beta*(model.Psi0-model.TrC)  )...
                + 0.5*tmpV;

            if ~isstruct(model.kern.transforms(1))
                fhandle = str2func([model.kern.transform(1) 'Transform']);
                gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
            else
                fhandle = str2func([model.kern.transforms(1).type 'Transform']);
                if ~isfield(model.kern.transforms(1), 'transformsettings')
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
                else
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact', model.kern.transforms(1).transformsettings);
                end
            end
        else
            gKern(1) = 0;
        end
    end

    %%% Compute Gradients with respect to X_u %%%
    gKX = kernGradX(model.kern, model.X_u, model.X_u);

    % The 2 accounts for the fact that covGrad is symmetric
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end

    if learnInducing
        % Allocate space for gX_u
        gX_u = zeros(model.k, model.q);
        % Compute portion associated with gK_u
        for i = 1:model.k
            for j = 1:model.q
                gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
            end
        end

        gInd = gInd1 + gInd2 + gX_u(:)';
    end

    % If the inducing points are fixed (tied to the latent points) then
    % X_u=K_t*dynamics.vardist.means and the derivatives w.r.t theta_t must be
    % amended with the appropriate partial derivatives. gInd must be passed,
    % in that case, as an argument to the function which calculates the
    % derivatives for the reparametrized quantities.
    if isfield(model, 'fixInducing') & model.fixInducing
        if learnInducing
            gIndRep = gInd;
        end
    else
        gIndRep=[];
    end

    gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;

    %----------------   This replaces vargpTimeDynamicsPriorReparamGrads

    % gVarmeansLik and gVarcovsLik are serialized into 1x(NxQ) vectors.
    % Convert them to NxQ matrices, i.e. fold them column-wise.
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);

    if ~isempty(gIndRep)
        gIndRep = reshape(gIndRep, dynModel.N, dynModel.q); %%%%%
    end

    % The second term in the parenthesis, corresponds to the KL term. The
    % multiplier will set it to zero, if we need only the derivatives
    % corresponding only to the likelihood part.
    gVarmeans = dynModel.Kt * (gVarmeansLik - includeKL* dynModel.vardist.means);

    gVarcovs = zeros(dynModel.N, dynModel.q); % memory preallocation

    % The following quantities (in the loop) are needed to be calculated in
    % a single loop wrt q,
    if m==1
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;
            Bt_q = eye(dynModel.N) + LambdaH_q*LambdaH_q'.*dynModel.Kt;

            % Invert Bt_q
            Lbt_q = jitChol(Bt_q)';
            G1 = Lbt_q \ diag(LambdaH_q);
            G = G1*dynModel.Kt;

            % Find Sq
            Sq{q} = dynModel.Kt - G'*G; %%%%%
            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            trGradKL{q} = -0.5*(BhatKt*Bhat + dynModel.vardist.means(:,q) * dynModel.vardist.means(:,q)');
            IBK = eye(dynModel.N) - BhatKt;
        end
    else
        for q=1:dynModel.q
            trGradKL{q} = 0;
        end
    end

    sumTrGradKL = 0;
    for q=1:dynModel.q
        % If includeKL is set to 0, then the KL part will be multiplied
        % with 0, thus being ignored.
        gVarcovs(:,q) = - (Sq{q} .* Sq{q}) * (gVarcovsLik(:,q) + includeKL * 0.5*dynModel.vardist.covars(:,q));

        diagVarcovs = repmat(gVarcovsLik(:,q)', dynModel.N,1);
        trGradKL{q} = trGradKL{q} + IBK .*  diagVarcovs * IBK';
        trGradKL{q} = trGradKL{q} + dynModel.vardist.means(:,q) * gVarmeansLik(:,q)';

        % In case gInd is empty then the inducing points are not reparametrized
        % (are not fixed to the variational means) we need not amend further the
        % derivative w.r.t theta_t, otherwise we have to do that.
        if ~isempty(gIndRep)
            trGradKL{q} = trGradKL{q} + dynModel.vardist.means(:,q) * gIndRep(:,q)';
        end

        sumTrGradKL = sumTrGradKL + trGradKL{q};
    end
    gSharedCoeff = gSharedCoeff + sumTrGradKL;

    % Serialize (unfold column-wise) gVarmeans and gVarcovs from NxQ matrices
    % to 1x(NxQ) vectors
    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';

    %-------------------------------------------------------------------

    % Variational variances are positive: Now that the final covariances
    % are obtained we amend with the partial derivatives due to the
    % exponential transformation to ensure positiveness.
    gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';

    if isfield(model, 'fixInducing') && model.fixInducing && learnInducing
        % If there are dynamics the derivative must further be amended with the
        % partial deriv. due to the mean reparametrization.
        if isfield(model, 'dynamics') && ~isempty(model.dynamics)
            gInd = reshape(gInd,model.k,model.q);
            %gInd = gInd' * model.dynamics.Kt;
            gInd =  model.dynamics.Kt * gInd;
            gInd = gInd(:)';
        end
        %gVarmeans(model.inducingIndices, :) = gVarmeans(model.inducingIndices,
        %:) + gInd; % This should work AFTER reshaping the matrices...but here
        %we use all the indices anyway.
        gVarmeans = gVarmeans + gInd;
        gInd = []; % Inducing points are not free variables anymore, they dont have derivatives on their own.
    end

    gVar = [gVarmeans gVarcovs];

    if isfield(model.vardist,'paramGroups')
        gVar = gVar*model.vardist.paramGroups;
    end

    % In case we are in the phase where the vardistr. is initialised (see above
    % for the variance of the kernel), beta is kept fixed. For backwards
    % compatibility this can be controlled either with the learnBeta field or
    % with the initVardist field. The later overrides the first.
    if isfield(model, 'learnBeta') && model.learnBeta
        gBetaFinal = gBeta;
    else
        gBetaFinal = 0*gBeta;
    end
    if isfield(model, 'initVardist')
        if model.initVardist == 1
            gBetaFinal = 0*gBeta;
        else
            gBetaFinal = gBeta;
        end
    end

    gSharedVar = gSharedVar + gVar;

    if ~learnInducing
        gInd = [];
    end

    % At this point, gDynKern will be [] if there are no dynamics.
    gPrivAll = [gPrivAll gInd gKern gBetaFinal];
end

gDynKern = kernGradient(dynModel.kern, dynModel.t, gSharedCoeff);

%%%% ...... was commented...
if isfield(model, 'dynamics') && ~isempty(model.dynamics)
    if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic2')
        if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
            gDynKern(2) = 0;
        end
    end

    %___NEW: assume that the second rbf/matern etc kernel is last in the
    %compound kernel
    %if numel(model.dynamics.kern.comp) > 3
    if isfield(model.dynamics, 'learnSecondVariance') && ~model.dynamics.learnSecondVariance   %%%%% NEW
        gDynKern(end) = 0;
    end
    %end
    %___
end

%---- NEW 2012: This is to fix selectively some of the kernel's parameters
if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
    % The field model.dynamics.fixedVariance must have the
    % indexes of the gradient vector that correspond to
    % variances of kernels of the matern class and we wish to
    % not learn them.
    if isfield(model.dynamics, 'fixedKernVariance') && ~isempty(model.dynamics.fixedKernVariance)
        gDynKern(model.dynamics.fixedKernVariance) = 0;
    end
end
%----

g = [gSharedVar gDynKern gPrivAll];

end

% The bound is written as:
% ll = KL + ll1 + ll2 + ...
% For the derivatives, it's a bit trickier, since the functions we have
% from vargplvm calculate the derivatives for ll+KL in a mixed way (because
% the derivatives w.r.t mu_bar and lambda exist ALSO in the likelihood
% parts, due to reparametrising mu and S with Kt. So, we split as follows:
% [gVarAll gDynKern gPriv1 gPriv2 ... ]
% TODO: for varcovs, perform (Sq.*Sq)*[large sum]
%   and for varmeans, perform Kt * [large sum
% TODO: add switch so that one model can be dynamical, another can be
%   static
function g = gDyn(modelAll)

gPrivAll = [];
gSharedCoeff = 0;
dynModel = modelAll.dynamics;
gSharedVar = zeros(1,dynModel.vardist.nParams);

if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
    seqStart=1;
    seq = dynModel.seq;
    for i=1:length(dynModel.seq)
        seqEnd = seq(i);
        sumTrGradKL{i} = zeros(seqEnd-seqStart+1, seqEnd-seqStart+1);
        seqStart = seqEnd+1;
    end
end

for m=1:modelAll.numModels
    model = modelAll.comp{m}; % current model

    % Include calculations for the KL term only once, for the first model.
    if m == 1
        includeKL = 1;
    else
        includeKL = 0;
    end

    % Likelihood terms (coefficients)
    [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model);

    % Get (in three steps because the formula has three terms) the gradients of
    % the likelihood part w.r.t the data kernel parameters, variational means
    % and covariances (original ones). From the field model.vardist, only
    % vardist.means and vardist.covars and vardist.lantentDimension are used.
    [gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1');
    [gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2);
    [gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);
    gKern3 = kernGradient(model.kern, model.X_u, gK_uu);

    % At this point, gKern gVarmeansLik and gVarcovsLik have the derivatives for the
    % likelihood part. Sum all of them to obtain the final result.
    gKern = gKern0 + gKern1 + gKern2 + gKern3;
    gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

    if strcmp(model.kern.type, 'rbfardjit')
        % different derivatives for the variance, which is super-numerically stable for
        % this particular kernel
        if model.learnSigmaf == 1
            gKern(1) = 0.5*model.d*( - model.k+ sum(sum(model.invLat.*model.invLat))/model.beta - model.beta*(model.Psi0-model.TrC)  )...
                + 0.5*tmpV;

            if ~isstruct(model.kern.transforms(1))
                fhandle = str2func([model.kern.transform(1) 'Transform']);
                gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
            else
                fhandle = str2func([model.kern.transforms(1).type 'Transform']);
                if ~isfield(model.kern.transforms(1), 'transformsettings')
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
                else
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact', model.kern.transforms(1).transformsettings);
                end
            end
        else
            gKern(1) = 0;
        end
    end

    %%% Compute Gradients with respect to X_u %%%
    gKX = kernGradX(model.kern, model.X_u, model.X_u);

    % The 2 accounts for the fact that covGrad is symmetric
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end

    % Allocate space for gX_u
    gX_u = zeros(model.k, model.q);
    % Compute portion associated with gK_u
    for i = 1:model.k
        for j = 1:model.q
            gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
        end
    end

    gInd = gInd1 + gInd2 + gX_u(:)';

    % If the inducing points are fixed (tied to the latent points) then
    % X_u=K_t*dynamics.vardist.means and the derivatives w.r.t theta_t must be
    % amended with the appropriate partial derivatives. gInd must be passed,
    % in that case, as an argument to the function which calculates the
    % derivatives for the reparametrized quantities.
    if isfield(model, 'fixInducing') & model.fixInducing
        gIndRep = gInd;
    else
        gIndRep=[];
    end
    gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;

    %----------------   This replaces vargpTimeDynamicsPriorReparamGrads

    % gVarmeansLik and gVarcovsLik are serialized into 1x(NxQ) vectors.
    % Convert them to NxQ matrices, i.e. fold them column-wise.
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);

    if ~isempty(gIndRep)
        gIndRep = reshape(gIndRep, dynModel.N, dynModel.q); %%%%%
    end

    % The second term in the parenthesis, corresponds to the KL term. The
    % multiplier will set it to zero, if we need only the derivatives
    % corresponding only to the likelihood part.
    gVarmeans = dynModel.Kt * (gVarmeansLik - includeKL* dynModel.vardist.means);
    gVarcovs = zeros(dynModel.N, dynModel.q); % memory preallocation

    if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;

            % The calculations are performed for each sequence independently
            % because correlations between sequences are not captured and, thus,
            % most of the matrices involved are block diagonal, so that in each of
            % the following loops only one block is considered.
            seqStart=1;
            for i=1:length(dynModel.seq)
                seqEnd = seq(i);
                Bt_q = eye(seqEnd-seqStart+1) + LambdaH_q(seqStart:seqEnd,1)*LambdaH_q(seqStart:seqEnd,1)'.*dynModel.Kt(seqStart:seqEnd,seqStart:seqEnd);

                % Invert Bt_q
                Lbt_q = jitChol(Bt_q)';
                G1 = Lbt_q \ diag(LambdaH_q(seqStart:seqEnd,1));
                G = G1*dynModel.Kt(seqStart:seqEnd, seqStart:seqEnd);

                % Find Sq
                Sq = dynModel.Kt(seqStart:seqEnd, seqStart:seqEnd) - G'*G;

                gVarcovs(seqStart:seqEnd,q) = - (Sq .* Sq) * (gVarcovsLik(seqStart:seqEnd,q) + ...
                    includeKL* 0.5*dynModel.vardist.covars(seqStart:seqEnd,q));

                % Find the coefficient for the grad. wrt theta_t (params of Kt)
                G1T=G1';
                Bhat=G1T*G1;
                BhatKt=G1T*G;

                if includeKL
                    trGradKL = -0.5*(BhatKt*Bhat + dynModel.vardist.means(seqStart:seqEnd,q) * dynModel.vardist.means(seqStart:seqEnd,q)');
                else
                    trGradKL = 0;
                end
                IBK = eye(seqEnd-seqStart+1) - BhatKt;
                diagVarcovs = repmat(gVarcovsLik(seqStart:seqEnd,q)', seqEnd-seqStart+1,1);
                trGradKL = trGradKL + IBK .*  diagVarcovs * IBK';
                trGradKL = trGradKL + dynModel.vardist.means(seqStart:seqEnd,q) * gVarmeansLik(seqStart:seqEnd,q)';

                % In case gInd is empty then the inducing points are not reparametrized
                % (are not fixed to the variational means) we need not amend further the
                % derivative w.r.t theta_t.
                if ~isempty(gIndRep)
                    trGradKL = trGradKL + dynModel.vardist.means(seqStart:seqEnd,q) * gIndRep(seqStart:seqEnd,q)';
                end
                sumTrGradKL{i} = sumTrGradKL{i} + trGradKL;
                seqStart = seqEnd+1;
            end
            %sumTrGradKL = sumTrGradKL + trGradKL;
        end
    else
        sumTrGradKL = 0;
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;
            Bt_q = eye(dynModel.N) + LambdaH_q*LambdaH_q'.*dynModel.Kt;

            % Invert Bt_q
            Lbt_q = jitChol(Bt_q)';
            G1 = Lbt_q \ diag(LambdaH_q);
            G = G1*dynModel.Kt;

            % Find Sq
            Sq = dynModel.Kt - G'*G;

            % If onlyLikelihood is set to 1, then the KL part will be multiplied
            % with zero, thus being ignored.
            gVarcovs(:,q) = - (Sq .* Sq) * (gVarcovsLik(:,q) + includeKL * 0.5*dynModel.vardist.covars(:,q));

            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            if includeKL
                trGradKL = -0.5*(BhatKt*Bhat + dynModel.vardist.means(:,q) * dynModel.vardist.means(:,q)');
            else
                trGradKL = 0;
            end

            IBK = eye(dynModel.N) - BhatKt;
            diagVarcovs = repmat(gVarcovsLik(:,q)', dynModel.N,1);
            trGradKL = trGradKL + IBK .*  diagVarcovs * IBK';
            trGradKL = trGradKL + dynModel.vardist.means(:,q) * gVarmeansLik(:,q)';

            % In case gInd is empty then the inducing points are not reparametrized
            % (are not fixed to the variational means) we need not amend further the
            % derivative w.r.t theta_t, otherwise we have to do that.
            if ~isempty(gIndRep)
                trGradKL = trGradKL + dynModel.vardist.means(:,q) * gIndRep(:,q)';
            end
            sumTrGradKL = sumTrGradKL + trGradKL;
        end
        gSharedCoeff = gSharedCoeff + sumTrGradKL;
    end
    % Serialize (unfold column-wise) gVarmeans and gVarcovs from NxQ matrices
    % to 1x(NxQ) vectors
    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';
    %-------------------------------------------------------------------

    % Variational variances are positive: Now that the final covariances
    % are obtained we amend with the partial derivatives due to the
    % exponential transformation to ensure positiveness.
    gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';

    if isfield(model, 'fixInducing') & model.fixInducing
        % If there are dynamics the derivative must further be amended with the
        % partial deriv. due to the mean reparametrization.
        if isfield(model, 'dynamics') && ~isempty(model.dynamics)
            gInd = reshape(gInd,model.k,model.q);
            %gInd = gInd' * model.dynamics.Kt;
            gInd =  model.dynamics.Kt * gInd;
            gInd = gInd(:)';
        end
        %gVarmeans(model.inducingIndices, :) = gVarmeans(model.inducingIndices,
        %:) + gInd; % This should work AFTER reshaping the matrices...but here
        %we use all the indices anyway.
        gVarmeans = gVarmeans + gInd;
        gInd = []; % Inducing points are not free variables anymore, they dont have derivatives on their own.
    end
    gVar = [gVarmeans gVarcovs];

    if isfield(model.vardist,'paramGroups')
        gVar = gVar*model.vardist.paramGroups;
    end

    if isfield(model, 'dynamics') && ~isempty(model.dynamics)
        if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic2')
            if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
                gDynKern(2) = 0;
            end
        end
        %___NEW: assume that the second rbf/matern etc kernel is last in the
        %compound kernel
        %if numel(model.dynamics.kern.comp) > 3
        if isfield(model.dynamics, 'learnSecondVariance') && ~model.dynamics.learnSecondVariance   %%%%% NEW
            gDynKern(end) = 0;
        end
        %end
        %___
    end

    % In case we are in the phase where the vardistr. is initialised (see above
    % for the variance of the kernel), beta is kept fixed. For backwards
    % compatibility this can be controlled either with the learnBeta field or
    % with the initVardist field. The later overrides the first.
    if isfield(model, 'learnBeta') && model.learnBeta
        gBetaFinal = gBeta;
    else
        gBetaFinal = 0*gBeta;
    end
    if isfield(model, 'initVardist')
        if model.initVardist == 1
            gBetaFinal = 0*gBeta;
        else
            gBetaFinal = gBeta;
        end
    end

    gSharedVar = gSharedVar + gVar;
    % At this point, gDynKern will be [] if there are no dynamics.
    gPrivAll = [gPrivAll gInd gKern gBetaFinal];
end

if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
    seqStart=1;
    gDynKern = size(dynModel.kern.nParams,1);
    for i=1:length(dynModel.seq)
        seqEnd = seq(i);
        gDynKern = gDynKern + kernGradient(dynModel.kern, dynModel.t(seqStart:seqEnd), sumTrGradKL{i});
        seqStart = seqEnd+1;
    end
else
    gDynKern = kernGradient(dynModel.kern, dynModel.t, gSharedCoeff);
end
g = [gSharedVar gDynKern gPrivAll];

end

function [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model)

gPsi1 = model.beta * model.m * model.B';
gPsi1 = gPsi1'; % because it is passed to "kernVardistPsi1Gradient" as gPsi1'...

gPsi2 = (model.beta/2) * model.T1;

gPsi0 = -0.5 * model.beta * model.d;

gK_uu = 0.5 * (model.T1 - (model.beta * model.d) * model.invLmT * model.C * model.invLm);

sigm = 1/model.beta; % beta^-1

PLm = model.invLatT*model.P;
tmpV = sum(sum(PLm.*PLm));
gBeta = 0.5*(model.d*(model.TrC + (model.N-model.k)*sigm -model.Psi0) ...
    - model.TrYY + model.TrPP ...
    + (1/(model.beta^2)) * model.d * sum(sum(model.invLat.*model.invLat)) + sigm*tmpV);

if ~isstruct(model.betaTransform)
    fhandle = str2func([model.betaTransform 'Transform']);
    gBeta = gBeta*fhandle(model.beta, 'gradfact');
else
    fhandle = str2func([model.betaTransform.type 'Transform']);
    gBeta = gBeta*fhandle(model.beta, 'gradfact', model.betaTransform.transformsettings);
end

g_Lambda = repmat(-0.5*model.beta*model.d, 1, model.N);
end


function out_fem_knw(FemMat, CondTensor, OutputFile)
% OUT_FEM_KNW Write Cauchy conductivity file (.knw)
%
% INPUT:
%    - TessMat    : Brainstorm FEM head model mesh (tetrahedral or hexahedral)
%    - CondTensor : [nLayers x 6] or [xx, yy, zz, xy, yz, zx]
%    - OutputFile : Full path to output file

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Takfarinas Medani, 2019
%          Francois Tadel, 2020

% Indices of all elements
index_elem = (1:size(FemMat.Tissue, 1))';

% Anisotropic: Tensor value given for each element
if (length(CondTensor) == length(index_elem))
    values = [index_elem, CondTensor];
% Isotropic: Same value for all the elements of a give tissues, or same value for all the elements
else
    if (numel(CondTensor) == 6)
        CondTensor = repmat(CondTensor(:)', length(FemMat.TissueLabels), 1);
    else
        CondTensor = reshape(CondTensor,[],6);
    end
    % Check dimensions
    if size(CondTensor,1) ~= length(FemMat.TissueLabels)
        error(['Input mismatch: ' num2str(size(CondTensor,1)) ' conductivity tensors for  '  num2str(length(FemMat.TissueLabels)) ' tissues.']);
    end
    values = [index_elem, CondTensor(FemMat.Tissue,:)];
end

% Open file
[fid, message] = fopen(OutputFile, 'wt');
if (fid < 0)
    error(['Could not create file: ' message]);
end
% Write file
% Note: the spaces between values are important
fprintf(fid, 'BOI - TENSORVALUEFILE\n');
fprintf(fid, '========================================================\n');
fprintf(fid, '========================================================\n');
fprintf(fid,'BOI - TENSOR\n');
fprintf(fid,'         %d   %1.5f       %1.5f       %1.5f\n             %1.5f       %1.5f       %1.5f\n', values');
fprintf(fid,'EOI - TENSOR\n');
fprintf(fid,'========================================================\n');
fprintf(fid,'========================================================\n');
fprintf(fid,'EOI - TENSORVALUEFILE\n');
% Close file
fclose(fid);


function noise = ncnmNoiseExpandParam(noise, params)

% NCNMNOISEEXPANDPARAM Expand null category noise model's structure from param vector.
% FORMAT
% DESC returns a null category noise model structure filled with the
% parameters in the given vector. This is used as a helper function to
% enable parameters to be optimised in, for example, the NETLAB
% optimisation functions.
% ARG noise : the noise structure in which the parameters are to be
% placed.
% ARG param : vector of parameters which are to be placed in the
% noise structure.
% RETURN noise : noise structure with the given parameters in the
% relevant locations.
%
% SEEALSO : ncnmNoiseParamInit, ncnmNoiseExtractParam, noiseExpandParam
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% NOISE

noise.bias = params(1:noise.numProcess);
noise.gamman = params(noise.numProcess+1);
if noise.gammaSplit
  noise.gammap = params(noise.numProcess+2);
else
  noise.gammap = noise.gamman;
end


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function plotGeoverErrorNoRot(decentr_state, fig_num, label)

%% Build struct with rel poses and ground truth
rel_poses = {};
for robot_i = 1:numel(decentr_state)
    r_i_data = decentr_state{robot_i};
    for query_i = 1:numel(r_i_data.place_matches)
        match = r_i_data.place_matches{query_i};
        if (numel(match) > 0)
            rel_pose = [];
            rel_pose.Sim_M_Q = match.Sim_M_Q;
            rel_pose.est_dist = norm(match.Sim_M_Q(1:3, 4));
            gt_p_W_M = ...
                decentr_state{match.robot_i}.gt_T_W_C{...
                match.frame_i}(1:3, 4);
            gt_p_W_Q = r_i_data.gt_T_W_C{query_i}(1:3, 4);
            rel_pose.gt_dist = ...
                norm(gt_p_W_M - gt_p_W_Q);
            rel_poses = [rel_poses; {rel_pose}];
        end
    end
end

%% Eval errors: position
figure(fig_num);
pos_error = cellfun(@(x) norm(x.est_dist - x.gt_dist), rel_poses);
subplot(1, 2, 1);
plot(pos_error);
title([label ': position errors']);
xlabel('rel pose #');
ylabel('position error [m]');

subplot(1, 2, 2);
plot(sort(pos_error), 1:numel(pos_error));
title([label ': position errors'])
xlabel('position error [m]');
ylabel('num rel poses with lower error');
end


function roi = dtiRoiErode(roi, cropAxes)
%
% roi = dtiRoiErode(roi, [cropAxes=[1 1 1]])
%
% Erodes the roi by n points along any of the three axes. E.g.,
% cropAxes = [0 1 1] will erode by 1 point along Y and Z,
% cropAxes = [3 2 2] will erode by 3 points along X, and 2 points
% along Y and Z.
%
% HISTORY:
% 2009.08.19 RFD wrote it.

if(~exist('cropAxes','var')||isempty(cropAxes))
    cropAxes = [1 1 1];
end
cropAxes = cropAxes(:)';
if(numel(cropAxes)~=3), error('numel(cropAxes) ~= 3!'); end

cropAxes(cropAxes>0) = cropAxes(cropAxes>0)+1;
stEl = ones(cropAxes+1);

if(isstruct(roi))
    for(ii=1:numel(roi))
        [roiImg, imgXform, bb] = dtiRoiToImg(roi(ii));
        roiImg = imerode(roiImg, stEl);
        roi(ii).coords = dtiRoiCoordsFromImg(roiImg, imgXform, bb);
    end
else
    [roiImg, imgXform, bb] = dtiRoiToImg(roi);
    roiImg = imerode(roiImg, stEl);
    roi = dtiRoiCoordsFromImg(roiImg, imgXform, bb);
end

return;


function edgeBoxesSweeps()
% Parameter sweeps for Edges Boxes object proposals.
%
% Running the parameter sweeps requires altering internal flags.
% The sweeps are not well documented, use at your own discretion.
%
% Structured Edge Detection Toolbox      Version 3.01
% Code written by Piotr Dollar and Larry Zitnick, 2014.
% Licensed under the MSR-LA Full Rights License [see license.txt]

% define parameter sweeps
rt = 'D:\code\research\edges\';
expNms = {'alpha','beta','minScore','edgeMinMag','edgeMergeThr',...
  'clusterMinMag','maxAspectRatio','minBoxArea','gamma','kappa'};
expNms=expNms(1:end); opts=createExp(rt,expNms); maxn=inf;

% run training and testing jobs
jobs = createJobs(rt,opts,maxn);
tic, for i=1:length(jobs), edgeBoxes(jobs{i}{:}); end; toc

% plot all results
for e=1:length(expNms)
  eval={'data',boxesData('split','val'),'names',{opts{e}.name},...
    'resDir',[rt 'boxes/sweeps/'],'maxn',maxn,'fName',expNms{e}};
  boxesEval(eval{:},'thrs',.7);
  boxesEval(eval{:},'thrs',.5:.05:1,'cnts',1000);
end

end

function jobs = createJobs( rt, opts, maxn )
% create jobs
M='models/forest/modelBsds'; M=load(M); M=M.model; M.opts.nThreads=1;
data=boxesData('split','val'); fs=data.imgs(1:min(end,maxn));
opts=[opts{:}]; N=length(opts); jobs=cell(1,N); D=zeros(1,N);
for e=1:N, opts(e).name=[rt 'boxes/sweeps/' opts(e).name '-val.mat']; end
for e=1:N, D(e)=exist(opts(e).name,'file')==2; jobs{e}={fs,M,opts(e)}; end
[~,K]=unique({opts.name},'stable'); D=D(K); jobs=jobs(K); jobs=jobs(~D);
fprintf('nJobs = %i\n',length(jobs));
end

function opts = createExp( rt, expNm )

% if expNm is a cell, call recursively and return
if( iscell(expNm) )
  N=length(expNm); opts=cell(1,N);
  for e=1:N, opts{e}=createExp(rt,expNm{e}); end; return;
end

% setup opts
opts=edgeBoxes(); opts.minScore=0;
N=100; optsDefault=opts; opts=opts(ones(1,N));
switch expNm
  case 'alpha'
    vs=45:5:75; N=length(vs);
    for e=1:N, opts(e).alpha=vs(e)/100; end
  case 'beta'
    vs=60:5:90; N=length(vs);
    for e=1:N, opts(e).beta=vs(e)/100; end
  case 'minScore'
    vs=[0 5 10 25 50 100]; N=length(vs);
    for e=1:N, opts(e).minScore=vs(e)/1000; end
  case 'edgeMinMag'
    vs=[0 50 100 200 400]; N=length(vs);
    for e=1:N, opts(e).edgeMinMag=vs(e)/1000; end
  case 'edgeMergeThr'
    vs=[25 50 100 200 400]; N=length(vs);
    for e=1:N, opts(e).edgeMergeThr=vs(e)/100; end
  case 'clusterMinMag'
    vs=[0 50 100 200 400]; N=length(vs);
    for e=1:N, opts(e).clusterMinMag=vs(e)/100; end
  case 'maxAspectRatio'
    vs=1:5; N=length(vs);
    for e=1:N, opts(e).maxAspectRatio=vs(e); end
  case 'minBoxArea'
    vs=[100 250 500 1000 2500 5000]; N=length(vs);
    for e=1:N, opts(e).minBoxArea=vs(e); end
  case 'gamma'
    vs=[25 50 100 200 400 10000]; N=length(vs);
    for e=1:N, opts(e).gamma=vs(e)/100; end
  case 'kappa'
    vs=50:25:200; N=length(vs);
    for e=1:N, opts(e).kappa=vs(e)/100; end
  otherwise, error('invalid exp: %s',expNm);
end

% produce final set of opts and find default opts
O=1:N; opts=opts(O); d=0;
for e=1:N, if(isequal(optsDefault,opts(e))), d=e; break; end; end
if(d==0), disp(expNm); assert(false); end; opts(d).name='Default';
for e=1:N, if(e~=d), opts(e).name=[expNm int2str2(vs(e),5)]; end; end

end


function varargout=rdmat(varargin)
%
% [tm,signal,Fs,siginfo]=rdmat(recordName)
%
% Import a signal in physical units from a *.mat file generated by WFDB2MAT.
% Required Parameters:
%
% recorName
%       String specifying the name of the *.mat file.
%
% Outputs are:
%
% tm
%       A Nx1 array of doubles specifying the time in seconds.
% signal
%       A NxM matrix of doubles contain the signals in physical units.
% Fs
%       A 1x1 integer specifying the sampling frequency in Hz for the entire record.
%siginfo
%       A LxN cell array specifying the signal siginfo. Currently it is a
%       structure with the following fields:
%
%        siginfo.Units
%        siginfo.Baseline
%        siginfo.Gain
%        siginfo.Description
%
% NOTE:
%       You can use the WFDB2MAT command in order to convert the record data into a *.mat file,
%       which can then be loaded into MATLAB/Octave's workspace using the LOAD command.
%       This sequence of procedures is quicker (by several orders of magnitude) than calling RDSAMP.
%       The LOAD command will load the signal data in raw units, use RDMAT to load the signal in physical units.
%
% KNOWN LIMITATIONS:
%       This function currently does support several of the features described
%       in the WFDB record format (such as multiresolution signals) :
%          http://www.physionet.org/physiotools/wag/header-5.htm
%       If you are not sure that the record (or database format) you are reading is
%       supported, you can do an integrity check by comparing the output with RDSAMP:
%
%       [tm,signal,Fs,siginfo]=rdmat('200m');
%       [tm2,signal2]=rdsamp('200m');
%       if(sum(abs(signal-signal2)) !=0);
%          error('Record not compatible with RDMAT');
%       end
%
%
% Written by Ikaro Silva, 2014
% Last Modified: November 26, 2014
% Version 1.2
%
% Since 0.9.7
%
% %Example:
% wfdb2mat('mitdb/200')
%tic;[tm,signal,Fs,siginfo]=rdmat('200m');toc
%tic;[tm2,signal2]=rdsamp('200m');toc
% sum(abs(signal-signal2))
%
%
% See also RDSAMP, WFDB2MAT

%endOfHelp

%Set default pararameter values
inputs={'recordName'};
defGain=200; %Default value for missing gains
wfdbNaN=-32768; %This should be the case for all WFDB signal format types currently supported by RDMAT

for n=1:nargin
    if(~isempty(varargin{n}))
        eval([inputs{n} '=varargin{n};'])
    end
end

outputs={'tm','val','Fs','siginfo'};
fid = fopen([recordName, '.hea'], 'rt');
if(fid==-1)
    error(['Could not open file: ' recordName '.hea !'])
end

%Following the documentation described in :
%http://www.physionet.org/physiotools/wag/header-5.htm
%to parse the header file

%Skip any comment lines
str=fgetl(fid);
while(strcmp(str(1),'#'))
    str=fgetl(fid);
end

%Process Record Line Info
info=textscan(str,'%s %u %f %u %s %s');
M=info{2}; %Number of signals present
Fs=info{3};

%Process Signal Specification lines. Assumes no comments between lines.
siginfo=[];
for m = 1:M
    str=fgetl(fid);
    info=textscan(str,'%s %s %s %u %u %f %u %u %s');
    gain=info{3}{:};

    %Get Signal Units if present
    ind=strfind(gain,'/');
    if(~isempty(ind))
        siginfo(m).Units=gain(ind+1:end);
        gain=gain(1:ind-1);
    end

    %Get Signal Baseline if present
    ind=strfind(gain,'(');
    if(~isempty(ind))
        ind2=strfind(gain,')');
        siginfo(m).Baseline=str2num(gain(ind+1:ind2-1));
        gain=gain(1:ind-1);
    else
        %If Baseline is missing, set it equal to ADC Zero
        adc_zero=info{5};
        if(~isempty(adc_zero))
            siginfo(m).Baseline=double(adc_zero);
        else
            error('Could not obtain signal baseline');
        end
    end

    %Get Signal Gain
    gain=str2num(gain);
    if(gain==0)
        %Set gain to default value in this case
        gain=defGain;
    end
    siginfo(m).Gain=double(gain);

    %Get Signal Descriptor
    siginfo(m).Description=info{9}{:};

end
fclose(fid);

load([recordName '.mat']);
val(val==wfdbNaN)= NaN;
for m = 1:M
    %Convert from digital units to physical units.
    % Mapping should be similar to that of rdsamp.c:
    % http://www.physionet.org/physiotools/wfdb/app/rdsamp.c
    val(m, :) = (val(m, :) - siginfo(m).Baseline ) / siginfo(m).Gain;
end

%Reshape to the Toolbox's standard format
val=val';

%Generate time vector
N=size(val,1);
tm =linspace(0,(N-1)/Fs,N);

for n=1:nargout
    eval(['varargout{n}=' outputs{n} ';'])
end

end


function [binned_data_spike_counts binned_labels binned_site_info] = load_binned_data_and_convert_firing_rates_to_spike_counts(binned_data_file_name, bin_width)

% This function allows one to load binned-format data and convert the binned_data for each site into spike counts.
%   The reason this function is needed is because paricular classifiers, such as the Poisson Naive Bayes classifier,
%   operate on integer data, however create_time_averaged_binned_data_from_raster data
%   creates firing rates that are not integers, thus by using this method, the firing rate data will be converted into integer spike counts).
%
%
% Inputs arguments:
%
%   1.  binned_data_file_name: the file name of data that is in binned format (that contains binned_data which has firing rate data)
%
% Optional input arguemnts:
%
%   bin_width: the bin width that each column of binned_data{iSite} was averaged over to create firing rates. If this argument is a vector, then the values
%       correspond to the bin widths of each column of binned_data{iSite}; if this value is a scalar then it is assumed that all bin widths are the same size.
%       If this argument is not given then this method assumes there is a variable binned_site_info.binning_parameters.bin_width that will be used as the bin width (this variable
%       will exist if the function create_binned_data_from_raster_data was used to create the binned data.
%
% Output arguments:
%
%   1. binned_data_spike_counts:  binned_data that has been converted into spike counts
%   2. binned_labels:  the binned labels
%   3. binned_site_info: the binned site info
%

%==========================================================================

%     This code is part of the Neural Decoding Toolbox.
%     Copyright (C) 2013 by Ethan Meyers (emeyers@mit.edu)
%
%     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.
%
%     This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

%==========================================================================

   load(binned_data_file_name)

   if nargin < 2
        bin_width = binned_site_info.binning_parameters.bin_width;   % will either be a number, or if custom bin widths are used, this will be a vector
   end

   c = 1;
   for iSite = 1:size(binned_data, 2)   % go through each site and convert it into

       if isscalar(bin_width)
           binned_data_spike_counts{iSite} =  binned_data{iSite} .* bin_width;
       else
           binned_data_spike_counts{iSite} = binned_data{iSite} .* repmat(bin_width, size(binned_data{iSite}, 1), 1);
       end

       % check to make sure that binned_data_spike_counts only contains integers showing that the conversion worked correctly
       %if length(find(rem(binned_data_spike_counts{iSite}, 1) ~= 0))
       %if length(find(rem(binned_data_spike_counts{iSite}, 1) > 10^-12)) % changed it to this to deal with numerical precision issues

       % changed to make this work with Octave (rounding errors can occur in both directions)
       deviations_from_intergers_matrix = min(rem(binned_data_spike_counts{iSite}, 1), 1 - rem(binned_data_spike_counts{iSite}, 1));

       if length(find(deviations_from_intergers_matrix > 10^-12))
           sites_with_failed_conversions(c) = iSite;
           c = c + 1;
       else
           binned_data_spike_counts{iSite} = round(binned_data_spike_counts{iSite});  % round data to get rid of small numerical imprecisions
       end

   end

   if exist('sites_with_failed_conversions')
        warning(['The following sites did not correctly convert to spike counts: ' num2str(sites_with_failed_conversions)]);
   end

end


function [lossValue_images, unaryDerivative, pairwiseDerivative, predictions] = vl_logisticScoreLoss_pairwiseCompactModel( unaryPotentials, pairwisePotentials, labels, dzdy, computeMaxMarginals )
%vl_logisticScoreLoss_pairwiseCompactModel implements the logistic loss on top of the structured scores
%
% The joint score:
%   S(y, theta) = \sum_i y_i * theta^U_i + \sum_ij y_i * y_j * \theta^P_{i,j,k_ij}
% where y_i \in \{0,1\} are the variables and theta^U, \theta_P - potentials
% i indexes the nodes, ij - the edges; k_ij - the cluster index of edge ij
%
% The individual scores are based on the max-marginals of the joint score:
%   s_i = \max_{ y: y_i = 1 } S(y, theta) - \max_{ y: y_i = 0 } S(y, theta)
%
% The final loss is computed as the sum of logistics on top of the individual scores:
%   loss = \sum_{i is positive} v( s_i ) + \sum_{i is negative} v( -s_i )
%
% If the number of nodes is <= 20 than the max-marginals are computed exacly using the exhaustive search, otherwise the approximations are used, see computeMinMarginalsPairwiseBinary.m
%
% Usage:
%   [lossValue_images, unaryDerivative, pairwiseDerivative, predictions] = vl_logisticScoreLoss_pairwiseCompactModel( unaryPotentials, pairwisePotentials, labels, dzdy, computeMaxMarginals )
%
% Input:
%   unaryPotentials - unary potentials parameterized by \theta^U, double[1 x 1 x 1 x numNodes], where numNodes is the number of all nodes in the batch
%   pairwisePotentials - pairwise potentials parameterizws by \theta^P, double[1 x 1 x numClusters x numNodes]
%   labels - cell array providing information about the batch, cell[numImages x 1], each cell has
%       candidateBatchIds - indices of the nodes of the current image, double[ numNodesInImage x 1 ]
%       classGroundTruth - class labels of the nodes of the current image, double[ numNodesInImage x 1 ] with labels 0 or 1
%       instanceGroundTruth - indices of the gound-truth objects of the current image, double[ numNodesImage x 1 ] of 0 (background), 1, ..., numInstances
%       clusteredEdges - information about clusters of the edges, structure with fields:
%           bbIds - indices of the nodes connected with an edge, double[numEdges x 2] of 1,...,numNodesInImage
%           clusterId - IDs of clusters for the edges, double[numEdges x 1]
%   dzdy - if empty, only forward pass is computed, otherwise the gradient is multiplied by dzdy
%   computeMaxMarginals - flag showing whether to compute output max-marginals (default: false)
%
% Output:
%   lossValue_images - values of the loss for all images in a batch, double[numImages x 1]
%   unaryDerivative - derivatives w.r.t. the \theta^U, same size as unaryPotentials
%   pairwiseDerivative - derivatives w.r.t. the \theta^P, same size as pairwisePotentials
%   predictions - if computeMaxMarginals == false than labels for the nodes, double[numNodesImage x 1]
%           if computeMaxMarginals == true than cell[numImages x 1], each cell has
%              bestLabeling - labels for the nodes, double[numNodesImage x 1]
%              maxMarginals - max-marginals

if ~exist('computeMaxMarginals', 'var') || isempty(computeMaxMarginals)
    computeMaxMarginals = false;
end

%% preparation
numImages = length(labels);
numNodes = zeros(numImages, 1);
numEdges = zeros(numImages, 1);
numLabels = 2;
for iImage = 1 : numImages
    numNodes( iImage ) = length( labels{iImage}.candidateBatchIds );
    numEdges( iImage ) = size(labels{ iImage }.clusteredEdges.bbIds, 1);
end

if size(unaryPotentials, 1) ~= 1 ||  size(unaryPotentials, 2) ~= 1 || size(unaryPotentials, 3) ~= 1 || size(unaryPotentials, 4) ~= sum(numNodes)
    error('Unary potentials are of incorrect size');
end
numClusters = size(pairwisePotentials, 3);
if size(pairwisePotentials, 1) ~= 1 ||  size(pairwisePotentials, 2) ~= 1 || size(pairwisePotentials, 3) ~= numClusters * 1 || size(pairwisePotentials, 4) ~= sum(numEdges)
    error('Pairwise potentials are of incorrect size');
end

if ~isempty(dzdy)
    % compute derivatives
    unaryDerivative = zeros( size(unaryPotentials), 'like', unaryPotentials );
    pairwiseDerivative = zeros( size(pairwisePotentials), 'like', pairwisePotentials );
else
    unaryDerivative = [];
    pairwiseDerivative = [];
end

%% start computations
nodeStartIndex = 0;
edgeStartIndex = 0;
lossValue_images = nan(numImages, 1);
predictions = cell(numImages, 1);
for iImage = 1 : numImages
    %% extract potentials
    nodeIds = nodeStartIndex + (1 : numNodes( iImage ));
    edgeIds = edgeStartIndex + (1 : numEdges( iImage ));
    nodeStartIndex = nodeStartIndex + numNodes( iImage );
    edgeStartIndex = edgeStartIndex + numEdges( iImage );

    % extract unaries
    unaries = reshape( unaryPotentials(:,:,:,nodeIds), 1, numNodes( iImage ) );

    % extract pairwise potentials by mapping only one cluster to the edge
    edgeEnds = labels{ iImage }.clusteredEdges.bbIds;
    edgeClusters = labels{ iImage }.clusteredEdges.clusterId;

    pairwise = pairwisePotentials(:,:,:,edgeIds);

    potentialsId = edgeClusters(:) + numClusters * (0 : numEdges( iImage ) - 1)';
    pairwise = reshape( pairwise( potentialsId ), 1, numEdges( iImage ) );

    % prepare terms for energy minimizationsc
    unaryTerms = [ -double(unaries(:)), zeros(numel(unaries), 1)];
    pairwiseTerms = [ edgeEnds, -double(pairwise(:)), zeros(numel(pairwise), 3) ];

    % labels{iImage}.classGroundTruth - 0 for bkg, 1 for obj
    % labels{iImage}.instanceGroundTruth - 0 for bkg, i for detection object #i, exactly one detection for object i should be present
    % labelsBinary - 1 - head, 2 - bkg

    % compute all the max-marginals
    if numNodes( iImage ) <= 20
        [minMarginals, minMarginals_args] = computeMinMarginalsBinaryMex( unaryTerms, pairwiseTerms );
    else
        [minMarginals, ~, minMarginals_args] = computeMinMarginalsPairwiseBinary( unaryTerms, pairwiseTerms );
    end
    maxMarginals = -minMarginals;

    scores = maxMarginals(:, 1) - maxMarginals(:, 2);
    [~, bestLabeling] = max( maxMarginals, [], 2 );

    % compute the logistic loss
    labelsBinary_GT = 2 * labels{iImage}.classGroundTruth - 1; % convert GT from (0 for bkg, 1 for obj) to (1 - head, -1 - bkg)
    expGtScores = exp( - scores .* labelsBinary_GT );

    numBkg = sum( labels{iImage}.classGroundTruth == 0 );
    numObj = sum( labels{iImage}.classGroundTruth == 1 );
    weightObj = 1 / numObj;
    weightBkg = 1 / numBkg;
    weights = weightBkg * ones( numNodes( iImage ), 1 );
    weights( labels{iImage}.classGroundTruth(:) == 1 ) = weightObj;
    weights = weights(:) / sum(weights(:)) * numel(weights);

    lossValue_images(iImage) = sum( log( 1 + expGtScores(:) ) .* weights(:) );

    if ~computeMaxMarginals
        predictions{iImage} = bestLabeling;
    else
        predictions{iImage}.maxMarginals = maxMarginals;
        predictions{iImage}.bestLabeling = bestLabeling;
    end

    if ~isempty(dzdy)
        % compute derivatives
        curUnaryDerivative = zeros(1, 1, 1, numNodes( iImage ), 'like', unaryDerivative);
        curPairwiseDerivative = zeros(1, 1, numClusters, numEdges( iImage ), 'like', pairwiseDerivative);

        % derivative of the loss w.r.t. the scores
        scoreDerivatives = (expGtScores(:) ./ (1 + expGtScores(:))) .* (-labelsBinary_GT(:)) .* weights(:);

        % derivative of the loss w.r.t. the max-marginals
        mmDerivates = [ ones( numNodes( iImage ), 1 ), -ones( numNodes( iImage ), 1 ) ];
        mmDerivates = bsxfun(@times, mmDerivates, scoreDerivatives);

        % derivative of the loss w.r.t. the potentials
        for iNode = 1 : numNodes( iImage )
            for iLabel = 1 : 2
                curArg = permute( minMarginals_args(iNode, iLabel, :), [3, 1, 2]);

                % unaries
                curMask = curArg(:) == 0; % the node belongs to the head class: 0 in this notaion
                curUnaryDerivative( curMask ) = curUnaryDerivative( curMask ) + 1 * mmDerivates(iNode, iLabel);

                % pairwise
                curMask = curArg( edgeEnds(:,1) ) == 0 & curArg( edgeEnds(:,2) ) == 0; % both incident nodes belong to the head class: 0 in this notaion
                curIds = edgeClusters(curMask) + numClusters * (find(curMask) - 1);
                curPairwiseDerivative( curIds ) = curPairwiseDerivative( curIds ) + 1 * mmDerivates(iNode, iLabel);

            end
        end

        unaryDerivative(:,:,:,nodeIds) = curUnaryDerivative;
        pairwiseDerivative(:,:,:,edgeIds) = curPairwiseDerivative;
    end
end

if ~isempty(dzdy)
    unaryDerivative = unaryDerivative * dzdy;
    pairwiseDerivative = pairwiseDerivative * dzdy;
end

end


function mat2ades(dat, fileName, Fs, chanlabel, chanunit, dattype, datunit)

% write in the current folder ADES and DAT files from matrix in MATLAB workspace
% data = matrix of data (nbchannel * time points) - the data have to be in microVolt
% fileName = string of the output files without extension ; the ades and dat files will have the same name
% FS = sampling rate
% labels = cell-array with channel labels
% labelType : 'EEG' or 'MEG'
%
% Data are stored in a binary file which name is exactly the same than the header file except the extension: .dat
% The samples are stored as float, 4 bytes per sample, little endian. The channels are multiplexed.
%
% Sophie Chen - January 2014
% Modified by Robert Oostenveld - February 2019

%% generate the ADES file
adesFile = [fileName '.ades'];

fid = fopen_or_error(adesFile, 'wt');

fprintf(fid, '#ADES header file\r\n');
fprintf(fid, 'samplingRate = %d\r\n', Fs);
fprintf(fid, 'numberOfSamples = %d\r\n', size(dat,2));

for lab = 1:length(dattype)
  fprintf(fid, 'Unit = %s, %s\r\n', dattype{lab}, datunit{lab});
end

for lab = 1:length(chanlabel)
  fprintf(fid, '%s = %s\r\n', chanlabel{lab}, chanunit{lab});
end

fclose(fid);

%% generate the DAT file

datFile = [fileName '.dat'];

fad = fopen_or_error(datFile, 'wb');
fwrite(fad, dat, 'float32', 'l');
fclose(fad);
end


function [out_vec] = tgn_model(in_vec)
% tgn_model.m - TGn channel model simulation
global numPkts PER_snr snc_idxs snc_dlys snr_idx H_iid H_dline P_tap Kf_tap Kv_tap R_tx R_rx;

% Channel Model Test Setup
%    0 - AWGN channel
%    1 - Channel D (no Doppler)
%    2 - Channel D (with Doppler)
test_setup = 1;

% PER graph settings (actual values are user-selected (GUI))
%%%%% numPkts = 200; %100
%%%%% PER_snr = 29:33; %15:1:21;

% Input parameters
Clk = in_vec(1);
Ntx = in_vec(2);
Nrx = in_vec(3);

% Channel inputs
in_len = length(in_vec(4:end))/4;
ch_in  = zeros(4, in_len);
ch_in(1,:) = in_vec(0*in_len+3+(1:in_len)).';
ch_in(2,:) = in_vec(1*in_len+3+(1:in_len)).';
ch_in(3,:) = in_vec(2*in_len+3+(1:in_len)).';
ch_in(4,:) = in_vec(3*in_len+3+(1:in_len)).';
testch_out = ch_in;
ch_in = ch_in(1:Ntx,:);  % keep only used Tx ant. inputs

% Channel outputs
ch_out = zeros(4, in_len);

if (test_setup==0) %%%%%%%%%%%%%%%  AWGN channel

    if (Clk==0)  snr_idx = 1;  end

    % Add AWGN to Rx streams (reduce by 5dB, due to upsampling)
    snr_val = PER_snr(snr_idx);
    for i=1:Nrx
        testch_out(i,:) = awgn(testch_out(i,:), snr_val-5, 'measured');
    end

    % Next SNR Value for PER Curve...
    if (mod(Clk,numPkts)==(numPkts-1) && snr_idx<length(PER_snr))
        snr_idx = snr_idx+1;
    end

    % Return output
    out_vec = reshape(testch_out.', 4*in_len, 1);

else  %%%%%%%%%%%%%%%  TGn Channel Model D

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% For Channel D (NLOS) %%%%%
chD_K = -inf;  %%% not used (since NLOS), K = -inf dB
chD_dly    = [    0     10     20     30     40     50     60     70     80     90    110    140    170    200    240    290    340    390];
chD_wdth   = [   10     10     10     10     10     10     10     10     10     20     30     30     30     40     50     50     50     50];
% Cluster 1 - tap powers, delays
chD_pow_c1 = [    0   -0.9   -1.7   -2.6   -3.5   -4.3   -5.2   -6.1   -6.9   -7.8   -9.0  -11.1  -13.7  -16.3  -19.3  -23.2   -inf   -inf];
chD_aoa_c1 = [158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9  158.9];
chD_asa_c1 = [ 27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7   27.7];
chD_aod_c1 = [332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1  332.1];
chD_asd_c1 = [ 27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4   27.4];
% Cluster 2 - tap powers, delays
chD_pow_c2 = [ -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -6.6   -9.5  -12.1  -14.7  -17.4  -21.9  -25.5   -inf];
chD_aoa_c2 = [320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2  320.2];
chD_asa_c2 = [ 31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4   31.4];
chD_aod_c2 = [ 49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3   49.3];
chD_asd_c2 = [ 32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1   32.1];
% Cluster 3 - tap powers, delays
chD_pow_c3 = [ -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf   -inf  -18.8  -23.2  -25.2  -26.7];
chD_aoa_c3 = [276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1  276.1];
chD_asa_c3 = [ 37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4   37.4];
chD_aod_c3 = [275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9  275.9];
chD_asd_c3 = [ 36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8   36.8];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

num_taps = length(chD_dly);

% define imag. component
j = sqrt(-1);

% Initialize channel model
if (Clk==0)

    snr_idx = 1;

    % Initialize random generator
    %12345
    %randn('state',12345);

    % initialize variables
    snc_idxs = zeros(num_taps, 12, 16);
    snc_dlys = zeros(num_taps, 12, 16);
    P_tap  = zeros(num_taps, 1);
    Kf_tap = zeros(num_taps, 1);
    Kv_tap = zeros(num_taps, 1);
    R_tx   = zeros(Ntx, Ntx, num_taps);
    R_rx   = zeros(Nrx, Nrx, num_taps);

    % Initialize channel delay line
    tot_dly = sum(chD_wdth)/10;
    H_dline = zeros(Ntx, tot_dly);

    % Initialize calculations for each tap
    for m=1:num_taps

        % initialize Doppler-filtered, Rayleigh fading for tap
        for y=1:16 % max size = 4x4
            s_idxs = 0;
            s_dlys = 0;
            [out_val, s_dlys, s_idxs] = sinc_dop_rv(s_dlys, s_idxs, 256);
            snc_idxs(m,:,y) = s_idxs;
            snc_dlys(m,:,y) = s_dlys;
        end

        % Compute overall tap power
        P_tap(m) = 10^(chD_pow_c1(m)/10) + 10^(chD_pow_c2(m)/10) + 10^(chD_pow_c3(m)/10);
        P_tap(m) = sqrt(P_tap(m));
        % Compute K-factor for tap (convert dB to linear)
        if (m==1)  K = 10^(chD_K/10);
        else       K = 0;              end
        Kf_tap(m) = sqrt(K/(K+1));
        Kv_tap(m) = sqrt(1/(K+1));
        % Compute correlation matrices
        sq2 = sqrt(2);
        ds=pi;    % antenna spacing
        Dc = 4;   % matrix index for distance zero
        ci=1:3;   % cluster index vector
        mxx=1:20;  % 2nd sum index vector, Rxx
        mxy=0:20;  % 2nd sum index vector, Rxy
        dph=60*(pi/180); % delta phi = pi (-180 to +180 degrees)
        Q = [ 10^(chD_pow_c1(m)/10) 10^(chD_pow_c2(m)/10) 10^(chD_pow_c3(m)/10) ];
        otx  = (pi/180)*[ chD_asd_c1(m) chD_asd_c2(m) chD_asd_c3(m) ];
        orx  = (pi/180)*[ chD_asa_c1(m) chD_asa_c2(m) chD_asa_c3(m) ];
        phtx = (pi/180)*[ chD_aod_c1(m) chD_aod_c2(m) chD_aod_c3(m) ];
        phrx = (pi/180)*[ chD_aoa_c1(m) chD_aoa_c2(m) chD_aoa_c3(m) ];
        % Tx correlations...
        normQtx = sum(Q.*(1-exp(-sq2*dph./otx)));   % norm. constants
        Qtx = Q/normQtx;
        for D = -3:3
            Rxx_c1 = sum( besselj(2*mxx,D*ds)./((sq2/otx(1)).^2+(2*mxx).^2).*cos(2*mxx*phtx(1)).*...
                          (sq2/otx(1) + exp(-dph*sq2/otx(1)).*(2*mxx.*sin(2*mxx*dph) - (sq2/otx(1))*cos(2*mxx*dph))) );
            Rxx_c2 = sum( besselj(2*mxx,D*ds)./((sq2/otx(2)).^2+(2*mxx).^2).*cos(2*mxx*phtx(2)).*...
                          (sq2/otx(2) + exp(-dph*sq2/otx(2)).*(2*mxx.*sin(2*mxx*dph) - (sq2/otx(2))*cos(2*mxx*dph))) );
            Rxx_c3 = sum( besselj(2*mxx,D*ds)./((sq2/otx(3)).^2+(2*mxx).^2).*cos(2*mxx*phtx(3)).*...
                          (sq2/otx(3) + exp(-dph*sq2/otx(3)).*(2*mxx.*sin(2*mxx*dph) - (sq2/otx(3))*cos(2*mxx*dph))) );
            Rxx_c = [Rxx_c1 Rxx_c2 Rxx_c3];
            Rxx(Dc+D) = besselj(0,D*ds)+4*sum(Qtx./(otx.*sq2).*Rxx_c);
            % cross-correlation, real and imag
            Rxy_c1 = sum( besselj(2*mxy+1,D*ds)./((sq2/otx(1)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phtx(1)).*...
                          (sq2/otx(1) - exp(-dph*sq2/otx(1)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/otx(1))*cos((2*mxy+1)*dph))) );
            Rxy_c2 = sum( besselj(2*mxy+1,D*ds)./((sq2/otx(2)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phtx(2)).*...
                          (sq2/otx(2) - exp(-dph*sq2/otx(2)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/otx(2))*cos((2*mxy+1)*dph))) );
            Rxy_c3 = sum( besselj(2*mxy+1,D*ds)./((sq2/otx(3)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phtx(3)).*...
                          (sq2/otx(3) - exp(-dph*sq2/otx(3)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/otx(3))*cos((2*mxy+1)*dph))) );
            Rxy_c = [Rxy_c1 Rxy_c2 Rxy_c3];
            Rxy(Dc+D) = 4*sum(Qtx./(otx.*sq2).*Rxy_c);
        end
        % Form Tx corr. matrix
        Rtx = [ Rxx(Dc)   Rxx(Dc+1) Rxx(Dc+2) Rxx(Dc+3); ...
                Rxx(Dc-1) Rxx(Dc)   Rxx(Dc+1) Rxx(Dc+2); ...
                Rxx(Dc-2) Rxx(Dc-1) Rxx(Dc)   Rxx(Dc+1); ...
                Rxx(Dc-3) Rxx(Dc-2) Rxx(Dc-1) Rxx(Dc)    ];
        Rtx = Rtx + j*...
              [ Rxy(Dc)   Rxy(Dc+1) Rxy(Dc+2) Rxy(Dc+3); ...
                Rxy(Dc-1) Rxy(Dc)   Rxy(Dc+1) Rxy(Dc+2); ...
                Rxy(Dc-2) Rxy(Dc-1) Rxy(Dc)   Rxy(Dc+1); ...
                Rxy(Dc-3) Rxy(Dc-2) Rxy(Dc-1) Rxy(Dc)    ];
        Rtx = Rtx(1:Ntx,1:Ntx); % keep only Ntx-by-Ntx elements
        R_tx(:,:,m) = Rtx^(1/2);

        % Rx correlations...
        normQrx = sum(Q.*(1-exp(-sq2*dph./orx)));   % norm. constants
        Qrx = Q/normQrx;
        for D = -3:3
            % cross-correlation, real parts
            Rxx_c1 = sum( besselj(2*mxx,D*ds)./((sq2/orx(1)).^2+(2*mxx).^2).*cos(2*mxx*phrx(1)).*...
                          (sq2/orx(1) + exp(-dph*sq2/orx(1)).*(2*mxx.*sin(2*mxx*dph) - (sq2/orx(1))*cos(2*mxx*dph))) );
            Rxx_c2 = sum( besselj(2*mxx,D*ds)./((sq2/orx(2)).^2+(2*mxx).^2).*cos(2*mxx*phrx(2)).*...
                          (sq2/orx(2) + exp(-dph*sq2/orx(2)).*(2*mxx.*sin(2*mxx*dph) - (sq2/orx(2))*cos(2*mxx*dph))) );
            Rxx_c3 = sum( besselj(2*mxx,D*ds)./((sq2/orx(3)).^2+(2*mxx).^2).*cos(2*mxx*phrx(3)).*...
                          (sq2/orx(3) + exp(-dph*sq2/orx(3)).*(2*mxx.*sin(2*mxx*dph) - (sq2/orx(3))*cos(2*mxx*dph))) );
            Rxx_c = [Rxx_c1 Rxx_c2 Rxx_c3];
            Rxx(Dc+D) = besselj(0,D*ds)+4*sum(Qrx./(orx.*sq2).*Rxx_c);
            % cross-correlation, real and imag
            Rxy_c1 = sum( besselj(2*mxy+1,D*ds)./((sq2/orx(1)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phrx(1)).*...
                          (sq2/orx(1) - exp(-dph*sq2/orx(1)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/orx(1))*cos((2*mxy+1)*dph))) );
            Rxy_c2 = sum( besselj(2*mxy+1,D*ds)./((sq2/orx(2)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phrx(2)).*...
                          (sq2/orx(2) - exp(-dph*sq2/orx(2)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/orx(2))*cos((2*mxy+1)*dph))) );
            Rxy_c3 = sum( besselj(2*mxy+1,D*ds)./((sq2/orx(3)).^2+(2*mxy+1).^2).*sin((2*mxy+1)*phrx(3)).*...
                          (sq2/orx(3) - exp(-dph*sq2/orx(3)).*((2*mxy+1).*sin((2*mxy+1)*dph) + (sq2/orx(3))*cos((2*mxy+1)*dph))) );
            Rxy_c = [Rxy_c1 Rxy_c2 Rxy_c3];
            Rxy(Dc+D) = 4*sum(Qrx./(orx.*sq2).*Rxy_c);
        end
        % Form Rx corr. matrix
        Rrx = [ Rxx(Dc)   Rxx(Dc+1) Rxx(Dc+2) Rxx(Dc+3); ...
                Rxx(Dc-1) Rxx(Dc)   Rxx(Dc+1) Rxx(Dc+2); ...
                Rxx(Dc-2) Rxx(Dc-1) Rxx(Dc)   Rxx(Dc+1); ...
                Rxx(Dc-3) Rxx(Dc-2) Rxx(Dc-1) Rxx(Dc)    ];
        Rrx = Rrx + j*...
              [ Rxy(Dc)   Rxy(Dc+1) Rxy(Dc+2) Rxy(Dc+3); ...
                Rxy(Dc-1) Rxy(Dc)   Rxy(Dc+1) Rxy(Dc+2); ...
                Rxy(Dc-2) Rxy(Dc-1) Rxy(Dc)   Rxy(Dc+1); ...
                Rxy(Dc-3) Rxy(Dc-2) Rxy(Dc-1) Rxy(Dc)    ];
        Rrx = Rrx(1:Nrx,1:Nrx); % keep only Nrx-by-Nrx elements
        R_rx(:,:,m) = (Rrx^(1/2)).';
    end
end

% Without Doppler, generate channel matrices
if (test_setup==1)
    H_tap = zeros(Nrx,Ntx,num_taps);
    for m=1:num_taps
        % Gaussian RV's for Rayleigh fading
        H_iid = randn(Nrx, Ntx) + j*randn(Nrx, Ntx);
        H_iid = H_iid/sqrt(2);

        % Fixed, LOS matrix
        H_f = ones(Nrx, Ntx);   % LOS (since AoA=AoD=45)

        % Variable, NLOS matrix (Rayleigh matrix)
        H_v = R_rx(:,:,m)*H_iid*R_tx(:,:,m);

        % Compute channel tap
        H_tap(:,:,m) = P_tap(m)*( Kf_tap(m)*H_f + Kv_tap(m)*H_v );
    end
end

% Process all input samples...
for i=1:in_len

    % Add new sample to delay line
    H_dline = [ch_in(:,i) H_dline(:,1:end-1)];

    % Apply channel filter
    H_dline_idx = 1;
    H_out = zeros(Nrx,1);
    for m=1:num_taps

        % With Doppler, update 'H_tap' every sample
        if (test_setup==2)
            % Generate Doppler-filtered, Rayleigh fading
            H_iid = zeros(1, Nrx*Ntx);
            for y=1:Nrx*Ntx
                s_idxs = snc_idxs(m,:,y);
                s_dlys = snc_dlys(m,:,y);
                [out_val, s_dlys, s_idxs] = sinc_dop_rv(s_dlys, s_idxs, 256);
                snc_idxs(m,:,y) = s_idxs;
                snc_dlys(m,:,y) = s_dlys;
                H_iid(y) = out_val;
            end
            H_iid = reshape(H_iid, Nrx, Ntx);

            % Fixed, LOS matrix
            H_f = ones(Nrx, Ntx);   % LOS (since AoA=AoD=45)

            % Variable, NLOS matrix (Rayleigh matrix)
            H_v = R_rx(:,:,m)*H_iid*R_tx(:,:,m);

            % Compute channel tap
            H_tap(:,:,m) = P_tap(m)*( Kf_tap(m)*H_f + Kv_tap(m)*H_v );
        end

        % Apply tap to inputs (considering tap width)
        for k = 1:(chD_wdth(m)/10)
if 1 % 10 ns (100 MHz) sampling
            H_out = H_out + H_tap(:,:,m)*H_dline(:,H_dline_idx);
else % 50 ns (20 MHz) sampling
            if mod(H_dline_idx,5)==1
                H_out = H_out + H_tap(:,:,m)*H_dline(:,1+floor(H_dline_idx/5));
            end
end
            H_dline_idx = H_dline_idx + 1;
        end
    end

    % Store channel output
    ch_out(1:Nrx,i) = H_out;
end

% Add AWGN to Rx streams (reduce by 5dB, due to upsampling)
snr_val = PER_snr(snr_idx);
for i=1:Nrx
    ch_out(i,:) = awgn(ch_out(i,:), snr_val-5, 'measured');
end

% Next SNR Value for PER Curve...
if (mod(Clk,numPkts)==(numPkts-1) && snr_idx<length(PER_snr))
    snr_idx = snr_idx+1;
end

% Return output
out_vec = reshape(ch_out.', 4*in_len, 1);

end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function test_bug2524

% WALLTIME 00:10:00
% MEM 1gb
% DEPENDENCY ft_write_mri

data = zeros(5,5,5);
trans = diag([5 5 5 1]);

tmp = tempname;
ft_write_mri([tmp,'.nii'],data,'transform',trans,'dataformat','nifti');
mri = ft_read_mri([tmp,'.nii']);

assert(mri.transform(1,1)==5);

% use a command line tool from FSL
[r,s] = system('which fslhd');
if r==0
  disp(r)
  disp(s)
else
  % use hard-coded FSL version
  s = '/opt/fsl/5.0.9/bin/fslhd';
end
[r,s] = system([deblank(s) ' ' tmp '.nii | grep pixdim']);
disp(r)
disp(s)

assert(numel(strfind(s,'5.0'))==3);

delete([tmp,'.nii']);


clear

load('results/results_clnf_cross-data_general.mat');
load('results/menpo_labels.mat');
[clnf_error, frontal_ids] = compute_error_menpo_small( labels,  experiment.shapes);
clnf_error_frontal = clnf_error(frontal_ids);
clnf_error_profile = clnf_error(~frontal_ids);

load('results/menpo_train_chehra.mat');
[drmf_error, frontal_ids] = compute_error_menpo_small( labels,  shapes);
drmf_error_frontal = drmf_error(frontal_ids);
drmf_error_profile = drmf_error(~frontal_ids);

load('results/menpo_train_sdm.mat');
for i = 1:numel(experiment.shapes)
    shapes{i} = shapes{i}+0.5;
end
[sdm_error, frontal_ids] = compute_error_menpo_small( labels,  shapes);
sdm_error_frontal = sdm_error(frontal_ids);
sdm_error_profile = sdm_error(~frontal_ids);

load('results/Menpo_train_pocr.mat');
[pocr_error, frontal_ids] = compute_error_menpo_small( labels,  experiments.shapes);
pocr_error_frontal = pocr_error(frontal_ids);
pocr_error_profile = pocr_error(~frontal_ids);

load('results/results_ceclm_cross-data.mat');
[ceclm_error, frontal_ids] = compute_error_menpo_small( labels,  experiment.shapes);
ceclm_error_frontal = ceclm_error(frontal_ids);
ceclm_error_profile = ceclm_error(~frontal_ids);

load('results/tcdcn_menpo.mat');
for i = 1:numel(shapes)
    shapes{i} = shapes{i};
end

[tcdcn_error, frontal_ids] = compute_error_menpo_small(labels, shapes);
tcdcn_error_frontal = tcdcn_error(frontal_ids);
tcdcn_error_profile = tcdcn_error(~frontal_ids);

load('results/CFAN_menpo_train.mat');
for i = 1:numel(shapes)
    shapes{i} = shapes{i}-0.5;
end

[cfan_error, frontal_ids] = compute_error_menpo_small(labels, shapes);
cfan_error_frontal = cfan_error(frontal_ids);
cfan_error_profile = cfan_error(~frontal_ids);

load('results/menpo_train_3DDFA.mat');
for i = 1:numel(shapes)
    shapes{i} = shapes{i}-0.5;
end

[error_3ddfa, frontal_ids] = compute_error_menpo_small(labels, shapes);
error_3ddfa_frontal = error_3ddfa(frontal_ids);
error_3ddfa_profile = error_3ddfa(~frontal_ids);

load('results/Menpo-CFSS_train.mat');
shapes = cell(size(estimatedPoseFull,1),1);

for i = 1:numel(shapes)
    shape = cat(2, estimatedPoseFull(i,1:68)', estimatedPoseFull(i,69:end)');
    shapes{i} = shape-0.5;
end

[cfss_error, frontal_ids] = compute_error_menpo_small(labels, shapes);
cfss_error_frontal = cfss_error(frontal_ids);
cfss_error_profile = cfss_error(~frontal_ids);


function FPt(option, up)
%FPT detects Fiducial Points from PPG peak and trough annotations
% as specified in PC's literature review.
%	            FPt(option, up)
%
%	Inputs:
%		option          the option which has led to this function being used
%       up              universal parameters structure
%
%	Outputs:
%       ...
%

fprintf('\n--- Identifying Fiducial Points ');
log_int_respSig = 1;             % Has value 1 unless this is a final respiratory signal

for subj = up.paramSet.subj_list

    %% Skip if this processing has been done previously
    iden_resp_sig_file_ending
    savepath = [up.paths.data_save_folder, num2str(subj), ending];
    filecontents = whos('-file', savepath);
    var_names = extractfield(filecontents, 'name');
    rel_log = zeros(size(var_names));
    if strcmp(option(1:3), 'ekg')
        rel_string = [option(1:3), up.paths.filenames.qrss];
    else
        rel_string = [option(1:3), up.paths.filenames.pulse_peaks];
    end
    for s = 1 : length(var_names)
        if strfind(var_names{s}, rel_string)
            rel_log(s) = 1;
        end
    end
    rel_var_names = var_names(logical(rel_log));
    for rel_var_name_no = 1 : length(rel_var_names)
        eval(['save_name = ''' option(1:3), up.paths.filenames.fid_pts, rel_var_names{rel_var_name_no}(4:end) ''';']);
        exist_log = check_exists(savepath, save_name);
        if exist_log
            continue
        end

        %% Load relevant data
        loadpath = [up.paths.data_save_folder, num2str(subj), up.paths.filenames.int_respSigs];

        if strcmp(option(1:3), 'ppg')
            rel_name1 = rel_var_names{rel_var_name_no};
            rel_name2 = [option(1:3) up.paths.filenames.elim_vhf];
            load(loadpath, rel_name1, rel_name2);
            eval(['rel_data.beats = ' rel_name1 ';']);
            eval(['rel_data.s = ' rel_name2 ';']);
            eval(['rel_data.fs = ' rel_name1 '.fs;']);
            eval(['rel_data.timings = ' rel_name1 '.timings;']);
        else
            %% ECG
            rel_name1 = rel_var_names{rel_var_name_no};
            rel_name2 = [option(1:3) up.paths.filenames.elim_vhf];
            load(loadpath, rel_name1, rel_name2);
            eval(['rel_data.beats = ' rel_name1 ';']);
            eval(['rel_data.s = ' rel_name2 ';']);
            eval(['rel_data.fs = ' rel_name1 '.fs;']);
            eval(['rel_data.timings = ' rel_name1 '.timings;']);
        end

        %% Identify Fiducial Points

        if strcmp(option(1:3), 'ppg')
            %% PPG Peaks
            % Find peaks as max between detected onsets
            temp.p_max.t = nan(length(rel_data.beats.tr.t)-1,1);
            temp.p_max.v = nan(length(rel_data.beats.tr.t)-1,1);
            for beat_no = 1 : (length(rel_data.beats.tr.t)-1)
                rel_range = find(rel_data.s.t >= rel_data.beats.tr.t(beat_no) & rel_data.s.t < rel_data.beats.tr.t(beat_no+1));
                [~, rel_el] = max(rel_data.s.v(rel_range));
                temp.p_max.t(beat_no) = rel_data.s.t(rel_range(rel_el));
                temp.p_max.v(beat_no) = rel_data.s.v(rel_range(rel_el));
            end

            %% PPG Troughs

            % Find troughs as min between detected peaks
            temp.tr_min.t = nan(length(temp.p_max.t)-1,1);
            temp.tr_min.v = nan(length(temp.p_max.t)-1,1);
            for beat_no = 1 : (length(temp.p_max.t)-1)
                rel_range = find(rel_data.s.t >= temp.p_max.t(beat_no) & rel_data.s.t < temp.p_max.t(beat_no+1));
                [~, rel_el] = min(rel_data.s.v(rel_range));
                temp.tr_min.t(beat_no) = rel_data.s.t(rel_range(rel_el));
                temp.tr_min.v(beat_no) = rel_data.s.v(rel_range(rel_el));
            end

        else
            %% ECG Peaks
            % Used to find peaks as max between detected onsets, but it now
            % appears that this doesn't work if it detects the onsets as a
            % mixture of Q and S waves.

            temp.p_max = rel_data.beats.p;

            %% ECG Troughs
            % Find troughs as min within search range 0.1s before peaks
            temp.tr_min.t = nan(length(temp.p_max.t),1);
            temp.tr_min.v = nan(length(temp.p_max.t),1);
            thresh = 0.1;
            for beat_no = 1 : (length(temp.p_max.t))
                rel_range = find(rel_data.s.t >= (temp.p_max.t(beat_no) - thresh) & rel_data.s.t < temp.p_max.t(beat_no));
                % used to be:     rel_range = find(rel_data.s.t >= (temp.p_max.t(beat_no) - thresh) & rel_data.s.t < (temp.p_max.t(beat_no) + thresh));
                [~, rel_el] = min(rel_data.s.v(rel_range));
                if ~isempty(rel_el)   % it is empty if the peak is at the very first element of the signal
                    temp.tr_min.t(beat_no) = rel_data.s.t(rel_range(rel_el));
                    temp.tr_min.v(beat_no) = rel_data.s.v(rel_range(rel_el));
                end
            end
            % get rid of any nans (arise if the peak is at the very first element of the signal)
            bad_els = isnan(temp.tr_min.t);
            temp.tr_min.t = temp.tr_min.t(~bad_els);
            temp.tr_min.v = temp.tr_min.v(~bad_els);

            % very ocassionally it picks out the same trough or peak twice (if two consecutive peaks are ridiculously close together so share some of the same search range)
            [~, rel_els, ~] = unique(temp.tr_min.t);
            temp.tr_min.t = temp.tr_min.t(rel_els);
            temp.tr_min.v = temp.tr_min.v(rel_els);

        end

        % Carry forward detected peaks and onsets
        temp.det_p.t = rel_data.beats.p.t;
        temp.det_p.v = rel_data.beats.p.v;
        temp.det_tr.t = rel_data.beats.tr.t;
        temp.det_tr.v = rel_data.beats.tr.v;

        % carry forward fs and timings
        temp.fs = rel_data.fs;
        temp.timings = rel_data.timings;

        %% Save processed data
        eval([save_name ' = temp;']);
        save_or_append_data

    end

end

end


function test_bug1600

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_checkdata ft_datatype_source

% The problem: ft_checkdata(volume, 'datatype', 'source') does not seem to
% convert the inside back to vectorial representation

% verify the problem
volume         = [];
volume.dim     = [3 4 5];
volume.anatomy = zeros(3,4,5);
volume.anatomy(2,2:3,2:4) = rand(1,2,3);
volume.inside  = volume.anatomy>0;
volume.transform = eye(4);

source = ft_checkdata(volume, 'datatype', 'source');

assert(all(size(source.inside)==[60 1]));


%Bicycle Car-like vehicle class
%
% This concrete class models the kinematics of a car-like vehicle (bicycle
% or Ackerman model) on a plane.  For given steering and velocity inputs it
% updates the true vehicle state and returns noise-corrupted odometry
% readings.
%
% Methods::
%   Bicycle      constructor
%   add_driver   attach a driver object to this vehicle
%   control      generate the control inputs for the vehicle
%   deriv        derivative of state given inputs
%   init         initialize vehicle state
%   f            predict next state based on odometry
%   Fx           Jacobian of f wrt x
%   Fv           Jacobian of f wrt odometry noise
%   update       update the vehicle state
%   run          run for multiple time steps
%   step         move one time step and return noisy odometry
%
% Plotting/display methods::
%   char             convert to string
%   display          display state/parameters in human readable form
%   plot             plot/animate vehicle on current figure
%   plot_xy          plot the true path of the vehicle
%   Vehicle.plotv    plot/animate a pose on current figure
%
% Properties (read/write)::
%   x               true vehicle state: x, y, theta (3x1)
%   V               odometry covariance (2x2)
%   odometry        distance moved in the last interval (2x1)
%   rdim             dimension of the robot (for drawing)
%   L               length of the vehicle (wheelbase)
%   alphalim        steering wheel limit
%   maxspeed        maximum vehicle speed
%   T               sample interval
%   verbose         verbosity
%   x_hist          history of true vehicle state (Nx3)
%   driver          reference to the driver object
%   x0              initial state, restored on init()
%
% Examples::
%
% Odometry covariance (per timstep) is
%       V = diag([0.02, 0.5*pi/180].^2);
% Create a vehicle with this noisy odometry
%       v = Bicycle( 'covar', diag([0.1 0.01].^2 );
% and display its initial state
%       v
% now apply a speed (0.2m/s) and steer angle (0.1rad) for 1 time step
%       odo = v.step(0.2, 0.1)
% where odo is the noisy odometry estimate, and the new true vehicle state
%       v
%
% We can add a driver object
%      v.add_driver( RandomPath(10) )
% which will move the vehicle within the region -10<x<10, -10<y<10 which we
% can see by
%      v.run(1000)
% which shows an animation of the vehicle moving for 1000 time steps
% between randomly selected wayoints.
%
% Notes::
% - Subclasses the MATLAB handle class which means that pass by reference semantics
%   apply.
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6
%   Peter Corke,
%   Springer 2011
%
% See also RandomPath, EKF.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef Bicycle < Vehicle

    properties
        % state
        L           % length of vehicle

        steermax
        accelmax
        vprev
        steerprev
    end

    methods

        function veh = Bicycle(varargin)
        %Bicycle.Bicycle Vehicle object constructor
        %
        % V = Bicycle(OPTIONS)  creates a Bicycle object with the kinematics of a
        % bicycle (or Ackerman) vehicle.
        %
        % Options::
        % 'steermax',M    Maximu steer angle [rad] (default 0.5)
        % 'accelmax',M    Maximum acceleration [m/s2] (default Inf)
        %--
        % 'covar',C       specify odometry covariance (2x2) (default 0)
        % 'speedmax',S    Maximum speed (default 1m/s)
        % 'L',L           Wheel base (default 1m)
        % 'x0',x0         Initial state (default (0,0,0) )
        % 'dt',T          Time interval (default 0.1)
        % 'rdim',R        Robot size as fraction of plot window (default 0.2)
        % 'verbose'       Be verbose
        %
        % Notes::
        % - The covariance is used by a "hidden" random number generator within the class.
        % - Subclasses the MATLAB handle class which means that pass by reference semantics
        %   apply.
        %
        % Notes::
        % - Subclasses the MATLAB handle class which means that pass by reference semantics
        %   apply.

            veh = veh@Vehicle(varargin{:});

            veh.x = zeros(3,1);

            opt.L = 1;
            opt.steermax = 0.5;
            opt.accelmax = Inf;

            veh = tb_optparse(opt, veh.options, veh);
            veh.vprev = 0;
            veh.x = veh.x0;
        end

        function xnext = f(veh, x, odo, w)
            %Bicycle.f Predict next state based on odometry
            %
            % XN = V.f(X, ODO) is the predicted next state XN (1x3) based on current
            % state X (1x3) and odometry ODO (1x2) = [distance, heading_change].
            %
            % XN = V.f(X, ODO, W) as above but with odometry noise W.
            %
            % Notes::
            % - Supports vectorized operation where X and XN (Nx3).
            if nargin < 4
                w = [0 0];
            end

            dd = odo(1) + w(1); dth = odo(2) + w(2);

            % straightforward code:
            % thp = x(3) + dth;
            % xnext = zeros(1,3);
            % xnext(1) = x(1) + (dd + w(1))*cos(thp);
            % xnext(2) = x(2) + (dd + w(1))*sin(thp);
            % xnext(3) = x(3) + dth + w(2);
            %
            % vectorized code:

            thp = x(:,3) + dth;
            %xnext = x + [(dd+w(1))*cos(thp)  (dd+w(1))*sin(thp) ones(size(x,1),1)*dth+w(2)];
            xnext = x + [dd*cos(thp)  dd*sin(thp) ones(size(x,1),1)*dth];
        end

        function [dx,u] = deriv(veh, t, x, u)
            %Bicycle.deriv  Time derivative of state
            %
            % DX = V.deriv(T, X, U) is the time derivative of state (3x1) at the state
            % X (3x1) with input U (2x1).
            %
            % Notes::
            % - The parameter T is ignored but  called from a continuous time integrator such as ode45 or
            %   Simulink.

            % implement acceleration limit if required
            if ~isinf(veh.accelmax)
                if (u(1) - veh.vprev)/veh.dt > veh.accelmax
                    u(1) = veh.vprev + veh.accelmax * veh.dt;
                elseif (u(1) - veh.vprev)/veh.dt < -veh.accelmax
                    u(1) = veh.vprev - veh.accelmax * veh.dt;
                end
                veh.vprev = u(1);
            end

            % implement speed and steer angle limits
            u(1) = min(veh.speedmax, max(u(1), -veh.speedmax));
            u(2) = min(veh.steermax, max(u(2), -veh.steermax));

            % compute the derivative
            dx = zeros(3,1);
            dx(1) = u(1)*cos(x(3));
            dx(2) = u(1)*sin(x(3));
            dx(3) = u(1)/veh.L * tan(u(2));
        end

        function odo = update(veh, u)
            %Bicycle.update Update the vehicle state
            %
            % ODO = V.update(U) is the true odometry value for
            % motion with U=[speed,steer].
            %
            % Notes::
            % - Appends new state to state history property x_hist.
            % - Odometry is also saved as property odometry.

            % update the state
            dx = veh.dt * veh.deriv([], veh.x, u);
            veh.x = veh.x + dx;

            % compute and save the odometry
            odo = [ norm(dx(1:2)) dx(3) ];
            veh.odometry = odo;

            veh.x_hist = [veh.x_hist; veh.x'];   % maintain history
        end

        function J = Fx(veh, x, odo)
        %Bicycle.Fx  Jacobian df/dx
        %
        % J = V.Fx(X, ODO) is the Jacobian df/dx (3x3) at the state X, for
        % odometry input ODO (1x2) = [distance, heading_change].
        %
        % See also Bicycle.f, Vehicle.Fv.
            dd = odo(1); dth = odo(2);
            thp = x(3) + dth;

            J = [
                1   0   -dd*sin(thp)
                0   1   dd*cos(thp)
                0   0   1
                ];
        end

        function J = Fv(veh, x, odo)
            %Bicycle.Fv  Jacobian df/dv
            %
            % J = V.Fv(X, ODO) is the Jacobian df/dv (3x2) at the state X, for
            % odometry input ODO (1x2) = [distance, heading_change].
            %
            % See also Bicycle.F, Vehicle.Fx.
            dd = odo(1); dth = odo(2);
            thp = x(3);

            J = [
                cos(thp)    0
                sin(thp)    0
                0           1
                ];
        end

        function s = char(veh)
        %Bicycle.char Convert to a string
        %
        % s = V.char() is a string showing vehicle parameters and state in
        % a compact human readable format.
        %
        % See also Bicycle.display.

            ss = char@Vehicle(veh);

            s = 'Bicycle object';
            s = char(s, sprintf('  L=%g, steer.max=%g, accel.max=%g', veh.L, veh.steermax, veh.accelmax));
            s = char(s, ss);
        end
    end % method

end % classdef


function Gamma = ehmminit(data,T,options)
%
% Initialise the ehmm chain using iterative vanilla K=2 HMMs
%
% INPUT
% data      observations, a struct with X (time series) and C (classes, optional)
% T         length of observation sequence
% options   structure with the training options
% Sind
%
% OUTPUT
% Gamma     p(state given X)
%
% Author: Diego Vidaurre, Aarhus University / Oxford , 2020

if ~isfield(options,'maxorder')
    [~,order] = formorders(options.order,options.orderoffset,...
        options.timelag,options.exptimelag);
    options.maxorder = order;
end
K = options.K;
if isfield(options,'initTestSmallerK') && options.initTestSmallerK
    warning('Option initTestSmallerK ignored')
end
% provided_baseline = ...
%     (isfield(options,'ehmm_baseline_data') && ~isempty(options.ehmm_baseline_data)) ...
%     || (isfield(options,'ehmm_baseline_w') && ~isempty(options.ehmm_baseline_w));
% r = 1; stop = false;

if isfield(options,'ehmm_baseline_data') && ~isempty(options.ehmm_baseline_data)
    baseline = computeBaseline(options);
elseif (isfield(options,'ehmm_baseline_w') && ~isempty(options.ehmm_baseline_w))
    if isstruct(options.ehmm_baseline_w)
        baseline = options.ehmm_baseline_w;
    else
        baseline = struct(); baseline.Mu_W = options.ehmm_baseline_w;
    end
else
    baseline = computeBaseline(options,data.X,T);
end

[hmm,I,G] = run_short_hmm_batches(data,T,options,K);
Gamma = findStandoutChains(T-options.order,hmm,baseline,I,G,K);

end

function [hmm,I,G] = run_short_hmm_batches(data,T,options,K)
L = 10000; step = 2000; threshold = 20;
N = length(T); order = options.order;
hmm = []; I = []; G = [];
for j = 1:N
    t0 = sum(T(1:j-1)); t0g = t0 - (j-1) * order;
    t =  t0 + (1:(L+order)); tg = t0g + (1:L);
    while t(end) <= T
        dat = data; dat.X = dat.X(t,:);
        [hm,g] = run_short_hmm(dat,length(t),options,K);
        if isempty(hmm)
            hmm = hm;
        else
            for k = 1:length(hm.state)
                if sum(g(:,k))>threshold
                    hmm.state(end+1) = hm.state(k);
                end
            end
        end
        for k = 1:length(hm.state)
            if sum(g(:,k))>threshold
                I = [I; [tg(1) tg(end)]];
                G = [G single(g(:,k))];
            end
        end
        t = t + step; tg = tg + step;
    end
end
end

function [hmm,Gamma] = run_short_hmm(data,T,options,K)
options.K = K;
GammaInit = initGamma_random(T-options.maxorder,K,...
    min(median(double(T))/10,500));
hmm = struct('train',struct());
hmm.K = K;
hmm.train = options;
hmm.train.ndim = size(data.X,2);
hmm.train.cyc = hmm.train.cyc;
hmm.train.verbose = 0; %%%%
hmm.train.episodic = 0;
hmm.train.Pstructure = true(options.K);
hmm.train.Pistructure = true(1,options.K);
hmm.train.stopcriterion = 'FreeEnergy';
if isfield(options,'DirichletDiag_Init')
    hmm.train.DirichletDiag = options.DirichletDiag_Init;
end
hmm = hmmhsinit(hmm);
if isfield(options,'DirichletDiag_Init')
    hmm.train.DirichletDiag = options.DirichletDiag;
end
[hmm,residuals] = obsinit(data,T,hmm,GammaInit);
data.C = NaN(size(data.C,1),hmm.K);
[hmm,Gamma] = hmmtrain(data,T,hmm,GammaInit,residuals);
end

function Gamma = findStandoutChains(T,hmm,baseline,I,G,nchains)
K = length(hmm.state);
hmm.state(K+1).W = baseline;
hmm.train.Pstructure = true(K+1); hmm.train.Pistructure = true(1,K+1);
hmm.train.stopcriterion = 'FreeEnergy';
fit = hmmspectramar([],[],hmm);
c = zeros(K,1);
ndim = size(fit.state(K+1).psd,2);
psd = []; for n = 1:ndim, psd = [psd; fit.state(K+1).psd(:,n,n)]; end
psd = psd ./ sum(psd);
W = zeros(length(baseline.Mu_W),K);
PSD = zeros(length(psd),K);
for k = 1:K
    psdk = []; for n = 1:ndim, psdk = [psdk; fit.state(k).psd(:,n,n)]; end
    psdk = psdk ./ sum(psdk);
    c(k) = corr(psdk,psd);
    W(:,k) = hmm.state(k).W.Mu_W;
    PSD(:,k) = psdk;
end
[~,jj] = min(c);
while length(jj) < nchains
   for k = 1:K
      c(k) = max(c(k),corr(PSD(:,k),PSD(:,jj(end))));
   end
   [~,j] = min(c); jj = [jj j];
end
Gamma = zeros(sum(T),nchains);
for k = 1:nchains
   Gamma(I(jj(k),1):I(jj(k),2),k) = G(:,jj(k));
end
end

%
% function Gamma = estimateGamma(data,T,W,Wbaseline,options)
% orders = formorders(options.order,options.orderoffset,options.timelag,options.exptimelag);
% [XX,Y] = formautoregr(data.X,T,orders,orders(end),1);
% K = size(W,2);
% % r = zeros(size(Y,1),K+1);
% % for k = 1:K
% %     r(:,k) = (XX * W(:,k) - Y).^2;
% % end
% % r(:,K+1) = (XX * Wbaseline - Y).^2;
% ehmm = struct('train',struct());
% ehmm.K = options.K;
% ehmm.train = options;
% ehmm = hmmhsinit(ehmm,[],T);
% for k = 1:K
%     ehmm.state(k).W.Mu_W = W(:,k);
% end
% ehmm.state(K+1).W.Mu_W = Wbaseline;
% ehmm.Omega = struct('Gam_rate',1,'Gam_shape',1);
% np = size(W,1); ndim = 1;
% for n = 1:ndim
%     ehmm.state_shared(n).Mu_W = zeros((K+1)*np,1);
% end
% for k = 1:K+1
%     ind = (k-1)*np + (1:np);
%     for n = 1:ndim
%         ehmm.state_shared(n).Mu_W(ind) = ehmm.state(k).W.Mu_W;
%     end
% end
% Gamma = hsinference(data,T,ehmm,Y,options,XX,zeros(sum(T)-length(T)*options.order,K));
% end
%
%