%{
 * Copyright (C) 2020-2030, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function [H_LC, P, min_cost, final_result, results] = optimizeIoU(opt, X, Y, intrinsic, display)
    % prepare for random initilization (not use for now)
    for i = 1:1
        theta_x = optimvar('theta_x', 1, 1,'LowerBound',-180,'UpperBound',180); % 1x1
        theta_y = optimvar('theta_y', 1, 1,'LowerBound',-180,'UpperBound',180); % 1x1
        theta_z = optimvar('theta_z', 1, 1,'LowerBound',-180,'UpperBound',180); % 1x1
        T = optimvar('T', 1, 3,'LowerBound',-0.5,'UpperBound',0.5);
%         theta_x = 0;
%         theta_y = 10;
%         theta_z = 20;
%         T = [ 1 2 3];  % 1x3
%         costIoU(theta_x, theta_y, theta_z, T, X, Y, intrinsic);

        prob = optimproblem;
        f = fcn2optimexpr(@costIoU, theta_x, theta_y, theta_z, T, X, Y, intrinsic);
        prob.Objective = f;

        noise = (rand([1,6]) - 0.5) * 2; % -1 to 1
        if i==1
            x0.theta_x = opt(1);
            x0.theta_y = opt(2) ;
            x0.theta_z = opt(3);
            x0.T = [0 0 0];
        elseif i==2
            x0.theta_x = 82.0122;
            x0.theta_y = -0.0192 ;
            x0.theta_z = 87.7953;
            x0.T = [0.0228
                   -0.2070
                   -0.0783];
        else
            x0.theta_x = opt(1) + noise(1) * 10;
            x0.theta_y = opt(2) + noise(2) * 10;
            x0.theta_z = opt(3) + noise(3) * 10;
            x0.T = [0 0 0] + noise(4:6) * 0.1;
        end

    %             options = optimoptions('fmincon', 'MaxIter',5e2,'Display','iter', 'TolX', 1e-12, 'FunctionTolerance', 1e-8, 'MaxFunctionEvaluations', 3e4);
        options = optimoptions('fmincon', 'MaxIter',5e2, 'TolX', 1e-12, 'Display','off', 'FunctionTolerance', 1e-8, 'MaxFunctionEvaluations', 3e4);
        % options.UseParallel = true;
        % showproblem(prob);
        % [sol, fval, exitflag, output] = solve(prob, x0, 'Options',options);
        [sol, fval, ~, ~] = solve(prob, x0, 'Options', options);
        R_final = rotx(sol.theta_x) * roty(sol.theta_y) * rotz(sol.theta_z);
        H_LC = eye(4);
        H_LC(1:3, 1:3) = R_final;
        H_LC(1:3, 4) = sol.T';

    %             disp('H_LC: ')
    %             disp('     R:')
    %             disp(H_LC(1:3, 1:3))
    %             disp('     T:')
    %             disp(-inv(H_LC(1:3, 1:3))*H_LC(1:3, 4))
        P = intrinsic * [eye(3) zeros(3,1)] * H_LC;
        results(i).total_cost = fval;
        results(i).RMSE = sqrt(fval/size(Y,2));
        results(i).H = H_LC;
        results(i).P = P;
        if display
            disp('-- H_LC: ')
            disp('------- R:')
            disp(H_LC(1:3, 1:3))
            disp('------- T:')
            disp(-inv(H_LC(1:3, 1:3))*H_LC(1:3, 4))
            disp('-- cost:')
            disp(results(i).total_cost)
            disp('-- RMSE:')
            disp(results(i).RMSE)
        end
    end

    [min_cost, k] = min([results(:).total_cost]);
    [max_cost, ~] = max([results(:).total_cost]);
    [min_RMSE, ~] = min([results(:).RMSE]);
    final_result.RMSE = min_RMSE;
    final_result.std = std([results(:).total_cost]);
    final_result.min_cost = min_cost;
    final_result.max_cost = max_cost;
    final_H = results(k).H;
    final_P = results(k).P;
    final_result.H = final_H;
    final_result.P = final_P;
    if display
%         disp("std:")
%         disp(final_result.std)
%         disp("minimum cost:")
%         disp(min_cost)
%         disp('H_LC: ')
%         disp(' R:')
%         disp(results(k).H(1:3, 1:3))
%         disp(' RPY (XYZ):')
%         disp(rad2deg(rotm2eul(results(k).H(1:3, 1:3), "XYZ")))
%         disp(' T:')
%         disp(-inv(results(k).H(1:3, 1:3))*results(k).H(1:3, 4))
%         disp("max cost:")
%         disp(max_cost)
%         disp('H_LC: ')
%         disp(' R:')
%         disp(results(j).H(1:3, 1:3))
%         disp(' RPY (XYZ):')
%         disp(rad2deg(rotm2eul(results(j).H(1:3, 1:3), "XYZ")))
%         disp(' T:')
%         disp(-inv(results(j).H(1:3, 1:3))*results(j).H(1:3, 4))
    end
end


function [pairwiseInteractions, pairwiseSolutions] = simulatePairwiseInteractions(pairwiseModelFolder, varargin)
% This function predicts the outcome of pairwise simulations in every
% combination from a given list of pairwise models. The pairwise models
% need to be created first with the function joinModelsPairwiseFromList.
% This script requires the COBRA Toolbox function solveCobraLP. Due to the
% coupling constraints on the pairwise models, the simulations cannot
% currently be run with optimizeCbModel.
%
% Below is a description of all possible consequences for the two joined
% organisms. Please note that the outcomes depend on the two genome-scale
% reconstructions joined and are highly dependent on the applied
% constraints.
%
% * Competition: both organisms grow slower in co-growth than separately
%   (same outcome for both).
%
% * Parasitism: one organism grows faster in co-growth than separately, while
%   the other grows slower in co-growth than separately. Outcome for
%   faster-growing organism: Parasitism_Taker, for slower-growing organism:
%   Parasitism_Giver
%
% * Amensalism: one organism's growth is unaffected by co-growth,  while
%   the other grows slower in co-growth than separately. Outcome for
%   unaffected organism: Amensalism_Unaffected, for slower-growing organism:
%   Amensalism_Affected
%
% * Neutralism: both organisms' growths are unaffected by co-growth (same
%   outcome for both)
%
% * Commensalism: one organism's growth is unaffected by co-growth,  while
%   the other grows fatser in co-growth than separately. Outcome for
%   unaffected organism: Commensalism_Giver, for slower-growing organism:
%   Commensalism_Taker
%
% * Mutualism: both organisms growth faster in co-growth than separately
%   (same outcome for both)
%
% Please note: the function takes the name of the folder containing
% previously created pairwise models as the input, e.g.:
% pairwiseInteractions, pairwiseSolutions] =
% simulatePairwiseInteractions('/Users/almut.heinken/Documents/PairwiseModels');
%
% USAGE:
%     [pairwiseInteractions, pairwiseSolutions] = simulatePairwiseInteractions(pairwiseModelFolder, varargin)
%
% INPUTS:
%     pairwiseModelFolder:     Folder where pairwise models and pairwise model info file are located
%
% OPTIONAL INPUTS:
%     inputDiet:               Cell array of strings with three columns containing
%                              exchange reaction model abbreviations, lower bounds,
%                              and upper bounds.
%                              If no diet is input then the input pairwise models
%                              will be used with unchanged constraints.
%     saveSolutionsFlag:       If true, flux solutions are stored (may result in
%                              large output files)
%     numWorkers:              Number of workers in parallel pool if desired
%     sigD:                    Difference in growth rate that counts as significant
%                              change (by default: 10%)
%
% OUTPUTS:
%     pairwiseInteractions:    Table with computed pairwise and single growth
%                              rates for all entered microbe-microbe models
%
% OPTIONAL OUTPUT:
%     pairwiseSolutions:       Table with all computed flux solutions saved
%                              (may result in large output files)
%
% .. Authors:
%       - Almut Heinken, 02/2018
%       - Almut Heinken, 02/2020: Inputs changed for more efficient computation

parser = inputParser();  % Parse input parameters
parser.addRequired('pairwiseModelFolder', @ischar);
parser.addParameter('inputDiet', {}, @iscell)
parser.addParameter('saveSolutionsFlag', false, @(x) isnumeric(x) || islogical(x))
parser.addParameter('numWorkers', 0, @(x) isnumeric(x))
parser.addParameter('sigD', 0.1, @(x) isnumeric(x))

parser.parse(pairwiseModelFolder, varargin{:});

pairwiseModelFolder = parser.Results.pairwiseModelFolder;
inputDiet = parser.Results.inputDiet;
saveSolutionsFlag = parser.Results.saveSolutionsFlag;
numWorkers = parser.Results.numWorkers;
sigD = parser.Results.sigD;

%% initialize COBRA Toolbox and parallel pool
global CBT_LP_SOLVER
if isempty(CBT_LP_SOLVER)
    initCobraToolbox
end
solver = CBT_LP_SOLVER;

if numWorkers > 0
    % with parallelization
    poolobj = gcp('nocreate');
    if isempty(poolobj)
        parpool(numWorkers)
    end
end
environment = getEnvironment();

pairwiseSolutions = {};
if saveSolutionsFlag == true
    pairwiseSolutions{1, 1} = 'pairedModelID';
    pairwiseSolutions{1, 2} = 'PairwiseSolution';
    pairwiseSolutions{1, 3} = 'SingleModel1Solution';
    pairwiseSolutions{1, 4} = 'SingleModel2Solution';
end

pairwiseInteractions{1, 1} = 'pairedModelID';
pairwiseInteractions{1, 2} = 'ModelID1';
pairwiseInteractions{1, 3} = 'ModelID2';
pairwiseInteractions{1, 4} = 'pairedGrowth_Model1';
pairwiseInteractions{1, 5} = 'pairedGrowth_Model2';
pairwiseInteractions{1, 6} = 'singleGrowth_Model1';
pairwiseInteractions{1, 7} = 'singleGrowth_Model2';
pairwiseInteractions{1, 8} = 'Outcome_Model1';
pairwiseInteractions{1, 9} = 'Outcome_Model2';
pairwiseInteractions{1, 10} = 'Total_Outcome';

growthRates={};
solutionsTmp={};

if isfile([pairwiseModelFolder filesep 'pairedModelInfo.mat'])
load([pairwiseModelFolder filesep 'pairedModelInfo.mat'],'pairedModelInfo')
else
    error('Pairwise models have not been created. Please run joinModelsPairwiseFromList first.')
end

info=pairedModelInfo;
parfor i = 1:size(info, 1)
    restoreEnvironment(environment);
    changeCobraSolver(solver, 'LP', 0, -1);
    changeCobraSolverParams('LP', 'logFile', 0);

    % load the model
    filename=[pairwiseModelFolder filesep info{i,1}];
    pairedModel=readCbModel(filename);
    pairedModelOrg=pairedModel;
    % if a diet was input
    if ~isempty(inputDiet)
        pairedModel = useDiet(pairedModel, inputDiet);
    end
    % for each paired model, set both biomass objective functions as
    % objectives
    biomass1 = strcat(info{i, 2}, '_', info{i, 3});
    biomass2 = strcat(info{i, 4}, '_', info{i, 5});
    model1biomass = find(ismember(pairedModel.rxns, biomass1));
    pairedModel.c(model1biomass, 1) = 1;
    model2biomass = find(ismember(pairedModel.rxns, biomass2));
    pairedModel.c(model2biomass, 1) = 1;
    % enforce minimal growth
    pairedModel = changeRxnBounds(pairedModel, biomass1, 0.001, 'l');
    pairedModel = changeRxnBounds(pairedModel, biomass2, 0.001, 'l');
    % calculate joint biomass
    solutionPaired = solveCobraLP(buildLPproblemFromModel(pairedModel,false));
    % separate growth
    % silence model 2 and optimize model 1
    % load the model again to avoid errors
    pairedModel=pairedModelOrg;
    % if a diet was input
    if ~isempty(inputDiet)
        pairedModel = useDiet(pairedModel, inputDiet);
    end
    pairedModel = changeObjective(pairedModel, biomass1);
    % disable flux through the second model
    pairedModel = changeRxnBounds(pairedModel, pairedModel.rxns(strmatch(strcat(info{i, 4}, '_'), pairedModel.rxns)), 0, 'b');
    % calculate single biomass
    solutionSingle1 = solveCobraLP(buildLPproblemFromModel(pairedModel,false));
    % silence model 1 and optimize model 2
    % load the model again to avoid errors
    pairedModel=pairedModelOrg;
    % if a diet was input
    if ~isempty(inputDiet)
        pairedModel = useDiet(pairedModel, inputDiet);
    end
    pairedModel = changeObjective(pairedModel, biomass2);
    % disable flux through the first model
    pairedModel = changeRxnBounds(pairedModel, pairedModel.rxns(strmatch(strcat(info{i, 2}, '_'), pairedModel.rxns)), 0, 'b');
    % calculate single biomass
    solutionSingle2 = solveCobraLP(buildLPproblemFromModel(pairedModel,false));

    % save results temporarily
    if solutionPaired.stat==1
        growthRates{i}{1,1} = solutionPaired.full(model1biomass);
        growthRates{i}{1,2} = solutionPaired.full(model2biomass);
    else
        growthRates{i}{1,1} = 0;
        growthRates{i}{1,2} = 0;
    end
    if solutionSingle1.stat==1
        growthRates{i}{1,3} = solutionSingle1.full(model1biomass);
    else
        growthRates{i}{1,3} = 0;
    end
    if solutionSingle2.stat==1
        growthRates{i}{1,4} = solutionSingle2.full(model2biomass);
    else
        growthRates{i}{1,4} = 0;
    end

    solutionsTmp{i}{1,1} = solutionPaired;
    solutionsTmp{i}{1,2} = solutionSingle1;
    solutionsTmp{i}{1,3} = solutionSingle2;
end

% backup results for large number of simulations
if size(pairedModelInfo, 1) > 1000
save('growthRates','growthRates','-v7.3');
save('solutionsTmp','solutionsTmp','-v7.3');
end

for i = 1:size(pairedModelInfo, 1)
    solPaired = solutionsTmp{i}{1,1};
    solSingle1 = solutionsTmp{i}{1,2};
    solSingle2 = solutionsTmp{i}{1,3};

    grPaired1 = growthRates{i}{1,1};
    grPaired2 = growthRates{i}{1,2};
    grSingle1 = growthRates{i}{1,3};
    grSingle2 = growthRates{i}{1,4};

    % interpret computed growth rates-only if solutions are feasible
    if solPaired.stat ~= 0 && solSingle1.stat ~= 0 && solSingle2.stat ~= 0
        [iAFirstModel, iASecondModel, iATotal] = analyzePairwiseInteractions(grPaired1, grPaired2, grSingle1, grSingle2, sigD);
    end
    %% list all results
    % fill out the results table with the model information
    pairwiseInteractions{i + 1, 1} = pairedModelInfo{i, 1};
    pairwiseInteractions{i + 1, 2} = pairedModelInfo{i, 2};
    pairwiseInteractions{i + 1, 3} = pairedModelInfo{i, 4};
    % combined growth
    pairwiseInteractions{i + 1, 4} = grPaired1;
    pairwiseInteractions{i + 1, 5} = grPaired2;
    pairwiseInteractions{i + 1, 6} = grSingle1;
    pairwiseInteractions{i + 1, 7} = grSingle2;
    % make sure that infeasible solutions are flagged
    if solPaired.stat == 0 || solSingle1.stat == 0 || solSingle2.stat == 0
        pairwiseInteractions{i + 1, 8} = 'Infeasible';
        pairwiseInteractions{i + 1, 9} = 'Infeasible';
        pairwiseInteractions{i + 1, 10} = 'Infeasible';
    else
        % save the results
        pairwiseInteractions{i + 1, 8} = iAFirstModel;
        pairwiseInteractions{i + 1, 9} = iASecondModel;
        pairwiseInteractions{i + 1, 10} = iATotal;
    end
    %% store the solutions if storeSolutionFlag==true
    if saveSolutionsFlag == true
        pairwiseSolutions{i + 1, 1} = pairedModelInfo{i, 1};
        pairwiseSolutions{i + 1, 2} = solPaired.full;
        pairwiseSolutions{i + 1, 3} = solSingle1.full;
        pairwiseSolutions{i + 1, 4} = solSingle2.full;
    end
end

end

function [iAFirstOrg, iASecondOrg, iATotal] = analyzePairwiseInteractions(pairedGrowthOrg1, pairedGrowthOrg2, singleGrowthOrg1, singleGrowthOrg2, sigD)
% This function evaluates the outcome of pairwise growth of two organisms
% compared with the two organisms separately. There are six possible
% outcomes of the interaction, and nine possible outcomes from the
% perspective of each organism.

if abs(1 - (pairedGrowthOrg1 / singleGrowthOrg1)) < sigD
    % first microbe unaffected - all possible cases resulting froms
    % second microbe's growth
    if abs(1 - (pairedGrowthOrg2 / singleGrowthOrg2)) < sigD
        % second microbe unaffected
        iAFirstOrg = 'Neutralism';
        iASecondOrg = 'Neutralism';
        iATotal = 'Neutralism';
    elseif abs((pairedGrowthOrg2 / singleGrowthOrg2)) > 1 + sigD
        % second microbe grows better
        iAFirstOrg = 'Commensalism_Giver';
        iASecondOrg = 'Commensalism_Taker';
        iATotal = 'Commensalism';
    elseif abs((singleGrowthOrg2 / pairedGrowthOrg2)) > 1 + sigD
        % second microbe grows slower
        iAFirstOrg = 'Amensalism_Unaffected';
        iASecondOrg = 'Amensalism_Affected';
        iATotal = 'Amensalism';
    else
        % if no case fits - needs inspection!
        iAFirstOrg = 'No_Result';
        iASecondOrg = 'No_Result';
        iATotal = 'No_Result';
    end
elseif abs((pairedGrowthOrg1 / singleGrowthOrg1)) > 1 + sigD
    % first microbe grows better - all possible cases resulting froms
    % second microbe's growth
    if abs(1 - (pairedGrowthOrg2 / singleGrowthOrg2)) < sigD
        % second microbe unaffected
        iAFirstOrg = 'Commensalism_Taker';
        iASecondOrg = 'Commensalism_Giver';
        iATotal = 'Commensalism';
    elseif abs((pairedGrowthOrg2 / singleGrowthOrg2)) > 1 + sigD
        % second microbe grows better
        iAFirstOrg = 'Mutualism';
        iASecondOrg = 'Mutualism';
        iATotal = 'Mutualism';
    elseif abs((singleGrowthOrg2 / pairedGrowthOrg2)) > 1 + sigD
        % second microbe grows slower
        iAFirstOrg = 'Parasitism_Taker';
        iASecondOrg = 'Parasitism_Giver';
        iATotal = 'Parasitism';
    else
        % if no case fits - needs inspection!
        iAFirstOrg = 'No_Result';
        iASecondOrg = 'No_Result';
        iATotal = 'No_Result';
    end
elseif abs((singleGrowthOrg1 / pairedGrowthOrg1)) > 1 + sigD
    % first microbe grows slower - all possible cases resulting froms
    % second microbe's growth
    if abs(1 - (pairedGrowthOrg2 / singleGrowthOrg2)) < sigD
        % second microbe unaffected
        iAFirstOrg = 'Amensalism_Affected';
        iASecondOrg = 'Amensalism_Unaffected';
        iATotal = 'Amensalism';
    elseif abs((pairedGrowthOrg2 / singleGrowthOrg2)) > 1 + sigD
        % second microbe grows better
        iAFirstOrg = 'Parasitism_Giver';
        iASecondOrg = 'Parasitism_Taker';
        iATotal = 'Parasitism';
    elseif abs((singleGrowthOrg2 / pairedGrowthOrg2)) > 1 + sigD
        % second microbe grows slower
        iAFirstOrg = 'Competition';
        iASecondOrg = 'Competition';
        iATotal = 'Competition';
    else
        % if no case fits - needs inspection!
        iAFirstOrg = 'No_Result';
        iASecondOrg = 'No_Result';
        iATotal = 'No_Result';
    end
else
    % if no case fits - needs inspection!
    iAFirstOrg = 'No_Result';
    iASecondOrg = 'No_Result';
    iATotal = 'No_Result';
end
end


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


% run_simSteamShaped_fast.m
% Jamie Near, Sunnybrook Research Institute 2022.
%
% USAGE:
% out=run_simSteamShaped_fast(spinSys);
%
% DESCRIPTION:
% This script simulates a STEAM experiment with fully shaped RF
% pulses. Coherence order filtering is employed to eliminate unwanted
% coherences.  Furthermore, simulations are run at various locations in
% space to account for the within-voxel spatial variation of the metabolite
% signal.  Summation across spatial positions is performed. The MATLAB
% parallel computing toolbox (parfor loop) was used to accelerate the
% simulations.  Acceleration is currently performed in the direction of the
% slice selective pulse along the x-direction, but this can be changed.  Up
% to a factor of 12 acceleration can be achieved using this approach.  If
% the parallel processing toolbox is not available, then replace
% the "parfor" loop with a "for" loop.
%
% INPUTS:
% To run this script, there is technically only one input argument:
% spinSys           = spin system to simulate
%
% However, the user should also edit the following parameters as
% desired before running the function:
% RFWaveform        = name of rf pulse waveform used for both (2nd & 3rd) selective 90 degree pulses.
% Tp                = duration of rf pulses[ms]
% Bfield            = Magnetic field strength in [T]
% Npts              = number of spectral points
% sw                = spectral width [Hz]
% Bfield            = magnetic field strength [Tesla]
% lw                = linewidth of the output spectrum [Hz]
% thkX              = slice thickness of x refocusing pulse [cm]
% thkY              = slice thickness of y refocusing pulse [cm]
% fovX              = full simulation FOV in the x direction [cm]
% fovY              = full simulation FOV in the y direction [cm]
% nX                = number of spatial grid points to simulate in x-direction
% nY                = number of spatial grid points to simulate in y-direction
% taus              = vector of pulse sequence timings  [ms]
%
% OUTPUTS:
% out               = Simulation results, summed over all space.

function out=run_simSteamShaped_fast(sys)
tic
% ************INPUT PARAMETERS**********************************
rfWaveform='ex40.b4_384_14.pta'; %name of RF pulse waveform.
Tp=1.920; %duration of RF pulses[ms]
flipAngle=90;  %Flip Angle of the RF pulses [degrees]
Npts=8192; %number of spectral points
sw=6000; %spectral width [Hz]
Bfield=2.89; %magnetic field strength [Tesla]
lw=1; %linewidth of the output spectrum [Hz]
thkX=2.5; %slice thickness of x RF pulse [cm]
thkY=2.5; %slice thickness of y RF pulse [cm]
fovX=5; %size of the full simulation Field of View in the x-direction [cm]
fovY=5; %size of the full simulation Field of View in the y-direction [cm]
nX=48; %Number of grid points to simulate in the x-direction
nY=48; %Number of grid points to simulate in the y-direction
tau1=6; %TE for STEAM sequence [ms]
tau2=32; %TM for STEAM sequence [ms]
centreFreq=2.3; %Centre frequency of simulation [ppm]
% ************END OF INPUT PARAMETERS**********************************

%set up spatial grid
x=linspace(-fovX/2,fovX/2,nX); %X positions to simulate [cm]
y=linspace(-fovY/2,fovY/2,nY); %y positions to simulate [cm]

%Load RF waveform
RF=io_loadRFwaveform(rfWaveform,'exc',0);

gamma=42577000; %gyromagnetic ratio

%Load spin systems
%load spinSystems
%sys=eval(['sys' spinSys]);

%If length of RF pulse is >200 pts, resample to 100 pts to reduce
%computational workload
if size(RF.waveform,1)>200
    RF=rf_resample(RF,100);
end

Gx=(RF.tbw/(Tp/1000))/(gamma*thkX/10000); %[G/cm]
Gy=(RF.tbw/(Tp/1000))/(gamma*thkY/10000); %[G/cm]

%Initialize structures:
d_temp=cell(1,1);
d=cell(1,1);

%loop through space: If you are using the parfor loops below, and you are
%using an older version of MATLAB (e.g.R2012), don't forget to initialize
%the parallel processing toolbox workers using 'matlabpool open N' (for N
%workers, 12 max).  I don't think this is necessary for newer versions of
%MATLAB.

%First loop through all x-positions, simulating only the first refocusing
%pulse.
%First loop through x direction (first refoc pulse only);

%for X=1:length(x)  %Use this if you don't have the MATLAB parallel processing toolbox
parfor X=1:length(x)  %Use this if you have the MATLAB parallel processing toolbox
        disp(['Executing X-position ' num2str(X) ' of ' num2str(length(x)) '!!!']);
        d_temp{X}=sim_steam_shaped_fastRF1(Bfield,sys,tau1,tau2,RF,Tp,x(X),Gx,flipAngle,centreFreq);
end

%calculate the average density matrix (Doing this inside a separate for
%loop because I couldn't figure out how to do this inside the parfor loop):
for X=1:length(x)
        d{1}=sim_dAdd(d{1},d_temp{X});
end

% %Initialize structures:
out_temp=cell(length(y),1);
out=struct([]);

%Now loop through y direction (second refoc pulse only);
%for Y=1:length(y) %Use this if you don't have the MATLAB parallel processing toolbox
parfor Y=1:length(y) %Use this if you do have the MATLAB parallel processing toolbox
%            disp(['Executing Y-position ' num2str(Y) ' of ' num2str(length(y)) '!!!']);
            out_temp{Y}=sim_steam_shaped_fastRF2(d{1},Npts,sw,Bfield,lw,sys,tau1,tau2,...
                RF,Tp,y(Y),Gy,flipAngle,centreFreq);
end

%Now combine the outputs;  Again, doing this inside a separate for loop
%becuase I can't figure out how to do this inside the parfor loop:
for Y=1:length(y)
            out=op_addScans(out,out_temp{Y});
end

%For consistent scaling across different shaped simulations, we need to :
%1.  Scale down by the total number of simulations run (since these were
%    all added together.
numSims=(nX*nY);
out=op_ampScale(out,1/numSims);

%2.  Scale by the total size of the simulated region, relative to the size
%    of the voxel.
voxRatio=(thkX*thkY)/(fovX*fovY);
out=op_ampScale(out,1/voxRatio);

toc
end

%Nested Function #1
function d = sim_steam_shaped_fastRF1(Bfield,sys,TE,TM,RF,tp,dx,Gx,flipAngle,centreFreq)
%
% USAGE:
% d = sim_steam_shaped_fastRF1(Bfield,sys,TE,TM,RF,tp,dx,Gx,flipAngle,centreFreq)
%
% DESCRIPTION:
% This function simulates only the first bit of the STEAM experiment, up to
% the beginning of the third RF pulse.  The excitation is
% simulated as an instantaneous rotation, and the refocusing pulse is
% simulated as a shaped rotation.
%
% This code is designed to be used in highly-accelerated shaped simulations,
% using the method described by Yan Zhang et al. Med Phys 2017;44(8):
% 4169-78.
%
% This code employs coherence order filtering to select only desired
% signal.  Finally, this code simulates the spectrum at a given point in
% space (x), given the values of the slice selection gradient (Gx).  In
% order to fully simulate the STEAM experiment, you have to run this
% simulation many times at various points in space (x), followed by
% sim_steam_shaped_fastRF2.m, at all points in space (y).
%
% INPUTS:
% Bfield    = main magnetic field strength in [T]
% sys       = spin system definition structure
% TE        = echo time in [ms].
% TM        = mixing time in [ms].
% RF        = RF pulse definition structure for RF pulses (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% dx        = position offset in x-direction (corresponding to 2nd STEAM pulse) [cm]
% Gx        = gradient strength for 2nd STEAM pulse [G/cm]
% flipAngle = flip angle of refocusing pulses [degrees] (Optional.  Default = 90 deg)
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using STEAM
%             sequence.
if nargin<10
    centreFreq=2.3;
    if nargin<9
        flipAngle=90;
    end
end

%In the steam sequence, it can be common to use an asymmetric RF pulse for the
%90-degree pulse waveform.  In this case, it is conventional for the 2nd
%and 3rd rf pulses to be time-reversed versions of eachother, with the 2nd
%pulse (RF1) being a max-phase pulse, and the 3rd pulse (RF2) being a
%min-phase pulse (i.e. the long tails of both pulse occurring during the TM
%period so that the TE is minimized).  Here, check if the RF pulse is
%asymmetric and if so, make sure that the 2nd and 3rd pulses are max-phase
%and min-phase, respectively:
if RF.rfCentre>0.5
    RF1=rf_timeReverse(RF);
    RF2=RF;
else
    RF1=RF;
    RF2=rf_timeReverse(RF);
end

%Check that the TE and TM values are not too short
if TE<(RF1.rfCentre*tp*2)
    error('ERROR:  TE cannot be less than duration of RF pulse! ABORTING!!');
end
if TM<(RF2.rfCentre*tp*2)
    error('ERROR:  TM cannot be less than duration of RF pulse! ABORTING!!');
end

%Set centre frequency
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

%Calculate new delays by subtracting the pulse duration from tau1 and tau2;
delays=zeros(2);
delays(1)=TE-(RF1.rfCentre*tp*2);
delays(2)=TM-(RF2.rfCentre*tp*2);
if sum(delays<0)
    error(['ERROR! The following taus are too short: ' num2str(find(delays<0)) '.']);
end

%BEGIN PULSE SEQUENCE************
d=sim_excite(d,H,'x');                                    %EXCITE
d=sim_COF(H,d,1);                                       %Keep only +1-order coherences
d=sim_evolve(d,H,delays(1)/2000);                         %Evolve by delays(1)/2
d=sim_gradSpoil(d,H,[Gx,0,0],[dx,0,0],tp*RF1.rfCentre);     %Prewind gradient for 2nd 90 degree pulse (Not sure why, but this only works when Gx is positive.  Intiutively, Gx amplitude should be the opposite of the slice select gradient (i.e. -Gx), but this does not seem to work).
d=sim_shapedRF(d,H,RF1,tp,flipAngle,90,dx,Gx);             %1st shaped 90 degree selective pulse
d=sim_COF(H,d,0);                                       %Keep only 0-order coherences
d=sim_evolve(d,H,(delays(2))/1000);                       %Evolve by delays(2)
%END PULSE SEQUENCE**************

%After running this many times along x, the density matrices should be
%averaged, and then the average density matrix should be passed through
%'sim_press_shaped_fastRef2' at various different y-positions.

end

%Nested Function #2
function out = sim_steam_shaped_fastRF2(d,n,sw,Bfield,linewidth,sys,TE,TM,RF,tp,dy,Gy,flipAngle,centreFreq)
%
% USAGE:
% out = sim_steam_shaped_fastRF(d,n,sw,Bfield,linewidth,sys,TE,TM,RF,tp,dy,Gy,flipAngle,centreFreq)
%
% DESCRIPTION:
% This function simulates only the last bit of the STEAM experiment, from the
% the beginning of the third STEAM pulse, to the end.  The RF
% pulse is simulated as a shaped rotation.
%
% This code is designed to be used in highly-accelerated shaped simulations,
% using the method described by Yan Zhang et al. Med Phys 2017;44(8):
% 4169-78.

% This code employs coherence order filtering to select only desired
% signal.
%
% Finally, this code simulates the spectrum at a given point in space (y),
% given the values of the slice selection gradient (Gy).  In order
% to fully simulate the STEAM experiment, you have to first run
% sim_steam_shaped_fastRF1.m at all points in space (x), followed by
% this code, at all points in space (y).
%
% INPUTS:
% d         = starting density matrix (obtained using 'sim_steam_shaped_fastRF1.m')
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% TE        = echo time in [ms].
% TM        = mixing time in [ms].
% RF        = RF pulse definition structure for selective RF pulses (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% dx        = position offset in x-direction (corresponding to first refocusing pulse) [cm]
% dy        = position offset in y-direction (corresponding to second refocusing pulse) [cm]
% Gx        = gradient strength for 2nd STEAM RF pulse [G/cm]
% Gy        = gradient strength for 3rd STEAM RF pulse [G/cm]
% flipAngle = flip angle of RF pulses [degrees] (Optional.  Default = 90 deg)
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using STEAM
%             sequence.

if nargin<14
    centreFreq=2.3;
    if nargin<13
        flipAngle=90;
    end
end

%In the steam sequence, it can be common to use an asymmetric RF pulse for the
%90-degree pulse waveform.  In this case, it is conventional for the 2nd
%and 3rd rf pulses to be time-reversed versions of eachother, with the 2nd
%pulse (RF1) being a max-phase pulse, and the 3rd pulse (RF2) being a
%min-phase pulse (i.e. the long tails of both pulse occurring during the TM
%period so that the TE is minimized).  Here, use the asymmetry factor of
%the pulse to make sure that the 2nd and 3rd pulses are max-phase and
%min-phase, respectively:
if RF.rfCentre>0.5
    RF1=rf_timeReverse(RF);
    RF2=RF;
else
    RF1=RF;
    RF2=rf_timeReverse(RF);
end

%Check that the TE and TM values are not too short
if TE<(RF1.rfCentre*tp*2)
    error('ERROR:  TE cannot be less than duration of RF pulse! ABORTING!!');
end
if TM<(RF2.rfCentre*tp*2)
    error('ERROR:  TM cannot be less than duration of RF pulse! ABORTING!!');
end

%Set centre frequency
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H]=sim_Hamiltonian(sys,Bfield);

%Calculate new delays by subtracting the pulse duration from TE and TM;
delays=zeros(2);
delays(1)=TE-(RF1.rfCentre*tp*2);
delays(2)=TM-(RF2.rfCentre*tp*2);
if sum(delays<0)
    error(['ERROR! The following delays are too short: ' num2str(find(delays<0)) '.']);
end

%BEGIN PULSE SEQUENCE************
d=sim_shapedRF(d,H,RF2,tp,flipAngle,90,dy,Gy);          %2nd shaped 90 degree selective pulse
d=sim_gradSpoil(d,H,[0,Gy,0],[0,dy,0],tp*RF1.rfCentre);   %Rewind gradient for 3rd 90 degree pulse (Not sure why, but this only works when Gy is positive.  Intiutively, Gx amplitude should be the opposite of the slice select gradient (i.e. -Gy), but this does not seem to work).
d=sim_COF(H,d,-1);                                      %Keep only -1 coherences
d=sim_evolve(d,H,delays(1)/2000);                            %Evolve by delays(1)/2
[out,~]=sim_readout(d,H,n,sw,linewidth,90);      %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq='steam';
out.te=TE;
out.tm=TM;
out.sim='shaped';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;

end