%% Loading file paths
clear
close all
addpath('src/utility/')
addpath('src/')
addpath('../mesh2tri/')

[file,path] = uigetfile({'*.csv'},...
    'Please select the .csv file... containing SDF of an object', ...
    '../', 'MultiSelect','off');
if isequal(file, 0)
    error('No file selected.');
end

file_path = [path, file];
sdf = csvread(file_path)';
voxelGrid.size = ones(1, 3) * sdf(1);
voxelGrid.range = sdf(2 : 7);
sdf = sdf(8 : end);

voxelGrid.x = linspace(voxelGrid.range(1), voxelGrid.range(2), voxelGrid.size(1));
voxelGrid.y = linspace(voxelGrid.range(3), voxelGrid.range(4), voxelGrid.size(2));
voxelGrid.z = linspace(voxelGrid.range(5), voxelGrid.range(6), voxelGrid.size(3));
[x, y, z] = ndgrid(voxelGrid.x, voxelGrid.y, voxelGrid.z);
voxelGrid.points = reshape(cat(4, x, y, z), [], 3)';
voxelGrid.interval = (voxelGrid.range(2) - voxelGrid.range(1)) / (voxelGrid.size(1) -1);
voxelGrid.truncation = 1.2 * voxelGrid.interval; %1.2
voxelGrid.disp_range = [-inf, voxelGrid.truncation];
voxelGrid.visualizeArclength = 0.01 * sqrt(voxelGrid.range(2) - voxelGrid.range(1));
clearvars x y z

sdf = min(max(sdf, -voxelGrid.truncation), voxelGrid.truncation);

%% marching-primitives
tic
[x] = MPS(sdf, voxelGrid, 'minArea', 5);
toc

%% triangularization and compression
[mesh_original] = meshSuperquadrics(x, 'Arclength', voxelGrid.visualizeArclength);
% compression
mesh = reducepatch(mesh_original.f, mesh_original.v, 0.1); %0.05-2
stl = triangulation(mesh.faces, mesh.vertices);

ifsave = true;
[pathname,name,ext] = fileparts([path, file]);
if ifsave
    x_save = single(x);
    save(fullfile(pathname, [name,'_sq.mat']),'x_save')
    stlwrite(stl,fullfile(pathname, [name,'_sq.stl']), 'binary')
end

%% visualize

close all
view_vector = [151, -40];
light_vector = [190,10];
camera_roll = 50;
color = [145,163,176] ./255;

% rearrange sdf to 3D array for region connection checking
sdf3d_region = reshape(sdf, voxelGrid.size(1), voxelGrid.size(2), voxelGrid.size(3));
% rearrange sdf to 3D array for visualization
sdf3d = permute(sdf3d_region, [2, 1, 3]);

[mesh_gt.f, mesh_gt.v] = plyread(...
    fullfile(pathname, [name,'_watertight.ply']),'tri');

figure(1)
trisurf(mesh_gt.f,mesh_gt.v(:,1),mesh_gt.v(:,2),mesh_gt.v(:,3), ...
    'FaceColor', color, 'FaceAlpha', 1, 'EdgeColor', 'none')
axis equal
view(view_vector)
camroll(camera_roll)
light
lightangle(light_vector(1), light_vector(2))
material dull
axis(voxelGrid.range)
grid off
axis off
title('ground truth mesh from marching cubes')

figure(2)
trisurf(mesh.faces,mesh.vertices(:,1),mesh.vertices(:,2),mesh.vertices(:,3), ...
    'FaceColor', color, 'FaceAlpha', 1, 'EdgeColor', 'none')
axis equal
view(view_vector)
camroll(camera_roll)
light
lightangle(light_vector(1), light_vector(2))
material dull
axis(voxelGrid.range)
grid off
axis off
title('superquadrics representation from marching primitives')

figure(3)
trisurf(mesh_gt.f,mesh_gt.v(:,1),mesh_gt.v(:,2),mesh_gt.v(:,3), ...
    'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none')
hold on
trisurf(mesh.faces,mesh.vertices(:,1),mesh.vertices(:,2),mesh.vertices(:,3), ...
    'FaceColor', color, 'FaceAlpha', 1, 'EdgeColor', 'none')
view(view_vector)
camroll(camera_roll)
axis equal
light
lightangle(light_vector(1), light_vector(2))
material dull
axis(voxelGrid.range)
grid off
axis off
hold off
title('overlapping recovered representation with the ground truth')


function [Classification_accuracy,p1,m1,classes]=simclass(datalearn, datatest,v,c, measure, p, m,pl)
%Inputs:
% datalearn: put your data file in matrix form, rows indicates samples and
% columns features of the data.
% datatest: put your data file in matrix form, rows indicates samples and
% columns features of the data.
% v:   how many columns of features you are using e.g. [1:4] means you will
% use first four columns as features of your data.
% c: column where you class labels are. They should be in numerical form.
% measure: Which quantifier in OWA operators you want to use.
% 1=Basic RIM quantifier
% 2=Quadratic quantifier
% 3=Exponential quantifier
% 4=Trigonometric quantifier
% 5=O'Hagans method.
% p: parameter in generalized Lukasiewics similarity, can be studied as a range of
% parameter values and then given as a vector i.e. p=[0.1:0.1:5].
% alpha: alpha value in OWA operators.
% Can be given as vector i.e. alpha=[0.1:0.1:5].
% pl: do you want to plot how the parameter changes in p and alpha changes the
% mean classification accuracies and variances.
%
%OUTPUTS:
% Mean_accuracy: Mean classification accuracy with best parameter values in
% p and m:
% p1 and m1: best parameter values w.r.t. mean classification accuracy.
% classes: Class information. To which class the sample was classified (in testing set data, datatest)

sv_name = 'results1'; % mat file where results are stored ('' = "no storing")

[datatest, lc1, cs1] = init_data(datatest,v,c);
[datalearn, lc2, cs2] = init_data(datalearn,v,c);

w_opt = 0; % Do we use weight optimization. Not implemented in this version.

%Initializations
N=1;
rn=1;
fitness = zeros(1,N);
fitness_id = zeros(1,N);
fitness_dif = zeros(1,N);
Means = zeros(length(p),length(m),length(rn));
Vars = zeros(length(p),length(m),length(rn));
Maxsf = zeros(length(p),length(m),length(rn));
Minsf = zeros(length(p),length(m),length(rn));

Means_fit_dif = zeros(length(p),length(m),length(rn));
Vars_fit_dir = zeros(length(p),length(m),length(rn));
Ideal_var = zeros(length(p),length(m), length(lc1),length(rn));

for n = 1 : 1
    rn_ideal = ones(1,length(lc1));
    for j = 1:length(m)
        for i = 1:length(p)
            y = [p(i), m(j),measure]; % p and m values and similarity measure

            for k = 1 : 1
                ideals(:,:,k) = idealvectors(datalearn, y); % idealvectors
                if w_opt == 0
                  [fitness(k), class, Simil] = calcfit(datatest, ideals(:,:,k), y);
                end
            end
            classinfo(i,j,1:length(class))=class;
            Means(i,j,n) = mean(fitness);
            Vars(i,j,n) = var(fitness);
            Maxsf(i,j,n) = max(fitness);
            Minsf(i,j,n) = min(fitness);
            fitness=[];
        end
    end
    tmp=max(max(Means));
    [p1,m1]=find(tmp==Means);
    Classification_accuracy=Means(p1(1),m1(1));
    classes=classinfo(p1(1),m1(1),:);
    if pl==1
        [X,Y] = meshgrid(m,p);
        figure
        surfc(X,Y,Means(:,:,n))
        title('Classification accuracies')
        xlabel('alpha-values')
        ylabel('p-values')
        zlabel('Classification accuracy')
    end
    clear Y
end
if length(sv_name) ~= 0
    save(sv_name)
end


function s = addtls(s)
%ADDTLS Add the t location-scale distribution.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.6.7 $  $Date: 2004/01/24 09:35:08 $

j = length(s) + 1;
s(j).name = 't location-scale';
s(j).code = 'tlocationscale';
s(j).pnames = {'mu' 'sigma' 'nu'};
s(j).pdescription = {'location' 'scale' 'shape'};
s(j).prequired = [false false false];
s(j).fitfunc = @tlsfit;
s(j).likefunc = @tlslike;
s(j).cdffunc = @tlscdf;
s(j).pdffunc = @tlspdf;
s(j).invfunc = @tlsinv;
s(j).statfunc = @tlsstat;
s(j).loginvfunc = [];
s(j).logcdffunc = [];
s(j).hasconfbounds = false;
s(j).censoring = true;
s(j).paramvec = true;
s(j).support = [-Inf Inf];
s(j).closedbound = [false false];
s(j).iscontinuous = true;
s(j).islocscale = false;
s(j).uselogpp = false;

% ==== t Location-Scale distribution functions ====

% these distribution functions do not yet handle arrays of parameters

function y = tlspdf(x, mu, sigma, nu)
%TLSPDF T location-scale probability density function (pdf).
sigma(sigma <= 0) = NaN;
nu(nu <= 0) = NaN;

y = tpdf((x - mu)./sigma,nu)./sigma;

function p = tlscdf(x ,mu, sigma, nu)
%TLSCDF T location-scale cumulative distribution function (cdf).
sigma(sigma <= 0) = NaN;
nu(nu <= 0) = NaN;

p = tcdf((x - mu)./sigma,nu);

function x = tlsinv(p, mu, sigma, nu)
%TLSINV Inverse of the t location-scale cumulative distribution function (cdf).
sigma(sigma <= 0) = NaN;
nu(nu <= 0) = NaN;

x = tinv(p,nu).*sigma + mu;

function r = tlsrnd(mu, sigma, nu, varargin)
%TLSRND Random arrays from the t location-scale distribution.
sigma(sigma <= 0) = NaN;
nu(nu <= 0) = NaN;

[err, sizeOut] = statsizechk(3,mu,sigma,nu,varargin{:});
if err > 0
    error('stats:tlsrnd:InconsistentSizes','Size information is inconsistent.');
end

r = mu + sigma.*trnd(nu,sizeOut);

function [m,v] = tlsstat(mu, sigma, nu)
%TLSSTAT Mean and variance for the t location-scale distribution.
sigma(sigma <= 0) = NaN;
nu(nu <= 0) = NaN;

if nu <= 1
    m = NaN;
else
    m = mu;
end
if nu <= 2
    v = Inf;
else
    v = sigma.^2 .* nu ./ (nu - 2);
end

function [nlogL,acov] = tlslike(params,data,cens,freq)
%TLSLIKE Negative log-likelihood for the t location-scale distribution.
if nargin < 4 || isempty(freq), freq = ones(size(data)); end
if nargin < 3 || isempty(cens), cens = zeros(size(data)); end

nlogL = tls_nloglf(params, data, cens, freq);
if nargout > 1
    acov = mlecov(params, data, 'nloglf',@tls_nloglf, 'cens',cens, 'freq',freq);
end

% ==== t location-scale fitting functions ====

function [phat,pci] = tlsfit(x,alpha,cens,freq,opts)

if nargin < 2 || isempty(alpha), alpha = .05; end
if nargin < 3 || isempty(cens), cens = zeros(size(x)); end
if nargin < 4 || isempty(freq), freq = ones(size(x)); end
if nargin < 5, opts = []; end

% Robust estimators for the mean and std dev of a normal, and method
% of moments on t-kurtosis for nu
xunc = x(cens == 0);
k = max(kurtosis(xunc), 4);
start = [median(xunc), 1.253.*mad(xunc), 2.*(2.*k-3)./(k-3)];

% The default options include turning fminsearch's display off.  This
% function gives its own warning/error messages, and the caller can turn
% display on to get the text output from fminsearch if desired.
options = statset(statset('tlsfit'), opts);
tolBnd = options.TolBnd;
options = optimset(options);

% Maximize the log-likelihood with respect to mu, sigma, and nu.
[phat,nll,err,output] = ...
    fminsearch(@tls_nloglf, start, options, x, cens, freq, tolBnd);
if (err == 0)
    % fminsearch may print its own output text; in any case give something
    % more statistical here, controllable via warning IDs.
    if output.funcCount >= options.MaxFunEvals
        wmsg = 'Maximum likelihood estimation did not converge.  Function evaluation limit exceeded.';
    else
        wmsg = 'Maximum likelihood estimation did not converge.  Iteration limit exceeded.';
    end
    if phat(3) > 100 % degrees of freedom became very large
       wmsg = sprintf('%s\n%s', wmsg, ...
                      'The normal distribution might provide a better fit.');
    end
    warning('stats:tlsfit:IterOrEvalLimit',wmsg);
elseif (err < 0)
    error('stats:tlsfit:NoSolution',...
          'Unable to reach a maximum likelihood solution.');
end

if nargout > 1
    acov = mlecov(phat, x, 'nloglf',@tls_nloglf, 'cens',cens, 'freq',freq);
    probs = [alpha/2; 1-alpha/2];
    se = sqrt(diag(acov))';

    % Compute the CI for mu using a normal approximation for muhat.
    pci(:,1) = norminv(probs, phat(1), se(1));

    % Compute the CI for sigma using a normal approximation for
    % log(sigmahat), and transform back to the original scale.
    % se(log(sigmahat)) is se(sigmahat) / sigmahat.
    logsigci = norminv(probs, log(phat(2)), se(2)./phat(2));
    pci(:,2) = exp(logsigci);

    % Compute the CI for nu using a normal distribution for nuhat.
    pci(:,3) = norminv(probs, phat(3), se(3));
end

function nll = tls_nloglf(parms, x, cens, freq, tolBnd)
%TLS_NLOGLF Objective function for t location-scale maximum likelihood.
mu = parms(1);
sigma = parms(2);
nu = parms(3);

% Restrict sigma and nu to the open interval (0, Inf).
if nargin > 4
    if sigma < tolBnd || nu < tolBnd
        nll = Inf;
        return
    end
end

t = (x - mu) ./ sigma;
w = nu + (t.^2);
logw = log(w);

L = -.5.*(nu+1).*logw + gammaln(.5.*(nu+1)) - gammaln(.5.*nu) + 0.5.*nu.*log(nu) - log(sigma) - .5.*log(pi);
ncen = sum(freq.*cens);
if ncen > 0
    cen = (cens == 1);
    if nu < 1e7  % Use the standard formula
        Scen = betainc(nu ./ w(cen), .5.*nu, 0.5) ./ 2;

        % Reflect for negative t.
        reflect = (t(cen) < 0);
        Scen(reflect) = 1 - Scen(reflect);

    else  % Use a normal approximation.
        Scen = log(0.5 * erfc(t(cen) ./ sqrt(2)));
    end
    L(cen) = log(Scen);
end
nll = -sum(freq .* L);

% Don't yet have dbetainc, so can't compute an analytic gradient with censoring.
%
% if nargout > 1
%     dL1 = (nu+1).*t./(w.*sigma);
%     dL2 = t.*dL1 - 1./sigma;
%     dL3 = .5.*(-logw - (nu+1)./w + psi(.5.*(nu+1)) - psi(.5.*nu) + log(nu) + 1);
%     if ncen > 0
% %         dL1(cen) = ;
% %         dL2(cen) = ;
% %         dL3(cen) = ;
%     end
%     ngrad = -[sum(freq .* dL1) sum(freq .* dL2) sum(freq .* dL3)];
% end


function [ G ] = gsp_bunny()
%GSP_BUNNY Create a graph of the stanford bunny
%   Usage :  G = gsp_bunny();
%
%   Output parameters:
%       G           : Resulting graph
%
%   'gsp_bunny()' creates a graph from the pointcloud of the Stanford Bunny model.
%
%   Example:::
%
%           G = gsp_bunny();
%           gsp_plot_graph(G);
%
%   References: turk1994zippered

    %Load the point cloud
    P = gsp_pointcloud('bunny');

    %Create the graph from the point cloud using an epsilon-neighborhood
    %connectivity
    param.type = 'knn';
    param.rescale = 1;
    param.center = 1;

    %Compute it
    G = gsp_nn_graph(double(P), param);
    %Reduce vertex size for plotting
    G.plotting.vertex_size = 10;
end


function show_sample(samples)
% a simple visualization tool using isosurface to show each 3D sample.

n = size(samples,1);
for i = 1 : n
    the_sample = squeeze(samples(i,:,:,:,:));

    figure;
    p = patch(isosurface(the_sample,0.05));
    set(p,'FaceColor','red','EdgeColor','none');
    daspect([1,1,1])
    view(3); axis tight
    camlight
    lighting gouraud;
    axis off;
    set(gcf,'Color','white');
    set(gca,'position',[0,0,1,1],'units','normalized');
    axis tight;
    %title(i);
    pause;
    close(gcf);
end


function [offsets,WI,parents,I] = bone_forest(C,E,P,BE)
  % [offsets,WI,parents] = bone_forest(C,E,P,BE)
  %
  % See also: writeBF.m

  if nargin == 2 || isempty(BE)
    BE = E;
    P = [];
  end
  [BE,I] = sortrows(BE);
  % point properties
  P_parents = zeros(numel(P),1);
  P_offsets = C(P,:);
  P_WI = (1:numel(P))';
  % bone properties
  % bones form tree(s)
  assert(numel(unique(BE(:,2))) == numel(BE(:,2)));
  % 0 if not dest otherwise index in list
  index_map = zeros(max(BE(:)),1);
  index_map(BE(:,2)) = 1:size(BE,1);
  roots = setdiff(BE(:,1),BE(:,2));
  % add roots to index map
  index_map(roots) = size(BE,1)+(1:numel(roots));
  % parents after adding roots
  BE_parents = index_map(BE(:,1));
  % roots have no mother or father
  BE_parents = [BE_parents; zeros(numel(roots),1)];
  BE_WI = [1:size(BE,1) zeros(1,numel(roots))]';
  BE_offsets = bone_offsets(C([BE(:,2); roots],:),BE_parents);
  % we'll first put "point" handles
  % then bone's (recall that in BF each joint needs an entry, including roots)
  parents = [P_parents;numel(P).*(BE_WI~=0)+BE_parents];
  WI = [P_WI;numel(P).*(BE_WI~=0)+BE_WI];
  offsets = [P_offsets;BE_offsets];
end


function rgb = labinterp(xi,rgb,xo,varargin)
% Interpolate an sRGB colormap in the Lab colorspace.
%
% Syntax:
%  rgb = labinterp(xi,rgb,xo,<interp1 options>)

M = [...
	+3.2406255,-1.5372080,-0.4986286;...
	-0.9689307,+1.8757561,+0.0415175;...
	+0.0557101,-0.2040211,+1.0569959];
wpt = [0.95047,1,1.08883]; % D65
%
rgb = liRGB2Lab(rgb,M,wpt);
rgb = interp1(xi,rgb,xo,varargin{:});
rgb = liLab2RGB(rgb,M,wpt);
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%labinterp
function rgb = liGammaCor(rgb)
% Gamma correction of sRGB data.
idx = rgb <= 0.0031308;
rgb(idx) = 12.92 * rgb(idx);
rgb(~idx) = real(1.055 * rgb(~idx).^(1/2.4) - 0.055);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%liGammaCor
function rgb = liGammaInv(rgb)
% Inverse gamma correction of sRGB data.
idx = rgb <= 0.04045;
rgb(idx) = rgb(idx) / 12.92;
rgb(~idx) = real(((rgb(~idx) + 0.055) / 1.055).^2.4);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%liGammaInv
function lab = liRGB2Lab(rgb,M,wpt) % Nx3 <- Nx3
% Convert a matrix of sRGB values to Lab.
%
%lab = applycform(rgb,makecform('srgb2lab','AdaptedWhitePoint',wpt));
%
% RGB2XYZ:
xyz = liGammaInv(rgb) / M.';
% Remember to include my license when copying my implementation.
% XYZ2Lab:
xyz = bsxfun(@rdivide,xyz,wpt);
idx = xyz>(6/29)^3;
F = idx.*(xyz.^(1/3)) + ~idx.*(xyz*(29/6)^2/3+4/29);
lab(:,2:3) = bsxfun(@times,[500,200],F(:,1:2)-F(:,2:3));
lab(:,1) = 116*F(:,2) - 16;
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%liRGB2Lab
function rgb = liLab2RGB(lab,M,wpt) % Nx3 <- Nx3
% Convert a matrix of Lab values to sRGB.
%
%rgb = applycform(lab,makecform('lab2srgb','AdaptedWhitePoint',wpt));
%
% Lab2XYZ
tmp = bsxfun(@rdivide,lab(:,[2,1,3]),[500,Inf,-200]);
tmp = bsxfun(@plus,tmp,(lab(:,1)+16)/116);
idx = tmp>(6/29);
tmp = idx.*(tmp.^3) + ~idx.*(3*(6/29)^2*(tmp-4/29));
xyz = bsxfun(@times,tmp,wpt);
% Remember to include my license when copying my implementation.
% XYZ2RGB
rgb = max(0,min(1, liGammaCor(xyz * M.')));
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%cbLab2RGB
% Copyright (c) 2017 Stephen Cobeldick
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
% http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and limitations under the License.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%license


function varargout = witps(varargin)
% VL_WITPS  Inverse thin-plate spline warping
%   [X1,X2]=VL_WITPS(XP1,XP2,Y,Yp) computes the inverse thin-plate spline
%   (TPS) warp of the points XP1,XP2.
%
%   Remark::
%     The inverse of a thin-plate spline in general is NOT a
%     thin-plate spline and some splines do not have an inverse.  This
%     function uses Gauss-Newton to compute a set of points (X1,X2)
%     such that [XP1,XP2]=VL_WTPS(X1,X2,Y,Yp).
%
%   See also: VL_WTPS(), VL_HELP().
[varargout{1:nargout}] = vl_witps(varargin{:});


function combo_test09 ( )

%*****************************************************************************80
%
%% COMBO_TEST09 tests I4VEC_SEARCH_BINARY_A and I4VEC_SORT_INSERT_A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'COMBO_TEST09\n' );
  fprintf ( 1, '  Integer vectors.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I4VEC_SORT_INSERT_A ascending sorts;\n' );
  fprintf ( 1, '  I4VEC_SEARCH_BINARY_A searches a ascending sorted vector.\n' );

  a(1:n) = [ 6, 7, 1, 0, 4, 3, 2, 1, 5, 8 ]';

  i4vec_print ( n, a, '  Before ascending sort:' );

  a = i4vec_sort_insert_a ( n, a );

  i4vec_print ( n, a, '  After ascending sort:' );

  b = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now search for an instance of the value %d\n', b );

  index = i4vec_search_binary_a ( n, a, b );

  fprintf ( 1, '\n' );
  if ( index == 0 )
    fprintf ( 1, '  The value does not occur.\n' );
  else
    fprintf ( 1, '  The value occurs at index = %d\n', index );
  end

  return
end


function step_distance(nlp, bounds)
    % constraints for step length and step width

    domain = nlp.Plant;
    x = domain.States.x;

    [right_foot_frame] = sys.frames.RightFoot(domain);
    [left_foot_frame] = sys.frames.LeftFoot(domain);
    right_foot = getCartesianPosition(domain, right_foot_frame);
    left_foot = getCartesianPosition(domain, left_foot_frame);
    constraint = tomatrix(left_foot(1:2) - right_foot(1:2));
    constraint_func = SymFunction(['step_distance_',domain.Name], constraint, {x});
    lb = [bounds.constrBounds.stepWidth.lb
        bounds.constrBounds.stepLength.lb];
    ub = [bounds.constrBounds.stepWidth.ub
        bounds.constrBounds.stepLength.ub];
    addNodeConstraint(nlp, constraint_func, {'x'}, 'first', lb, ub, 'NonLinear');

    %     wt = 0.3662;
    %     lb = [-wt-0.015
    %         0-0.03];
    %     ub = [-wt+0.015
    %         0+0.03];
    %     addNodeConstraint(nlp, constraint_func, {'x'}, floor(nlp.NumNode/2)+1, lb, ub, 'NonLinear');
end


%{
Yahoo! TVSum50 Dataset.
- Function to compute a pairwise F1 score
%}

function [ out ] = pairwise_f1( score,portion )
%PAIRWISE_F1 Summary of this function goes here

    addpath('../knapsack');

    if size(score,1) < size(score,2),
        error('score must be column-wise matrix; each column is an observation');
    end

    L = size(score,1); % sequence length
    N = size(score,2); % number of annotations

    % Pre-compute knapsack solution
    y = cell(1,N);
    for i=1:N
        [~,y{i}] = knapsack(ones(L,1), score(:,i), fix(portion*L));
    end

    for i=1:N,
        val = 0; % accumulative f-measure
        for j=1:N,
            if i==j, continue; end
            cp = classperf(y{i},y{j},'Positive',1,'Negative',0);
            prec = cp.CorrectRate;
            rec = cp.Sensitivity;
            f1score = 2*(prec*rec)/(prec+rec);

            val = val+f1score;
        end
        val = val / (N-1);
        out(i) = val;
    end
end


function Index = SVM_AFdetection_withTrainingModel(training_feature,training_class,test_feature,test_class)
%******************************************************
% $ This function is used for calculating the SVM-based AF detection
% indices. You need to train the SVM model first using the training_feature
% and training_class, and then using the test_feature and test_class for calculating the AF
% detection incices
%
% $ Reference:
% Q. Li, C. Y. Liu, J. Oster and G. D. Clifford. Chapter: Signal processing
% and feature selection preprocessing for classification in noisy healthcare data. In book: Machine Learning for Healthcare Technologies, Edition: 1st, Publisher: IET, Editors: David A. Clifton, 2016.
%
% $ Variable declaration:
% Input:
% training_feature: the feature metrix from AF detection, you need to run
% AF_feature function first to obtain the AF features, each row is a
% feature vector from an RR interval time series
% training_class: the labels of AF for the trained RR ineterval time series, N*1
% metrix, 1 for AF and 0 for non-AF
% test_feature: the feature metrix from AF detection, you need to run
% AF_feature function first to obtain the AF features, each row is a
% feature vector from an RR interval time series
% test_class: the labels of AF for the tested RR ineterval time series, N*1
% metrix, 1 for AF and 0 for non-AF
% Output:
% Index: AF detection indices output, with the defined order of indices of TP FN FP TN Se Sp Acc PPV NPV J in turn.
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information

addpath('E:\My algorithms\Matlab_SVM_tool\libsvm-3.20\matlab');

meanx=nanmean(training_feature);
stdx=nanstd(training_feature);

xtrain=bsxfun(@minus, training_feature, meanx);
xtrain=bsxfun(@rdivide, xtrain,stdx);

xtest=bsxfun(@minus, test_feature, meanx);
xtest=bsxfun(@rdivide, xtest,stdx);
model = svmtrain(training_class,xtrain, '-b 1');

[predict_all, accuracy_test, ytest_out] = svmpredict(test_class, xtest, model, '-b 1');

TP=length(find(test_class==1 & predict_all==1));
FP=length(find(test_class==0 & predict_all==1));
FN=length(find(test_class==1 & predict_all==0));
TN=length(find(test_class==0 & predict_all==0));
Se=TP/(TP+FN);
Sp=TN/(TN+FP);
Acc=(TP+TN)/length(test_class);
PPV=TP/(TP+FP);
NPV=TN/(TN+FN);
J=Se+Sp-1;
Index=[TP FN FP TN Se Sp Acc PPV NPV J];
end


function [sys,x0,str,ts]=ADRC_1(t,x,u,flag,r0,h,B01,B02,r)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,r0,h,B01,B02);
    case 3
        sys=mdlOutputs(x,r);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=4;
    sizes.NumOutputs=2;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,r0,h,B01,B02)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),r0,h);
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*fh;
    e2=x(3)-u(2);
    sys(3)=x(3)+h*(x(4)-B01*e2+u(3));
    sys(4)=x(4)+h*(-B02*e2);

function sys=mdlOutputs(x,r)
    e3=x(1)-x(3);
    sys(1)=r*e3-x(4);
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end


%NEURC Automatic neural network classifier
%
% 	W = NEURC(A,UNITS)
% 	W = A*NEURC([],UNITS)
% 	W = A*NEURC(UNITS)
%
% INPUT
%   A      Dataset
%   UNITS  Number of units
%          Default: 0.2 x size smallest class in A.
%
% OUTPUT
%   W      Trained feed-forward neural network mapping
%
% DESCRIPTION
% Automatically trained feed-forward neural network classifier with UNITS
% units in a single hidden layer. Training, by LMNC, is stopped when the
% performance on an artificially generated tuning set of 1000 samples per
% class (based on k-nearest neighbour interpolation) does not improve anymore.
%
% NEURC always tries three random initialisations, with fixed random seeds,
% and returns the best result according to the tuning set. This is done in
% order to obtain a reproducable result.
%
% If UNITS is NaN it is optimised by REGOPTC. This may take a long
% computing time and is often not significantly better than the default.
%
% Uses the Mathworks' neural network toolbox.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, LMNC, BPXNC, GENDATK, REGOPTC

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: neurc.m,v 1.9 2008/07/03 09:11:44 duin Exp $

function argout = neurc (varargin)

  checktoolbox('nnet');

	mapname = 'AutoNeuralNet';
  argin = shiftargin(varargin,'integer');
  argin = setdefaults(argin,[],[]);

  if mapping_task(argin,'definition')
    argout = define_mapping(argin,'untrained',mapname);

  elseif mapping_task(argin,'training')			% Train a mapping.

    [a,units] = deal(argin{:});
    n_attempts = 3;			% Try three different random initialisations.
    [m,k] = size(a);
    if isempty(units)
      cs = classsizes(a);
      units = ceil(0.2*min(cs));
    end

  	if isnan(units) % optimize complexity parameter: number of neurons
			defs = {[]};
			parmin_max = [1,30];
			w = regoptc(a,mfilename,{units},defs,[1],parmin_max,testc([],'soft'),0);
			return
  	end

		islabtype(a,'crisp');
		isvaldfile(a,1,2); % at least 1 object per class, 2 classes
		a = testdatasize(a);
		a = setprior(a,getprior(a,0));

		% train a network.
		% Reproducability: always use same seeds.
		randstate = randreset(1); opt_err = inf; opt_mapping = [];

		% Try a number of random initialisations.
		s = sprintf('%i neural network initializations: ',n_attempts);
		prwaitbar(n_attempts,s);
		for attempt = 1:n_attempts
			prwaitbar(n_attempts,attempt,[s int2str(attempt)]);
			prwarning(4,'training with initialisation %d of %d',attempt,n_attempts);
			t = gendatk(a,1000,2,1); 			% Create tuning set based on training set.
			w = lmnc(a,units,inf,[],t);		% Find LMNC mapping.
			e = t*w*testc;								% Calculate classification error.
			if (e < opt_err)
				% If this is the best of the three repetitions, store it.
				opt_mapping = w; opt_err = e;
			end
		end
		prwaitbar(0);
    randreset(randstate); % return original state

		% Output is best network found.
		argout = setname(opt_mapping,mapname);

  else % Evaluation

    [a,w] = deal(argin{1:2});
		nodatafile(a);
		data = getdata(w);

		if (length(data) > 1)

	    % "Old" neural network - network is second parameter: unpack.
  	  data = getdata(w); weights = data{1};
    	pars = data{2}; numlayers = length(pars);

	    output = a;                       % Output of first layer: dataset.
  	  for j = 1:numlayers-1
    	  % Number of inputs (n_in) and outputs (n_out) of neurons in layer J.
      	n_in = pars(j); n_out = pars(j+1);

	      % Calculate output of layer J+1. Note that WEIGHTS contains both
  	    % weights (multiplied by previous layer's OUTPUT) and biases
    	  % (multiplied by ONES).

      	this_weights = reshape(weights(1:(n_in+1)*n_out),n_in+1,n_out);
	      output = sigm([output,ones(m,1)]*this_weights);

  	    % Remove weights of this layer.
    	  weights(1:(n_in+1)*n_out) = [];
	    end
		else
			% "New" neural network: unpack and simulate using the toolbox.
			net = data{1};
			output = sim(net,+a')';
		end;

		% 2-class case, therefore 1 output: 2nd output is 1-1st output.
		if (size(output,2) == 1)
			output = [output (1-output)];
		end

		% Output is mapped dataset.
		argout = setdat(a,output,w);

	end

return


classdef prtRegressPlsda < prtRegress
    % prtRegressPlsda  Partial least squares discriminant regression
    %
    %    REGRESS = prtRegressPlsda returns a Partial least squares
    %    discriminant regressor
    %
    %    REGRESS = prtRegressPlsda(PROPERTY1, VALUE1, ...) constructs a
    %    prtRegressPlsda object REGRESS with properties as specified by
    %    PROPERTY/VALUE pairs.
    %
    %    A prtRegressPlsda object inherits all properties from the abstract
    %    Regress prtRegress. In addition is has the following properties:
    %
    %    nComponents  -  The number of components
    %    Bpls         -  The regression weights, estimated during training
    %    xMeans       -  The xMeans, estimated during training
    %    yMeans       -  The yMeana, estimated during training
    %
    %    For information on the partial least squares discriminant
    %    algorithm, please refer to the following URL:
    %
    %    http://en.wikipedia.org/wiki/Partial_least_squares_regression
    %
    %    A prtRegressPlsda object inherits the TRAIN, RUN, CROSSVALIDATE and
    %    KFOLDS methods from prtAction. It also inherits the PLOT method
    %    from prtRegress.
    %
    %    Example:
    %
    %   TestDataSet = prtDataGenNoisyLine;         % Create some test and
    %   TrainingDataSet = prtDataGenNoisyLine;     % training data
    %   regress = prtRegressPlsda;                % Create a regressor
    %   regress = regress.train(TrainingDataSet); % Train
    %   regressed = run(regress, TestDataSet);    % Test
    %   regress.plot;
    %
    %    See also prtRegress, prtRegressLslr, prtRegressRvm

    properties (SetAccess=private)
        name = 'Partial Least Squares Discriminant' % Partial Least Squares Discriminant
        nameAbbreviation = 'PLSDA' % PLSDA
        isNativeMary = true;  % True
    end

    properties
        % w is a DataSet.nDimensions x 1 vector of projection weights
        % learned during Fld.train(DataSet)
        nComponents = 2;
    end

    properties (SetAccess=protected)
        Bpls     % The prediction weights
        loadings % T
        xFactors % P
        yFactors % Q
        yMeansFactor % Factor to be added into regression output (accounts for X means and yMeans);
    end

    methods

        function Obj = prtRegressPlsda(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end

        function Obj = set.nComponents(Obj,val)
            if ~prtUtilIsPositiveInteger(val)
                error('prt:prtRegressPlsda:nComponents','nComponents must be a positive integer');
            end
            Obj.nComponents = val;
        end

    end

    methods (Access=protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)

            X = DataSet.getObservations;

            Y = DataSet.getY;

            maxComps = min(size(X));
            if Obj.nComponents > maxComps;
                Obj.nComponents = maxComps;
            end

            xMeans = mean(X,1);
            yMeans = mean(Y,1);
            X = bsxfun(@minus, X, xMeans);
            Y = bsxfun(@minus, Y, yMeans);

            [Obj.Bpls, R, Obj.xFactors, Obj.yFactors, Obj.loadings, U] = prtUtilSimpls(X,Y,Obj.nComponents);  %#ok<ASGLU,NASGU>

            Obj.yMeansFactor = yMeans - xMeans*Obj.Bpls;

        end

        function DataSet = runAction(Obj,DataSet)
            yOut = bsxfun(@plus,DataSet.getObservations*Obj.Bpls, Obj.yMeansFactor);
            DataSet = DataSet.setObservations(yOut);
        end

        function xOut = runActionFast(Obj,xIn,ds) %#ok<INUSD>
           xOut = bsxfun(@plus,xIn*Obj.Bpls, Obj.yMeansFactor);
        end
    end
end


function [in3] = l2in3(l)
% Convert volume from liters to cubic inches.
% Chad Greene 2012
in3 = l*61.023744095;


function [ferns,hsPr] = fernsClfRemoveTrainingData( hs, remove_data_idx, ferns )
% Compute cross-validation error of random ferns classifier
%
% See "Fast Keypoint Recognition in Ten Lines of Code" by Mustafa Ozuysal,
% Pascal Fua and Vincent Lepetit, CVPR07.
%
% Dimensions:
%  M - number ferns
%  S - fern depth
%  F - number features
%  N - number input vectors
%  H - number classes
%
% USAGE
%  [ferns,hsPr] = fernsClfTrain( data, hs, [varargin] )
%
% INPUTS
%  data     - [NxF] N length F feature vectors
%  hs       - [Nx1] target output labels in [1,H]
%  varargin - additional params (struct or name/value pairs)
%   .S        - [10] fern depth (ferns are exponential in S)
%   .M        - [50] number of ferns to train
%   .thrr     - [0 1] range for randomly generated thresholds
%   .bayes    - [1] if true combine probs using bayes assumption
%   .ferns    - [] if given reuse previous ferns (recompute pFern)
%   .cv_sets  - [] if given, N x 1 vector containing the index of each
%   .cv_nsets - [10] if cv_sets not specified, then the data is split into
%               cv_nsets randomly chosen sets.
%
% OUTPUTS
%  fracwrong_cv  - [1x1] fraction of training examples predicted
%               incorrectly in hsPr_cv
%  hsPr_cv    - [Nx1] predicted cross-validation-based classification of
%               each training example
%  probs_cv   - [NxH] predicted cross-validation-based output label
%               probabilities for each training example
%  ferns      - learned fern model from all training data with the
%               following fields
%   .fids     - [MxS] feature ids for each fern for each depth
%   .thrs     - [MxS] threshold corresponding to each fid
%   .pFern    - [2^SxHxM] learned log probs at fern leaves
%   .bayes    - if true combine probs using bayes assumption
%   .inds     - [NxM] cached indices for original training data
%   .H        - number classes
%
% EXAMPLE
%  N=5000; H=5; d=2; [xs0,hs0,xs1,hs1]=demoGenData(N,N,H,d,1,1);
%  fernPrm=struct('S',4,'M',50,'thrr',[-1 1],'bayes',1);
%  tic, [ferns,hsPr0]=fernsClfTrain(xs0,hs0,fernPrm); toc
%  tic, hsPr1 = fernsClfApply( xs1, ferns ); toc
%  e0=mean(hsPr0~=hs0); e1=mean(hsPr1~=hs1);
%  fprintf('errors trn=%f tst=%f\n',e0,e1); figure(1);
%  subplot(2,2,1); visualizeData(xs0,2,hs0);
%  subplot(2,2,2); visualizeData(xs0,2,hsPr0);
%  subplot(2,2,3); visualizeData(xs1,2,hs1);
%  subplot(2,2,4); visualizeData(xs1,2,hsPr1);
%
% See also fernsClfTrain, fernsClfApply, fernsInds
%
% Kristin Branson 2011-06-09, based on fernsClfTrain from:
%
% Piotr's Image&Video Toolbox      Version 2.50
% Copyright 2010 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

% get additional parameters and check dimensions
%dfs={'S',10,'M',50,'thrr',[0 1],'bayes',1,'ferns',[],'cv_sets',[],'cv_nsets',10};
%[S,M,thrr,bayes,ferns,cv_sets,cv_nsets]=getPrmDflt(varargin,dfs,1);
H=max(hs);
[M,S] = size(ferns.fids);
assert(all(hs>0)); assert(S<=20);

% grab inds for data we are removing
inds_remove = ferns.inds(remove_data_idx,:);

% remove inds from ferns
ferns.inds(remove_data_idx,:) = [];

% update counts
pFern = ferns.counts;
edges = 1:2^S;
for m = 1:M,
  for h = 1:H,
    pFern(:,h,m) = pFern(:,h,m) - reshape(histc(inds_remove(hs(remove_data_idx)==h,m),edges),[2^S,1]);
  end
end
ferns.counts = pFern;

% update pFern
if( ferns.bayes<=0 )
  norm = 1./sum(pFern,2);
  % KB: use bsxfun instead of loop
  pFern = bsxfun(@times,pFern,norm);
  %for h=1:H, pFern(:,h,:)=pFern(:,h,:).*norm; end
else
  norm = 1./sum(pFern,1);
  % KB: use bsxfun instead of loop
  pFern = bsxfun(@times,pFern,norm);
  %for s=1:2^S, pFern(s,:,:)=pFern(s,:,:).*norm; end
  pFern=log(pFern);
end

% store pFern and compute output values
ferns.pFern=pFern; clear pFern;
if(nargout==2), hsPr=fernsClfApply([],ferns,ferns.inds); end

end


function [ w, x ] = gm_rule_set_old ( rule, dim_num, point_num )

%*****************************************************************************80
%
%% GM_RULE_SET_OLD sets a Grundmann-Moeller rule.  (OBSOLETE VERSION)
%
%  Discussion:
%
%    This version of the computation is no longer used.  The direct
%    application of the formula results in overflows and inaccuracies
%    very quickly.
%
%    This rule returns weights and abscissas of a Grundmann-Moeller
%    quadrature rule for the DIM_NUM-dimensional unit simplex.
%
%    The dimension POINT_NUM can be determined by calling GM_RULE_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Axel Grundmann, Michael Moeller,
%    Invariant Integration Formulas for the N-Simplex
%    by Combinatorial Methods,
%    SIAM Journal on Numerical Analysis,
%    Volume 15, Number 2, April 1978, pages 282-290.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%    0 <= RULE.
%
%    Input, integer DIM_NUM, the spatial dimension.
%    1 <= DIM_NUM.
%
%    Input, integer POINT_NUM, the number of points in the rule.
%
%    Output, real W(POINT_NUM), the weights.
%
%    Output, real X(DIM_NUM,POINT_NUM), the abscissas.
%
  s = rule;
  d = 2 * s + 1;
  k = 0;
  n = dim_num;
  one_pm = 1;

  for i = 0 : s

    weight = r8_factorial ( n ) ...
      * ( one_pm *  ( d + n - 2 * i )^d ) ...
      / ( 2^(2*s) * r8_factorial ( i ) * r8_factorial ( d + n - i ) );

    one_pm = - one_pm;

    beta_sum = s - i;
    more = 0;
    beta = [];
    h = 0;
    t = 0;

    while ( 1 )

      [ beta, more, h, t ] = comp_next ( beta_sum, dim_num + 1, ...
        beta, more, h, t );

      k = k + 1;

      w(k) = weight;

      x(1:dim_num,k) = ( 2 * beta(2:dim_num+1)' + 1 ) / ( d + n - 2 * i );

      if ( ~more )
        break
      end

    end

  end

  return
end


function varargout = invpos(varargin)
% INVPOS  Returns model of 1./x

switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)
        varargout{1} = 1./varargin{1};

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char' % YALMIP sends 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.

            varargout{1} = (cone([2;X-t],X+t));
            varargout{2} = struct('convexity','convex','monotonicity','decreasing','definiteness','positive','model','graph');
            varargout{3} = X;
        else
        end
    otherwise
end


function [ y, m, d ] = day_borrow_julian ( y, m, d )

%*****************************************************************************80
%
%% DAY_BORROW_JULIAN borrows days from months in a Julian date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, integer M, integer D, a year, month, and day
%    representing a date.  On input, D might be negative.
%
%    Output, integer Y, integer M, integer D, a year, month, and day
%    representing a date.  On output,
%    M should have decreased by one month, and D gone up by the
%    number of days in the month we "cashed in".  Y may be affected
%    if the input value of M was 1.
%
  while ( d <= 0 )

    m = m - 1;

    [ y, m ] = month_borrow_julian ( y, m );

    days = month_length_julian ( y, m );

    d = d + days;

  end

  return
end


echo off
%load seed.mat
rand('seed',0);
echo on
% This script shows how to use the ga. You should see the demos for
% more information as well. gademo1, gademo2, gademo3
global bounds

% Crossover Operators
xFns = 'arithXover heuristicXover simpleXover';
xOpts = [2 0; 2 3; 2 0];

% Mutation Operators
mFns = 'boundaryMutation multiNonUnifMutation nonUnifMutation unifMutation';
mOpts = [4 0 0;6 10 3;4 10 3;4 0 0];

% Termination Operators
termFns = 'maxGenTerm';
termOps = [10];

% Selection Function
selectFn = 'normGeomSelect';
selectOps = [0.06];

% Evaluation Function takes two options
% prob to use gradient, prob to perform Lamarkian evolution
evalFn = 'gaZBGradEval';
evalOps = [1.00 1.00];

% Bounds on the variables
bounds = [-3 12.1; 4.1 5.8];

% GA Options [epsilon float/binar display]
gaOpts=[1e-6 1 1];

% Generate an intialize population of size 80
startPop = initializega(80,bounds,evalFn,evalOps,[1e-6 1]);
evalOps = [1.00 0.00]; % 1 - Peform learning 0-Do not update

% Lets run the GA using Baldwinian Evolution

[x endPop bestPop trace]=ga(bounds,evalFn,evalOps,startPop,gaOpts,...
    termFns,termOps,selectFn,selectOps,xFns,xOpts,mFns,mOpts);

% x is the best solution found
x
pause

% endPop is the ending population
endPop
pause

% bestPop is the best solution tracked over generations
bestPop
pause

% trace is a trace of the best value and average value of generations
trace
pause

% Plot the best over time
clf
plot(trace(:,1),trace(:,2));
pause

% Add the average to the graph
hold on
plot(trace(:,1),trace(:,3));
pause

% Lets run the GA using Lamarkian Evolution
evalOps = [1.00 1.00];

[x endPop bestPop trace]=ga(bounds,evalFn,evalOps,startPop,gaOpts,...
    termFns,termOps,selectFn,selectOps,xFns,xOpts,mFns,mOpts);

% x is the best solution found
x
pause

% endPop is the ending population
endPop
pause

% bestPop is the best solution tracked over generations
bestPop
pause

% trace is a trace of the best value and average value of generations
trace
pause

% Plot the best over time
clf
plot(trace(:,1),trace(:,2));
pause

% Add the average to the graph
hold on
plot(trace(:,1),trace(:,3));
pause


% jointprob() - rejection of odd columns of a data array  using
%              joint probability of the values in that column (and
%              using the probability distribution of all columns).
%
% Usage:
%   >>  [jp rej] = jointprob( signal );
%   >>  [jp rej] = jointprob( signal, threshold, jp, normalize, discret);
%
%
% Inputs:
%   signal     - one dimensional column vector of data values, two
%                dimensional column vector of values of size
%                sweeps x frames or three dimensional array of size
%                component x sweeps x frames. If three dimensional,
%                all components are treated independently.
%   threshold  - Absolute threshold. If normalization is used then the
%                threshold is expressed in standard deviation of the
%                mean. 0 means no threshold.
%   jp         - pre-computed joint probability (only perform thresholding).
%                Default is the empty array [].
%   normalize  - 0 = do not not normalize entropy. 1 = normalize entropy.
%                2 is 20% trimming (10% low and 10% high) proba. before
%                normalizing. Default is 0.
%   discret    - discretization variable for calculation of the
%                discrete probability density. Default is 1000 points.
%
% Outputs:
%   jp         - normalized joint probability  of the single trials
%                (size component x sweeps)
%   rej        - rejected matrix (0 and 1, size comp x sweeps)
%
% Remark:
%   The exact values of joint-probability depend on the size of a time
%   step and thus cannot be considered as absolute.
%
% See also: realproba()

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.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

function [jp, rej] = jointprob( signal, threshold, oldjp, normalize, discret );

if nargin < 1
	help jointprob;
	return;
end;
if nargin < 2
	threshold = 0;
end;
if nargin < 3
	oldjp = [];
end;
if nargin < 4
	normalize = 0;
end;
if nargin < 5
	discret = 1000;
end;

if size(signal,2) == 1 % transpose if necessary
	signal = signal';
end;

[nbchan pnts sweeps] = size(signal);
jp  = zeros(nbchan,sweeps);

if exist('oldjp') & ~isempty( oldjp ) % speed up the computation
	jp = oldjp;
else
	for rc = 1:nbchan

		% COMPUTE THE DENSITY FUNCTION
		% ----------------------------
		[ dataProba sortbox ] = realproba( signal(rc, :), discret );

		% compute all entropy
		% -------------------
		for index=1:sweeps
			datatmp = dataProba((index-1)*pnts+1:index*pnts);
			jp(rc, index) = - sum( log( datatmp ) );
			     % - sum( datatmp .* log( datatmp ) ); would be the entropy
		end;
	end;

	% normalize the last dimension
	% ----------------------------
	if normalize
        tmpjp = jp;
        if normalize == 2,
            tmpjp = sort(jp);
            tmpjp = tmpjp(round(length(tmpjp)*0.1):end-round(length(tmpjp)*0.1));
        end;
        try,
            switch ndims( signal )
             case 2,	jp = (jp-mean(tmpjp)) / std(tmpjp);
             case 3,	jp = (jp-mean(tmpjp,2)*ones(1,size(jp,2)))./ ...
                  (std(tmpjp,0,2)*ones(1,size(jp,2)));
            end;
        catch, error('Error while normalizing'); end;
	end;
end

% reject
% ------
if threshold ~= 0
    if length(threshold) > 1
    	rej = (threshold(1) > jp) | (jp > threshold(2));
    else
    	rej = abs(jp) > threshold;
    end;
else
	rej = zeros(size(jp));
end;

return;


function [minFlux,maxFlux,Vmin,Vmax] = fluxVariability(model,optPercentage,osenseStr,rxnNameList,verbFlag, allowLoops)
%fluxVariability Performs flux variablity analysis
%
% [minFlux,maxFlux] = fluxVariability(model,optPercentage,osenseStr,rxnNameList,verbFlag, allowLoops)
%
%INPUT
% model             COBRA model structure
%
%OPTIONAL INPUTS
% optPercentage     Only consider solutions that give you at least a certain
%                   percentage of the optimal solution (Default = 100
%                   or optimal solutions only)
% osenseStr         Objective sense ('min' or 'max') (Default = 'max')
% rxnNameList       List of reactions for which FVA is performed
%                   (Default = all reactions in the model)
% verbFlag          Verbose output (opt, default false)
% allowLoops        Whether loops are allowed in solution. (Default = true)
%                   See optimizeCbModel for description
%
%OUTPUT
% minFlux           Minimum flux for each reaction
% maxFlux           Maximum flux for each reaction
%
%OPTIONAL OUTPUT
% Vmin          Matrix of column flux vectors, where each column is a
%               separate minimization.
% Vmax          Matrix of column flux vectors, where each column is a
%               separate maximization.
%

% Markus Herrgard  8/21/06 Original code.
% Ronan Fleming   01/20/10 Take the extremal flux from the flux vector,
%                          not from the objective since this is invariant
%                          to the value and sign of the coefficient
% Ronan Fleming   27/09/10 Vmin,Vmax

if (nargin < 2)
    optPercentage = 100;
end
if (nargin < 3)
    osenseStr = 'max';
end
if (nargin < 4)
    rxnNameList = model.rxns;
end
if (nargin < 5)
    verbFlag = false;
end
if (nargin < 6)
    allowLoops = true;
end
if (isempty(optPercentage))
    optPercentage = 100;
end
if (isempty(osenseStr))
    osenseStr = 'max';
end
if (isempty(rxnNameList))
    rxnNameList = model.rxns;
end

% LP solution tolerance
global CBT_LP_PARAMS
if (exist('CBT_LP_PARAMS', 'var'))
    if isfield(CBT_LP_PARAMS, 'objTol')
        tol = CBT_LP_PARAMS.objTol;
    else
        tol = 1e-6;
    end
    if isfield(CBT_LP_PARAMS, 'minNorm')
        minNorm = CBT_LP_PARAMS.minNorm;
    else
        minNorm = 0;
    end
else
    tol = 1e-6;
    minNorm = 0;
end

% Determine constraints for the correct space (0-100% of the full space)
if (sum(model.c ~= 0) > 0)
    hasObjective = true;
    optSol = optimizeCbModel(model,osenseStr, 0, allowLoops);
    if (optSol.stat > 0)
        objRxn = model.rxns(model.c~=0);
        if (strcmp(osenseStr,'max'))
            objValue = floor(optSol.f/tol)*tol*optPercentage/100;
        else
            objValue = ceil(optSol.f/tol)*tol*optPercentage/100;
        end
    else
        error('Infeasible problem - no optimal solution!');
    end
else
    hasObjective = false;
end

if (verbFlag == 1)
    h = waitbar(0,'Flux variability analysis in progress ...');
end
if (verbFlag > 1)
    fprintf('%4s\t%4s\t%10s\t%9s\t%9s\n','No','Perc','Name','Min','Max');
end

if (~isfield(model,'b'))
    model.b = zeros(size(model.S,1),1);
end
% Set up the general problem
[nMets,nRxns] = size(model.S);
rxnListFull = model.rxns;
LPproblem.c = model.c;
LPproblem.lb = model.lb;
LPproblem.ub = model.ub;
LPproblem.csense(1:nMets) = 'E';
LPproblem.csense = LPproblem.csense';
if hasObjective
    LPproblem.A = [model.S;columnVector(model.c)'];
    LPproblem.b = [model.b;objValue];
    if (strcmp(osenseStr,'max'))
        LPproblem.csense(end+1) = 'G';
    else
        LPproblem.csense(end+1) = 'L';
    end
else
    LPproblem.A = model.S;
    LPproblem.b = model.b;
end

% %solve to generate initial basis
LPproblem.osense = -1;
tempSolution = solveCobraLP(LPproblem);
LPproblem.basis = tempSolution.basis;

% Loop through reactions
maxFlux = zeros(length(rxnNameList), 1);
minFlux = zeros(length(rxnNameList), 1);

if length(minNorm)> 1 || minNorm > 0
    Vmin=zeros(nRxns,nRxns);
    Vmax=zeros(nRxns,nRxns);
    %minimizing the Euclidean norm gets rid of the loops, so there
    %is no need for a second slower MILP approach
    allowLoops=1;
else
    Vmin=[];
    Vmax=[];
end

solutionPool = zeros(length(model.lb), 0);
if ~exist('matlabpool') || (matlabpool('size') == 0) %aka nothing is active
    m = 0;
    for i = 1:length(rxnNameList)
        if mod(i,10) == 0, clear mex, end
        if (verbFlag == 1),fprintf('iteration %d.  skipped %d\n', i, round(m));end
        LPproblem.c = zeros(nRxns,1);
        rxnBool=strcmp(rxnListFull,rxnNameList{i});
        LPproblem.c(rxnBool) = 1; %no need to set this more than 1
        % do LP always
        LPproblem.osense = -1;
        LPsolution = solveCobraLP(LPproblem);
        %take the maximum flux from the flux vector, not from the obj -Ronan
        maxFlux(i) = LPsolution.full(LPproblem.c~=0);

        %minimise the Euclidean norm of the optimal flux vector to remove
        %loops -Ronan
        if length(minNorm)> 1 || minNorm > 0
            QPproblem=LPproblem;
            QPproblem.lb(LPproblem.c~=0)=maxFlux(i)-1e-12;
            QPproblem.ub(LPproblem.c~=0)=maxFlux(i)+1e12;
            QPproblem.c(:)=0;
            %Minimise Euclidean norm using quadratic programming
            if length(minNorm)==1
                minNorm=ones(nRxns,1)*minNorm;
            end
            QPproblem.F = spdiags(minNorm,0,nRxns,nRxns);
            %quadratic optimization
            solution = solveCobraQP(QPproblem);
            if isempty(solution.full)
                pause(eps)
            end
            Vmax(:,rxnBool)=solution.full(1:nRxns,1);
        end

        LPproblem.osense = 1;
        LPsolution = solveCobraLP(LPproblem);
        %take the maximum flux from the flux vector, not from the obj -Ronan
        minFlux(i) = LPsolution.full(LPproblem.c~=0);

        %minimise the Euclidean norm of the optimal flux vector to remove
        %loops
        %minimise the Euclidean norm of the optimal flux vector to remove
        %loops
        if length(minNorm)> 1 || minNorm > 0
            QPproblem=LPproblem;
            QPproblem.lb(LPproblem.c~=0)=maxFlux(i)-1e-12;
            QPproblem.ub(LPproblem.c~=0)=maxFlux(i)+1e12;
            QPproblem.c(:)=0;
            QPproblem.F = spdiags(minNorm,0,nRxns,nRxns);
            %Minimise Euclidean norm using quadratic programming
            if length(minNorm)==1
                minNorm=ones(nRxns,1)*minNorm;
            end
            QPproblem.F = spdiags(minNorm,0,nRxns,nRxns);
            %quadratic optimization
            solution = solveCobraQP(QPproblem);
            Vmin(:,rxnBool)=solution.full(1:nRxns,1);
        end

        if ~allowLoops
            if any( abs(LPproblem.c'*solutionPool - maxFlux(i)) < tol) % if any previous solutions are good enough.
                % no need to do anything.
                m = m+.5;
            else
                LPproblem.osense = -1;
                LPsolution = solveCobraMILP(addLoopLawConstraints(LPproblem, model));
                maxFlux(i) = LPsolution.obj/1000;
              end
            if any( abs(LPproblem.c'*solutionPool - minFlux(i)) < tol)
                m = m+.5;
                % no need to do anything.
            else
                LPproblem.osense = 1;
                LPsolution = solveCobraMILP(addLoopLawConstraints(LPproblem, model));
                minFlux(i) = LPsolution.obj/1000;
            end
        end
        if (verbFlag == 1)
            waitbar(i/length(rxnNameList),h);
        end
        if (verbFlag > 1)
            fprintf('%4d\t%4.0f\t%10s\t%9.3f\t%9.3f\n',i,100*i/length(rxnNameList),rxnNameList{i},minFlux(i),maxFlux(i));
        end
    end
else % parallel job.  pretty much does the same thing.
    parfor i = 1:length(rxnNameList)
        %if mod(i,10) == 0, clear mex, end
        %if (verbFlag == 1),fprintf('iteration %d.  skipped %d\n', i, round(m));end
        c = zeros(nRxns,1);
        c(strcmp(rxnListFull,rxnNameList{i})) = 1000;
        if allowLoops % do LP
            LPsolution = solveCobraLP(struct(...
                'A', LPproblem.A,...
                'b', LPproblem.b,...
                'lb', LPproblem.lb,...
                'ub', LPproblem.ub,...
                'csense', LPproblem.csense,...
                'c',c,...
                'osense',-1, ...
                'basis', LPproblem.basis ...
            ));
            %take the maximum flux from the flux vector, not from the obj -Ronan
            maxFlux(i) = LPsolution.full(c~=0);
            %LPproblemb.osense = 1;
            LPsolution = solveCobraLP(struct(...
                'A', LPproblem.A,...
                'b', LPproblem.b,...
                'lb', LPproblem.lb,...
                'ub', LPproblem.ub,...
                'csense', LPproblem.csense,...
                'c',c,...
                'osense',1, ... %only part that's different.
                'basis', LPproblem.basis ...
            ));
            minFlux(i) = LPsolution.full(c~=0);
        else
            LPsolution = solveCobraMILP(addLoopLawConstraints(struct(...
                'A', LPproblem.A,...
                'b', LPproblem.b,...
                'lb', LPproblem.lb,...
                'ub', LPproblem.ub,...
                'csense', LPproblem.csense,...
                'c',c,...
                'osense',-1 ...
            ), model));
            maxFlux(i) = LPsolution.obj/1000;

            LPsolution = solveCobraMILP(addLoopLawConstraints(struct(...
                'A', LPproblem.A,...
                'b', LPproblem.b,...
                'lb', LPproblem.lb,...
                'ub', LPproblem.ub,...
                'csense', LPproblem.csense,...
                'c',c,...
                'osense',1 ...
            ), model));%
            minFlux(i) = LPsolution.obj/1000;
        end
    end
end

if (verbFlag == 1)
	if ( regexp( version, 'R20') )
        	close(h);
	end
end

maxFlux = columnVector(maxFlux);
minFlux = columnVector(minFlux);


function bsv_test04 ( )

%*****************************************************************************80
%
%% BSV_TEST04 varies the location of the left boundary A in the Burgers equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BSV_TEST04:\n' );
  fprintf ( 1, '  Solution of steady viscous Burgers equation.\n' );
  fprintf ( 1, '  Vary the left boundary location A around the value -1.\n' );

  a_test = [ -1.04, -1.02, -1.01, -1.005, -1.0, -0.995, -0.99, -0.98, -0.96 ];
  test_num = length ( a_test );
  b = +1.0;
  alpha = 1.0;
  beta = -1.0;
  nu = 0.1;
  n = 161;
  output = 0;

  u = zeros(n,test_num);

  fprintf ( 1, '\n' );

  for test = 1 : test_num

    a = a_test(test);

    fprintf ( 1, '  Using A = %g\n', a );

    u(:,test) = bsv ( a, b, alpha, beta, nu, n, output );

  end

  x = linspace ( a, b, n );

  figure ( 4 )
  clf
  hold on
  plot ( x, u, 'b-', 'LineWidth', 3 );
  plot ( [a,b], [0.0,0.0], 'r-', 'LineWidth', 2 )
  grid on
  title ( 'A = -1.04, -1.02, -1.01, -1.005, -1, -0.995, -0.99, -0.98, -0.96' )
  xlabel ( '<--- X --->' )
  ylabel ( '<---U(X) --->' )
  axis ( [ a, b, -1.5, alpha ] )
  hold off
  filename = 'bsv_test04.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot to file "%s".\n', filename );

  return
end


function nlogL = DN_nlogL_norm(y)
% DN_nlogL_norm     Negative log likelihood of data coming from a Gaussian distribution.
%
% Fits a Gaussian distribution to the data using the normfit function in
% Matlab's Statistics Toolbox and returns the negative log likelihood of the
% data coming from a Gaussian distribution using the normlike function.
%
%---INPUT:
% y, a vector of data

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

[muhat, sigmahat] = normfit(y);
nlogL = normlike([muhat, sigmahat],y)/length(y);

% ** Somehow this just scales with length, regardless of the distribution of y?

end


function g = rbfOutputGradX(model, X)

% RBFOUTPUTGRADX Evaluate derivatives of a RBF model's output with respect to inputs.
% FORMAT
% DESC returns the derivatives of the outputs of an periodic radial basis function model with
% respect to the inputs to the model. Currently a wrapper for rbfjacob.
% ARG model : the model for which the derivatives will be computed.
% ARG X : the locations at which the derivatives will be computed.
% RETURN g : the gradient of the output with respect to the inputs.
%
% SEEALSO : rbfOutputGrad, modelOutputGradX, rbfjacob
%
% COPYRIGHT : Neil D. Lawrence, 2008

% MLTOOLS

g = rbfjacob(model, X);


% Concatenate the neighbouring frames vectors to higher dimensional vectors
% Input is Number of dimension x number of frames
function [y] = ExpandContext(x, context, window_type)
[dim nFr] = size(x);

if nargin<3
    window_type = 'null';
end

if context>1
    half_context = (context-1)/2;
    idx = [ones(1,half_context) 1:nFr ones(1,half_context)*nFr];
    x = x(:,idx);
    %x = [ repmat(x(:,1),1,half_context) x repmat(x(:,end), 1, half_context) ];
    if 0    % this implementation may be slow
        y = [];
        for i=1:context
            y = [y; x(:,i:end-context+i)];
        end
    else
        if strcmp(class(x), 'gpuArray')
            y = gpuArray.zeros(dim*context,nFr);
        else
            y = zeros(dim*context,nFr);    % allocate memory first
        end
        context = gather(context);
        for i=1:context
            y((i-1)*dim+1:i*dim,:) = x(:,i:end-context+i);
        end
    end
else
    y = x;
end

switch window_type
    case 'null'
        window = [];
    case 'Hamming'
        window = hamming(context);
        window = repmat(window, 1, dim);
        window = reshape(window', dim*context, 1);
        window = diag(window);
        y = window * y;
    otherwise
        fprintf('Unkown window type\n');
        return;
end


function beta = l1plusTV(H, Ht, A, At, y, sizeImage, TVlambda, p, normfac, err, insweep, decfac, tol)

% Algorithm for solving problems of the form:
% min ||x||_p + TVlambda*TV(A'x)s.t. ||y-Hx||_2 < err

% Inputs
% H - Forward Measurement Operator
% Ht - Backward Measurement Operator
% A - Sparsifying Forward Transform
% At - Sparsifying Backward Transform
% y - collected data
% sizeImage - size of Image matrix
% TVlambda - multiplication factor for TV norm
% p - non-convex norm (default 1)
% normfac - highest singular value of A (default 1)
% err - two norm mismatch (default 1e-6)
% insweep - number of sweeps for internal loop (default 50)
% decfac - lambda decrease factor for outside loop (default 0.5)
% tol - tolerance for convergence (default 1e-4)

% copyright (c) Angshul Majumdar 2010

N = sizeImage;
c = 4;

if nargin < 9
    p = 1;
end
if nargin < 10
    normfac = 1;
end
if nargin < 11
    err = 1e-6;
end
if nargin < 12
    insweep = 50;
end
if nargin < 13
    decfac = 0.5;
end
if nargin < 14
    tol = 1e-4;
end

alpha = 1.1*normfac;
x_initial = Ht(y);
z_initial = zeros(length(TV(x_initial)),1);

x = x_initial; z = z_initial;

lambdaInit = decfac*max(abs(Ht(y))); lambda = lambdaInit;

f_current = norm(y-H(x_initial)) + lambda*norm(x,p) + TVlambda*norm(TV(At(x)),1);

while lambda > lambdaInit*tol
    lambda
    for i = 1:insweep
        f_previous = f_current;

        Wtilde = abs(x).^(p-2);
        W = 1 + (lambda/2)*Wtilde;

        b = x + (1/alpha)*(Ht(y-H(x)));
        btilde = (1./sqrt(W)).*b;
        z = (c*z + weightedTV(b-weightedTVtrans(z)))./((2*alpha/TVlambda)*(abs(weightedTV(x))).^(2-p)+c);
        x = 1./sqrt(W).*btilde - (1./sqrt(W)).*weightedTVtrans(z);

        f_current = norm(y-H(x_initial)) + lambda*norm(x,p) + TVlambda*norm(TV(x),1);

        if norm(f_current-f_previous)/norm(f_current + f_previous)<tol
            break;
        end
    end
    if norm(y-H(x))<err
        break;
    end
    lambda = decfac*lambda;
end
beta = x;

%%%%%% Functions for computing TV and TVtranspose %%%%%%%

    function wFvec = weightedTV(Xvec)
        wFvec = TV((1./sqrt(W)).*Xvec);
    end

    function wXvec = weightedTVtrans(Fvec)
        wXvec = (1./sqrt(W)).*TVtrans(Fvec);
    end

    function Fvec = TV(Xvec)
        Ivec = At(Xvec); % image pixels
        X = reshape(Ivec,N(1),N(2)); % sparse transform coefficients

        FX = zeros(N);
        FY = zeros(N);

        FX(1:end-1,:) = diff(X);
        FY(:,1:end-1) = diff(X')';

        Fvec = [FX(:)' FY(:)']';
    end

    function Xvec = TVtrans(Fvec)
        FX = reshape(Fvec(1:end/2),N(1),N(2));
        FY = reshape(Fvec(1+end/2:end),N(1),N(2));

        DX = zeros(N(1)+1,N(2));
        DY = zeros(N(1),N(2)+1);
        DX(2:end-1,:) = FX(1:end-1,:);
        DY(:,2:end-1) = FY(:,1:end-1);

        XT = -diff(DX);
        YT = -diff(DY')';

        X = XT + YT; % image
        Xvec = A(X(:));
    end
end


function [mix] = spm_mix (y,m,verbose)
% Fit a multivariate Gaussian Mixture model using VB
% FORMAT [mix] = spm_mix (y,m,verbose)
%
% y          [N x d] data matrix containing N samples of d-dim data
% m          Number of mixture components
% verbose    Set to 1 to see evolution of free energy, 0 otherwise
%            (default=1)
%
% mix        Returned model
%
%--------------------------------------------------------------------------
% The fields in mix are:
%
% m                The number of components
% fm               The negative free energy. This decomposes into
%                  fm=acc-kl_proportions-kl_covs-kl_centres
%
% acc              model accuracy
% kl_proportions   complexity penalty for cluster proportions
% kl_covs          complexity penalty for cluster covariances
% kl_centres       complexity penalty for cluster centres
%
% Fields:
%
% lambda           Post mixers, q(pi|D) = D(lambda)
% gamma            [m x N] matrix of belonging probabilities
% state(s).a       Post precisions, q(Gamma|D)=W(a,B)
% state(s).B
% state(s).C       Post covariance
% state(s).m       Post mean, q(mu|D)=N(m_s,beta_s Gamma_s)
% state(s).beta
% state(s).prior   Estimated mixing proportions
%
%                  In the field prior:
%
% lambda_0         Prior mixers, p(pi) = D(lambda_0)
% a_0,B_0          Prior precisions, p(Gamma)=W(a_0,B_0)
% m_0,beta_0       Prior means, p(mu)=N(m_0,beta_0 Gamma_s)
%
%__________________________________________________________________________
% Copyright (C) 2007-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mix.m 5962 2014-04-17 12:47:43Z spm $

% This code implements the algorithm in:
%
% See Attias, H. (2000) A Variational
% Bayesian framework for Graphical Models, NIPS.
%
% See also:
%
% W.D. Penny (2001) Variational Bayes for d-dimensional Gaussian mixture models
% Wellcome Department of Imaging Neuroscience, University College London.
%
% for Negative Free Energy expression and validation.

if nargin < 3
    verbose=1;
end
[N,d]=size(y);

% Put model info into data structure format
mix.nin=d;
mix.m=m;

% PRIORS
% Set mixing priors
lambda_0=1;
% Set mean priors
m_0=mean(y);
beta_0=1;
% Set precision priors
% These settings set the prior covariance matrix to be
% equal to the 0.01*d*identity matrix - if original data is
% zmuv, this is reasonable
a_0=d;
B_0=0.01*d*eye(d);
%B_0=d*eye(d);

% Also, store in data structure ready for output
mix.prior.lambda_0=lambda_0;
mix.prior.m_0=m_0;
mix.prior.beta_0=beta_0;
mix.prior.a_0=a_0;
mix.prior.B_0=B_0;

sd=0;
for i=1:d,
  sd=sd+psi((a_0+1-i)/2);
end
log_tilde_gamma_0=sd-log(det(B_0))+d*log(2);

if (m==1)
    % For a single component we just have a Gaussian model

    % Set posterios
    state(1).a=N+a_0;
    state(1).beta=N+beta_0;
    state(1).m=m_0;
    Cy=cov(y);
    state(1).B=N*Cy+B_0;

    fm=spm_lg_gamma (d,0.5*state(1).a);
    fm=fm-spm_lg_gamma (d,0.5*a_0);
    fm=fm-0.5*d*N*log(pi);
    fm=fm+0.5*d*(log(beta_0)-log(state(1).beta));
    fm=fm+0.5*a_0*log(det(B_0));
    fm=fm-0.5*state(1).a*log(det(state(1).B));

    mix.m=m;
    mix.fm=fm;
    state(1).C=state(1).B/state(1).a;
    mix.state=state;

    mix.priors=1;
    return;
end

% Run kmeans on the data
% to initialise posterior
[priors,means,covs] = spm_kmeans(y,m);

% Add pseudo-counts to ML priors
priors=priors+(1/N);
lambda=priors;
for s=1:m,
  state(s).m=means(s,:);
  state(s).beta=priors(s)*N+beta_0;
  state(s).a=priors(s)*N+a_0;
  Cov=covs(:,:,s);
  % If component cov is rank deficient set to prior
  if rank(Cov) < size(Cov,1)
    state(s).B=B_0;
  else
    state(s).B=priors(s)*N*Cov;
  end
  N_bar(s)=priors(s)*N;
end

% Start algorithm
lik=[];
tol=0.0001;
max_loops=32;
for loops=1:max_loops,

    % E-step
    lambda_tot=sum(lambda);
    for s=1:m,
        state(s).bar_gamma=state(s).a*inv(state(s).B);
        log_tilde_pi(s)=psi(lambda(s))-psi(lambda_tot);
        sd=0;
        for i=1:d,
            sd=sd+psi((state(s).a+1-i)/2);
        end
        log_tilde_gamma(s)=sd-log(det(state(s).B))+d*log(2);

        tilde_pi(s)=exp(log_tilde_pi(s));
        tilde_gamma(s)=exp(log_tilde_gamma(s));
        for n=1:N,
            gamma(s,n)=tilde_pi(s)*tilde_gamma(s)^0.5;
            dy=(y(n,:)-state(s).m);
            gamma(s,n)=gamma(s,n)*(exp(-0.5*dy*state(s).bar_gamma*dy')+eps)*exp(-d/(2*state(s).beta));
        end
    end

    gamma_n=sum(gamma);
    for s=1:m,
        if mean(gamma_n) > eps
            % If component still exists
            gamma(s,:)=gamma(s,:)./gamma_n;
        end
    end

    % M-step

    % Part-I
    for s=1:m,
        pi_bar(s)=mean(gamma(s,:))+eps;
        N_bar(s)=N*pi_bar(s)+eps;
        bar_mu(s,:)=(1/N_bar(s))*sum(gamma(s,:)'*ones(1,d).*y);

    end

    % get weighted means and covariances
    for s=1:m,
        state(s).bar_sigma=zeros(d,d);
        for n=1:N,
            dy=y(n,:)-bar_mu(s,:);
            state(s).bar_sigma=state(s).bar_sigma+gamma(s,n).*(dy'*dy);
        end
        state(s).bar_sigma=(state(s).bar_sigma)/N_bar(s);

        if isnan(state(s).bar_sigma)
            state(s).bar_sigma
        end
    end

    % Now compute the free energy
    f1=-spm_kl_dirichlet(lambda,lambda_0*ones(1,m),log_tilde_pi);
    for s=1:m,
        f2(s)=-spm_kl_wishart(state(s).a,state(s).B,a_0,B_0);

        % KL-method for computing f3(s)
        Cs=state(s).B/(state(s).beta*state(s).a);
        C0=state(s).B/(beta_0*state(s).a);
        f3(s)=-spm_kl_normal(state(s).m,Cs,m_0,C0);

        % Check f3(s)
        check_f3=-0.5*d*(log(state(s).beta)-log(beta_0)+(beta_0/state(s).beta)-1);
        dm=state(s).m-m_0;
        check_f3=check_f3-0.5*dm*beta_0*state(s).a*inv(state(s).B)*dm';
        f3(s)=check_f3;

        f4(s)=N_bar(s)*log_tilde_pi(s)-sum(gamma(s,:).*log(gamma(s,:)+eps));
        LaB=0;
        for i=1:d,
            LaB=LaB+psi((state(s).a+1-i)/2);
        end
        LaB=LaB+d*log(2)-log(det(state(s).B));
        f5(s)=0.5*N_bar(s)*(-d*log(2*pi)+LaB-trace(state(s).bar_gamma*state(s).bar_sigma)-(d/state(s).beta));
        fkl(s)=f2(s)+f3(s)+f4(s)+f5(s);
        fkl_adj(s)=f2(s)+f3(s)+(f4(s)+f5(s))/N_bar(s);
    end
    fm=f1+sum(f2)+sum(f3)+sum(f4)+sum(f5);

    acc=sum(f4)+sum(f5);
    kl_proportions=-f1;
    kl_covs=-sum(f2);
    kl_centres=-sum(f3);

    if verbose
        disp(sprintf('Iter=%d, F1=%1.2f, F2=%1.2f, F3=%1.2f, F4=%1.2f, F5=%1.2f, Fm=%1.2f',loops,f1,sum(f2),sum(f3),sum(f4),sum(f5),fm));
    end

    % Convergence criterion
    oldlik=lik;
    lik=fm;
    if (loops>1)
        if abs((lik-oldlik)/lik) < tol
            break;
        end
    end;

    % Part-II

    for s=1:m,
        % Posterior mixing coefficients
        lambda(s)=N_bar(s)+lambda_0;
        % Posterior means
        state(s).m=(N_bar(s)*bar_mu(s,:)+beta_0*m_0)/(N_bar(s)+beta_0);
        state(s).beta=N_bar(s)+beta_0;
        % Posterior precisions
        state(s).a=N_bar(s)+a_0;
        dy=bar_mu(s,:)-m_0;
        state(s).B=N_bar(s)*state(s).bar_sigma + (N_bar(s)*beta_0*dy'*dy)/(N_bar(s)+beta_0)+B_0;
    end
end

% Put variables into data structure
mix.m=m;
mix.fm=fm;
mix.acc=acc;
mix.kl_proportions=kl_proportions;
mix.kl_covs=kl_covs;
mix.kl_centres=kl_centres;
mix.state=state;
mix.lambda=lambda;
mix.gamma=gamma;

% Put info into data structure
for j=1:m,
    mix.state(j).prior=pi_bar(j);
    mix.state(j).C=mix.state(j).B/mix.state(j).a;
end


function sir = SIR(Ori,Est)

  s = Ori(:);

  t = Est(:);

  sir = 10*log10( (s'*s) / ((s-t)'*(s-t)) );


function g = coeff_var_dct(I)

temp1=dct2(I);
temp2=temp1(:);
temp3=temp2(2:end);

%g=kurtosis(temp3);
g=coeff_var_gen_gauss(temp3);


function dataout=modeWPE_NLMS_BSS(nummics, numrefs, datain)
%
% Perform wpe, nlms, and bss.
% nummics:              no. of mic channels
% numrefs:              no. of reference channels
% datain:               input data
% dataout:              output data
%

config;

%% perform wpe
addpath('wpe_v1.33');

mic=datain(:, 1:2);
ref=datain(:, 3);

cfgs='wpe_v1.33/settings/local.m';
early=wpe(mic, cfgs);

%% perform nlms
addpath('Speex-AEC-matlab-master');

%    Usage:
%
%       speex_mdf_out = speex_mdf(Fs, u, d, filter_length, frame_size, dbg_var_name);
%
%       Fs                  sample rate
%       u                   speaker signal, column vector in range [-1; 1]
%       d                   microphone signal, column vector in range [-1; 1]
%       filter_length       typically 250ms, i.e. 4096 @ 16k FS
%                           must be a power of 2
%       frame_size          typically 8ms, i.e. 128 @ 16k Fs
%                           must be a power of 2
%       dbg_var_name        internal state variable name to trace.
%                           Default: 'st.leak_estimate'.
%
%    Jonathan Rouach <jonr@waves.com>
%
filter_length=stftshift*AEC_FLEN;
frame_size=stftshift;
nearend1=speex_mdf(fs, ref, early(:, 1), filter_length, frame_size);
nearend2=speex_mdf(fs, ref, early(:, 2), filter_length, frame_size);
nearend=[nearend1.e, nearend2.e];

%% perform stft
addpath('stft2');

M=nummics;
N=M;

Xtf=cell(M, 1);
for m=1:M
    Xtf{m}=stft(nearend(:, m), stftshift, fftsize, false);
end
[K, T]=size(Xtf{1});

Ytf=cell(M, 1);
for m=1:M
    Ytf{m}=zeros(K, T);
end

%% params go to config

%% space for bss
% the weighted correlation matrices
C1=cell(K, 1);
C2=cell(K, 1);
for k=1:K
    C1{k}=STABLE_EPS*eye(M, M);
    C2{k}=STABLE_EPS*eye(M, M);
end

% demixing matrices
Demix=cell(K, 1);
for k=1:K
    Demix{k}=eye(N, M);
end

%% perform bss
for tau=1:T
    %% perform bss
    Bssout=zeros(K, M);

    %
    % calculate nonlinearity
    %
    phi1=0;
    phi2=0;

    for k=1:K
        x=zeros(M, 1);
        for m=1:M
            x(m)=Xtf{m}(k, tau);
        end

        y=Demix{k}*x;
        % output data
        Bssout(k, :)=y.';

        phi1=phi1+abs(y(1))^2;
        phi2=phi2+abs(y(2))^2;
    end

    phi1=(1-BF_FORGET)*(phi1+VAR_BIAS)^((GAMMA-2)/2);
    phi2=(1-BF_FORGET)*(phi2+VAR_BIAS)^((GAMMA-2)/2);

    % update the demixing matrices
    for k=1:K
        %
        % accumulate the weighted correlation
        %
        x=zeros(M, 1);
        for m=1:M
            x(m)=Xtf{m}(k, tau);
        end

        C1{k}=BF_FORGET*C1{k}+phi1*(x*x');
        C2{k}=BF_FORGET*C2{k}+phi2*(x*x');

        %
        % solve gev problem
        %
        [Ev, Ed]=eig(C2{k}+BF_DIAGLOAD*eye(M, M), C1{k}+BF_DIAGLOAD*eye(M, M));
        if Ed(1, 1)>=Ed(2, 2)
            e1=Ev(:, 1);
            e2=Ev(:, 2);
        else
            e1=Ev(:, 2);
            e2=Ev(:, 1);
        end

        D=[e1'; e2'];

        %
        % solve the scaling ambiguity
        %
        A=inv(D);

        if abs(A(1, 1))>=abs(A(2, 1))
            a1=A(1, 1);
        else
            a1=A(2, 1);
        end

        if abs(A(2, 2))>=abs(A(1, 2))
            a2=A(2, 2);
        else
            a2=A(1, 2);
        end

        D=diag([a1; a2])*D;
        Demix{k}=D;
    end

    for m=1:M
        Ytf{m}(:, tau)=Bssout(:, m);
    end
end

%% perform istft and output signal
dataout=zeros(dataLength(T, stftshift, fftsize ), N);
for n=1:N
    dataout(:, n)=istft(Ytf{n}, stftshift, false);
end

end


function B = ordfilt2(varargin)
%ORDFILT2 Perform 2-D order-statistic filtering.
%   B=ORDFILT2(A,ORDER,DOMAIN) replaces each element in A by the
%   ORDER-th element in the sorted set of neighbors specified by
%   the nonzero elements in DOMAIN.
%
%   B = ORDFILT2(A,ORDER,DOMAIN,S), where S is the same size as
%   DOMAIN, uses the values of S corresponding to the nonzero
%   values of DOMAIN as additive offsets.
%
%   B = ORDFILT2(...,PADOPT) controls how the matrix boundaries
%   are padded.  PADOPT may be 'zeros' (the default) or
%   'symmetric'.  If PADOPT is 'zeros', A is padded with zeros at
%   the boundaries.  If PADOPT is 'symmetric', A is symmetrically
%   extended at the boundaries.
%
%   Class Support
%   -------------
%   The class of A may be numeric or logical.  The class of B is
%   the same as the class of A, unless the additive offset form of
%   ORDFILT2 is used, in which case the class of B is double.
%
%   Example
%   -------
%   Use a maximum filter on snowflakes.png with a [5 5] neighborhood.  This is
%   equivalent to imdilate(image,strel('square',5)).
%
%       A = imread('snowflakes.png');
%       B = ordfilt2(A,25,true(5));
%       imview(A), imview(B)
%
%   Remarks
%   -------
%   DOMAIN is equivalent to the structuring element used for
%   binary image operations. It is a matrix containing only 1's
%   and 0's; the 1's define the neighborhood for the filtering
%   operation.
%
%   For example, B=ORDFILT2(A,5,ONES(3,3)) implements a 3-by-3
%   median filter; B=ORDFILT2(A,1,ONES(3,3)) implements a 3-by-3
%   minimum filter; and B=ORDFILT2(A,9,ONES(3,3)) implements a
%   3-by-3 maximum filter.  B=ORDFILT2(A,4,[0 1 0; 1 0 1; 0 1 0])
%   replaces each element in A by the maximum of its north, east,
%   south, and west neighbors.
%
%   See also MEDFILT2.

%   Copyright 1993-2003 The MathWorks, Inc.
%   $Revision: 5.17.4.5 $  $Date: 2003/08/23 05:53:07 $

[A,order,domain,s,padopt,msg] = ParseInputs(varargin{:});

domainSize = size(domain);
center = floor((domainSize + 1) / 2);
[r,c] = find(domain);
r = r - center(1);
c = c - center(2);
padSize = max(max(abs(r)), max(abs(c)));
originalSize = size(A);
if (strcmp(padopt, 'zeros'))
    A = padarray(A, padSize * [1 1], 0, 'both');
elseif (strcmp(padopt, 'ones'))
    % padopt of 'ones' is for support of medfilt2; it is
    % undocumented
    A = padarray(A, padSize * [1 1], 1, 'both');
else
    A = padarray(A, padSize * [1 1], 'symmetric', 'both');
end
Ma = size(A,1);
offsets = c*Ma + r;

% make sure that offsets are valid
if ~isreal(offsets) || any(floor(offsets) ~= offsets) || any(~isfinite(offsets))
    %should never get here
    eid = sprintf('Images:%s:internalError', mfilename);
    msg = 'Internal error: bad OFFSETS.';
    error(eid,'%s',msg);
end

if isempty(s)
  %ORDFILT2(A,ORDER,DOMAIN)
  B = ordf(A, order, offsets, [padSize padSize] + 1, ...
             originalSize, domainSize);
else
  %ORDFILT2(A,ORDER,DOMAIN,S,PADOPT)
  B = ordf(A, order, offsets, [padSize padSize] + 1, ...
           originalSize, domainSize, s);
end

%%%
%%% ParseInputs
%%%
function [A,order,domain,s,padopt,msg] = ParseInputs(varargin)

A = [];
order = [];
domain = [];
s = [];
padopt = 'zeros';
msg = '';

% checknargin(3,5,nargin,mfilename);

A = varargin{1};
order = varargin{2};
domain = varargin{3};
options = {'zeros', 'ones', 'symmetric'};
% padopt of 'ones' is for supporting medfilt2; it is undocumented.

if (nargin == 4)
  if (ischar(varargin{4}))
    padopt = checkstrs(varargin{4},options,mfilename,'PADOPT',4);
  else
    s = varargin{4};
  end

elseif (nargin == 5)
  s = varargin{4};
  padopt = checkstrs(varargin{5},options,mfilename,'PADOPT',5);
end

% make sure that arguments are valid
% checkinput(order,'double',{'real','scalar','integer'},mfilename, ...
%           'ORDER',2);

if ~isempty(s)
  if (~isa(A, 'double'))
    A = double(A);
  end
%  checkinput(A, 'double', {'2d','real'}, mfilename, 'A', 1);
  s = s(find(domain));
%  checkinput(s, 'double', 'real', mfilename, 'S', 4);
else
%  checkinput(A, {'numeric','logical'}, {'2d','real'}, mfilename, 'A', 1);
end


function degree = keast_degree ( rule )

%*****************************************************************************80
%
%% KEAST_DEGREE returns the degree of a Keast rule for the tetrahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Keast,
%    Moderate Degree Tetrahedral Quadrature Formulas,
%    Computer Methods in Applied Mechanics and Engineering,
%    Volume 55, Number 3, May 1986, pages 339-348.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer DEGREE, the degree of the rule.
%
  if ( rule == 1 )
    degree = 0;
  elseif ( rule == 2 )
    degree = 1;
  elseif ( rule == 3 )
    degree = 2;
  elseif ( rule == 4 )
    degree = 3;
  elseif ( rule == 5 )
    degree = 4;
  elseif ( rule == 6 )
    degree = 4;
  elseif ( rule == 7 )
    degree = 5;
  elseif ( rule == 8 )
    degree = 6;
  elseif ( rule == 9 )
    degree = 7;
  elseif ( rule == 10 )
    degree = 8;
  else
    degree = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KEAST_DEGREE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of RULE = %d\n', rule );
    error ( 'KEAST_DEGREE - Fatal error!' )
  end

  return
end


function [camPoints, prjPoints, Nodes, Edges] = getMatchedNodes(whiteLight, colorGrid, camCorners, prjW, prjH, verbose)
%% Get matched Nodes on both camera and projector image.
% Tthe outputs are matched camera and projector 2D point pairs.

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%% Step 1. Extract grid mask on the white board
[imBWGrid, imColorGridMasked] = ImgProc.segColorGrid(whiteLight, colorGrid, camCorners, verbose);
imSize = size(imBWGrid);

%% Step 2. Skeletonize
imSkele = ImgProc.bw2skele(imBWGrid);

if (verbose)
    figure;
    imshowpair(imBWGrid, imSkele, 'Montage');
    title('Grid mask and skeletonized grid mask');
end

%% Step 3. Extract edge, node endpoint and # neighbors image
[imEdge, imNode, ~, ~] = extractStructs(imSkele);

%% Step 4. Pre-extract Nodes and Edges structures for cleaning
[Edges, Nodes] = ImgProc.skeleToStruct(imNode, imEdge);

% get rid unnormally long edges
    hEdge = Edges(logical([Edges.isH]));
    hEdge = hEdge([hEdge.Area] < mean([hEdge.Area]) + 2*std([hEdge.Area]));

    vEdge = Edges(~logical([Edges.isH]));
    vEdge = vEdge([vEdge.Area] < mean([vEdge.Area]) + 2*std([vEdge.Area]));

    Edges = [hEdge; vEdge];

% find isolated edges (area == 1) and remove those edges from imEdge
% and add to imNodes
labels = [Edges.Area] == 1;
isoEdges = Edges(labels);
vecNumNodes = cellfun(@numel, {isoEdges.nodes});
leafEdgeIdx = vecNumNodes == 1;
nodeEdgeIdx = vecNumNodes == 2;

imEdge([isoEdges(nodeEdgeIdx).PixelIdxList]) = 0;
imNode([isoEdges(nodeEdgeIdx).PixelIdxList]) = 1;

% Extract Nodes and Edges from cleaned imEdge and imNode
[Edges, Nodes] = ImgProc.skeleToStruct(imNode, imEdge);
numNodes = numel(Nodes);
numEdges = numel(Edges);

% imMix = imEdge + imNode*3;
% figure;
% title('imMix - Edges and Nodes');
% imagesc(imMix);

%% step 5. create labeled edge and node images for debugging

if (verbose)
%     imEdgeLabel = zeros(size(imNode), 'uint16');
%     imNodeLabel = zeros(size(imNode), 'uint16');
%
%     for i = 1:numEdges
%         imEdgeLabel(Edges(i).PixelIdxList) = i;
%     end
%
%     for i = 1:numNodes
%         imNodeLabel(Nodes(i).PixelIdxList) = i;
%     end
%
%     subplot(2,2,3);drawnow
%     title('imEdgeLabel - Labeled Edges');
%     imagesc(imEdgeLabel);
%     caxis([0 0.1]);
%     myMap = [0, 0, 0; 1, 1, 1];
%     colormap(myMap);
%
%     subplot(2,2,4);drawnow
%     title('imNodeLabel - Labeled Nodes');
%     imagesc(imNodeLabel);
%     caxis([0 0.1]);
%     myMap = [0, 0, 0; 1, 1, 1];
%     colormap(myMap);
end

%% step 6. create the ajacency matrix and graph from Edges and Nodes
A = zeros(numNodes, numNodes);

    for i = 1:numEdges
    curEdge = Edges(i);

    if (numel(curEdge.nodes) == 2)% ignore end point and end edge
        edgeLength = numel(curEdge.PixelIdxList);
        A(curEdge.nodes(1), curEdge.nodes(2)) = edgeLength;
        A(curEdge.nodes(2), curEdge.nodes(1)) = edgeLength;
    end

end

%% step 6-7. create the ajacency matrix and graph from Edges and Nodes and traverse the grid to assign four neighbors to Nodes
Nodes = ImgProc.traverseGrid(Nodes, Edges);

%% step 8. get horizontal and vertical edges
horiEdges = Edges([Edges.isH] == 1);
vertEdges = Edges([Edges.isH] == 0);

imHoriEdge = false(imSize);
imHoriEdge(vertcat(horiEdges.PixelIdxList)) = 1;

imVertEdge = false(imSize);
imVertEdge(vertcat(vertEdges.PixelIdxList)) = 1;

%% step 9. read color grid and convert RGB to labels
[imAllLabel, imHoriLabel, imVertLabel] = ImgProc.colorToLabel(imColorGridMasked, imNode, imHoriEdge, imVertEdge, verbose);

%% step 10. assign each node horizontal and vertical color labels
[Nodes(:).horiColor] = deal(-1);
[Nodes(:).vertColor] = deal(-1);

% imNodeWide = imdilate(imNode, strel('disk', 1));
imNodeWide = imdilate(imNode, strel('square', 3));

imHoriWide = imdilate(imHoriEdge, strel('disk', 1));
imHoriWide = logical(imHoriWide .* (~imNodeWide));

imVertWide = imdilate(imVertEdge, strel('disk', 1));
imVertWide = logical(imVertWide .* (~imNodeWide));

% labeled images (faster than imreconstruct)
imHoriWideL = bwlabel(imHoriWide);
imVertWideL = bwlabel(imVertWide);

for i = 1:numel(Nodes)
    hEdges = [Nodes(i).hEdges];
    hEdges = hEdges(hEdges > 0);
    curHoriEdgePixIdx = vertcat(Edges(hEdges).PixelIdxList);

%     imMarker = false(imSize);
%     imMarker(curHoriEdgePixIdx) = 1;
%     imWideRecon = imreconstruct(imMarker, imHoriWide, 4);
%     curHoriEdgeLabels = imHoriLabel(imWideRecon > 0);

    hLabels = imHoriWideL(curHoriEdgePixIdx);
    idx = ismember(imHoriWideL, hLabels(hLabels>0));
    curHoriEdgeLabels = imHoriLabel(idx);

    % vertical
    vEdges = [Nodes(i).vEdges];
    vEdges = vEdges(vEdges > 0);
    curVertEdgePixIdx = vertcat(Edges(vEdges).PixelIdxList);

%     imMarker = false(imSize);
%     imMarker(curVertEdgePixIdx) = 1;
%     imWideRecon = imreconstruct(imMarker, imVertWide, 4);
%     curVertEdgeLabels = imVertLabel(imWideRecon > 0);

    vLabels = imVertWideL(curVertEdgePixIdx);
    idx = ismember(imVertWideL, vLabels(vLabels>0));
    curVertEdgeLabels = imVertLabel(idx);

    % vote majority
    Nodes(i).horiColor = mode(curHoriEdgeLabels);
    Nodes(i).vertColor = mode(curVertEdgeLabels);
end

%% step 11. correct each Node's label by majority voting on each stripe

% NodesBackup = Nodes; % keep a copy of old Nodes for debug
Nodes = ImgProc.correctColors(Nodes);

%% step 12. decode the De Bruijn sequence

Nodes = ImgProc.decodeDebruijn(Nodes, prjW, prjH);
[camPoints, prjPoints] = extractNodesPosition(Nodes);

% fill missing coordinates by majority vote on the same stripe
for i = 1:numel(Nodes)
    % find all nodes on the same horizontal line
    Nnb = ImgProc.findNbInDir(Nodes, i, 'N', 0);
    Snb = ImgProc.findNbInDir(Nodes, i, 'S', 0);
    Enb = ImgProc.findNbInDir(Nodes, i, 'E', 0);
    Wnb = ImgProc.findNbInDir(Nodes, i, 'W', 0);

    % activeRow
    hNodeIdx = [flip(Wnb), i, Enb];
    activeRow = [Nodes(hNodeIdx).activeRow];
    if(nnz(activeRow > 0))
        [Nodes(hNodeIdx).activeRow] = deal(mode(activeRow(activeRow > 0)));
    end

    % activeCol
    vNodeIdx = [flip(Nnb), i, Snb];
    activeCol = [Nodes(vNodeIdx).activeCol];
    if(nnz(activeCol > 0))
        [Nodes(vNodeIdx).activeCol] = deal(mode(activeCol(activeCol > 0)));
    end
end

% fill missing node prj coordinates using linear interpolation
for i = 1:numel(Nodes)
    % find all nodes on the same horizontal line
    Nnb = ImgProc.findNbInDir(Nodes, i, 'N',0);
    Snb = ImgProc.findNbInDir(Nodes, i, 'S',0);
    Enb = ImgProc.findNbInDir(Nodes, i, 'E',0);
    Wnb = ImgProc.findNbInDir(Nodes, i, 'W',0);

    hNodeIdx = [flip(Wnb), i, Enb];
%     hStripes = [Nodes(hNodeIdx).hEdges];

    vNodeIdx = [flip(Nnb), i, Snb];
%     vStripes = [Nodes(vNodeIdx).vEdges];

    % fill -1 with linear interpolation

    % horizontal stripes
    vecCol = [Nodes(hNodeIdx).activeCol];
    vecCol = num2cell(fillmissing(vecCol, 'linear', 'MissingLocations', vecCol<0));
    [Nodes(hNodeIdx).activeCol] = vecCol{:};

    vecRow = [Nodes(hNodeIdx).activeRow];
    vecRow = num2cell(fillmissing(vecRow, 'linear', 'MissingLocations', vecRow<0));
    [Nodes(hNodeIdx).activeRow] = vecRow{:};

    % vertical stripes
    vecRow = [Nodes(vNodeIdx).activeRow];
    vecRow = num2cell(fillmissing(vecRow, 'linear', 'MissingLocations', vecRow<0));
    [Nodes(vNodeIdx).activeRow] = vecRow{:};

    vecCol = [Nodes(vNodeIdx).activeCol];
    vecCol = num2cell(fillmissing(vecCol, 'linear', 'MissingLocations', vecCol<0));
    [Nodes(vNodeIdx).activeCol] = vecCol{:};
end

%% Local functions
function [camPoints, prjPoints, validNodeIdx] = extractNodesPosition(Nodes)

validNodes = [Nodes.activeCol] > 0 & [Nodes.activeRow] > 0;
validNodeIdx = find(validNodes);

camPoints = vertcat(Nodes(validNodes).Centroid);
prjPoints = [[Nodes(validNodes).activeCol]', [Nodes(validNodes).activeRow]'];
end

%% convert skeleton image to node, edge endpoints images
function [imEdges, imNodes, imEndPoints, imNeighbor] = extractStructs(imSkele)
imNeighbor = imfilter(double(imSkele), [1, 1, 1; 1, 0, 1; 1, 1, 1]);
imNeighbor = imNeighbor .* imSkele;

imEdges = imNeighbor == 2;
imEndPoints = imNeighbor == 1;
imNodes = logical(imSkele - imEdges - imEndPoints);
end
end


function [newnode,newelem]=mergesurf(node,elem,varargin)
%
% [newnode,newelem]=mergesurf(node1,elem1,node2,elem2,...)
%
% merge two or more triangular meshes and split intersecting elements
%
% author: Qianqian Fang <q.fang at neu.edu>
%
% input:
%      node: node coordinates, dimension (nn,3)
%      elem: tetrahedral element or triangle surface (nn,3)
%
% output:
%      newnode: the node coordinates after merging, dimension (nn,3)
%      newelem: tetrahedral element or surfaces after merging (nn,4) or (nhn,5)
%
% note: you can call meshcheckrepair for the output newnode and
% newelem to remove the duplicated nodes or elements
%
% example:
%
%   [node1,face1,elem1]=meshabox([0 0 0],[10 10 10],1,1);
%   [node2,face2,elem2]=meshasphere([5 5 10],3,0.3,3);
%   [newnode,newface]=mergesurf(node1,face1,node2,face2);
%   plotmesh(newnode,newface,'x>5');
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

len=length(varargin);
newnode=node;
newelem=elem;
if(len>0 && mod(len,2)~=0)
   error('you must give node and element in pairs');
end
for i=1:2:len
   no=varargin{i};
   el=varargin{i+1};
   [newnode,newelem]=surfboolean(newnode,newelem,'all',no,el);
end


function y = sum_square_pos( x, dim )

%SUM_SQUARE_POS   Sum of squares of the positive parts.
%   For vectors, SUM_SQUARE(X) is the sum of the squares of the positive
%   parts of X; i.e., SUM( MAX(X,0)^2 ). X must be real.
%
%   For matrices, SUM_SQUARE_POS(X) is a row vector containing the
%   application of SUM_SQUARE_POS to each column. For N-D arrays, the
%   SUM_SQUARE_POS operation is applied to the first non-singleton
%   dimension of X.
%
%   SUM_SQUARE_POS(X,DIM) takes the sum along the dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_SQUARE_POS(X,...) is convex and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be convex (or affine). DIM must
%       always be constant.

error( nargchk( 1, 2, nargin ) );
if nargin == 2,
    y = sum( square_pos( x ), dim );
else
    y = sum( square_pos( x ) );
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [rtk,ins_align_flag]=ins_align(rtk,obsr_,obsb_,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
ins_align_flag=0;

if norm(rtk.sol.pos)~=0
    [vel,ins_align_flag]=tdcp2vel(rtk,nav,obsr_,rtk.oldobsr);
end

[rtk,~]=gnss_solver(rtk,obsr_,obsb_,nav);

rtk.oldobsr=obsr_;

if rtk.sol.stat~=glc.SOLQ_NONE&&ins_align_flag==1
    % initialize ins
    pos=xyz2blh(rtk.sol.pos);
    yaw=vel2yaw(vel);
    att=[0 0 yaw];
    avp0=[att,vel,pos]';
    ins=ins_init(rtk.opt.ins,avp0);

    % correct lever arm for position and velocity
    ins.pos=ins.pos-ins.Mpv*ins.Cnb*ins.lever;
    ins.vel=ins.vel-ins.Cnb*askew(ins.web)*ins.lever;
    rtk.ins=ins;
else
    ins_align_flag=0;
end

return


function meshfaces_test02 ( )

%*****************************************************************************80
%
%% MESHFACES_TEST02: A rectangle is subdivided into three.
%
%  Modified:
%
%    23 August 2014
%
%  Author:
%
%    Darren Engwirda
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MESHFACES_TEST02\n' );
  fprintf ( 1, '  A rectangle is subdivided into three.\n' );
  fprintf ( 1, '  The middle rectangle is thin.\n' );

  node = [0.0, 0.0; 1.0,0.0; 1.0,1.0; 0.0,1.0; 1.01,0.0; 1.01,1.0; 3.0,0.0; 3.0,1.0];
  edge = [1,2; 2,3; 3,4; 4,1; 2,5; 5,6; 6,3; 5,7; 7,8; 8,6];

  face{1} = [1,2,3,4];
  face{2} = [5,6,7,2];
  face{3} = [8,9,10,6];
%
%  Since we don't save the output, we will just see an image of the mesh.
%
  meshfaces ( node, edge, face );

  filename = 'test02.png'
  print ( '-dpng', filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  An image of the mesh was saved as "%s"\n', filename );

  return
end


function point = polygonPoint(poly, pos)
%POLYGONPOINT Extract a point from a polygon
%
%   POINT = polygonPoint(POLYGON, POS)
%
%
%   Example
%   polygonPoint
%
%   See also
%   polygons2d, polylinePoint
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2009-04-30,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

% eventually copy first point at the end to ensure closed polygon
if sum(poly(end, :) == poly(1,:))~=2
    poly = [poly; poly(1,:)];
end

% number of points to compute
nPoints = length(pos(:));

% number of vertices in polygon
Nv = size(poly, 1)-1;

% allocate memory results
point = zeros(nPoints, 2);

% iterate on points
for i = 1:nPoints
    % compute index of edge (between 0 and Nv)
    ind = floor(pos(i));

    % special case of last point of polyline
    if ind==Nv
        point(i,:) = poly(end,:);
        continue;
    end

    % format index to ensure being on polygon
    ind = min(max(ind, 0), Nv-1);

    % position on current edge
    t = min(max(pos(i)-ind, 0), 1);

    % parameters of current edge
    x0 = poly(ind+1, 1);
    y0 = poly(ind+1, 2);
    dx = poly(ind+2,1)-x0;
    dy = poly(ind+2,2)-y0;

    % compute position of current point
    point(i, :) = [x0+t*dx, y0+t*dy];
end


function f2 = p00_f2 ( problem, x )

%*****************************************************************************80
%
%% P00_F2 evaluates the second derivative for any problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the values of the variables.
%
%    Output, real F2, the second derivative.
%
  if ( problem == 1 )
    f2 =  p01_f2 ( x );
  elseif ( problem == 2 )
    f2 =  p02_f2 ( x );
  elseif ( problem == 3 )
    f2 =  p03_f2 ( x );
  elseif ( problem == 4 )
    f2 =  p04_f2 ( x );
  elseif ( problem == 5 )
    f2 =  p05_f2 ( x );
  elseif ( problem == 6 )
    f2 =  p06_f2 ( x );
  elseif ( problem == 7 )
    f2 =  p07_f2 ( x );
  elseif ( problem == 8 )
    f2 =  p08_f2 ( x );
  elseif ( problem == 9 )
    f2 =  p09_f2 ( x );
  elseif ( problem == 10 )
    f2 =  p10_f2 ( x );
  elseif ( problem == 11 )
    f2 =  p11_f2 ( x );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_F2 - Fatal error!\n' );
    fprintf ( 1, '  Illegal problem number PROBLEM = %d\n', problem );
    error ( 'P02_F2 - Fatal error!' );
  end

  return
end