function [bbox_pred, bbox_pred_in_region, bbox_pred_in_region_quantized] = decode_loc_probs_to_bbox_targets(...
    bbox_in, class_indices, loc_prob_vectors, conf)
% decode_loc_probs_to_bbox_targets: given the input bounding boxes (bbox_in),
% the category ids of each bounding box (class_indices), and the predicted
% probability vectors (loc_prob_vectors) of each input bounding box it
% returns the location of the predicted bounding boxes.
%
% INPUTS:
% 1) bbox_in: a N x 4 array with the input bounding box coordinates in the
% form of [xi0,yi0,xi1,yi1] where (xi0,yi0) is the tot-left corner and
% (xi1,yi1) is the bottom-right corner. N is the number of bounding boxes.
% 2) class_indices: a N x 1 array with the category id of each input
% bounding box.
% 3) loc_prob_vectors: a M x K x C x N array with the predicted probability
% vectors of each input bounding box; N is the number of bounding boxes, C
% is the number of categoris, M is the resolution of the target
% probabilities specified by the input parameter conf.resolution and K is
% the number of target probability vectors per bounding box and per
% category. For the InOut model K = 2 (1 vector for each axis), for the
% Border model K = 4 (2 vectors for each axis; e.g. for the x axis we have
% 1 probability vector for the left border and 1 probability vector for the
% right bordr), and for the Combined model K = 6 (3 vectors for each axis;
% e.g. for the x axis we have 1 probability vector for the in-out elements,
% 1 probability vector for the left border, and 1 probability vector
% for the right border).
% 4) conf: struct with the configuration parameters of LocNet:
%   4.1) conf.scale_ratio: (scalar value) the scaling factor of the search region
%        w.r.t. the input bounding boxes. Specifically, given an input
%        bounding boxes, in order for LocNet to localize the target
%        bounding box it "look" in a search region that is obtained by scaling
%        the input bounding box by a scaling factor. The scaling factor is
%        given by the scale_ratio parameter.
%   4.2) conf.resolution: scalar value; In order for the LocNet to localize
%   the target bounding boxes inside the search region, it considers a division
%   of the search region in M horizontal stripes (rows) and M vertical stripes
%   (columns). The value of M is given by the parameter conf.resolution.
%   4.3) conf.num_classes: (scalar value) number of categories
%   4.4) conf.loc_type: string wiht the type of the localization model that
%   the LocNet implements. The possible options are: 'inout','borders', or 'combined'
%
% OUTPUTS:
% 1) bbox_pred: a N x 5 array with the predicted bounding box coordinates in the
% form of [xt0,yt0,xt1,yt1,c] where (xt0,yt0) is the tot-left corner, (xt1,yt1)
% is the bottom-right corner, and c is the category id of the bounding box.
% 3) bbox_pred_in_region: a N x 5 array with the predicted bounding box
% coordinates in the form of [xtr0,ytr0,xtr1,ytr1] where (xtr0,ytr0) is
% the tot-left corner, (xtr1,ytr1) is the bottom-right corner, and c is the
% category id of the bounding box. The coordinates are w.r.t. the
% corresponding search regions.
% 2) bbox_target_quantized: a N x 5 array with the predicted bounding box
% coordinates in the form of [xtq0,ytq0,xtq1,ytq1] where (xtq0,ytq0) is
% the tot-left corner, (xtq1,ytq1) is the bottom-right corner, and c is the
% category id of the bounding box. The coordinates are w.r.t. the
% corresponding search regions and quantized in discrite values between 1
% and resolution.
%
% This file is part of the code that implements the following paper:
% Title      : "LocNet: Improving Localization Accuracy for Object Detection"
% Authors    : Spyros Gidaris, Nikos Komodakis
% Institution: Universite Paris Est, Ecole des Ponts ParisTech
% ArXiv link : http://arxiv.org/abs/1511.07763
% code       : https://github.com/gidariss/LocNet
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2016 Spyros Gidaris
%
% Title     : "LocNet: Improving Localization Accuracy for Object Detection"
% ArXiv link: http://arxiv.org/abs/1511.07763
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------

resolution  = conf.resolution;
scale_ratio = conf.scale_ratio;
loc_type    = conf.loc_type;

assert(size(loc_prob_vectors,1) == conf.resolution);

% get the coordinates of the search regions
bbox_search_region = scale_bboxes(bbox_in, scale_ratio);

% given the predicted probability vectors and the category ids, estimate
% the most likely bounding box location of the objects of interest; the
% predicted bounding boxes locations are w.r.t. the search region
% bbox_search_region and quantized in discrite values between 1 and
% resolution (in the paper the resolution parameter is referred as M)
switch loc_type
    case 'inout'
        bbox_pred_in_region_quantized = decode_in_out_probabilities(loc_prob_vectors, class_indices);
    case 'borders'
        bbox_pred_in_region_quantized = decode_borders_probabilities(loc_prob_vectors, class_indices);
    case 'combined'
        bbox_pred_in_region_quantized = decode_combined_probabilities(loc_prob_vectors, class_indices);
    otherwise
        error('Invalid localization type %s',loc_type)
end

% get the coordinates of the predicted bounding boxes w.r.t. the original
% image and in pixel units.
[bbox_pred, bbox_pred_in_region] = decode_quantized_location(...
    bbox_pred_in_region_quantized, resolution, bbox_search_region);
bbox_pred = [bbox_pred, class_indices];
end

function [real_coord, real_coord_region] = decode_quantized_location(quantized, resolution, bbox_search_region)
search_width  = bbox_search_region(:,3)-bbox_search_region(:,1)+1;
search_height = bbox_search_region(:,4)-bbox_search_region(:,2)+1;

% transform the coordinates of the predicted bounding boxes in pixel units
real_coord_region      = quantized;
real_coord_region(:,1) = (quantized(:,1)-1).*(search_width /resolution) + 0.5;
real_coord_region(:,2) = (quantized(:,2)-1).*(search_height/resolution) + 0.5;
real_coord_region(:,3) = (quantized(:,3)-1).*(search_width /resolution) + 0.5;
real_coord_region(:,4) = (quantized(:,4)-1).*(search_height/resolution) + 0.5;

% get the coordinates of the predicted bounding boxes w.r.t. the image
real_coord      = real_coord_region;
real_coord(:,1) = real_coord_region(:,1)+bbox_search_region(:,1);
real_coord(:,3) = real_coord_region(:,3)+bbox_search_region(:,1);
real_coord(:,2) = real_coord_region(:,2)+bbox_search_region(:,2);
real_coord(:,4) = real_coord_region(:,4)+bbox_search_region(:,2);
end

function best_location = decode_in_out_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodInOut(loc_prob_vectors(:,1,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodInOut(loc_prob_vectors(:,2,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function best_location = decode_combined_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodCombined(loc_prob_vectors(:,1:3,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodCombined(loc_prob_vectors(:,4:6,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function best_location = decode_borders_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodBorders(loc_prob_vectors(:,1:2,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodBorders(loc_prob_vectors(:,3:4,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodCombined(Probs)
% Given the predicted Combined probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
PositiveProbs = max(  Probs,min_prob);
NegativeProbs = max(1-Probs,min_prob);

LogInsideProbs   = -log(squeeze(PositiveProbs(:,1,:)));
LogBordersProbs0 = -log(squeeze(PositiveProbs(:,2,:)))';
LogBordersProbs1 = -log(squeeze(PositiveProbs(:,3,:)))';

LogOutsideProbs     = -log(squeeze(NegativeProbs(:,1,:)));
LogNonBordersProbs0 = -log(squeeze(NegativeProbs(:,2,:)))';
LogNonBordersProbs1 = -log(squeeze(NegativeProbs(:,3,:)))';

resolution            = size(LogInsideProbs,1);
num_elems             = size(LogOutsideProbs,2);
LogInsideProbsCumSum  = cumsum([zeros(1,num_elems,'single');LogInsideProbs], 1)';
LogOutsideProbsCumSum = cumsum([zeros(1,num_elems,'single');LogOutsideProbs],1)';

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogInsideProbsCumSum(:,b+1)-LogInsideProbsCumSum(:,a))-...
            (LogOutsideProbsCumSum(:,b+1)-LogOutsideProbsCumSum(:,a)) + ...
            (LogBordersProbs0(:,a) + LogBordersProbs1(:,b)) - ...
            (LogNonBordersProbs0(:,a) + LogNonBordersProbs1(:,b));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodInOut(Probs)
% Given the predicted InOut probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
InsideProbs      = max(  Probs,min_prob);
OutsideProbs     = max(1-Probs,min_prob);
LogInsideProbs   = -log(squeeze(InsideProbs(:,1,:)));
LogOutsideProbs  = -log(squeeze(OutsideProbs(:,1,:)));

resolution            = size(LogInsideProbs,1);
num_elems             = size(LogOutsideProbs,2);
LogInsideProbsCumSum  = cumsum([zeros(1,num_elems,'single');LogInsideProbs], 1)';
LogOutsideProbsCumSum = cumsum([zeros(1,num_elems,'single');LogOutsideProbs],1)';

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogInsideProbsCumSum(:,b+1)-LogInsideProbsCumSum(:,a))-...
            (LogOutsideProbsCumSum(:,b+1)-LogOutsideProbsCumSum(:,a));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodBorders(Probs)
% Given the predicted Borders probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
PositiveProbs    = max(  Probs,min_prob);
LogBordersProbs0 = -log(squeeze(PositiveProbs(:,1,:)))';
LogBordersProbs1 = -log(squeeze(PositiveProbs(:,2,:)))';

resolution = size(LogBordersProbs0,2);
num_elems = size(LogBordersProbs0,1);

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogBordersProbs0(:,a) + LogBordersProbs1(:,b));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end


function [OffDec,OffMask] = Operator(Problem,ParentDec,ParentMask,rbm,dae,Site,allZero,allOne)
% The operator of MOEA/PSL
%
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    Parent1Mask = ParentMask(1:end/2,:);
    Parent2Mask = ParentMask(end/2+1:end,:);
    Parent1Dec  = ParentDec(1:end/2,:);
    Parent2Dec  = ParentDec(end/2+1:end,:);

    %% Binary variation
    if any(Site)
        other   = ~allZero & ~allOne;
        OffTemp = BinaryCrossover(rbm.reduce(Parent1Mask(Site,other)),rbm.reduce(Parent2Mask(Site,other)));
        OffTemp = rbm.recover(OffTemp);
        OffMask = false(size(OffTemp,1),size(Parent1Mask,2));
        OffMask(:,other)  = OffTemp;
        OffMask(:,allOne) = true;
    else
        OffMask = [];
    end
    OffMask = [OffMask;BinaryCrossover(Parent1Mask(~Site,:),Parent2Mask(~Site,:))];
    OffMask = BinaryMutation(OffMask);

    %% Real variation
    if any(Problem.encoding~=4)
        if any(Site)
            OffDec = RealCrossover(dae.reduce(Parent1Dec(Site,:)),dae.reduce(Parent2Dec(Site,:)));
            OffDec = dae.recover(OffDec);
        else
            OffDec = [];
        end
        OffDec = [OffDec;RealCrossover(Parent1Dec(~Site,:),Parent2Dec(~Site,:))];
        OffDec = RealMutation(OffDec,Problem.lower,Problem.upper);
        OffDec(:,Problem.encoding==4) = 1;
    else
        OffDec = ones(size(OffMask));
    end
end

function Offspring = BinaryCrossover(Parent1,Parent2)
% Uniform crossover

    k = rand(size(Parent1)) < 0.5;
    Offspring1    = Parent1;
    Offspring2    = Parent2;
    Offspring1(k) = Parent2(k);
    Offspring2(k) = Parent1(k);
    Offspring     = [Offspring1;Offspring2];
end

function Offspring = BinaryMutation(Offspring)
% Bitwise mutation

    Site = rand(size(Offspring)) < 1/size(Offspring,2);
    Offspring(Site) = ~Offspring(Site);
end

function Offspring = RealCrossover(Parent1,Parent2)
% Simulated binary crossover

    disC  = 20;
    [N,D] = size(Parent1);
    beta  = zeros(N,D);
    mu    = rand(N,D);
    beta(mu<=0.5) = (2*mu(mu<=0.5)).^(1/(disC+1));
    beta(mu>0.5)  = (2-2*mu(mu>0.5)).^(-1/(disC+1));
    beta = beta.*(-1).^randi([0,1],N,D);
    Offspring = [(Parent1+Parent2)/2+beta.*(Parent1-Parent2)/2
                 (Parent1+Parent2)/2-beta.*(Parent1-Parent2)/2];
end

function Offspring = RealMutation(Offspring,Lower,Upper)
% Polynomial mutation

    disM  = 20;
    [N,D] = size(Offspring);
    Lower = repmat(Lower,N,1);
    Upper = repmat(Upper,N,1);
    Site  = rand(N,D) < 1/D;
    mu    = rand(N,D);
    temp  = Site & mu<=0.5;
    Offspring       = min(max(Offspring,Lower),Upper);
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                      (1-(Offspring(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp = Site & mu>0.5;
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                      (1-(Upper(temp)-Offspring(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
    Offspring       = min(max(Offspring,Lower),Upper);
end


function [ output_args ] = test_gmm(dataset)
%TEST_GMM Summary of this function goes here
%   Detailed explanation goes here
addpath('../NCM');
addpath('../common');

close all

% select dataset - '21' for Pavia University, '62' for the gulfport dataset
% '00' for generating unsupervised synthetic dataset
% '001' or '002' for running on the unsupervised synthetic image 1 or 2
if nargin < 1
    dataset = '001';
end

%% load dataset
SNR = 60;
% asphalt, basalt, concrete, conifer, grass, limestone, quartzite
chosen = [6,2,3,1];
rows = 60;
cols = 60;

options = struct('reduced_dim',10);
options.convergence_thresh = 0.0001;
options.show_fig = 0;

switch dataset
    case '00'
        Y_noise = 1e-3;
        Y_noise = rand(200,1) * Y_noise;

        gen_im_opts = [];
        gen_im_opts.chosen = chosen;
        gen_im_opts.noise_sigma = Y_noise;

        if 0 % synthetic unsupervised A
%             [I,E_gt,A_gt,names,wl,ws_gt,mus_gt,sigmas_gt,Y_noise] = ...
%                 generate_toy_image_end_var(rows, cols, Y_noise, chosen, 1);
            [I,E_gt,A_gt,names,wl,Y_noise,extra] = generate_toy_image_unified(...
                rows, cols, 'gmm', 'quadrant', 0, gen_im_opts);
            ws_gt = extra.ws;
            mus_gt = extra.mus;
            sigmas_gt = extra.sigmas;
            save('toy_image_end_var_3.mat','I','E_gt','A_gt','names','wl',...
                'ws_gt','mus_gt','sigmas_gt','Y_noise');
        elseif 0 % synthetic unsupervised B
            gen_im_opts.material_gaussian_centers_normalized = [0.5,0.25;...
                0.25,0.75; 0.75,0.75]; % (x,y)
            gen_im_opts.material_gaussian_sigma_normalized = 0.25;
            gen_im_opts.material_num_samples = 150;
            gen_im_opts.material_shape_width = 1.5;
            gen_im_opts.material_shape_width_sd = 0.5;
            gen_im_opts.material_gaussian_coefficient = 10;

            [I,E_gt,A_gt,names,wl,Y_noise,extra] = generate_toy_image_unified(...
                rows, cols, 'gmm', 'gaussian_dotted', 0, gen_im_opts);
            ws_gt = extra.ws;
            mus_gt = extra.mus;
            sigmas_gt = extra.sigmas;
            save('toy_image_end_var_3_B.mat','I','E_gt','A_gt','names','wl',...
                'ws_gt','mus_gt','sigmas_gt','Y_noise');
        else
%             load('toy_image_end_var_3.mat');
            load('toy_image_end_var_3_B.mat');
        end

%         noise_sigmas = noise_est_roger(I);
%         disp('The error of noise estimation is ');
%         mdiff(Y_noise,noise_sigmas);
%
%         noise_sigmas = noise_est_mlr(I);
%         mdiff(Y_noise,noise_sigmas);

        M = 4;
        options.beta1 = 5;
        options.beta2 = 5; % 0.005
        options.shrink_size = 5;
        options.beta2_decay = 0.05;
    case '001'
        load('toy_image_end_var_3.mat');

        M = 4;
        options.beta1 = 5;
        options.beta2 = 5; % 0.005
        options.shrink_size = 5;
        options.beta2_decay = 0.05;
    case '002'
        load('toy_image_end_var_3_B.mat');

        M = 4;
        options.beta1 = 0.1;
        options.beta2 = 0.1; % 0.005
        options.shrink_size = 5;
        options.beta2_decay = 0.05;
    case '01'
        load('toy_image_end_var_snr60_1111.mat');
        M = 4;
        options.beta1 = 2000;
        options.beta2 = 50; % 2e4
        options.eta = 0.05;
        options.rho1 = 5;
    case '2'
%         [I,R_gt,A_gt,names,wl] = load_pavia_university;
        load('../../data/PaviaUniversity_corrected.mat');
        % remove gravel and bitumen
        R_gt([3,7],:) = [];
        names([3,7]) = [];
        if 1
            A_gt(:,:,[3,7]) = [];
        else
            to_be_merged = [8,1];
            merged = [3,7];
            for j = 1:2
                A1 = A_gt(:,:,to_be_merged(j));
                A2 = A_gt(:,:,merged(j));
                A3 = A2 + A1;
                A_gt(:,:,to_be_merged(j)) = A3;
            end
            A_gt(:,:,[3,7]) = [];
        end
        M = 7;

        options.beta1 = 0.5;
        options.beta2 = 0.5; % 2e4
        options.rho1 = 0.01;
        options.rho2 = 0;
        options.shrink_size = 2;
        options.beta2_decay = 0.5;

    case '21'
        load('../../data/PaviaUniversity_A.mat');
        A_gt = double(A_gt);
        M = 5;

        options.beta1 = 5;
        options.beta2 = 5; % 2e4
        options.rho1 = 0;
        options.rho2 = 0;
        options.shrink_size = 2;
        options.beta2_decay = 0.05;
    case '3'
        [I,R_gt,A_gt,names,wl] = load_neon;
    case '6'
        [I,R_gt,A_gt,names,wl] = load_gulfport;
    case '62'
        load('../../data/muufl_gulfport_B.mat');
        M = 5;

%         remove_bands = [1:5,72-9:72];
%         [I,R_gt,wl] = remove_noisy_bands(I,R_gt,wl,remove_bands);

        options.beta1 = 5;
        options.beta2 = 5; % 2e4
        options.rho1 = 0;
        options.rho2 = 0;
        options.shrink_size = 1;
        options.beta2_decay = 0.05;
    otherwise
end
if exist('rgb','var') && ~isempty(rgb)
    I1 = rgb;
else
    I1 = retrieve_rgb(I,wl);
end
figure('name','RGB image of the original image');
imshow(I1);

[Y,A_gt,rows,cols] = reshape_hsi(I,A_gt);
[N,B] = size(Y);

if exist('ws_gt','var') && ~isempty(ws_gt)
    endmember_scatter_plot_end_var(Y,ws_gt,mus_gt,sigmas_gt,names);
    R_gt = zeros(M,B);
    for j = 1:M
        R_gt(j,:) = mean(Y(A_gt(:,j)==1,:), 1);
    end
else
    endmember_scatter_plot(Y,R_gt,{'Ground Truth'});
end
set(gcf,'name','Scatter plot of the original image with ground truth');

if exist('ws_gt','var') && ~isempty(ws_gt)
    opts = struct('show_approx',1,'w_jk',{ws_gt},'mu_jk',{mus_gt},'sigma_jk',...
        {sigmas_gt},'legend_names',{{'Ground truth GMM'}});
else
    opts = struct('legend_names',{{}});
end
hist_end_var(Y,A_gt,names,1,opts);
set(gcf,'name','Histogram of the ground truth pure pixels vs GMM');

I1 = reshape(Y, [rows, cols, B]);

D = 0.001^2 * eye(B);
options.D = D;

% [N,B] = size(Y);
% options.A_gt = A_gt;
% options.fix_A = 1;
% options.fix_sigma_jk = 1;

[A,R,w_jk,mu_jk,sigma_jk,extra] = gmm_hu(I1,M,options);
E = gmm_hu_endmember(I1,A,D,w_jk,mu_jk,sigma_jk);

%% Permute the results to accord with the GT for comparison
[error_M,error_A,best_p] = compare_2_endmembers(R_gt, R, A_gt, A, ...
    rows,cols,names,wl,1);

A = A*best_p';
R = best_p*R;
for i = 1:size(E,3)
    E(:,:,i) = best_p * E(:,:,i);
end
w_jk = w_jk(best_p*(1:M)');
mu_jk = mu_jk(best_p*(1:M)');
sigma_jk = sigma_jk(best_p*(1:M)');

if options.show_fig % Play the movie
    replay_scatter_abund(extra.frames_scatter, extra.frames_abund);
end

save(['result_gmm_',dataset,'.mat'],'A','R','E','w_jk','mu_jk','sigma_jk');

opts = struct('show_approx',1,'w_jk',{w_jk},'mu_jk',{mu_jk},'sigma_jk',...
    {sigma_jk},'legend_names',{{'Estimated distribution'}});
hist_end_var(Y,A_gt,names,1,opts);

function [ws,mus,sigmas] = create_endmember_params_1_1_1_1(E)
[M,B] = size(E);
ws = cell(1,M);
mus = cell(1,M);
sigmas = cell(1,M);

for j = 1:M
    ws{j} = 1;
    mus{j} = E(j,:);
    sigmas{j} = 0.01^2*eye(B);
end

% endmember variability of basalt
idx = 2;
major_dir = ones(B,1);
major_dir = major_dir/sqrt(sum(major_dir.^2));
sigmas{idx} = 0.01^2*eye(B) + 0.05^2*major_dir*major_dir';

% endmember variability of concrete
idx = 3;
major_dir = ones(B,1);
major_dir = major_dir/sqrt(sum(major_dir.^2));
sigmas{idx} = 0.01^2*eye(B) + 0.03^2*major_dir*major_dir';


function msm_to_mm_coordinate_integer_skewsymmetric ( output_filename, a )

%*****************************************************************************80
%
%% MSM_TO_MM_COORDINATE_INTEGER_SKEWSYMMETRIC writes a "matrix coordinate integer skew-symmetric" Matrix Market file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the name of the file to which the information
%    should be written.
%
%    Input, sparse matrix A, the NROW by NCOL matrix, stored in MATLAB sparse
%    matrix format, which is to be written to the file.
%
  [ nrow, ncol ] = size ( a );
  nnzeros = nnz ( a );

  fid = fopen ( output_filename, 'wt+' );

  if ( fid < 0 );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MSM_TO_MM_COORDINATE_INTEGER_SKEWSYMMETRIC - Fatal error!\n' );
    fprintf ( 1, '  Cannot open the output file.\n' );
    error ( 'MSM_TO_MM_COORDINATE_INTEGER_SKEWSYMMETRIC - Fatal error!');
  end;

  fprintf ( fid, '%%%%MatrixMarket matrix coordinate integer skew-symmetric\n');
  fprintf ( fid, '%%%%  Created by MSM_TO_MM_COORDINATE_INTEGER_SKEWSYMMETRIC.M\n' );
  fprintf ( fid, '  %d  %d  %d\n', nrow, ncol, nnzeros );

  for j = 1 : ncol
    [ rows, temp, vals ] = find ( a(:,j) );
    sz = size ( rows );
    for k2 = 1 : sz
      if ( j < rows(k2) )
        fprintf ( fid, '  %d  %d  %d\n', rows(k2), j, vals(k2) );
      end
    end

  end

  fclose ( fid );

  return
end


clear all;
clc;

addpath util;

load('profile.mat');

count=length(profile);

pts_num = 39;

% for i=1:length(profile)
%     profile{i}.tmp=LoadPTS(sprintf('./profile/%s/%s.pts', profile{i}.status, profile{i}.name));
% end

gt=zeros(pts_num,2,count);
for i=1:length(profile)
    gt(:,:,i)=profile{i}.gt;
end

% num = 5;
num = 4;

spacing=0.0005;
sampling = 0:spacing:0.35;
x = sampling;
y = repmat(x',1,num);
profile_errs=cell(num,1);
det=zeros(pts_num,2,count);

% participants = {'yang','he','wu','deng','tmp'};
participants = {'yang','he','wu','deng'};

for iter = 1:num
    for i=1:length(profile)
        det(:,:,i)=profile{i}.(participants{iter});
    end
    profile_errs{iter} = compute_diag_error( gt, det );
    for i=1:numel(sampling)
        y(i,iter) = sum(profile_errs{iter} < sampling(i)) / numel(profile_errs{iter});
    end
end

title = 'Menpo Profile Test Set';

% methods={'J. Yang et al','Z. He et al', 'W. Wu et al', 'J. Deng et al','tmp'};
methods={'J. Yang et al','Z. He et al', 'W. Wu et al', 'J. Deng et al'};

colors = lines(num);

x_limit = 0.03;

linewidth = 3;

fontsize = 12;

plot_ced(x, y, methods, strrep(title,'_',' '), ...
         x_limit, colors, linewidth, fontsize);

ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset;
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];

fig = gcf;
fig.PaperPositionMode = 'auto';
fig_pos = fig.PaperPosition;
fig.PaperSize = [fig_pos(3) fig_pos(4)]* 1.05;

print(fig,'-dpdf',[ title '.pdf']);


disp('Starting tests...')
test_digamma
%test_gammaln
test_trigamma
test_int_hist
%test_inv_posdef
test_java
test_logmulexp
test_logsumexp
test_mutable
%test_ndsum
%test_normcdf
test_normpdf
test_randgamma
test_randbeta
test_randbinom
test_randwishart
test_repmat
test_row_sum
test_sameobject
test_scale
test_solve_tri
test_sorted
test_sqdist
disp('All tests completed')


function rot3daxes(h,as,l,ang,axSym,killLines)
% rot3daxes preforms a simple rotation of an axis set created by make3daxes.
%
% rot3daxes(h,as,l,ang) rotates the surface objects with
% handles in array (as) in figure (h) by an angle (ang) about the axis
% defined by array (l).  The handles are assumed to be of surface objects
% generated by the function make3daxes and the angle is assumed to be in
% degrees.
%
% rot3daxes(...,axSym) plots the previous axis positions with linesytle
% axSym.
%
% rot3daxes(...,killLines) removes previous line objects from the figure,
% when killLines is set to true.
%
% Note: This function is intended to be called by animEuler.m
%
% Examples:
%     %rotate axes 45 degrees about z axis.
%     rot3daxes(h,as,[0,0,1],45,'--')

% Written by Dmitry Savransky 29 April 2009

if ~exist('axSym','var') || isempty(axSym)
    axSym = '--';
end
if ~exist('killLines','var')
    killLines = false;
end
%set figure and remove any previous line objects if requested
figure(h);

if killLines
    oldObjs = findobj(h,'Type','line');
    if ~isempty(oldObjs), delete(oldObjs); end
end

%plot orig axes
hold on
col = ['r','g','b'];
for j=1:3
    a = get(as(j),{'Xdata','Ydata','Zdata'});
    plot3(mean(a{1},2),mean(a{2},2),mean(a{3},2),[col(j),axSym],'LineWidth',2);
end

for j=1:20
    for i=1:3
        rotate(as(i),l,ang/20,[0,0,0])
    end
    pause(0.1);
end
% axis equal;
% axis([-1 1 -1 1 -1 1])
% view(45,15)
% grid on


function net = alexnet(labelNum,lossWeight,opts)
channel_num=256;
mag=0.1;
netname='alexnet';

partRate=1;
textureRate=0;
output_num=labelNum;
net=load('../nets/imagenet-caffe-alex.mat');

net.layers=net.layers(1:end-7);
net.layers{end-1}.learningRate=[1,5];
net=add_block_mask(net,lossWeight, opts,'6',3,3,256,channel_num,1,1,partRate,textureRate,mag); %%%%%%%%%%%%%%%%%
net.layers{end+1} = struct('type', 'pool', 'name', 'pool6', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;
net=add_block_mask(net,lossWeight,opts,'7',6,6,channel_num,4096,1,0,partRate,textureRate,mag); %%%%%%%%%%%%%%%%%

net = add_dropout(net, opts, 'fc-7') ;
net = add_block(net, opts, 'fc-8', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, 'fc-8') ;

net = add_block(net, opts, '9', 1, 1, 4096, output_num, 1, 0) ;
net.layers(end) = [] ;
net.meta.classes.name={'pos'};
net.meta.classes.description={'pos'};
net.meta.normalization.imageSize=net.meta.normalization.imageSize(1:3);
net.meta.netname=netname;
end


function [voxels,scale,translate] = read_binvox(filename)
         fid = fopen(filename,'r');
         % read header
         tline = fscanf(fid,'%s',1);
         if ~strcmp(tline,'#binvox')
             fprintf(['Error: first line reads [' tline '] instead of [#binvox]\n']);
         end
         version = fscanf(fid,'%d',1);
%          fprintf('reading binvox version %d\n',version);
         depth = -1;
         done = 0;
         while (~feof(fid)&&~done)
            tline = fscanf(fid,'%s',1);
            if strcmp(tline,'data')
                done = 1;
            elseif strcmp(tline,'dim')
                A=fscanf(fid,'%d',3);
                width = A(1);
                height= A(2);
                depth = A(3);
            elseif strcmp(tline,'translate')
                translate = fscanf(fid,'%f',3);
            elseif strcmp(tline,'scale')
                scale =fscanf(fid,'%f',1);
            end
         end

         scale = scale/width;
         totalsize = width * height * depth;
         voxels = zeros(width, height, depth);
         % read voxel data
         index = 0;end_index = 0;nr_voxels = 0;

         B = fread(fid,1,'uint8'); %read the linefeed char
         while((end_index < totalsize) && ~feof(fid))
              B = fread(fid,2,'uint8');
              value = B(1);
              count = B(2);
              end_index = index + count;

              if (end_index >=totalsize)
                  break;
              end

              for i=index:min(totalsize,end_index)
                  voxels(i+1) = value; % matlab index starts 1
              end

              if (value)
                  nr_voxels = nr_voxels+count;
              end
              index = end_index;
         end
         % matlab's strange ordering
         voxels = permute(voxels,[3,1,2]);
         fclose(fid);
end


function DCM = spm_dcm_erp_update(DCM,oldDCM,fields)
% Set priors over DCM model parameters for Bayesian updating
% FORMAT DCM = spm_dcm_erp_update(DCM,oldDCM,fields)
%
% DCM    - DCM structure to be updated
% oldDCM - inverted DCM with posterior moments
% fields - character array of fields to be updated: e.g.,{'A','B'}
%__________________________________________________________________________
% Copyright (C) 2013 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_erp_update.m 5801 2013-12-10 18:45:20Z guillaume $

% get prior structure for the sort of model
%==========================================================================
try, model = DCM.options.model; catch, model = 'CMC'; end

% get the posterior (expectation and covariance) moments of parameters
%--------------------------------------------------------------------------
Ep      = oldDCM.Ep;
Cp      = spm_unvec(diag(oldDCM.Cp),Ep);

% prior moments of parameters
%--------------------------------------------------------------------------
[pE,pC] = spm_dcm_neural_priors(DCM.A,DCM.B,DCM.C,model);

% fill-in prior moments
%--------------------------------------------------------------------------
for i = 1:length(fields)

    pE.(fields{i}) = Ep.(fields{i});
    pC.(fields{i}) = Cp.(fields{i});

end

% they surprising model structure
%==========================================================================
DCM.M.pE = pE;
DCM.M.pC = pC;


function [X]=tt_minres_selfprec2(A,  eps, varargin)
%Computation of the approximate TT-matrix inverse using self-prec method
%   [X]=TT_MINRES_SELFPREC2(A,EPS,OPTIONS) Computation of the approximate
%   TT-matrix inverse using Saad self-prec method. Options are provided
%   in form 'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2
%   and so on. The parameters are set to default (in brackets in the
%   following) The list of option names and default values are:
%       o matvec - type of the local matvec [ {mm+compr} | mmk2 ]
%       o max_rank - maximal TT-rank bound [1000]
%       o prec_type - left or right inversion [ {left} | right ]
%       o maxit - maximal number of iterations [10]
%       o tol - the requested inversion tolerance [EPS]
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

% matvec='mmk2';
matvec='mm+compr';
max_rank=1000;
prec_type='left';
tol=eps;
maxit=10;

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'matvec'
            matvec=varargin{i+1};
        case 'max_rank'
            max_rank=lower(varargin{i+1});
        case 'prec_type'
            prec_type=varargin{i+1};
        case 'maxit'
            maxit=varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

%d=max(size(A));
d=ndims(A);
ns=A.n;
%ns=tt_size(A);

I=tt_eye(ns, d); I=tt_matrix(I);
if (strcmp(prec_type, 'left'))
   % X=tt_scal(tt_transp(A), 1e-15);
   X=(1e-15)*conj(A');
else
  %  X=tt_scal(I, 1e-150);
   X=1e-150*I;
end;
normf=norm(I);
%normf=sqrt(tt_dot(tt_mat_to_vec(I), tt_mat_to_vec(I)));
err=1;
err_old=2;
sp=0;

for iout=1:maxit
    if (strcmp(matvec, 'mmk2'))
        resid=tt_mmk2(A, X, eps, max_rank);
    else
        %resid=tt_mm(A,X);
        resid=round(A*X,eps,max_rank*2);
        %resid=tt_mat_compr(resid, eps,max_rank*2);
    end;
    %resid=ttm_add(I, tt_scal(resid, -1));
    resid=I-resid;

    if (iout>1)||(strcmp(prec_type, 'left'))
        if (strcmp(matvec, 'mmk2'))
            Xresid=tt_mmk2(X, resid, eps,max_rank);
        else
            %Xresid=tt_mm(X,resid);
            Xresid=round(X*resid,eps,max_rank*2);
            %Xresid=tt_mat_compr(Xresid, eps,max_rank*2);

        end;
    else
        Xresid=resid;
    end;

    max_xrrank=max(rank(Xresid));

    if (strcmp(prec_type, 'left')) % we use left preconditioner (igmres)
        if (strcmp(matvec, 'mmk2'))
            w=tt_mmk2(A, Xresid, eps,max_rank);
            w=tt_mmk2(X, w, eps,max_rank);
        else
            w=round(A*Xresid,eps,max_rank);
            w=round(X*w,eps);
            %w=tt_mm(A,Xresid);
            %w=tt_mat_compr(w, eps,max_rank);
            %w=tt_mm(X,w);
%             w=tt_mat_compr(w, eps);
        end;
        %w = tt_mat_to_vec(w);
        %beta=sqrt(tt_dot(tt_mat_to_vec(Xresid),tt_mat_to_vec(Xresid)));
        beta=norm(Xresid);
        wr=dot(w,Xresid);
        %wr = tt_dot(w, tt_mat_to_vec(Xresid));
        ww=dot(w,w);

        H=zeros(2,1);
        H(1,1)=wr/(beta^2);
        H(2,1)=sqrt(ww/(beta^2)-wr^2/(beta^4));
        rhs=H(1,1)*beta;

        y = rhs/(H'*H);
        err = err*norm(H*y-[beta; 0])/beta;
        y=y/beta;

%         y = tt_dot(w,w);
%         if (y~=0)
%             y = tt_dot(w, tt_mat_to_vec(Xresid))/y;
%         else
%             y=1;
%         end;
    else % Use right preconditioner and fgmres
        if (strcmp(matvec, 'mmk2'))
            w=tt_mmk2(A, Xresid, eps,max_rank);
        else
            w=A*Xresid;
            w=round(w,eps);
%             w=tt_mat_compr(w, eps);
        end;
        w=tt_tensor(w);
        %w = tt_mat_to_vec(w);
        beta=norm(resid);
        %beta=sqrt(tt_dot(tt_mat_to_vec(resid),tt_mat_to_vec(resid)));
        wr=dot(w,tt_tensor(resid));
        ww=dot(w,w);
        %wr = tt_dot(w, tt_mat_to_vec(resid));
        %ww = tt_dot(w,w);

        H=zeros(2,1);
        H(1,1)=wr/(beta^2);
        H(2,1)=sqrt(ww/(beta^2)-wr^2/(beta^4));
        rhs=H(1,1)*beta;

        y = rhs/(H'*H);
        err = norm(H*y-[beta; 0])/normf;
        y=y/beta;

%         y2=ww;
%         if (y~=0)
%             y2 = wr/y2;
%         else
%             y2=1;
%         end;
    end;

    max_wrank=max(rank(w));

    %Xresid=tt_mat_to_vec(Xresid);
    Xresid=tt_tensor(Xresid);
    X=tt_tensor(X);
    %X=tt_mat_to_vec(X);
    %if (err<err_old)
        %X = tt_axpy(1, X, y, Xresid, eps, max_rank);
        X=round(X+y*Xresid,eps,max_rank);

        %err_old=err;
    %else
    %    sp=sp+1;
    %end;
    %max_xrank=max(tt_ranks(X));
    max_xrank=max(rank(X));
    %X = tt_vec_to_mat(X, ns, ns);
    X=tt_matrix(X,ns,ns);
%     if (strcmp(matvec, 'mmk2'))
%         resid=tt_mmk2(A, X, eps, max_rank);
%     else
%         resid=tt_mm(A, X);
% %         resid=tt_mat_compr(resid, eps);
%     end;
%     resid=ttm_add(I, tt_scal(resid, -1));
%     resid=tt_mat_to_vec(resid);
%
%     err_real=sqrt(tt_dot(resid,resid))/normf;
    err_real=0;
    fprintf('iter [%d], res_real=%3.3e, x_rank=%d, Xr_rank=%d, w_rank=%d, err=%3.3e\n', iout, err_real, max_xrank, max_xrrank, max_wrank, err);
    if (err<tol)||(sp>0)
        break;
    end

end
%keyboard;


% std_findsameica() - find groups of datasets with identical ICA decomposiotions
%
% Usage:
%        >> clusters = std_findsameica(ALLEEG);
% Inputs:
%   ALLEEG  - a vector of loaded EEG dataset structures of all sets in the STUDY set.
%
% Outputs:
%   cluster - cell array of groups of datasets
%
% Authors:  Arnaud Delorme, SCCN, INC, UCSD, July 2009-

% Copyright (C) Arnaud Delorme, SCCN, INC, UCSD, arno@sccn.ucsd.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 2 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% Coding notes: Useful information on functions and global variables used.

function cluster = std_findsameica(ALLEEG);

cluster = { [1] };
for index = 2:length(ALLEEG)

    found = 0;
    for c = 1:length(cluster)
        if all(size(ALLEEG(cluster{c}(1)).icaweights) == size(ALLEEG(index).icaweights))
            %if isequal(ALLEEG(cluster{c}(1)).icaweights, ALLEEG(index).icaweights)
            if sum(sum(abs(ALLEEG(cluster{c}(1)).icaweights-ALLEEG(index).icaweights))) < 2e-5
                cluster{c}(end+1) = index;
                found = 1;
                break;
            end;
        end;
    end;
    if ~found
        cluster{end+1} = index;
    end;
end;


function [training_data, mappingScore] = prepareTrainingData(model,printLevel,params)
% given a standard COBRA model, add thermodynamic data to it using
% the Component Contribution method
%
% INPUTS
% model
%
%
% OPTIONAL INPUTS
% params.use_cached_kegg_inchis
% params.use_model_pKas_by_default
% params.uf                          maximum uncertainty
%
%
% OUTPUTS:
% model
% .DfG0                 m x 1 array of component contribution estimated
%                       standard Gibbs energies of formation.
% .covf                 m x m estimated covariance matrix for standard
%                       Gibbs energies of formation.
% .uf                   m x 1 array of uncertainty in estimated standard
%                       Gibbs energies of formation. uf will be large for
%                       metabolites that are not covered by component
%                       contributions.

if ~exist('printLevel','var')
    printLevel = 0;
end
if ~exist('param','var')
    use_cached_kegg_inchis=true;
    use_model_pKas_by_default=true;
else
    if ~isfield(params,'use_cached_kegg_inchis')
        use_cached_kegg_inchis = true;
        % use_cached_kegg_inchis = false;
    else
        use_cached_kegg_inchis=params.use_cached_kegg_inchis;
    end
    if ~isfield(params,'use_model_pKas_by_default')
        use_model_pKas_by_default = true;
    else
        use_model_pKas_by_default=params.use_model_pKas_by_default;
    end
end
% load the training data (from TECRDB, Alberty, etc.)
training_data = loadTrainingData();

% get the InChIs for all the compounds in the training data
% (note that all of them have KEGG IDs)
kegg_inchies = getInchies(training_data.cids, use_cached_kegg_inchis);
inds = ismember(kegg_inchies.cids, training_data.cids);
training_data.std_inchi = kegg_inchies.std_inchi(inds);
training_data.std_inchi_stereo = kegg_inchies.std_inchi_stereo(inds);
training_data.std_inchi_stereo_charge = kegg_inchies.std_inchi_stereo_charge(inds);
training_data.nstd_inchi = kegg_inchies.nstd_inchi(inds);

% use the chemical formulas from the InChIs to verify that each and every
% reaction is balanced.
training_data = balanceReactionsInTrainingData(training_data);

% get the pKas for the compounds in the training data (using ChemAxon)
training_data.kegg_pKa = getTrainingDatapKas(training_data);

% match between the compounds in the model and the KEGG IDs used in the
% training data, and create the group incidence matrix (G) for the
% combined set of all compounds.
[training_data, mappingScore] = createGroupIncidenceMatrix(model, training_data);

% apply the reverse Legendre transform for the relevant training observations (typically
% apparent reaction Keq from TECRDB)
training_data = reverseTransformTrainingData(model, training_data, use_model_pKas_by_default);


function [x, y] = CheckRC(x, y, im)
y = max(y, 1);
y = min(y, size(im, 1));
x = max(x, 1);
x = min(x, size(im, 2));


classdef OptimizationMetricsPrinter_MMA < OptimizationMetricsPrinter

    methods (Access = protected)

        function printConvergenceVariables(obj,fid)
            kktnorm = obj.optimizer.historicalVariables.kktnorm;
            fprintf(fid,'Optimality tolerance: %f \n',kktnorm);
        end

    end

end


function b = iswholenum(x)
% Reason for this function is that isinteger() looks at the class of the number
% rather it's actual value. So isinteger(4) will return true but integer(4.0) will
% be false even though 4.0 = 4. This is a problem.
b = false;
if ~ismember(class(x), {'double','single','int8','uint8','int16','uint16','int32','uint32'});
    return;
end
b = (x == floor(x));


% original data folder
hrdir = '/scratch/mfr/DIV2K/DIV2K_train_HR/';
% configuration
p = 128;

% stride
stride = 16;

fnum = 1;
files = dir([hrdir '/' '*.png']);
savedir = 'train';

% 3 channels
h5create([savedir '/hr.h5'],  '/data', [p p 3 Inf], 'Datatype', 'single', 'ChunkSize', [p p 3 1]);

for k = 1:length(files)
    file = files(k).name;
    image = imread([hrdir '/' file]);
    image = im2double(image);

    [w h c] = size(image);
    i = 1;
    j = 1;

    while ((j + p <= h))
        hrpatch = image(i:i+p-1, j:j+p-1, :);

        h5write([savedir '/hr.h5'],  '/data', single(hrpatch),  [1 1 1 fnum], [p p 3 1]);

        fnum = fnum + 1;

        if (mod(fnum, 1000) == 0)
            sprintf('already generated %d patches\n', fnum)
        end

        j = j + stride;
        if (i + p > w)
            i = 1;
            j = j + stride;
        end
    end
end
sprintf('generated %d patches\n', fnum)


function test_bug1916

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_scalingfactor

if isempty(which('ft_scalingfactor'))
  % it is in the fieldtrip/private directory
  [ftver, ftpath] = ft_version;
  cd(fullfile(ftpath, 'private'));
end

old = 'mT/mm';
new = 'fT/km';

% the first time for any conversion takes 150 ms
stopwatch = tic;
factor = ft_scalingfactor(old, new);
toc(stopwatch)

n = 300;
old = repmat({old}, 1, n);
new = repmat({new}, 1, n);

% this used to work fine, because all inputs are the same
stopwatch = tic;
factor = cellfun(@ft_scalingfactor, old, new);
toc(stopwatch)

n = 100;
old = repmat({'T/cm' 'T/cm' 'T'}, 1, n);
new = repmat({'fT/m' 'fT/m' 'fT'}, 1, n);

% this used to be very slow, because of the A-A-B pattern
stopwatch = tic;
factor = cellfun(@ft_scalingfactor, old, new);
toc(stopwatch)


% general script for non-systematic LDPC simulation

% Things to Remember - Steps to follow ALWAYS !!!!!!!!
% 0. This file is good for non-systematic H only (errors calculated for all n)
% 1. Load H
% 2. Define SNR Range
% 3. Set Maximum number of iterations
% 4. Set Maximum number of codeword-errors for which to run simulation
% 5. Select decoder - Matlab or C-based

clear all
close all
clc
tic

%get H from somewhere , change R accordingly !!
load 128x256regular.mat H
% load 128x256regular_v6.mat  %for users of Matlab 6.5

ind=find(H==1);
[r,c]=ind2sub(size(H),ind);
[rows,cols]=size(H);
h=sparse(H);                         % for use with Matlab-based LDPC Decoder
n=cols;
k=n-rows;

% Find
% 1: maximum check degree
% 2: column indeces in each row which contain '1'
% 3: maximum variable degree
% 4: find column indeces in each row which contain '1'
[max_check_degree,check_node_ones,BIGVALUE_COLS,max_variable_degree,variable_node_ones,BIGVALUE_ROWS]=one_finder(H);

rand('seed',584);
randn('seed',843);

dB=[0:3];                           % range of SNR values in dB
SNRpbit=10.^(dB/10);                % Eb/No conversion from dB to decimal
No_uncoded=1./SNRpbit;              % since Eb=1
R=k/n;                              % code rate
No=No_uncoded./R;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
max_iter=10;                   %maximum number of decoder iterations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
maximum_blockerror=30;          % maximum blockerrors per SNR point
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

blockerrors=0;
biterrors=0;
block=0;
FER=zeros(1,length(dB));        % array for Frame Error Rate
BER=zeros(1,length(dB));        % array for Channel Error Rate
block_array=zeros(1,length(dB));

for z=1:length(SNRpbit), % loop for testing over range of SNR values

    biterrors=0;
    blockerrors=0;
    block=0;

    while(blockerrors<maximum_blockerror)  %while loop

        %%%%%%%%%%%%%%% u is the codeword to be transmitted %%%%%%%%%%%%%%%%%%%
        u=zeros(1,cols);  %%all zero codeword
        tx_waveform=bpsk(u);

        sigma=sqrt(No(z)/2);
        rx_waveform=tx_waveform + sigma*randn(1,length(tx_waveform));

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%% Use C - based LDPC Decoder %%%%%%%%%%%%%%
        %%% Fastest C-based LDPC Decoder  %%%%

        gamma_n=(4/No(z))*rx_waveform;
        vhat=decode_ldpc_new(max_iter,gamma_n,check_node_ones,max_check_degree,BIGVALUE_COLS-1,variable_node_ones,max_variable_degree,BIGVALUE_ROWS-1,rows,cols);
        uhat=vhat;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% Slightly slower C-based LDPC Decoder  %%%%

%         gamma_n=(4/No(z))*rx_waveform;
%         sg=zeros(rows,cols);
%         sg(ind)=gamma_n(c);
%
%         [vhat,iteration]=decode_ldpc(No(z),H,max_iter,sg,gamma_n);
%         uhat=vhat;
%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%% Use Matlab-based LDPC Decoder %%%%%%%%%%%%%%
%
%         [vhat,iteration]=decode_ldpc_matlab(rx_waveform,No(z),h,rows,cols,ind,r,c,max_iter);
%         uhat=vhat;

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

        Errors=zeros(1,length(vhat));
        Errors(find(u~=vhat))=1;

        if sum(Errors)~=0
            blockerrors=blockerrors+1;

            if rem(blockerrors,5)==0; %save statistics after every 5 blockerrors

                ber=biterrors/(block*n);
                fer=blockerrors/block;

                s=sprintf('%dx%d_regular_results.txt',n-k,n);
                fid = fopen(s,'a');
                fprintf(fid,'\n');
                fprintf(fid,'%s %2.1EdB\n','SNR =',dB(z));
                fprintf(fid,'%s %6.3E\n','BER =',ber);
                fprintf(fid,'%s %6.3E\n','FER =',fer);
                fprintf(fid,'%s %d\n','blocks =',block);
                fprintf(fid,'%s %d\n','blockerrors =',blockerrors);
                fprintf(fid,'%s %d\n','biterrors =',biterrors);
                fclose(fid);
            end

        end

        biterrors=biterrors+sum(Errors);
        block=block+1;

    end     %while loop

    block_array(z)=block; %counting blocks per SNR point
    BER(z)=biterrors/(block*n);
    FER(z)=blockerrors/block;
    fprintf(1,'\n\n Simulation finished for SNR: %d \n',dB(z))

%     save results.mat dB BER FER block_array

end      % loop for testing over range of SNR values

semilogy(dB,BER,'b-o')
title('Bit Error Rate')
ylabel('BER')
xlabel('Eb/No (dB)')
grid
figure
semilogy(dB,FER,'b-o')
title('Frame Error Rate')
ylabel('FER')
xlabel('Eb/No (dB)')
grid

toc

%   Copyright (c) 2005 by Shaikh Faisal Zaheer, faisal.zaheer@gmail.com
%   $Revision: 3.0 $  $Date: 26/12/2005 $


% Code used in Vidaurre et al. (2017) PNAS
%
% Detailed documentation and further examples can be found in:
% https://github.com/OHBA-analysis/HMM-MAR
% This pipeline must be adapted to your particular configuration of files.
%%%%%%%%%%%%%%%%%%%%%%%%%
%% SETUP THE MATLAB PATHS AND FILE NAMES
mydir = '/home/diegov/MATLAB/';
addpath(genpath([ mydir 'HMM-MAR']))

K = 12; % no. states
repetitions = 3; % to run it multiple times (keeping all the results)
DirData = [mydir 'data/HCP/TimeSeries/hippocampus_data/'];
DirOut = [mydir 'HCP/HMM/out/hippocampus/'];
TR = 0.72;
use_stochastic = 1; % set to 1 if you have loads of data

N = 820; % no. subjects
f = cell(N,1); T = cell(N,1);
% each .mat file contains the data (ICA components) for a given subject,
% in a matrix X of dimension (4800time points by 50 ICA components).
% T{j} contains the lengths of each session (in time points)
for j=1:N
    f{j} = [DirData 's' num2str(j) '.mat'];
    load(f{j}); T{j} = [1200 1200 1200 1200];
end

options = struct();
options.K = K; % number of states
options.order = 0; % no autoregressive components
options.zeromean = 0; % model the mean
options.covtype = 'full'; % full covariance matrix
options.Fs = 1/TR;
options.verbose = 1;
options.standardise = 1;
options.inittype = 'HMM-MAR';
options.cyc = 500;
options.initcyc = 10;
options.initrep = 3;

% stochastic options
if use_stochastic
    options.BIGNbatch = round(N/30);
    options.BIGtol = 1e-7;
    options.BIGcyc = 500;
    options.BIGundertol_tostop = 5;
    options.BIGforgetrate = 0.7;
    options.BIGbase_weights = 0.9;
end

% We run the HMM multiple times
for r = 1:repetitions
    [hmm, Gamma, ~, vpath] = hmmmar(f,T,options);
    save([DirOut 'HMMrun_rep' num2str(r) '.mat'],'Gamma','vpath','hmm')
    disp(['RUN ' num2str(r)])
end

%% Pull out metastates

for r = 1:repetitions
    figure(r)
    load([DirOut 'HMMrun_rep' num2str(r) '.mat'],'Gamma','hmm')
    subplot(1,2,1) % Figure 2B
    GammaSessMean = squeeze(mean(reshape(Gamma,[1200 4 N K]),1));
    GammaSubMean = squeeze(mean(GammaSessMean,1));
    [~,pca1] = pca(GammaSubMean','NumComponents',1);
    [~,ord] = sort(pca1);
    imagesc(corr(GammaSubMean(:,ord))); colorbar
    subplot(1,2,2) % Figure 2A
    P = hmm.P;
    for j=1:K, P(j,j) = 0; P(j,:) = P(j,:) / sum(P(j,:));  end
    imagesc(P(ord,ord),[0 0.25]); colorbar
    axis square
    hold on
    for j=0:13
        plot([0 13] - 0.5,[j j] + 0.5,'k','LineWidth',2)
        plot([j j] + 0.5,[0 13] - 0.5,'k','LineWidth',2)
    end
    hold off
end


function [Gamma,scale] = id_Gamma_inference(L,Pi,order)
% inference for independent samples (ignoring time structure)
% scale is the likelihood

L(L<realmin) = realmin;
Gamma = repmat(Pi,size(L,1),1) .* L;
Gamma = Gamma(1+order:end,:);
scale = sum(Gamma,2);
Gamma = rdiv(Gamma,scale);

end


filename='ImprovedBridge_hexahedra';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'IPOPT'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'STANDARD';
showBC = true;

nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function pos = getComPosition(obj)
    % Returns the symbolic representation of the robot manipulator
    %
    % Return values:
    % pos: the 3-Dimensional SO(3) position vector of the CoM of the system
    % @type SymExpression

    n_link = length(obj.Links);
    links = cell(1,n_link);
    for i=1:n_link
        links{i}.Mass = obj.Links(i).Mass;
        % links{i}.Inertia = obj.Links(i).Inertia;
        links{i}.gst0 = obj.Links(i).gst0;
        frame = obj.Links(i).Reference;
        while isempty(frame.TwistPairs)
            frame = frame.Reference;
            if isempty(frame)
                error('The coordinate system is not fully defined.');
            end
        end

        links{i}.TwistPairs = frame.TwistPairs;
        % links{i}.ChainIndices = frame.ChainIndices;
    end

    pos = eval_math_fun('ComputeComPosition',links);

end


function DCM = spm_meta_model(DCM)
% Meta-modelling of Bayes-optimal responses (Newton's method)
% FORMAT DCM = spm_meta_model(DCM)
%
% store estimates in DCM
%--------------------------------------------------------------------------
% DCM.M   - meta-model specification
%      M: [1 x m struct]   - hierarchical inference model (cf DEM.M)
%      G: [1 x s struct]   - generative process (for spm_ADEM or spm_ALAP)
%      U: [n x N double]   - n prior beliefs over N samples
%     pE: [1 x 1 struct]   - prior expectation of meta-model parameters
%     pC: [1 x 1 struct]   - prior variance of meta-model parameters
%
% DCM.xY  - data structure
%      y: [N x p double]   - N samples of a p-variate response
%     X0: [N x q double]   - q-variate confounds
%     dt: [1 x 1 double]   - size of time bin for each sample
%      Q: {[N x N double]} - precision component[s]
%
% DCM.xU  - input structure
%      u: [r x N double]   - r-variate input (hidden causes G in DEM)
%
% Computes (and stores in DCM_MM_???)
%--------------------------------------------------------------------------
% DCM.DEM - Inference (with MAP parameters)
% DCM.Ep  - conditional expectation
% DCM.Cp  - conditional covariances
% DCM.Eh  - conditional log-precision
% DCM.Ey  - conditional response
% DCM.F   - log-evidence
%
% This routine illustrates Bayesian meta modelling - the Bayesian inversion
% of a model of a Bayesian observer. This requires the specification of two
% models: An inference model used by the subject (specified by a DEM
% structure) and a meta-model (specified by a DCM structure). The inference
% model is completed by a response model to furnish the meta-model; where
% the response model takes the output of the (active) inference scheme
% specified by the DEM and generates an observed (behavioural or
% neurophysiological) response. Crucially either the inference model or
% the response model or both can have free parameters - that are optimised
% using Bayesian nonlinear system identification in the usual way.
%
% Although this routine is a function, it is expected that people will fill
% in the model-specific parts in a local copy, before running it.  The
% current example uses a model of slow pursuit and generates synthetic data
% (responses) to illustrate how it works. To replace these simulated data
% with real data, simply specify the DCM.xY (and xU fields) with
% empirical values. If other fields do not exist, exemplar fields will be filled in.
%
% The conditional density of the parameters and F values (log-evidence) can
% be used in the usual way for inference on parameters or Bayesian model
% comparison (as for other DCMs)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_meta_model.m 5788 2013-12-06 20:08:57Z karl $

% INFERENCE MODEL: DEM.M and DEM.G
%==========================================================================
try
    M = DCM.M;

catch

    % create illustrative (M,G) if not specified (EXAMPLE)
    %----------------------------------------------------------------------

    % hidden causes and states
    %======================================================================
    % x    - hidden states:
    %   x.o(1) - oculomotor angle
    %   x.o(2) - oculomotor velocity
    %   x.x(1) - target angle - extrinsic coordinates
    %
    % v    - causal states: force on target
    %
    % g    - sensations:
    %   g(1) - oculomotor angle (proprioception)
    %   g(2) - oculomotor velocity
    %   g(:) - visual input - intrinsic coordinates
    %----------------------------------------------------------------------

    % Set-up
    %======================================================================
    M(1).E.s = 1/2;                               % smoothness
    M(1).E.n = 4;                                 % order of
    M(1).E.d = 1;                                 % generalised motion

    % angular frequency of target motion (per time bin)
    %----------------------------------------------------------------------
    w  = 2*pi/32;

    % occlusion and visual mapping functions
    %----------------------------------------------------------------------
    occ = @(x) x < 1/2;
    vis = @(x) exp(-((-8:8)' - x.x(1) + x.o(1)).^2)*occ(x.x);

    % Generative model (M) (sinusoidal movement)
    %======================================================================
    % Endow the model with internal dynamics (a simple oscillator) so that is
    % recognises and remembers the trajectory to anticipate jumps in rectified
    % sinusoidal motion.

    % slow pursuit following with (second order) generative model
    %----------------------------------------------------------------------
    x.o = [0;0];                                  % motor angle & velocity
    x.x = 0;                                      % target location

    % level 1: Displacement dynamics and mapping to sensory/proprioception
    %----------------------------------------------------------------------
    M(1).f = @(x,v,P) [x.o(2); (v - x.o(1))/4 - x.o(2)/2; v - x.x];
    M(1).g = @(x,v,P) [x.o; vis(x)];
    M(1).x = x;                                   % hidden states
    M(1).V = exp(4);                              % error precision
    M(1).W = exp(4);                              % error precision

    % level 2: With hidden (memory) states
    %----------------------------------------------------------------------
    M(2).f  = @(x,v,P)[x(2); -x(1)]*v;
    M(2).g  = @(x,v,P) x(1);
    M(2).x  = [0; 0];                             % hidden states
    M(2).V  = exp(4);                             % error precision
    M(2).W  = exp(4);                             % error precision

    % level 3: Encoding frequency of memory states (U)
    %----------------------------------------------------------------------
    M(3).v = 0;
    M(3).V = exp(4);

    % generative model (G) (with no noise)
    %======================================================================

    % first level
    %----------------------------------------------------------------------
    G(1).f = @(x,v,a,P) [x.o(2); a/4 - x.o(2)/8; v - x.x];
    G(1).g = @(x,v,a,P) [x.o; vis(x)];
    G(1).x = x;                                  % hidden states
    G(1).U = sparse(1,[1 2],[1 1],1,19)*exp(4);  % motor gain

    % second level
    %----------------------------------------------------------------------
    G(2).v = 0;                                  % exogenous force
    G(2).a = 0;                                  % action force

end

% Check generative model
%--------------------------------------------------------------------------
M      = spm_DEM_M_set(M);

% Experimental input (C)
%==========================================================================
try
    xU    = DCM.xU;
    N     = size(xU.u,2);

catch

    % Sine wave cause (EXAMPLE)
    %----------------------------------------------------------------------
    N     = 64;                                 % length of data sequence
    xU.dt = 16;                                 % time step (ms)
    xU.u  = sin((1:N)*w).*((1:N) > 16);         % sinusoidal target motion
end

% Priors (U)
%==========================================================================
try
    U = DCM.U;

catch

    % (EXAMPLE) prior beliefs about target frequency (w)
    %----------------------------------------------------------------------
    U = zeros(1,N) + w;
end

% Data and confounds
%==========================================================================
try
    xY = DCM.xY;

catch

    % Generate simulated data (EXAMPLE)
    %----------------------------------------------------------------------

    % meta-model and TRUE parameters
    %----------------------------------------------------------------------
    MM.M    = M;
    MM.G    = G;
    MM.U    = U;

    P.G     = 2;
    P.W     = 4;
    [y DEM] = IS(P,MM,xU);

    % and simulate observation noise
    %----------------------------------------------------------------------
    xY.y    = y + spm_Q(1/2,N,1)*randn(N,1)/16;

    spm_figure('GetWin','Simulated respsone');
    spm_DEM_qU(DEM.qU,DEM.pU)

end

% DCT confounds
%==========================================================================
xY.X0  = spm_dctmtx(N,1);

% and [serial] correlations (precision components) AR model
%--------------------------------------------------------------------------
xY.Q   = {spm_Q(1/2,N,1)};

% META-MODEL parameters
%==========================================================================
% These parameterise the inference scheme function (IS) at the bottom of
% this script - this functions defines how the parameters are used and
% therefore optimised. Change this function to specify the meta-model

try
    pE    = DCM.pE;
    pC    = DCM.pC;

catch

    % prior expectations (EXAMPLE)
    %----------------------------------------------------------------------
    pE.G  = 1;
    pE.W  = 2;

    % prior covariance (EXAMPLE)
    %----------------------------------------------------------------------
    pC.G  = 1;
    pC.W  = 1;

end

% This completes the meta-model specification. The fields are now assembled
% and passed to spm_nlsi_GN (nonlinear system identification using Gauss-
% Newton-like gradient ascent).
%==========================================================================

% META-MODEL (MM)
%==========================================================================
MM.M   = M;
MM.G   = G;
MM.U   = U;

% hyperpriors (assuming about 99% signal to noise)
%--------------------------------------------------------------------------
hE     = 6 - log(var(spm_vec(xY.y)));
hC     = exp(-4);

% Meta-model
%--------------------------------------------------------------------------
MM.IS  = @IS;
MM.pE  = pE;
MM.pC  = pC;
MM.hE  = hE;
MM.hC  = hC;

% model inversion
%==========================================================================
MM.Nmax      = 16;
[Ep,Cp,Eh,F] = spm_nlsi_GN(MM,xU,xY);

% integrate (A)DEM scheme with MAP etimates
%--------------------------------------------------------------------------
[Ey,DEM] = IS(Ep,MM,xU);

% store estimates in DCM
%--------------------------------------------------------------------------
DCM.DEM = DEM;                  % Inference scheme (with MAP parameters)
DCM.M   = MM;                   % meta-model
DCM.xY  = xY;                   % data structure
DCM.xU  = xU;                   % input structure
DCM.Ep  = Ep;                   % conditional expectation
DCM.Cp  = Cp;                   % conditional covariances
DCM.Eh  = Eh;                   % conditional log-precision
DCM.Ey  = Ey;                   % conditional response
DCM.F   = F;                    % log-evidence

% save
%--------------------------------------------------------------------------
save(sprintf('DCM_MM_%s',date),'DCM', spm_get_defaults('mat.format'));

% and plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Parameter estimates');
subplot(2,1,1)
spm_plot_ci(Ep,Cp), hold on
Pp       = spm_vec(P);
pE       = spm_vec(pE);
plot(Pp(1),Pp(2),'.r' ,'MarkerSize',32), hold on
plot(pE(1),pE(2),'.g','MarkerSize',32), hold off
axis([0 4 0 8])
xlabel('Gain','FontSize',12)
ylabel('Precision','FontSize',12)
title('Posterior (blue), prior (green) and true (red) value','FontSize',16)

return

% inference scheme x = IS(p,M,U)
%==========================================================================
function [y,DEM] = IS(P,M,U)

% parameterise inference model (EXAMPLE)
%--------------------------------------------------------------------------
M.M(2).W = exp(P.W);

% reset random number generator (for action schemes)
%--------------------------------------------------------------------------
rng('default')

% Bayesian inference (spm_DEM, spm_LAP, spm_ADEM or spm_ALAP)
%--------------------------------------------------------------------------
DEM.M  = M.M;
DEM.G  = M.G;
DEM.U  = M.U;
DEM.C  = U.u;
DEM    = spm_ADEM(DEM);

% (Hidden) behavioural or electrophysiological response (EXAMPLE)
%--------------------------------------------------------------------------
x      = DEM.pU.x{1}(1,:);
y      = (P.G*x)';

return


function output = F_sparse_affine_transform(input, transform, bias, singlePrecision)

[D,M,N] = size(input);
if N>1
    input = reshape(input, D,M*N);
end

visible_nonzero_idx = find(sum(abs(input),2)>0);
visible_nonzero = full(input(visible_nonzero_idx,:));
if singlePrecision==1
    visible_nonzero = single(visible_nonzero);
end
output = bsxfun(@plus, transform(:,visible_nonzero_idx) * visible_nonzero, bias);    % only compute the nonzero elements

if N>1
    output = reshape(output, size(output,1), M,N);
end

end


%MSTRAJ Multi-segment multi-axis trajectory
%
% TRAJ = MSTRAJ(WP, QDMAX, TSEG, Q0, DT, TACC, OPTIONS) is a trajectory
% (KxN) for N axes moving simultaneously through M segment.  Each segment
% is linear motion and polynomial blends connect the segments.  The axes
% start at Q0 (1xN) and pass through M-1 via points defined by the rows of
% the matrix WP (MxN), and finish at the point defined by the last row of WP.
% The  trajectory matrix has one row per time step, and one column per
% axis.  The number of steps in the trajectory K is a function of the
% number of via points and the time or velocity limits that apply.
%
% - WP (MxN) is a matrix of via points, 1 row per via point, one column
%   per axis.  The last via point is the destination.
% - QDMAX (1xN) are axis speed limits which cannot be exceeded,
% - TSEG (1xM) are the durations for each of the K segments
% - Q0 (1xN) are the initial axis coordinates
% - DT is the time step
% - TACC (1x1) is the acceleration time used for all segment transitions
% - TACC (1xM) is the acceleration time per segment, TACC(i) is the acceleration
%   time for the transition from segment i to segment i+1.  TACC(1) is also
%   the acceleration time at the start of segment 1.
%
% TRAJ = MSTRAJ(WP, QDMAX, TSEG, [], DT, TACC, OPTIONS) as above but the
% initial coordinates are taken from the first row of WP.
%
% TRAJ = MSTRAJ(WP, QDMAX, Q0, DT, TACC, QD0, QDF, OPTIONS) as above
% but additionally specifies the initial and final axis velocities (1xN).
%
% Options::
% 'verbose'    Show details.
%
% Notes::
% - Only one of QDMAX or TSEG can be specified, the other is set to [].
% - If no output arguments are specified the trajectory is plotted.
% - The path length K is a function of the number of via points, Q0, DT
%   and TACC.
% - The final via point P(end,:) is the destination.
% - The motion has M segments from Q0 to P(1,:) to P(2,:) ... to P(end,:).
% - All axes reach their via points at the same time.
% - Can be used to create joint space trajectories where each axis is a joint
%   coordinate.
% - Can be used to create Cartesian trajectories where the "axes"
%   correspond to translation and orientation in RPY or Euler angle form.
% - If qdmax is a scalar then all axes are assumed to have the same
%   maximum speed.
%
% See also MTRAJ, LSPB, CTRAJ.

% 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

function [TG, t, info]  = mstraj(segments, qdmax, tsegment, q0, dt, Tacc, varargin)

    if isempty(q0)
        q0 = segments(1,:);
        segments = segments(2:end,:);
    end

    assert(size(segments,2) == size(q0,2), 'RTB:mstraj:badarg', 'WP and Q0 must have same number of columns');
    assert(xor(~isempty(qdmax), ~isempty(tsegment)), 'RTB:mstraj:badarg', 'Must specify either qdmax or tsegment, but not both');
    if isempty(qdmax)
        assert(length(tsegment) == size(segments,1), 'RTB:mstraj:badarg', 'Length of TSEG does not match number of segments');
    end
    if isempty(tsegment)
        if length(qdmax) == 1
            % if qdmax is a scalar assume all axes have the same speed
            qdmax = repmat(qdmax, 1, numcols(segments));
        end
        assert(length(qdmax) == size(segments,2), 'RTB:mstraj:badarg', 'Length of QDMAX does not match number of axes');
    end

    ns = numrows(segments);
    nj = numcols(segments);

    [opt,args] = tb_optparse([], varargin);

    if length(args) > 0
        qd0 = args{1};
    else
        qd0 = zeros(1, nj);
    end
    if length(args) > 1
        qdf = args{2};
    else
        qdf = zeros(1, nj);
    end

    % set the initial conditions
    q_prev = q0;
    qd_prev = qd0;

    clock = 0;      % keep track of time
    arrive = [];    % record planned time of arrival at via points

    tg = [];
    taxis = [];

    for seg=1:ns
        if opt.verbose
            fprintf('------------------- segment %d\n', seg);
        end

        % set the blend time, just half an interval for the first segment

        if length(Tacc) > 1
            tacc = Tacc(seg);
        else
            tacc = Tacc;
        end

        tacc = ceil(tacc/dt)*dt;
        tacc2 = ceil(tacc/2/dt) * dt;
        if seg == 1
            taccx = tacc2;
        else
            taccx = tacc;
        end

        % estimate travel time
        %    could better estimate distance travelled during the blend
        q_next = segments(seg,:);    % current target
        dq = q_next - q_prev;    % total distance to move this segment

        %% probably should iterate over the next section to get qb right...
        % while 1
        %   qd_next = (qnextnext - qnext)
        %   tb = abs(qd_next - qd) ./ qddmax;
        %   qb = f(tb, max acceleration)
        %   dq = q_next - q_prev - qb
        %   tl = abs(dq) ./ qdmax;

        if ~isempty(qdmax)
            % qdmax is specified, compute slowest axis

            qb = taccx * qdmax / 2;        % distance moved during blend
            tb = taccx;

            % convert to time
            tl = abs(dq) ./ qdmax;
            %tl = abs(dq - qb) ./ qdmax;
            tl = ceil(tl/dt) * dt;

            % find the total time and slowest axis
            tt = tb + tl;
            [tseg,slowest] = max(tt);

            info(seg).slowest = slowest;
            info(seg).segtime = tseg;
            info(seg).axtime = tt;
            info(seg).clock = clock;

            % best if there is some linear motion component
            if tseg <= 2*tacc
                tseg = 2 * tacc;
            end
        elseif ~isempty(tsegment)
            % segment time specified, use that
            tseg = tsegment(seg);
            slowest = NaN;
        end

        % log the planned arrival time
        arrive(seg) = clock + tseg;
        if seg > 1
            arrive(seg) = arrive(seg) + tacc2;
        end

        if opt.verbose
            fprintf('seg %d, slowest axis %d, time required %.4g\n', ...
                seg, slowest, tseg);
        end

        %% create the trajectories for this segment

        % linear velocity from qprev to qnext
        qd = dq / tseg;

        % add the blend polynomial
        qb = jtraj(q0, q_prev+tacc2*qd, 0:dt:taccx, qd_prev, qd);
        tg = [tg; qb(2:end,:)];

        clock = clock + taccx;     % update the clock

        % add the linear part, from tacc/2+dt to tseg-tacc/2
        for t=tacc2+dt:dt:tseg-tacc2
            s = t/tseg;
            q0 = (1-s) * q_prev + s * q_next;       % linear step
            tg = [tg; q0];
            clock = clock + dt;
        end

        q_prev = q_next;    % next target becomes previous target
        qd_prev = qd;
    end
    % add the final blend
    qb = jtraj(q0, q_next, 0:dt:tacc2, qd_prev, qdf);
    tg = [tg; qb(2:end,:)];
    info(seg+1).segtime = tacc2;
    info(seg+1).clock = clock;

    % plot a graph if no output argument
    if nargout == 0
        t = (0:numrows(tg)-1)'*dt;
        clf
        plot(t, tg, '-o');
        hold on
        plot(arrive, segments, 'bo', 'MarkerFaceColor', 'k');
        hold off
        grid
        xlabel('time');
        xaxis(t(1), t(end))
        return
    end
    if nargout > 0
        TG = tg;
    end
    if nargout > 1
        t = (0:numrows(tg)-1)'*dt;
    end
    if nargout > 2
        infout = info;
    end


function dt6 = dtiLoadTensorElements(teBasename)
% dt6 = dtiLoadTensorElements(teBasename)
%
%
% HISTORY:
% 2004.03.26 RFD (bobd@stanford.edu) Wrote it.
%
% TODO:
%
%

if(~exist('teBasename', 'var') | isempty(teBasename))
    [f,p] = uigetfile({'*.001';'*.*'},'Select a slice from the Tensor Elements file...');
    teBasename = fullfile(p,f(1:end-3));
end

if(teBasename(end) ~= '.')
    teBasename = [teBasename '.'];
end

nSlices = length(dir([teBasename,'*']));
d = dir([teBasename,'001']);
% OK- there are 6*4=24 bytes per voxel -> d.bytes/24 voxels. We assume that
% the bits are divided equally between X and Y.
xySize = sqrt(d.bytes./24);
imdim = [xySize,xySize,nSlices];
disp([mfilename ': I''m guessing that the image dimension is [',num2str(imdim),'] (based on file size).']);

npix = imdim(1)*imdim(2);
dt6 = zeros([imdim,6]);
disp(['Reading ',num2str(nSlices),' slices...']);
for(ii=1:imdim(3))
    fname = sprintf('%s%03d', teBasename, ii);
    fid = fopen(fname, 'rb', 'ieee-le');
    dt6(:,:,ii,:) = reshape(fread(fid, inf, 'float32'),[imdim(1),imdim(2),1,6]);
    fclose(fid);
end
return;