% evaluate simulated data - ICA and NMF
function separ(sep0, offset0, path_data, path_res, prename, niter, savethis, hint, sep_how)
% sep_how: string i - ica, n - nmf -> method for separation
if ~exist('hint', 'var')
    hint = 0;
end

if ~exist('sep_how', 'var')
    sep_how = 'in';
end

fprintf('Separation of components... \n')
for rr = 1: length(offset0)
    fprintf('\n%g: ',rr)
    for ll=1 : length(sep0)
        fprintf('.')
        p.namedir = [prename num2str(100*sep0(ll)) 'offset_' num2str(offset0(rr))];
        cd ([path_data p.namedir])
        for mm=1:niter

            namefile = [p.namedir '-iter_' num2str(mm)];
            load ([namefile '.mat'])
            %         ims(psf);
            %         SaveImageFULL('psf', 'pf');

            if sum(sep_how == 'i')>0 %ICA
                [icasig{mm}, A{mm}, W{mm}] = fastica (dveccr, 'numOfIC', 2, 'g', 'tanh');
                icapixICA{mm} = reshape(A{mm},32, 32, 2);
            end
            if sum(sep_how == 'n')>0 %NMF
                ncomp = 2; %number of components to be separated
                if hint
                    dvec_ind = squeeze(reshape(double(array2im(dpixc_ind)), p.nx*p.ny, 1, 2)); % vectors of resized images
%                     [out, bg(mm), bg_im]=backgroundoffset(dpixc);
                    [out, bg(mm), bg_im]=backgroundoffset(dpixc, 'no', 5, 20, 8); %empirical values...
                    dvec_bg = bg(mm)*ones(1, p.nx*p.ny);
%                     dvec_bg = p.offset*ones(1, p.nx*p.ny); %changed for offset 10...

                    blinkmatrand = rand(p.Nt, ncomp);
                    winit = [blinkmatrand,ones(p.Nt,1)];             %random weights will be assigned to firts two and bg fixed

                    hinit = [dvec_ind'; dvec_bg];       %original 'true' points + background
%                     hinit = [rand(ncomp, p.nx*p.ny); dvec_bg];
                    ncomp = ncomp+1; %background added
                    [w{mm},h{mm}, wtrace{mm},htrace{mm}]=nmf_test(double(dveccr'),ncomp+1,1,winit,hinit, [3], [3]);

                else
                    [w{mm},h{mm}]=nmf(double(dveccr'),ncomp,1);
                end
                icapixNMF{mm} = reshape(h{mm}',32,32,ncomp);
            end
            %             imstiled(icapixICA{mm});
            %             SaveImageFULL([p.namedir 'ICA_' num2str(mm)], 'p');
            %             imstiled(icapixNMF{mm});
            %             SaveImageFULL([p.namedir 'NMF_' num2str(mm)], 'p');
            close all

        end

        p.path_data = path_data;
        p.path = path_res;

        if savethis == 1
            fprintf('saving data \n');
            if ~(strcmp('p.path', 'p.path_data')) %not identical
                mkdir ([p.path p.namedir]);
                cd ([p.path p.namedir]);
            end
            save ([p.namedir '_separ'])
            writedata([],[],p,[p.namedir '_param'])
        end
    end
end

fprintf('\n')


classdef BiGE < ALGORITHM
% <many> <real/integer/label/binary/permutation>
% Bi-goal evolution

%------------------------------- Reference --------------------------------
% M. Li, S. Yang, and X. Liu, Bi-goal evolution for many-objective
% optimization problems, Artificial Intelligence, 2015, 228: 45-65.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = MatingSelection(Estimation(Population.objs,1/Problem.N^(1/Problem.M)));
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Population = EnvironmentalSelection([Population,Offspring],Problem.N);
            end
        end
    end
end


function [xhat,e,k,theta0,P,b] = celp9600(x,N,L,M,c,cb,Pidx)
%  celp --> 9600 bps CELP analyzer and synthesizer.
%
%    [xhat,e,k,theta0,P,b] = celp9600(x,N,L,M,c,cb,Pidx)
%
%    The function implements a 9600 bps CELP analyzer and synthesizer,
%    if the speech signal is sampled at 8 kHz, the frame size N is 20 ms
%    (160 samples), and the block duration L for the excitation sequence
%    selection is 5 ms (40 samples). Furthermore, assume that the
%    codebook has 1024 sequences which require 10 bit to send the index k,
%    and that the lag of the pitch filter, P, is searched in the range 16
%    to 160 which require 8 bit to represent exactly. Thus, the quantization
%    procedure only affects the M = 10 LPC parameters (inverse sine) coded
%    by 6 bits, the gain Theta0 coded by 7 bit, and the pitch filter coeff.
%    b coded by 8 bit.

Nx = length(x);                         % Signal length.
F  = fix(Nx/N);                         % No. of frames.
J  = N/L;                               % No. blocks per frame.

% Initialize output signals.
xhat   = zeros(Nx,1);                   % Synthesized signal.
e      = zeros(Nx,1);                   % Excitation signal.
k      = zeros(J,F);                    % Columns are excitation
theta0 = zeros(J,F);                    % parameters per frame.
P      = zeros(J,F);
b      = zeros(J,F);

ebuf  = zeros(Pidx(2),1);               % Vectors with previous excitation
ebuf2 = ebuf; bbuf = 0;                 % samples.
Zf = []; Zw = []; Zi = [];              % Memory hangover in filters.

for (f=1:F)
  %fprintf(1,'... Frame no. %g out of %g.\n',f,F);
  n = (f-1)*N+1:f*N;                    % Time index of current speech frame.

  [kappa,kf,theta0f,Pf,bf,ebuf,Zf,Zw] = celpana(x(n),L,M,c,cb,Pidx,bbuf,...
                                                                ebuf,Zf,Zw);

  sigma  = 2/pi*asin(kappa);
  sigma  = udecode(uencode(sigma,6),6);
  kappa  = sin(pi/2*sigma);
  theta0 = udecode(uencode(theta0,7,0.2),7,0.2);
  b      = udecode(uencode(b,8,1.4),8,1.4);

  [xhat(n),ebuf2,Zi] = celpsyn(cb,kappa,kf,theta0f,Pf,bf,ebuf2,Zi);

  % Output excitation signal and parameters for current frame.
  e(n)        = ebuf(Pidx(2)-N+1:Pidx(2));
  k(:,f)      = kf;
  theta0(:,f) = theta0f;
  P(:,f)      = Pf;
  b(:,f)      = bf; bbuf = bf(J);       % Last estimated b used in next frame.
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2010, John T. Ramshur, jramshur@gmail.com
%
% This file is part of HRVAS
%
% HRVAS 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.
%
% HRVAS 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 HRVAS.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function output=poincareHRV(ibi)
%poincareHRV(ibi) - calculates poincare HRV
%
%Inputs:    ibi = 2dim array containing [t (s),ibi (s)]
%
%Outputs:   output is a structure containg HRV.

    %check inputs
    ibi(:,2)=ibi(:,2).*1000; %convert ibi to ms
    %assumes ibi units are seconds

%     if abs(range(ibi(:,2)))<50 %assume ibi units are seconds
%             ibi(:,2)=ibi(:,2).*1000; %convert ibi to ms
%     end
%     if abs(range(diff(ibi(:,1))))>50 %assume time unites are ms
%         ibi(:,1)=ibi(:,1)./1000; %convert time to s
%     end

    sd=diff(ibi(:,2)); %successive differences
    rr=ibi(:,2);
    SD1=sqrt( 0.5*std(sd)^2 );
    SD2=sqrt( 2*(std(rr)^2) - (0.5*std(sd)^2) );

    %format decimal places
    output.SD1=round(SD1*10)/10; %ms
    output.SD2=round(SD2*10)/10; %ms

end


function [cl] = qt2cl(qt)
% Convert volume from US liquid quarts to centiliters.
% Chad Greene 2012
cl = qt*94.6352946;


function Edges = detectAreaBorders(images)

images(isnan(images))=0;
% Edges=double(edge(images, 'canny'));
images = images+1;
Edges = zeros(size(images));
for area=3:max(images(:))-1
    images_tmp = double(images==area);
    Edges=Edges | double(edge(images_tmp, 'canny'));
end
% figure;imagesc(Edges)


function plotData(x, y)
%PLOTDATA Plots the data points x and y into a new figure
%   PLOTDATA(x,y) plots the data points and gives the figure axes labels of
%   population and profit.

figure; % open a new figure window

% ====================== YOUR CODE HERE ======================
% Instructions: Plot the training data into a figure using the
%               "figure" and "plot" commands. Set the axes labels using
%               the "xlabel" and "ylabel" commands. Assume the
%               population and revenue data have been passed in
%               as the x and y arguments of this function.
%
% Hint: You can use the 'rx' option with plot to have the markers
%       appear as red crosses. Furthermore, you can make the
%       markers larger by using plot(..., 'rx', 'MarkerSize', 10);

plot(x, y, 'rx', 'MarkerSize', 10); % Plot the data
ylabel('Profit in $10,000s'); % Set the y􀀀axis label
xlabel('Population of City in 10,000s'); % Set the x􀀀axis label

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

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: SSD versus rotations, linearInter, HNSP level=4
%
%==============================================================================

clear, close all, help(mfilename);

setup2DHNSPData;
imgModel('set','imgModel','linearInter');
level = 4; omega = ML{level}.omega; m = ML{level}.m;
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

center = (omega(2:2:end)-omega(1:2:end))'/2;
trafo('set','trafo','rotation2D','c',center);

wc = pi/2*linspace(-1,1,101);  dc = zeros(size(wc));
figure(1); clf;
for j=1:length(wc),
  yc = trafo(wc(j),xc);
  Tc = imgModel(T,omega,yc);
  dc(j) = SSD(Tc,Rc,omega,m);
  viewImage(128+(Tc-Rc)/2,omega,m); drawnow;
  FAIRpause(1/6)
end;
figure; clf; p1 = plot(wc,dc);
%==============================================================================


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function output = local_search(objective, inputs)
% LOCAL_SEARCH: Function runs binary optimization by searching over the neighborhood
% of single model flips at each iteration

% Extract inputs
nVars = inputs.n_vars;
nEval = inputs.evalBudget;

% Generate initial condition and evaluate objective
model 	  = sample_models(1,nVars);
model_val = objective(model);

% determine the total number of iterations
nIter = ceil(nEval/nVars);

% Setup cells to store model, objective, and runtime
model_iter = zeros(nIter, nVars);
obj_iter   = zeros(nIter,1);
time_iter  = zeros(nIter,1);

% Declare counter
counter = 0;

for i=1:nIter

	% Update counter
	counter = counter + 1;

	ls_iter = tic;

	% Setup vector to store new objective values and difference
	new_obj  = zeros(nVars,1);
	diff_obj = zeros(nVars,1);

	for j=1:nVars

		% Setup new_model with one flipped variable
		new_model = model;
		new_model(j) = 1-new_model(j);

		% Evaluate objective
		new_obj(j)  = objective(new_model);
		diff_obj(j) = model_val - new_obj(j);

	end

	% Check if diff_obj is positive - improvement can be made
	if any(diff_obj > 0)

		% Choose optimal index to flip
    	[~, opt_idx] = max(diff_obj);
    	model(opt_idx) = 1 - model(opt_idx);
    	model_val = new_obj(opt_idx);

    end

	% Save models, model_obj, and runtime
	model_iter(counter,:) = model;
	obj_iter(counter)  = model_val;
	time_iter(counter) = toc(ls_iter);

end

% extend results
model_iter_new = zeros(nVars*nIter, nVars);
time_iter_new  = zeros(nVars*nIter, 1);
for i=1:nIter
	idx = nVars*(i-1)+1:nVars*i;
	model_iter_new(idx,:) = repmat(model_iter(i,:), nVars, 1);
	time_iter_new(idx) = interp1([0,1],[0,time_iter(i)],linspace(1/nVars,1,nVars));
end
obj_iter   = reshape(repmat(obj_iter, 1, nVars)', nVars*nIter, 1);

% save outputs
output = struct;
output.objVals  = obj_iter;
output.optModel = model_iter_new;
output.runTime  = time_iter_new;

end


function dz = dynamics(z,u)
% dz = dynamics(z,u)
%
% Computes the dynamics of a quadruple chain integrator
%
% INPUTS:
%   z = [4*nx,nt] = [position;velocity;acceleration;jerk]
%   u = [nx,nt] = snap
%
% OUTPUTS:
%   dz = dz/dt
%

nx = size(u,1);    % dimension of the position space
idx = (nx+1):(4*nx);   % index of velocity, acceleration, and jerk
dz = [z(idx,:);u];  % derivatives

end


function [node,elem,face]=volmap2mesh(img,ix,iy,iz,elemnum,maxvol,thickness,Amat,Bvec)
%
% [node,elem,face]=volmap2mesh(img,ix,iy,iz,thickness,elemnum,maxvol,A,B)
%
% convert a binary volume to tetrahedral mesh followed by an Affine transform
%
% author: Qianqian Fang (q.fang at neu.edu)
% date:   2008/01/12
%
% input:
%        img, ix,iy,iz, elemnum and  maxvol: see vol2mesh.m
%        thickness: scale z-dimension of the mesh to specified thickness,
%                   if thickness==0, scaling is bypassed
%        Amat: a 3x3 transformation matrix
%        Bvec: a 3x1 vector
%        Amat and Bvec maps the image index space to real world coordnate system by
%                   [x,y,z]_new=Amat*[x,y,z]_old+Bvec
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

[node,elem,face]=vol2mesh(img,ix,iy,iz,elemnum,maxvol);

node(:,1:3)=(Amat*node(:,1:3)'+repmat(Bvec(:),1,size(node,1)))';

if(thickness)
	zmin=min(node(:,3));
	zmax=max(node(:,3));
	node(:,3)=(node(:,3)-zmin)/(zmax-zmin)*thickness;
end


function title = p07_title ( )

%*****************************************************************************80
%
%% P07_TITLE returns a title for problem 7.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string TITLE, a title for the problem.
%
  title = 'The dying snake, ( x + sin(x) ) * e^(-x^2).';

  return
end


function [v1,v2,v3]=vehicle()

% [v1,v2,v3]=vehicle;
% creates 3 different vehicle configurations, which vary one
% from another only by the distance between prow and wings.

% -----------------------------------------------------------------------
% configuration # 1

% phisical world variables
v1.g_e=[0; 0; 9.81];	% Gravitational acceleration
v1.rho=1033;			% Marine water density

% mass
v1.m=1444.0;			% mass (kg)

% Rigid body mass matrix [Kg Kg*m; Kg*m Kg*m^2]

v1.Mrb=[	1444	0	0	0	252.7	0	;
			0	1444	0	-252.7	0	-2527	;
			0	0	1444	0	2527	0	;
			0	-252.7	0	142.8	0	0	;
			252.7	0	2527	0	2796	0	;
			0	-2527	0	0	0	2778	];

% Added mass matrix [Kg Kg*m; Kg*m Kg*m^2]

v1.Ma=[	55.7	0	0	0	0	0	;
			0	1460	0	0	0	-102	;
			0	0	1460	0	102	0	;
			0	0	0	83.4	0	0	;
			0	0	102	0	3401	0	;
			0	-102	0	0	0	3401	];

% inverse total mass matrix
v1.iM=inv(v1.Mrb+v1.Ma);

% geometric variables

v1.l=3.5;			% Fuselage length (m)
v1.d=0.7;			% Fuselage diameter (m)
v1.vol=1.39787;		% Fuselage volume (m^3)

% vectors

v1.P_b=[0; 0; 0];		% Pole wrt B (m)
v1.G_b=[-1.75; 0; 0.175];	% Center of mass wrt B (m)
v1.B_b=[-1.75; 0; 0];	% Center of buoyancy wrt B (m)

% wings

v1.sw=0.2;			% Wing surface (m^2)
v1.cw=0.4;			% Wing width (m)
v1.bw=0.5;			% Wing length (m)

% Position of wings wrt B along x (m) in config 1
lw=-1.65;

% distance of wing middle point from prow and from axis in config 1
dpfw =    -lw +0.25*v1.cw;
dafw = v1.d/2 +0.43*v1.bw;

% wings middle point positions wrt B in config 1
v1.P1_b = [-dpfw; dafw;    0];
v1.P2_b = [-dpfw;    0; dafw];
v1.P3_b = [-dpfw;-dafw;    0];
v1.P4_b = [-dpfw;    0;-dafw];

% tails

v1.st=0.2;			% Tail surface (m^2)
v1.ct=0.4;			% Tail width (m)
v1.bt=0.5;			% Tail length (m)

lt=-3.1;				% Position of tails along x wrt B (m)

% distance of tail middle point from prow and from axis
dpft =    -lt +0.25*v1.ct;
daft = v1.d/2 +0.43*v1.bt;

% tails middle point positions wrt B
v1.P5_b = [-dpft; daft;    0];
v1.P6_b = [-dpft;    0; daft];
v1.P7_b = [-dpft;-daft;    0];
v1.P8_b = [-dpft;    0;-daft];

% -----------------------------------------------------------------------
% configuration # 2

v2=v1;

% Position of wings wrt B along x (m) in config 2
lw=-1.15;

% distance of wing middle point from prow and from axis in config 2
dpfw =    -lw +0.25*v2.cw;
dafw = v2.d/2 +0.43*v2.bw;

% wings middle point positions wrt B in config 2
v2.P1_b = [-dpfw; dafw;    0];
v2.P2_b = [-dpfw;    0; dafw];
v2.P3_b = [-dpfw;-dafw;    0];
v2.P4_b = [-dpfw;    0;-dafw];

% -----------------------------------------------------------------------
% configuration # 3

v3=v1;

% Position of wings wrt B along x (m) in config 3
lw=-0.35;

% distance of wing middle point from prow and from axis in config 3
dpfw =    -lw +0.25*v3.cw;
dafw = v3.d/2 +0.43*v3.bw;

% wings middle point positions wrt B in config 2
v3.P1_b = [-dpfw; dafw;    0];
v3.P2_b = [-dpfw;    0; dafw];
v3.P3_b = [-dpfw;-dafw;    0];
v3.P4_b = [-dpfw;    0;-dafw];


%TIME_INTOVERSAMP
%
%   The purpose of this test is to determine whether special code for
%   handling the integer oversampling case in the factorization routines
%   pays off in any way.

Lr=[1048576*2,2400];
ar=[      512,   2];
Mr=[     1024, 800];

for ii=1:length(Lr)

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  [L, a, M]

  N=L/a;
  c=gcd(a,M);
  p=a/c;
  q=M/c;
  d=N/q;

  f=rand(L,1);
  gf=rand(p*q,c*d);
  c1=mex_dgt_fac_1(f,gf,a,M);

  f=rand(L,1);
  gf=rand(p*q,c*d);
  c2=mex_dgt_fac_2(f,gf,a,M);

end;


function [inplane_pix_size] = mrGetIPPixSize()
%function [inplane_pix_size] = mrGetIPPixSize()
%
% PURPOSE: Prompt the user for the inplane anatomy pixel size
% AUTHOR:  Poirson
% DATE:    07.16.97
% HISTORY: Based on routine by Geoff Boynton
% NOTES:   The default values [256/260,256/260,1/4] mean that
%          26 cm of brain image are interpolated on the 256 pixels
%          in the x and y dimensions (units are pixels/mm).
%          And that 1 pixel in the inplane direction equals 4mm thickness.
%

% 04/13/00 huk and nestares -- gets defPixSize from mrSESSION now
% 07/27/00 wandell, brewer  -- added extra check for reconParams field

global mrSESSION

qt=''''; %single quote character

if    isfield(mrSESSION,'fullInplaneSize') & ...
      isfield(mrSESSION,'reconParams') & ...
      isfield(mrSESSION.reconParams(1),'FOV') & ...
      isfield(mrSESSION.reconParams(1),'sliceThickness')
  defPixSize =  [mrSESSION.fullInplaneSize(1)/ mrSESSION.reconParams(1).FOV, ...
                 mrSESSION.fullInplaneSize(2)/mrSESSION.reconParams(1).FOV, ...
                 1/mrSESSION.reconParams(1).sliceThickness];
  disp('Calculating inplane anatomy voxel size from fullInplaneSize, FOV, and sliceThickness.');
else
  defPixSize = [256/260,256/260,1/4];
  disp('mrSESSION inplane voxel size info does not exist.  Default values set; check your protocol parameters.');
end

disp('Enter size of inplane anatomy pixels/mm in x,y and z directions');
%inplane_pix_size=input('Default is [256/260,256/260,1/4]: ');
defStr = mat2str(defPixSize);
inplane_pix_size=input(['Default is: ' defStr ': ']);
%Or go the default
if isempty(inplane_pix_size)
	inplane_pix_size= defPixSize;
end

return


function crfw = crfPairwiseTrain(vlab, hlab, pvSP, phSP, adjlist, pE, params, edgelen)

% get f1(y1, y2) = log(P(y1|x)) + log(P(y2|x))
% and f2(y1, y2) = I(y1=y2)*log(P(y1=y2|x)) + I(y1~=y2)*log(P(y1~=y2|x))

npair = 0;
for f = 1:numel(adjlist)
    npair = npair + size(adjlist{f}, 1);
end

f1 = zeros(npair, 49);
f2 = zeros(npair, 49);
f3 = zeros(npair, 49);
lab = zeros(npair, 1);
%nbpair = zeros(npair, 2);

c = 0;
for f = 1:numel(adjlist)
    npairf = size(adjlist{f}, 1);
    pg = [pvSP{f}(:, 1)  repmat(pvSP{f}(:, 2), 1, 5).*phSP{f}  pvSP{f}(:, 3)];
    labf = (vlab{f}==1)*1 + (vlab{f}==2).*(hlab{f}>0).*(hlab{f}+1) + (vlab{f}==3)*7;

    s1 = adjlist{f}(:, 1);
    s2 = adjlist{f}(:, 2);

    edgeperc = zeros(numel(s1), 2);
    nb = zeros(size(pg, 1), 1);
    for k = 1:numel(s1)
        nb(s1(k)) = nb(s1(k))+1;
        nb(s2(k)) = nb(s2(k))+1;
        edgeperc(k, 1) = edgelen{f}(s1(k), s2(k)) / (sum(edgelen{f}(s1(k), :))+sum(edgelen{f}(:, s1(k))));
        edgeperc(k, 2) = edgelen{f}(s1(k), s2(k)) / (sum(edgelen{f}(s2(k), :))+sum(edgelen{f}(:, s2(k))));
    end
    %nbpair(c+1:c+npairf, :) = [nb(s1) nb(s2)];
%    min(1./edgeperc(:, 1) + 1./edgeperc(:, 2))
%    max(1./edgeperc(:, 1) + 1./edgeperc(:, 2))
    % unaries normalize for number of edges, and pairwise normalize for
    % edge length
    for k1 = 1:7
        for k2 = 1:7
            f1(c+1:c+npairf, (k1-1)*7+k2) = log(pg(s1, k1))./nb(s1) + log(pg(s2, k2))./nb(s2);
            kp = params(k1 + (k2-1)*7, :);
            if k1==k2
                f2(c+1:c+npairf, (k1-1)*7+k2) = ...
                    log(kp(3)./(1+exp(-kp(1)-kp(2)*(log(pE{f})-log(1-pE{f}))))).*...
                    (edgeperc(:, 1) + edgeperc(:, 2));
            else
                f3(c+1:c+npairf, (k1-1)*7+k2) = ...
                    log(kp(3)./(1+exp(-kp(1)-kp(2)*(log(pE{f})-log(1-pE{f}))))).*...
                    (edgeperc(:, 1) + edgeperc(:, 2));
            end
        end
    end

    lab(c+1:c+npairf) = (labf(s1)>0 & labf(s2)>0).*((labf(s1)-1)*7 + labf(s2));

    c = c + npairf;
end

ind = find(lab==0);
lab(ind) = [];
f1(ind, :) = [];
f2(ind, :) = [];
f3(ind, :) = [];
%nbpair(ind, :) = [];

f2 = f2 - repmat(log(sum(exp(f2), 2)), [1 size(f2, 2)]);
f3 = f3 - repmat(log(sum(exp(f3), 2)), [1 size(f3, 2)]);

fnc{1} = f1;
fnc{2} = f2;
fnc{3} = f3;
%disp(numel(lab))

[p, err] = avep([1 0 0], fnc, lab);
disp(num2str([p err]))

crfw = fminsearch(@(x) objective(x, fnc, lab), [1 1 1]);

[p, err] = avep(crfw, fnc, lab);
disp(num2str([p err]))

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

function lpw = objective(w, f, lab)

correctind = [1:numel(lab)]' + (lab-1)*numel(lab);

lpw = 0;
tmpz = 0;
for k = 1:numel(f)
    %if k==1
        lpw = lpw + w(k)*f{k}(correctind);
        tmpz = tmpz + w(k)*f{k};
%     else
%         lpw = lpw + w(k)*f{k}(correctind)./nb(:, 1).^w(end) + w(k)*f{k}(correctind)./nb(:, 2).^w(end);
%         tmpz = tmpz + w(k)*f{k}./repmat(nb(:, 1).^w(end), [1 size(f{k},2)]) + w(k)*f{k}./repmat(nb(:, 2).^w(end), [1 size(f{k},2)]);
    %end
end
z = log(sum(exp(tmpz), 2));

lpw = -(sum(lpw - z));

%disp(num2str([lpw w]))

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

function [p err] = avep(w, f, lab)

correctind = [1:numel(lab)]' + (lab-1)*numel(lab);

lpw = 0;
tmpz = 0;
for k = 1:numel(f)
    %if k==1
        lpw = lpw + w(k)*f{k}(correctind);
        tmpz = tmpz + w(k)*f{k};
%     else
%         lpw = lpw + w(k)*f{k}(correctind)./nb(:, 1).^w(end) + w(k)*f{k}(correctind)./nb(:, 2).^w(end);
%         tmpz = tmpz + w(k)*f{k}./repmat(nb(:, 1).^w(end), [1 size(f{k},2)]) + w(k)*f{k}./repmat(nb(:, 2).^w(end), [1 size(f{k},2)]);
    %end
end
z = log(sum(exp(tmpz), 2));

p = mean(exp(lpw-z));

[tmp, guess] = max(tmpz, [], 2);
err = mean(guess~=lab);


vm.f=@(A,B).5*norm(A(:)+B(:)-Z(:))^2;
vm.g=@(A,B)A+B-Z;


%computes the key of the input audio (super simple variant)
%>
%> @param x: time domain sample data, dimension samples X channels
%> @param f_s: sample rate of audio data
%> @param afWindow: FFT window of length iBlockLength (default: hann), can be [] empty
%> @param iBlockLength: internal block length (default: 4096 samples)
%> @param iHopLength: internal hop length (default: 2048 samples)
%>
%> @retval cKey key string
% ======================================================================
function [cKey] = ComputeKey (x, f_s, afWindow, iBlockLength, iHopLength)

    % set default parameters if necessary
    if (nargin < 5)
        iHopLength = 2048;
    end
    if (nargin < 4)
        iBlockLength = 4096;
    end

    if (nargin < 3 || isempty(afWindow))
        afWindow = hann(iBlockLength, 'periodic');
    end

    % key names
    cMajor  = char ('C Maj','C# Maj','D Maj','D# Maj','E Maj','F Maj',...
        'F# Maj','G Maj','G# Maj','A Maj','A# Maj','B Maj');
    cMinor  = char ('c min','c# min','d min','d# min','e min','f min',...
        'f# min','g min','g# min','a min','a# min','b min');

    % template pitch chroma (Krumhansl major/minor)
    t_pc = [6.35 2.23 3.48 2.33 4.38 4.09 2.52 5.19 2.39 3.66 2.29 2.88
            6.33 2.68 3.52 5.38 2.60 3.53 2.54 4.75 3.98 2.69 3.34 3.17];
    t_pc = diag(1 ./ sum(t_pc, 2)) * t_pc;

    % compute FFT window function
    if (length(afWindow) ~= iBlockLength)
        error('window length mismatch');
    end

    % extract audio pitch chroma
    [v_pc, t] = computeFeature("SpectralPitchChroma", x, f_s, afWindow, iBlockLength, iHopLength);

    % average pitch chroma
    v_pc = mean(v_pc, 2);

    % compute manhattan distances for major and minor
    d = zeros(2,12);
    for (i = 0:11)
        d(:,i+1)= sum(abs(repmat(v_pc', 2, 1)-circshift(t_pc, [0 i])), 2);
    end
    [dist,iKeyIdx] = min(d,[],2);
    if (dist(1) < dist(2))
        cKey = deblank(cMajor(iKeyIdx(1), :));
    else
        cKey = deblank(cMinor(iKeyIdx(2), :));
    end
end


function [reduced_pot,successful] = reduce_pot(pot,tailnodes)
% Executes the reduce operation defined in
% Stable Local Computation with Conditional Gaussian Distributions
% Steffen L. Lauritzen
% Frank Jensen
% September 1999
% The potential pot is reduced if B contains any zero columns
% The test are restricted to the positions in tailnodes.
% Any columns successfully deleted are entered in the array successful
if nargin < 2
    tailnodes = pot.ctaildom;
end

successful = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Keep track of remaining tailnodes %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
rem_tailnodes = pot.ctaildom;
for i = tailnodes
    pos = find(i==rem_tailnodes);
    successful_red = [pos];
    red_scgcpot = cell(1,pot.dsize);
    j = 1;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Test whether all components of pot.scgpotc can be reduced %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    while ((j <= pot.dsize) & ~isempty(successful_red))
        [cpot,successful_red] = reduce_pot(pot.scgpotc{j},pos);
        red_scgcpot{j} = cpot;
        j = j + 1;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % If i is a reducible tailnode, then reduce the potential %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if ~isempty(successful_red)
        successful = [successful i];
        pot.scgpotc = red_scgcpot;
        rem_tailnodes = mysetdiff(rem_tailnodes,i);
    end;
end

pot.ctaildom = rem_tailnodes;
positions = find_equiv_posns(rem_tailnodes,pot.ctaildom);
pot.ctailsizes = pot.ctailsizes(positions);
pot.ctailsize = sum(pot.ctailsizes);
pot.domain = mysetdiff(pot.domain,successful);
reduced_pot = pot;


function [state,options] = psoplotbestf(options,state,flag)
% Plots the best, mean, and worst scores of particle swarm.

notinf = isfinite(state.Score) ;

if strcmp(flag,'init')
    set(gca,'NextPlot','add',...
        'XLabel',xlabel('Generation'),...
        'YLabel',ylabel('Score'))
%     line(state.Generation,max(state.Score),...
%         'Color','red',...
%         'Tag','Worst Scores',...
%         'Marker','.',...
%         'LineStyle','none')
    line(state.Generation,mean(state.Score(notinf)),...
        'Color','blue',...
        'Tag','Mean Scores',...
        'Marker','.',...
        'LineStyle','none')
    line(state.Generation,min(state.Score),...
        'Color','black',...
        'Tag','Best Scores',...
        'Marker','.',...
        'LineStyle','none')
elseif strcmp(flag,'done')
    legend({'Mean Score','Best Score'})
else
%     hworst = findobj(gca,'Tag','Worst Scores','Type','line') ;
    hmean = findobj(gca,'Tag','Mean Scores','Type','line') ;
    hbest = findobj(gca,'Tag','Best Scores','Type','line') ;
    x = [get(hmean,'XData'), state.Generation] ;
%     yworst = [get(hworst,'YData'), max(state.Score)] ;
    ymean = [get(hmean,'YData'), mean(state.Score(notinf))] ;
    ybest = [get(hbest,'YData'), min(state.Score)] ;
%     set(hworst,...
%         'XData',x,...
%         'YData',yworst)
    set(hmean,...
        'XData',x,...
        'YData',ymean)
    set(hbest,...
        'XData',x,...
        'YData',ybest)
    titletxt = sprintf('Best: %g Mean: %g',ybest(end),ymean(end)) ;
    title(titletxt)
end


%% DEMO 16: TIGRE 2D tomography
%
%
%  In demo we show how to do 2D tomography in TIGRE. It is exactly the same
%  as 3D.
%
%
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize
clear;
close all;
%% PARALLEL BEAM 2D

% VARIABLE                                   DESCRIPTION                    UNITS
%-------------------------------------------------------------------------------------
geo.DSD = 1536;                             % Distance Source Detector      (mm)
geo.DSO = 1000;                             % Distance Source Origin        (mm)
% Detector parameters

%
% 1- Make the detector Nx1
% 2- Make the image NxMx1

% Image parameters
geo.nVoxel=[256;256;1];                   % number of voxels              (vx)
geo.sVoxel=[256;256;1];                   % total size of the image       (mm)
geo.dVoxel=geo.sVoxel./geo.nVoxel;          % size of each voxel            (mm)

geo.nDetector=[512;  1];					% number of pixels              (px)
geo.dDetector=[0.8; geo.dVoxel(3)]; 					% size of each pixel            (mm)
geo.sDetector=geo.nDetector.*geo.dDetector; % total size of the detector    (mm)

% MAKE SURE THAT THE DETECTOR PIXELS SIZE IN V IS THE SAME AS THE IMAGE!

% Offsets
geo.offOrigin =[0;0;0];                     % Offset of image from origin   (mm)
geo.offDetector=[0; 0];

% Auxiliary
geo.accuracy=0.5;                           % Accuracy of FWD proj          (vx/sample)
geo.mode='parallel';
%% Define angles of projection and load phatom image

angles=linspace(0,2*pi,100);
phatom=single(phantom('Modified Shepp-Logan',geo.nVoxel(1)));
projections=Ax(phatom,geo,angles);
%% recosntruct

imgOSSART=OS_SART(projections,geo,angles,40);
imgASDPOCS=ASD_POCS(projections,geo,angles,40);

%% Plot
plotImg([imgOSSART;imgASDPOCS ],'Dim',3,'Slice',1); %top is ASD_POCS, bottom OSSART

%%
%%
%% And now Fan Beam
%%
% The same thing!

%% FAN BEAM 2D

% VARIABLE                                   DESCRIPTION                    UNITS
%-------------------------------------------------------------------------------------
geo.DSD = 1536;                             % Distance Source Detector      (mm)
geo.DSO = 1000;                             % Distance Source Origin        (mm)
% Detector parameters

% Image parameters
geo.nVoxel=[256;256;1];                   % number of voxels              (vx)
geo.sVoxel=[256;256;1];                   % total size of the image       (mm)
geo.dVoxel=geo.sVoxel./geo.nVoxel;          % size of each voxel            (mm)

geo.nDetector=[512;  1];					% number of pixels              (px)
geo.dDetector=[0.8; 1]; 					% size of each pixel            (mm)
geo.sDetector=geo.nDetector.*geo.dDetector; % total size of the detector    (mm)

% Offsets
geo.offOrigin =[0;0;0];                        % Offset of image from origin   (mm)              ;
geo.offDetector=[0; 0];

% Auxiliary
geo.accuracy=0.5;                           % Accuracy of FWD proj          (vx/sample)
geo.mode='cone';
%% Define angles of projection and load phatom image

angles=linspace(0,2*pi,100);
phatom=single(phantom('Modified Shepp-Logan',geo.nVoxel(1)));
% phatom=cat(3,phatom,phatom);
projections=Ax(phatom,geo,angles,'interpolated');
%% recosntruct

imgOSSART=OS_SART(projections,geo,angles,40);
% in 2D, the TV norm minimization happens in CPU, this causes the
% algorithms to be slower
imgASDPOCS=ASD_POCS(projections,geo,angles,40);

%% Plot
plotImg([imgOSSART;imgASDPOCS ],'Dim',3,'Slice',1); %top is ASD_POCS, nottom OSSART


function out = vscale(F)
%VSCALE   Vertical scale of a CHEBFUN3V.
%   VSCL = VSCALE(F) returns the maximal vertical scale of the components of a
%   CHEBFUN3V object F as determined by evaluating F on a coarse tensor-product
%   grid.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

for jj = 1:F.nComponents
    vscl(jj,1) = vscale(F.components{jj});
end
out = max(vscl);

end


classdef S2AxisFieldTri < S2AxisField
% a class represeneting a function on the sphere

  properties
    tri       % S2Triangulation
    values = vector3d  % function values
  end

  properties (Dependent = true)
    vertices
    antipodal
  end

  methods

    function sVF = S2AxisFieldTri(nodes,values)
      % initialize a spherical vector field

      if nargin == 0, return; end

      if isa(nodes,'function_handle')
        n = equispacedS2Grid('resolution',1.5*degree);
        values = nodes(n);
        nodes = n;
      end

      if isa(nodes,'S2Triangulation')
        sVF.tri = nodes;
      else
        sVF.tri = S2Triangulation(nodes);
      end

      sVF.values = values;

    end

    function v = get.vertices(S2F)
      v = S2F.tri.vertices;
    end

    function v = get.antipodal(S2F)
      v = S2F.tri.antipodal;
    end

    function S2F = set.vertices(S2F,v)
      if ~isempty(S2F.values), S2F.values = S2F.eval(v); end
      S2F.tri.vertices = v;
      S2F.tri.update;
    end

  end

end


clear all
syms a b c x
        % Rezolvarea primei ecuatii
sol1=solve(a*x^2+b*x+c)
pretty(sol1)
        % Rezolvarea ecuatiei a doua
sol2=solve('cos(2*x)+sin(x)=1')
        % Transformarea solutiei simbolice in numerica
numsol2=double(sol2)
        % Reprezentarea grafica a functiei pe intervalul dat
ezplot('cos(2*x)+sin(x)-1',[0, 2*pi])
hold on
grid
        % Marcarea solutiilor obtinute pe grafic
plot(numsol2,zeros(size(numsol2)),'rd')


function [c, ceq] = rendezvousNonlconFunc(x, iniOrbit, finOrbit, gmuXfr, minPe, maxAp, onlyOptBurn1)
%rendezvousNonlconFunc Summary of this function goes here
%   Detailed explanation goes here
    [~, ~, ~, ~, ~, ~, xfrOrbit, ~, ~] = rendezvousObjFunc(x, iniOrbit, finOrbit, gmuXfr, [1,1], onlyOptBurn1);

	[rAp, rPe] = computeApogeePerigee(xfrOrbit(1), xfrOrbit(2));
    c(1) = rAp - maxAp;
    c(2) = minPe - rPe;
    ceq = [];
end


function [cs,h]=m_contourf(long,lat,data,varargin)
%  M_CONTOURF Adds filled contours to a map
%    M_CONTOURF(LONG,LAT,DATA,...) is the same as M_CONTOUR except
%    that contours are filled. Areas of data above a given level are
%    filled, areas below are left blank or are filled by a lower level.
%    NaN's in the data leave holes in the filled plot/
%
%    [CS,H] = M_CONTOURF(...) returns contour matrix C as described in
%    CONTOURC and a vector H of handles to PATCH objects (for use by
%    CLABEL).
%
%    See also M_CONTOUR, CONTOURF

% Rich Pawlowicz (rich@ocgy.ubc.ca) 17/Jan/1998
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.

% 19/02/98 - type - should have been 'clip','patch', rather than 'off'.
%  9/12/98 - handle all-NaN plots without letting contour crash.
% 6/Nov/00 - eliminate returned stuff if ';' neglected (thx to D Byrne)

global MAP_PROJECTION

% Have to have initialized a map first

if isempty(MAP_PROJECTION)
  disp('No Map Projection initialized - call M_PROJ first!');
  return;
end

if min(size(long))==1  &&  min(size(lat))==1
 [long,lat]=meshgrid(long,lat);
end

[X,Y]=m_ll2xy(long,lat,'clip','on');  %First find the points outside

i=isnan(X);      % For these we set the *data* to NaN...
data(i)=NaN;

                 % And then recompute positions without clipping. THis
                 % is necessary otherwise contouring fails (X/Y with NaN
                 % is a no-no. Note that this only clips properly down
                 % columns of long/lat - not across rows. In general this
                 % means patches may nto line up properly a right/left edges.
if any(i(:)), [X,Y]=m_ll2xy(long,lat,'clip','patch'); end

if any(~i(:))
 [cs,h]=contourf(X,Y,data,varargin{:});
 set(h,'tag','m_contourf');
else
  cs=[];h=[];
end

if nargout==0
 clear cs h
end


function c = times(a,b)
% TIMES implements a.*b, where either a or b is an adiff object.

switch [class(a),class(b)]

case 'adiffdouble'
   c = adiff(b.*a.x, rowmult(b, a.dx), a.root);

case 'doubleadiff'
   c = adiff(a.*b.x, rowmult(a, b.dx), b.root);

case 'adiffadiff'
   checkroot(a,b);
   c = adiff( a.x.*b.x, rowmult(b.x, a.dx)+rowmult(a.x, b.dx), a.root);

otherwise
   error(['Can''t multiply (.*) ',class(a),' and ',class(b)]);

end


% The COBRAToolbox: testGeometricFBA.m
%
% Purpose:
%     - Tests whether geometricFBA works as intended.
%
% Authors:
%     - Thomas Pfau, Oct 2018

% save the current path
currentDir = pwd;

% The testmodel used is structured as follows:
%
%   <-> A -> B ---> C --> E <->
%        \               ^
%         \             /
%           -> F -->  G
%
% Thus the flux can be split between A -> B -> C -> E and A -> F -> G -> E

% load the test models
model = createToyModelForGeoFBA();
% model with Constraints
modelWConst = addCOBRAConstraints(model,{'R1'},400); % R1 restricted to a maximum flux of 400

%When detectDeadEnds is changed according to Ronans suggestion, we need to test
%multiple solvers.
solverPkgs = prepareTest('needsLP', true, 'minimalMatlabSolverVersion',8.0, 'excludeSolvers',{'pdco','ddqMinos','quadMinos','matlab'});
tol = 1e-4;
for k = 1:length(solverPkgs.LP)
    % set the solver
    changeCobraSolver(solverPkgs.LP{k}, 'LP', 0);
    fprintf('Testing geometricFBA with solver %s ...\n',solverPkgs.LP{k});
    sol = geometricFBA(model);
    assert(all(abs(sol(1:6) - 500) < tol))
    sol = geometricFBA(modelWConst);
    % flux is now centered between 0 and 400 for R11.. R3 and between 600
    % and 1000 for R4..6
    assert(all(abs(sol(1:3) - 200) < tol))
    assert(all(abs(sol(4:6) - 800) < tol))
end

fprintf('Done...\n');
cd(currentDir)


function [nlZ,dnlZ,post,K_mat,Q] = gplite_core(hyp,gp,compute_nlZ,compute_nlZ_grad)
%GPLITE_CORE Core kernel computations for lite GP regression.

%% Initialize GP hyperparameters

[N,D] = size(gp.X);         % Number of training points and dimension

Ncov = gp.Ncov;
Nnoise = gp.Nnoise;
Nmean = gp.Nmean;

% Output warping
outwarp_flag = isfield(gp,'outwarpfun') && ~isempty(gp.outwarpfun);
if outwarp_flag
    Noutwarp = gp.Noutwarp;
    hyp_outwarp = hyp(Ncov+Nnoise+Nmean+1:Ncov+Nnoise+Nmean+Noutwarp);
    if compute_nlZ_grad
        [y,dwarp_dt,dwarp_dtheta,d2warp_dthetadt] = gp.outwarpfun(hyp_outwarp,gp.y);
    else
        [y,dwarp_dt] = gp.outwarpfun(hyp_outwarp,gp.y);
    end
    if ~isempty(gp.s2)
        s2 = gp.s2 .* dwarp_dt.^2;  % Warped noise
    else
        s2 = [];
    end
else
    y = gp.y;
    s2 = gp.s2;
end

% Evaluate observation noise on training inputs
hyp_noise = hyp(Ncov+1:Ncov+Nnoise); % Get noise hyperparameters
if compute_nlZ_grad
    [sn2,dsn2] = gplite_noisefun(hyp_noise,gp.X,gp.noisefun,gp.y,s2);
else
    sn2 = gplite_noisefun(hyp_noise,gp.X,gp.noisefun,gp.y,s2);
end
sn2_mult = 1;  % Effective noise variance multiplier

% Evaluate mean function on training inputs
hyp_mean = hyp(Ncov+Nnoise+1:Ncov+Nnoise+Nmean); % Get mean function hyperparameters
if compute_nlZ_grad
    [m,dm] = gplite_meanfun(hyp_mean,gp.X,gp.meanfun,[],gp.meanfun_extras);
else
    m = gplite_meanfun(hyp_mean,gp.X,gp.meanfun,[],gp.meanfun_extras);
end

%% Observed covariance matrix inversion

% Compute kernel matrix K_mat
if gp.covfun(1) == 1
    ell = exp(hyp(1:D));
    sf2 = exp(2*hyp(D+1));
    K_mat = sq_dist(bsxfun(@rdivide,gp.X',ell));
    K_mat = sf2 * exp(-K_mat/2);
else
    hyp_cov = hyp(1:Ncov); % Get covariance function hyperparameters
    if compute_nlZ_grad
        [K_mat,dK_mat] = gplite_covfun(hyp_cov,gp.X,gp.covfun,[]);
    else
        K_mat = gplite_covfun(hyp_cov,gp.X,gp.covfun,[]);
    end
end

% Use Cholesky representation of posterior for non-small noise
Lchol = min(sn2) >= 1e-6;

if Lchol
    if isscalar(sn2)
        sn2div = sn2;
        sn2_mat = eye(N);
    else
        sn2div = min(sn2);
        sn2_mat = diag(sn2/sn2div);
    end

    for iter = 1:10
        [L,p] = chol(K_mat/(sn2div*sn2_mult)+sn2_mat);
        if p > 0; sn2_mult = sn2_mult*10; else; break; end
    end
    sl = sn2div*sn2_mult;
    if nargout > 2
        pL = L;             % L = chol(eye(n)+sW*sW'.*K)
    end
else
    if isscalar(sn2)
        sn2_mat = sn2*eye(N);
    else
        sn2_mat = diag(sn2);
    end
    for iter = 1:10     % Cholesky decomposition until it works
        [L,p] = chol(K_mat+sn2_mult*sn2_mat);
        if p > 0; sn2_mult = sn2_mult*10; else; break; end
    end
    sl = 1;
    if nargout > 2
        pL = -L\(L'\eye(N));    % L = -inv(K+inv(sW^2))
    end
end

alpha = L\(L'\(y-m)) / sl;     % alpha = inv(K_mat + diag(sn2)) * (y - m)  I

%% Integrated basis functions

if gp.intmeanfun > 0
    bb = gp.intmeanfun_mean(:);
    BB = gp.intmeanfun_var(:);

    H = gplite_intmeanfun(gp.X,gp.intmeanfun);
    plus_idx = (BB > 0);    % Non-delta parameters
    betabar = zeros(1,size(H,1));
    if any(~plus_idx)
        T_plus = diag(1./BB(plus_idx)) + H(plus_idx,:)*(L\(L'\H(plus_idx,:)')/sl);
        T_chol = chol(T_plus);
        betabar(plus_idx) = T_chol \ (T_chol' \ (bb(plus_idx)./BB(plus_idx) + H(plus_idx,:)*alpha));
        betabar(~plus_idx) = bb(~plus_idx);
    else
        T_plus = diag(1./BB) + H*(L\(L'\H')/sl);
        T_chol = chol(T_plus);
%        betabar(:) = T_plus \ (bb./BB + H*alpha);
        betabar(:) = T_chol \ (T_chol' \ (bb./BB + H*alpha));
    end
end

%% Negative log marginal likelihood computation
nlZ = []; dnlZ = []; Q = [];

if compute_nlZ
    Nhyp = size(hyp,1);

    if gp.intmeanfun > 0
        % Negative log marginal likelihood with integrated basis functions
        prec_idx = BB > 0 & isfinite(BB);
        inf_idx = isinf(BB);

        vagueall_flag = all(inf_idx);  % Vague prior on *all* basis functions?
        vagueany_flag = any(inf_idx);  % Some vague priors?
        precany_flag = any(prec_idx);  % Some precise priors?

        % Compute first quadratic term
        nu = y-m - H'*bb;
        if vagueall_flag
            nlZ_1 = nu'*alpha/2;
        else
            if precany_flag
                HBH_prec = H(prec_idx,:)'*bsxfun(@times,BB(prec_idx),H(prec_idx,:));
                N_mat = L'*L*sl + HBH_prec;
                N_chol = chol(N_mat);
                % Ninv = N_mat\eye(N);
                Ninv = N_chol\(N_chol'\eye(N));
                nlZ_1 = nu'*(Ninv*nu)/2;
            else
                nlZ_1 = nu'*(L\(L'\nu))/sl/2;
            end
        end

        % Compute second quadratic term (vague prior contribution)
        if vagueany_flag
            if ~precany_flag
                W = chol(H*(L\(L'\H'))/sl);
                % A = H'*((H*Kinv*H')\H);
                A_mat = H'*(W\(W'\H));
                nlZ_v = -nu'*(L\(L'\(A_mat*(L\(L'\nu)))))/sl^2/2;
            else
                Hinf = H(inf_idx,:);
                W = chol(Hinf*Ninv*Hinf');
                % A = Hinf'*((Hinf*Ninv*Hinf')\Hinf);
                A_mat = Hinf'*(W\(W'\Hinf));
                C = Ninv*A_mat*Ninv;
                nlZ_v = -nu'*C*nu/2;
            end
        else
            nlZ_v = 0;
        end

        % Compute determinants
        nldet = sum(log(diag(L)));                  % First component
        if precany_flag                             % Precise priors
            nldet = nldet + sum(log(BB(prec_idx)))/2;
            Tprec_idx = isfinite(BB(BB > 0));
            nldet = nldet + log(det(T_plus(Tprec_idx,Tprec_idx)))/2;
        end
        if vagueany_flag
            nldet = nldet + sum(log(diag(W)));
        end

        nlZ = nlZ_1 + nlZ_v + nldet + N*log(2*pi*sl)/2 - sum(inf_idx)*log(2*pi)/2;

    else
        % Compute negative log marginal likelihood
        nlZ = (y-m)'*alpha/2 + sum(log(diag(L))) + N*log(2*pi*sl)/2;
    end

    if outwarp_flag     % Jacobian correction for output warping
        nlZ = nlZ - sum(log(abs(dwarp_dt)));
    end

    if compute_nlZ_grad
        % Compute gradient of negative log marginal likelihood

        dnlZ = zeros(Nhyp,1);    % allocate space for derivatives

        if gp.intmeanfun > 0
            % Gradient with integrated basis functions
            Kinv = L\(L'\eye(N))/sl;
            if ~precany_flag; Ninv = Kinv; end
            chi = Ninv*nu;
            Q = Kinv - chi*chi';
            if vagueany_flag
                phi = Ninv*A_mat*chi;
                Q = Q - phi*phi' + 2*chi*phi';
                if ~precany_flag
                    Q = Q - Kinv*A_mat*Kinv;
                else
                    Q = Q - Ninv*A_mat*Ninv;
                end
            else
                phi = 0;
            end
            if precany_flag
                Q = Q - Kinv*H(prec_idx,:)'*(T_plus(Tprec_idx,Tprec_idx)\(H(prec_idx,:)*Kinv));
            end
        else
            Q = L\(L'\eye(N))/sl - alpha*alpha';
        end

        if gp.covfun(1) == 1
            for i = 1:D                             % Grad of cov length scales
                K_temp = K_mat .* sq_dist(gp.X(:,i)'/ell(i));
                dnlZ(i) = sum(sum(Q.*K_temp))/2;
            end
            dnlZ(D+1) = sum(sum(Q.*(2*K_mat)))/2;   % Grad of cov output scale
        else
            for i = 1:Ncov                          % Grad of cov hyperparameters
                dnlZ(i) = sum(sum(Q.*dK_mat(:,:,i)))/2;
            end
        end

        % Gradient of GP likelihood
        if isscalar(sn2)
            trQ = trace(Q);
            for i = 1:Nnoise; dnlZ(Ncov+i) = 0.5*sn2_mult*dsn2(i)*trQ; end
        else
            dgQ = diag(Q);
            for i = 1:Nnoise; dnlZ(Ncov+i) = 0.5*sn2_mult*sum(dsn2(:,i).*dgQ); end
            if outwarp_flag
                error('Input-dependent noise not supported with output warping yet.');
            end
        end

        % Gradient of mean function
        if Nmean > 0
            if gp.intmeanfun > 0
                % Mean function gradient with integrated basis functions
                dnlZ(Ncov+Nnoise+(1:Nmean)) = -dm'*(chi - phi);
            else
                dnlZ(Ncov+Nnoise+(1:Nmean)) = -dm'*alpha;
            end
        end

        % Gradient of output warping function
        if outwarp_flag && Noutwarp > 0
            if gp.intmeanfun > 0
                error('Integrated basis functions are not supported with output warping yet.');
            end
            for i = 1:Noutwarp
                dnlZ(Ncov+Nnoise+Nmean+i) = dwarp_dtheta(:,i)'*alpha ...
                    - sum(d2warp_dthetadt(:,i)./dwarp_dt);
            end
        end

    end
end

%% Output posterior struct if requested
if nargout > 2
    post.hyp = hyp;
    post.alpha = alpha;
    post.sW = ones(N,1)./sqrt(min(sn2)*sn2_mult);   % sqrt of noise precision vector
    post.L = pL;
    post.sn2_mult = sn2_mult;
    post.Lchol = Lchol;
    if gp.intmeanfun > 0
        post.intmean.HKinv = H*(L\(L'\eye(N))/sl);
         % Inverse reduced T (only positive variances)
        post.intmean.Tplusinv = T_chol \ (T_chol' \eye(size(T_chol)));
        post.intmean.betabar = betabar;
    end
end

end


function song = sing(str, bpm)
%SING   A basic keyboard for MATLAB using CHEBFUNs.
%   S = SING('STR1 STR2 STR3 ...') creates a CHEBFUN F corresponding to the
%   musical notes in each of the STR which vaguely correspond to Helmholtz pitch
%   notation (http://en.wikipedia.org/wiki/Helmholtz_pitch_notation).
%
%   S = SING('STR1 ...', BPM) alters the temp of the tune (in beats per minute).
%   The default value is 60bpm.
%
%   For example, S = SING('A'), produces a function corresponding to the note
%   A, S = SING('B') produces a B, and so on.
%
%   Basic chords are provided, for example S = SING('CEG') will produce a major
%   triad (http://en.wikipedia.org/wiki/Major_chord). The convention is for the
%   kth note to have 1/k times the amplitude of the first.
%
%   Empty spaces correspond to pause of a sixteenth note (semiquaver), and
%   commas separate notes without a pause.
%
%   If STR is in uppercase, it will last for a quarter note (crotchet), and
%   lower case notes for a sixteenth note (semiquaver).
%
%   The SING keyboard has three octaves (in A to G). S = SING('C-') produces a
%   low C, S = SING('C') produces a middle C, and S = SING('C+') produces a
%   high C.
%
%   Sharps are supported by using '#', but there is currently no notation for
%   flats.
%
%   Examples:
%    1. Ode to Joy:
%       S = SING('BG- BG- CA DB DB CA BG- AD- G-B G-B AD- BG- BG- AD- AD-');
%    2. God Save The Queen
%       S = SING('CG CG DG BG CG DG EC EC FC EC DG CG DG CG BG CG',30);
%
%   CHEBFUNs for these examples may be generated with SING(1) and SING(2),
%   respectively.
%
% See also SOUND, CHEBTUNE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Set up basic frequencies.
s0 = 2^(1/12);
f0 = 440*pi;
fp1 = 2*f0;
fm1 = .5*f0;

% Set tempo.
if ( nargin < 2 )
    bpm = 60;
    t0 = 1/4;
else
    t0 = 60/bpm/4;
end

q = chebfun('x', [0 t0]);   % Quarter note.
s = chebfun('x', [0 t0/4]); % Sixteenth note.

%% Middle

% Quarter notes
A = sin(f0*q);
As = sin(f0*s0*q);
B = sin(f0*s0^2*q);
C = sin(f0*s0^3*q);
Cs = sin(f0*s0^4*q);
D = sin(f0*s0^5*q);
Ds = sin(f0*s0^6*q);
E = sin(f0*s0^7*q);
F = sin(f0*s0^8*q);
Fs = sin(f0*s0^9*q);
G = sin(f0*s0^10*q);
Gs = sin(f0*s0^11*q);

% Sixteenth notes
a = sin(f0*s);
as = sin(f0*s0*s);
b = sin(f0*s0^2*s);
c = sin(f0*s0^3*s);
cs = sin(f0*s0^4*s);
d = sin(f0*s0^5*s);
ds = sin(f0*s0^6*s);
e = sin(f0*s0^7*s);
f = sin(f0*s0^8*s);
fs = sin(f0*s0^9*s);
g = sin(f0*s0^10*s);
gs = sin(f0*s0^11*s);

%% -

% Quarter notes
Am = sin(fm1*q);
Ams = sin(fm1*s0*q);
Bm = sin(fm1*s0^2*q);
Cm = sin(fm1*s0^3*q);
Cms = sin(fm1*s0^4*q);
Dm = sin(fm1*s0^5*q);
Dms = sin(fm1*s0^6*q);
Em = sin(fm1*s0^7*q);
Fm = sin(fm1*s0^8*q);
Fms = sin(fm1*s0^9*q);
Gm = sin(fm1*s0^10*q);
Gms = sin(fm1*s0^11*q);

% Sixteenth notes
am = sin(fm1*s);
ams = sin(fm1*s0*s);
bm = sin(fm1*s0^2*s);
cm = sin(fm1*s0^3*s);
cms = sin(fm1*s0^4*s);
dm = sin(fm1*s0^5*s);
dms = sin(fm1*s0^6*s);
em = sin(fm1*s0^7*s);
fm = sin(fm1*s0^8*s);
fms = sin(fm1*s0^9*s);
gm = sin(fm1*s0^10*s);
gms = sin(fm1*s0^11*s);

%% +

% Quarter notes
Ap = sin(fp1*q);
Aps = sin(fp1*s0*q);
Bp = sin(fp1*s0^2*q);
Cp = sin(fp1*s0^3*q);
Cps = sin(fp1*s0^4*q);
Dp = sin(fp1*s0^5*q);
Dps = sin(fp1*s0^6*q);
Ep = sin(fp1*s0^7*q);
Fp = sin(fp1*s0^8*q);
Fps = sin(fp1*s0^9*q);
Gp = sin(fp1*s0^10*q);
Gps = sin(fp1*s0^11*q);

% Sixteenth notes
ap = sin(fp1*s);
aps = sin(fp1*s0*s);
bp = sin(fp1*s0^2*s);
cp = sin(fp1*s0^3*s);
cps = sin(fp1*s0^4*s);
dp = sin(fp1*s0^5*s);
dps = sin(fp1*s0^6*s);
ep = sin(fp1*s0^7*s);
fp = sin(fp1*s0^8*s);
fps = sin(fp1*s0^9*s);
gp = sin(fp1*s0^10*s);
gps = sin(fp1*s0^11*s);

%%

% Easy access to example tunes.
if ( (nargin == 0) || isnumeric(str) )
    if ( nargin == 0 )
        str = 1;
    end

    switch str
        case 1 % Ode to Joy
            str = 'BG- BG- CA DB DB CA BG- AD- G-B G-B AD- BG- BG- AD- AD-';
        case 2 % God Save The Queen
            str = 'CG CG DG BG CG DG EC EC FC EC DG CG DG CG BG CG';
    end
end

% Build the CHEBFUN from the string of notes for the tune.
song = chebfun;
l = 0;
while ( ~isempty(str) )
    l = l + 1;
    k = 0;

    % Get the string for the next chord.
    for j = 1:numel(str)
        k = k + 1;
        if ( strcmp(str(k), ' ') || strcmp(str(k), ',') )
            break
        end
    end

    strk = str(1:k);
    str(1:k) = [];

    % Quarter note or sixteenth note?
    if ( isstrprop(strk(1), 'upper') )
        strk = upper(strk);
        songtmp = 0*A;
    else
        strk = lower(strk);
        songtmp = 0*a;
    end

    % Assemble the chord from its constituent notes.
    j = 0;
    while ( ~isempty(strk) )
        % Parse out one note from the chord.
        if ( (length(strk) > 1) && any(strcmpi(strk(2), {'+' '-' '#' '*'})) )
            if ( (length(strk) > 2) && any(strcmpi(strk(3), {'#' '*'})) )
                strjk = strk(1:3);
                strk(1:3) = [];
            else
                strjk = strk(1:2);
                strk(1:2) = [];
            end
        else
            strjk = strk(1);
            strk(1) = [];
        end

        % Set the amplitude of this note in the chord.
        j = j + 1;
        amp = .4/j;

        % Decode and add in the note.
        if ( strcmp(strjk, 'r') || strcmp(strjk, ' ') )  % Sixteenth rest.
            songtmp = join(songtmp, 0*s);
            break
        elseif ( strcmp(strjk, 'R') )                    % Quarter rest.
            songtmp = join(songtmp, 0*q);
            break
        else                                             % Actual note.
            songtmp = songtmp + amp*getNote(strjk);
        end
    end

    % Tack the chord onto the end of the song.
    song = join(song, songtmp);
end

    % Nested function for mapping note strings to note CHEBFUNs defined above.
    function note = getNote(s)
        switch (s)
            case 'A', note = A;
            case 'B', note = B;
            case 'C', note = C;
            case 'D', note = D;
            case 'E', note = E;
            case 'F', note = F;
            case 'G', note = G;

            case 'a', note = a;
            case 'b', note = b;
            case 'c', note = c;
            case 'd', note = d;
            case 'e', note = e;
            case 'f', note = f;
            case 'g', note = g;

            case 'A-', note = Am;
            case 'B-', note = Bm;
            case 'C-', note = Cm;
            case 'D-', note = Dm;
            case 'E-', note = Em;
            case 'F-', note = Fm;
            case 'G-', note = Gm;

            case 'a-', note = am;
            case 'b-', note = bm;
            case 'c-', note = cm;
            case 'd-', note = dm;
            case 'e-', note = em;
            case 'f-', note = fm;
            case 'g-', note = gm;

            case 'A+', note = Ap;
            case 'B+', note = Bp;
            case 'C+', note = Cp;
            case 'D+', note = Dp;
            case 'E+', note = Ep;
            case 'F+', note = Fp;
            case 'G+', note = Gp;

            case 'a+', note = ap;
            case 'b+', note = bp;
            case 'c+', note = cp;
            case 'd+', note = dp;
            case 'e+', note = ep;
            case 'f+', note = fp;
            case 'g+', note = gp;

            case 'a#', note = as;
            case 'c#', note = cs;
            case 'd#', note = ds;
            case 'f#', note = fs;
            case 'g#', note = gs;

            case 'A#', note = As;
            case 'C#', note = Cs;
            case 'D#', note = Ds;
            case 'F#', note = Fs;
            case 'G#', note = Gs;

            case 'a+#', note = aps;
            case 'c+#', note = cps;
            case 'd+#', note = dps;
            case 'f+#', note = fps;
            case 'g+#', note = gps;

            case 'A+#', note = Aps;
            case 'C+#', note = Cps;
            case 'D+#', note = Dps;
            case 'F+#', note = Fps;
            case 'G+#', note = Gps;

            case 'a-#', note = ams;
            case 'c-#', note = cms;
            case 'd-#', note = dms;
            case 'f-#', note = fms;
            case 'g-#', note = gms;

            case 'A-#', note = Ams;
            case 'C-#', note = Cms;
            case 'D-#', note = Dms;
            case 'F-#', note = Fms;
            case 'G-#', note = Gms;
        end
    end

end


function polygonal_surface_display ( node_file_name, face_file_name )

%*****************************************************************************80
%
%% POLYGONAL_SURFACE_DISPLAY displays a polygonal surface file.
%
%  Discussion:
%
%    This program reads two files defining the nodes and the faces
%    of a polygonal surface in 3D.
%
%    It displays the surface using MATLAB's graphics system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string NODE_FILE_NAME, the name of the node file.
%
%    Input, string FACE_FILE_NAME, the name of the face file.
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGONAL_SURFACE_DISPLAY\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the nodes and faces defining\n' );
  fprintf ( 1, '  a polygonal surface and display the object\n' );
  fprintf ( 1, '  as a MATLAB shape.\n' );
%
%  If at least one command line argument, it's the node file name.
%
  if ( nargin < 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGONAL_SURFACE_DISPLAY:\n' );
    node_file_name = input ( 'Enter the name of the node file:' );

  end
%
%  If at least two command line arguments, it's the face file name.
%
  if ( nargin < 2 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGONAL_SURFACE_DISPLAY:\n' );
    face_file_name = input ( 'Enter the name of the face file:' );

  end
%
%  Read the data.
%
  [ node_num, node_xyz ] = polygonal_surface_node_file_read ( node_file_name );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  r8mat_transpose_print_some ( 3, node_num, node_xyz, 1, 1, 3, 5, ...
    '  First five nodes:' );

  [ face_order, face_num, face_node ] = ...
    polygonal_surface_face_file_read ( face_file_name );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Face order =      %d\n', face_order );
  fprintf ( 1, '  Number of faces = %d\n', face_num );

  i4mat_transpose_print_some ( face_order, face_num, face_node, ...
    1, 1, face_order, 5, '  First five faces:' );
%
%  Display the image as a collection of polygons.
%
  handle = patch ( 'Vertices', node_xyz', 'Faces', face_node' );

  set ( handle, 'FaceColor', [0.5, 0.6, 0.8], 'EdgeColor', 'Black' );

  axis equal;
  grid on;

  xlabel ( '--X axis--' )
  ylabel ( '--Y axis--' )
  zlabel ( '--Z axis--' )
%
%  The TITLE function will interpret underscores in the title.
%  We need to unescape such escape sequences!
%
  title_string = s_escape_tex ( face_file_name );
  title ( title_string )
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGONAL_SURFACE_DISPLAY:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Discussion:
%
%    An I4MAT is an array of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Discussion:
%
%    An I4MAT is an array of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 10;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function [ face_order, face_num, face_node ] = ...
  polygonal_surface_face_file_read ( face_file_name )

%*****************************************************************************80
%
%% POLYGONAL_SURFACE_FACE_FILE_READ reads faces defining a surface.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Angel,
%    Interactive Computer Graphics,
%    a Top-Down Approach with OpenGL,
%    Addison-Wesley, 2000.
%
%  Parameters:
%
%    Input, string FACE_FILE_NAME, the name of the face file.
%
%    Output, integer FACE_ORDER, the order of the face.
%
%    Output, integer FACE_NUM, the number of face defined.
%
%    Output, real FACE_NODE(FACE_ORDER,FACE_NUM),
%    the nodes making up each face.
%
  [ face_order, face_num ] = i4mat_header_read ( face_file_name );

  face_node = i4mat_data_read ( face_file_name, face_order, face_num );

  return
end
function [ node_num, node_xyz ] = polygonal_surface_node_file_read ( ...
  node_file_name )

%*****************************************************************************80
%
%% POLYGONAL_SURFACE_NODE_FILE_READ reads node information from a polygonal surface node file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Angel,
%    Interactive Computer Graphics,
%    a Top-Down Approach with OpenGL,
%    Addison-Wesley, 2000.
%
%  Parameters:
%
%    Input, string NODE_FILE_NAME, the name of the node file.
%
%    Output, integer NODE_NUM, the number of nodes defined.
%
%    Output, real NODE_XYZ(3,NODE_NUM), the coordinates of nodes.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_file_name );

  node_xyz = r8mat_data_read ( node_file_name, dim_num, node_num );

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 5;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function s2 = s_escape_tex ( s1 )

%*****************************************************************************80
%
%% S_ESCAPE_TEX de-escapes TeX escape sequences.
%
%  Discussion:
%
%    In particular, every occurrence of the characters '\', '_',
%    '^', '{' and '}' will be replaced by '\\', '\_', '\^',
%    '\{' and '\}'.  A TeX interpreter, on seeing these character
%    strings, is then likely to return the original characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S1, the string to be de-escaped.
%
%    Output, string S2, a copy of the string, modified to avoid TeX escapes.
%
  s1_length = length ( s1 );

  s1_pos = 0;
  s2_pos = 0;
  s2 = [];

  while ( s1_pos < s1_length )

    s1_pos = s1_pos + 1;

    if ( s1(s1_pos) == '\' || ...
         s1(s1_pos) == '_' || ...
         s1(s1_pos) == '^' || ...
         s1(s1_pos) == '{' || ...
         s1(s1_pos) == '}' )
      s2_pos = s2_pos + 1;
      s2 = strcat ( s2, '\' );
    end

    s2_pos = s2_pos + 1;
    s2 = strcat ( s2, s1(s1_pos) );

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function [errors, perrors] = ...
    mcmcTestSuperpixelClassifier(spfeatures, imsegs, vclassifier, hclassifier)

spvlabels = {imsegs(:).vert_labels};
sphlabels = {imsegs(:).horz_labels};

% test vertical
[vdata, vlab, imind] = formatData(spfeatures, spvlabels);
vconf = test_boosted_dt_mc(vclassifier, vdata);
vconf = 1 ./ (1+exp(-vconf));
vconf = vconf ./ repmat(sum(vconf, 2), 1, size(vconf, 2));
[tmp, vmax] = max(vconf, [], 2);
verror = mean(vmax~=vlab)

vconferr = 0;
for k = 1:numel(vlab)
    vconferr = vconferr + 1 - vconf(k, vlab(k));
end
vconferr = vconferr / numel(vlab)

% test subclass
[hdata, hlab] = formatData(spfeatures, sphlabels);
hconf = test_boosted_dt_mc(hclassifier, hdata);
hconf = 1 ./ (1+exp(-hconf));
hconf = hconf ./ repmat(sum(hconf, 2), 1, size(hconf, 2));
[tmp, hmax] = max(hconf, [], 2);
herror = mean(hmax~=hlab)

hconferr = 0;
for k = 1:numel(hlab)
    hconferr = hconferr + 1 - hconf(k, hlab(k));
end
hconferr = hconferr / numel(hlab)

% get same_label error
%[serror, sconferr] = sameLabelError(spfeatures, spvlabels, sphlabels, ...
%    {imsegs(:).adjmat}, vclassifier, hclassifier)

errors.verr = verror;
errors.vcerr = vconferr;
errors.herr = herror;
errors.hcerr = hconferr;
%errors.serr = serror;
%errors.scerr = sconferr;

[hdata2, hlab2, imind] = formatData(spfeatures, spvlabels);
hconf2 = test_boosted_dt_mc(hclassifier, hdata2);
hconf2 = 1 ./ (1+exp(-hconf2));
hconf2 = hconf2 ./ repmat(sum(hconf2, 2), 1, size(hconf2, 2));

lab = (vlab==1)*1 + (vlab==2).*(hlab2+1) + (vlab==3)*7;

pEst = [vconf(:, 1) repmat(vconf(:, 2), 1, 5).*hconf2 vconf(:, 3)];
pTrue = (repmat((1:7), numel(lab), 1)==repmat(lab, 1, 7));

[pMean, pVal, estBias] = evaluateProbabilityEstimate(pEst(:), pTrue(:), 0.05);
perrors.pMean = pMean;
perrors.pVal = pVal;
perrors.estBias = estBias;

perrors.pMean2 = 0;
perrors.estBias2 = 0;
snv = 0;
for k = 1:5
    testind = find((imind > (1-k)*50) & (imind < k*50));
    trainind = setdiff((1:numel(imind)),testind)';

    [pMean, pVal, estBias] = evaluateProbabilityEstimate(pEst(trainind), pTrue(trainind), 0.05);
    perrors.pMeanTr{k} = pMean;

    pEstTest = pEst(testind);
    for i = 1:numel(pEstTest)
        [val, maxind] = min(abs(pVal-pEstTest(i)));
        pEstTest(i) = pMean(maxind);
    end
%    disp(num2str(numel(pEstTest)))
    [pMean, pVal, estBias, nv] = evaluateProbabilityEstimate(pEstTest, pTrue(testind), 0.05);
    snv = snv + nv;
    perrors.pMean2 = perrors.pMean2 + pMean.*nv;
    perrors.estBias2 = perrors.estBias2 + estBias/5;
end
perrors.pMean2 = perrors.pMean2 ./ max(snv, 1E-5);

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

function [data, lab, imind] = formatData(features, labels, labels2)
% concatenate data and labels, and give adjacency information

nimages = numel(features);
[tmp, nvars] = size(features{1});
% count superpixels
nsp = 0;
for f = 1:nimages
    nsp = nsp + sum(labels{f}~=0);
end
data = zeros(nsp, nvars);
lab = zeros(nsp, 1);
imind = zeros(nsp, 1);

% concatenate data
vc = 0;
for f = 1:nimages
    ind = find(labels{f}~=0);
    data(vc+1:vc+numel(ind), :) = features{f}(ind, :);
    lab(vc+1:vc+numel(ind)) = labels{f}(ind);
    imind(vc+1:vc+numel(ind)) = f;
    vc = vc + numel(ind);
end

function [serror, sconferr] = sameLabelError(features, vlabels, hlabels, adjmat, vclassifier, hclassifier)

nimages = numel(features);
[tmp, nvars] = size(features{1});
% count superpixels
nsp = 0;
for f = 1:nimages
    nsp = nsp + sum((vlabels{f}~=0) & ((hlabels{f}~=0) | (vlabels{f}~=2)));
end
data = zeros(nsp, nvars);
lab = zeros(nsp, 1);
adj = cell(nsp, 1);
issame = cell(nsp, 1);

% concatenate data
vc = 0;
nadj = 0;
for f = 1:nimages
    ind = find((vlabels{f}~=0) & ((hlabels{f}~=0) | (vlabels{f}~=2)));
    data(vc+1:vc+numel(ind), :) = features{f}(ind, :);
    for k = 1:numel(ind)
        restind = ind(k+1:end);
        adj{vc+k} = restind(find(adjmat{f}(ind(k), restind)));
        issame{vc+k} = zeros(numel(adj{vc+k}), 1);
        for k2 = 1:numel(adj{vc+k})
            issame{vc+k}(k2) = (vlabels{f}(ind(k))==vlabels{f}(adj{vc+k}(k2))) && ...
                (hlabels{f}(ind(k))==hlabels{f}(adj{vc+k}(k2)));
        end
        nadj = nadj  +numel(adj{vc+k});
    end
    vc = vc + numel(ind);
end

vconf = test_boosted_dt_mc(vclassifier, data);
vconf = 1 ./ (1+exp(-vconf));
vconf = vconf ./ repmat(sum(vconf, 2), 1, size(vconf, 2));
hconf = test_boosted_dt_mc(hclassifier, data);
hconf = 1 ./ (1+exp(-hconf));
hconf = hconf ./ repmat(sum(hconf, 2), 1, size(hconf, 2));

sconf = zeros(nadj, 1);
slab = zeros(nadj, 1);
sc = 0;
for k = 1:numel(adj)
    for k2 = 1:numel(adj{k})
        sc = sc + 1;
        slab(sc) = issame{k}(k2);
        sconf(sc) = sum(vconf(k, [1 3]).*vconf(k2, [1 3])) + ...
            sum((vconf(k, 2)*hconf(k, :)) .* (vconf(k2, 2)*hconf(k2, :)));
    end
end

ind1 = find(slab==1);
ind2 = find(slab==0);
disp(num2str([mean(sconf(ind1)) mean(sconf(ind2))]))
[sorted] = sort(sconf);
tval = sorted(floor(numel(sconf)*(1-mean(slab))));
serror = mean(slab~=(sconf>tval));
sconferr = mean(abs(slab-sconf));


classdef CellsClassifier < handle

    methods (Access = public, Static)

        function [full,empty,cut] = classifyCells(levelSet,connec)
            nodes = connec;
            indexes = (1:size(connec,1))';
            lsNodes = levelSet(nodes);
            phiCase = sum((sign(lsNodes)<0),2);

            full  = phiCase == size(nodes,2);
            empty = phiCase == 0;
            cut = indexes(~(full | empty));
        end

    end

end


function [ z, stats ] = solve_cosamp( Phi, K, y, x)
%SOLVEBPIC Solves the CoSaMP problem
    [M, N] = size(Phi);
    epsilon = 1e-6;
    %We now setup our recovery program.
    solver = spx.pursuit.single.CoSaMP(Phi, K);
    result = solver.solve(y);
    % recovered representation. N length vector
    z = result.z;
    stats.recoveredRepresentationVector  = z;
    % recovery error vector. N length vector
    h = x - z;
    stats.recoveryErrorVector = h;
    % l_2 norm of reconstruction error
    stats.recoveryErrorL2Norm = norm(h);
    % The K non-zero coefficients in x (set of indices)
    stats.T0 = find (x ~= 0);
    % The portion of recovery error over T0 K length vector
    stats.recoveryErrorVectorT0 = h(stats.T0);
    % Positions of other places (set of indices)
    stats.T0C = setdiff(1:N , stats.T0);
    % Recovery error at T0C places [N - K] length vector
    hT0C = h(stats.T0C);
    stats.recoveryErrorVectorT0C = hT0C;
    % The K largest indices after T0 in recovery error (set of indices)
    stats.T1 = spx.commons.signals.largest_indices(hT0C, K);
    % The recovery error component over T1. [K] length vector.
    hT1 = h(stats.T1);
    stats.recoveryErrorVectorT1 = hT1;
    % Remaining indices [N - 2K] set of indices
    stats.TRest = setdiff(stats.T0C , stats.T1);
    % Recovery error over remaining indices [N - 2K] length vector
    hTRest = h(stats.TRest);
    stats.recoveryErrorVectorTRest = hTRest;
    % largest indices of the recovered vector
    stats.TT0 = spx.commons.signals.largest_indices(z, K);
    % Support Overlap
    stats.supportOverlap = intersect(stats.T0, stats.TT0);
    % Support recovery ratio
    stats.supportRecoveryRatio = numel(stats.supportOverlap) / K;
    % measurement error vector [M] length vector
    e = y - Phi * z;
    stats.measurementError = e;
    % Norm of measurement error.  This must be less than epsilon
    stats.measurementErrorL2Norm = norm(e);
    % Ratio between the norm of recovery error and measurement error
    stats.recoveryToMeasurementErrorNormRatio = ...
        stats.recoveryErrorL2Norm / stats.measurementErrorL2Norm;
    % whether we consider the process to be success or not.
    % We consider success only if the support has been recovered
    % completely.
    stats.success = numel(stats.supportOverlap) == K;
end


%%***************************************************************************
%% blkeig: compute eigenvalue decomposition of a cell array
%%         whose contents are square matrices or the diagonal
%%         of a diagonal matrix.
%%
%% [d,V] = blkeig(blk,X);
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%***************************************************************************

    function [d,V] = blkeig(blk,X);

    spdensity = 0.5;

    if ~iscell(X);
       if strcmp(blk{1},'s');
          blktmp = blk{2};
          if (length(blktmp) == 1);
             if (nargout == 1);
                d = eig(full(X));
             elseif (nargout == 2);
                [V,d] = eig(full(X));
                d = diag(d);
             end
          else
             if (nargout == 2);
                V = sparse(length(X),length(X));
             end
	     d = zeros(sum(blktmp),1);
             xx = mexsvec(blk,X,0);
             blktmp2 = blktmp.*(blktmp+1)/2;
             s2 = [0, cumsum(blktmp2)];
             blksub{1,1} = 's'; blksub{1,2} = 0;
             s = [0, cumsum(blktmp)];
             for i = 1:length(blktmp)
                pos = [s(i)+1 : s(i+1)];
                blksub{2} = blktmp(i);
                Xsub = mexsmat(blksub,xx(s2(i)+1:s2(i+1)),0);
                if (nargout == 1);
                   lam = eig(Xsub);
                elseif (nargout == 2);
                   [evec,lam] = eig(Xsub);
                   lam = diag(lam);
                   V(pos,pos) = sparse(evec);
                end
                d(pos,1) = lam;
             end
          end
          n2 = sum(blktmp.*blktmp);
          if (nargout == 2);
             if (nnz(V) <= spdensity*n2);
                V = sparse(V);
             else
                V = full(V);
             end
          end
       elseif strcmp(blk{1},'l');
          if (nargout == 2);
             V = ones(size(X)); d = X;
          elseif (nargout == 1);
             d = X;
          end
       end
    else
       if (nargout == 2);
          V = cell(size(X));  d = cell(size(X));
          for p = 1:size(blk,1);
	     [d{p},V{p}] = blkeig(blk(p,:),X{p});
          end
       elseif (nargout == 1);
          d = cell(size(X));
          for p = 1:size(blk,1);
	     d{p} = blkeig(blk(p,:),X{p});
          end
       end
    end
%%***************************************************************************


% Group (split) position data by time or in specified number of partitions
%
% This function divides position data into groups based on pair of values (groupType, groupingValue).
% You can use it to split position data into predefined number of groups (2, 4, e.t.c.), or into
% groups of equal time.
%
%  USAGE
%   [groupIndices, edges] = general.groupPositions(pos, groupType, groupingValue)
%   pos             Position samples. Matrix of size at least Nx1.
%   groupType       String that defines type of grouping. Affects how groupingValue
%                   argument is used. 'time' defines group by time interval.
%                   'num' groups positions in specified number of groups.
%   groupingValue   Integer that specify value for grouping. If groupType == 'time'
%                   this argument should be a value of seconds.
%                   If groupType == 'num' this argument is the number of groups.
%
%   groupIndices    Vector of length equal to number of position samples. Each value
%                   indicates group belongingness. If groupIndices(3) == 1, means that
%                   position sample number 3 belongs to group number 1.
%   edges           Vector of edges that define groups. For example, if data is split
%                   in halves, then edges = [0 <middle_point> Inf].
%
%  EXAMPLE
%
%   pos = data.getPositions();
%   groupIndices = general.groupPositions(pos, 'num', 2); % divide data into halves
%   positionsOfFirstGroup = pos(groupIndices == 1, :);
%
%   general.groupPositions(pos, 'time', 1); % divide data into groups each duration of 1 second
%
function [groupIndices, edges] = groupPositions(pos, groupType, groupingValue)
    if nargin < 3
        error('BNT:numArgs', 'Incorrect number of parameters (type ''help general.groupPositions'' for details).');
    end

    if size(pos, 1) == 1 && size(pos, 2) > 2
        % this is probably a row vector, transpose it
        pos = pos';
    end

    if size(pos, 2) < 1
        error('Incorrect value for argument ''pos'' (type ''help general.groupPositions'' for details).');
    end

    if ~helpers.isstring(groupType, 'time', 'num')
        error('Incorrect value for argument ''groupType'' (type ''help general.groupPositions'' for details).');
    end

    if ~helpers.isdscalar(groupingValue, '>0')
        error('Incorrect value for argument ''groupingValue'', it should be >= 1 (type ''help general.groupPositions'' for details).');
    end

    post = pos(:, 1);
    duration = max(post) - min(post);

    if strcmpi(groupType, 'num')
        interval = duration / groupingValue;
        edges = 0:interval:duration;
        edges = min(post) + edges; % shift if for case min(post) ~= 0
        edges(end) = Inf;
    else
        interval = groupingValue;
        edges = 0:interval:duration;
        edges = min(post) + edges; % shift if for case min(post) ~= 0
        edges(end+1) = Inf;
    end

    [~, groupIndices] = histc(post, edges);
end


function varargout = drawGraph(varargin)
%DRAWGRAPH Draw a graph, given as a set of vertices and edges
%
%   drawGraph(NODES, EDGES)
%   Draws a graph specified by a set of nodes (array N-by-2 or N-by-3,
%   corresponding to coordinate of each node), and a set of edges (an array
%   NE-by-2, containing for each edge the first and the second node).
%   Default drawing is a red circle for nodes and a blue line for edges.
%
%   drawGraph(NODES, EDGES, FACES)
%   Also draws faces of the graph as patches.
%
%   drawGraph(GRAPH)
%   Passes argument in a srtucture with at least 2 fields named 'nodes' and
%   'edges', and possibly one field 'faces', corresponding to previously
%   described parameters.
%   GRAPH can also be a cell array, whose first element is node array,
%   second element is edges array, and third element, if present, is faces
%   array.
%
%
%   drawGraph(..., SNODES)
%   drawGraph(..., SNODES, SEDGES)
%   drawGraph(..., SNODES, SEDGES, SFACES)
%   Specifies the draw mode for each element, as in the classical 'plot'
%   function. To not display some elements, uses 'none'.
%
%
%   H = drawGraph(...)
%   Returns handle to the set of edges.
%
%   [HN, HE] = drawGraph(...)
%   Returns handle to the set of nodes and to the set of edges.
%
%   [HN, HE, HF] = drawGraph(...)
%   Also returns handle to the set of faces.
%
%   See Also
%   graphs, drawGraphEdges, fillGraphFaces, clipGraph, clipGraphPolygon
%

%   -----
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 17/07/2003.
%

%   HISTORY
%   10/02/2004 : documentation
%   06/04/2004 : change name
%   09/07/2004 : add faces
%   05/08/2004 : correct bug when drawing 2D graph
%   06/08/2004 : small bug for drawing poins (length instead of size(.,1))
%   09/08/2004 : rewrite code (separate 2D and 3D, use of plot instead of
%       line, manage faces if present in 2D and 3D, ...), add style
%       management, various input types, and documentation
%   22/09/2004 : correct bug in drawing faces
%   11/11/2005 : forgot a loop index for faces stored as cells
%   22/05/2009 add more drawing options

%% initialisations

% uses empty arrays by default for edges and faces
e = [];
f = [];

% default styles for nodes, edges, and faces

% nodes are drawn as red circles
sn = {'linestyle', 'none', 'color', 'r', 'marker', 'o'};

% edges are drawn as blue lines
se = {'linestyle', '-', 'color', 'b'};

% faces are cyan, their edges are not drawn
sf = {'EdgeColor', 'none', 'Facecolor', 'c'};

%% Process input arguments

% case of a call without arguments
if nargin == 0
    help drawGraph;
    return;
end

% ---------------------------------------------------------------
% First extract the graph structure

var = varargin{1};
if iscell(var)
    % graph is stored as a cell array: first cell is nodes, second one is
    % edges, and third is faces
    n = var{1};
    if length(var)>1
        e = var{2};
    end
    if length(var)>2
        f = var{3};
    end
    varargin(1) = [];
elseif isstruct(var)
    % graph is stored as a structure, with fields 'nodes', 'edges', and
    % eventually 'faces'.
    n = var.nodes;
    e = var.edges;
    if isfield(var, 'faces')
        f = var.faces;
    end
    varargin(1) = [];
else
    % graph is stored as set of variables: nodes, edges, and eventually
    % faces
    n = varargin{1};
    e = varargin{2};
    varargin(1:2) = [];

    if ~isempty(varargin)
        var = varargin{1};
        if isnumeric(var)
            % faces are stored in a numeric array of indices
            f = var;
            varargin(1) = [];
        elseif iscell(var)
            if ~ischar(var{1})
                % faces are stored in a cell array, each cell containing a
                % row vector of indices
                f = var;
                varargin(1) = [];
            end
        end
    end
end

% extract drawing style

if ~isempty(varargin)
    sn = concatArguments(sn, varargin{1});
end

if length(varargin)>1
    se = concatArguments(se, varargin{2});
end

if length(varargin)>2
    sf = concatArguments(sf, varargin{3});
end

%% Main drawing processing

hold on;

if size(n, 2) == 2
    % Draw a 2 dimensional graph ----------------------

    % Draw faces of the graph ------------
    if ~strcmp(sf{1}, 'none') && ~isempty(f)
        if iscell(f)
            % each face is contained in a cell.
            hf = zeros(size(f));
            for fi = 1:length(f)
                hf(fi) = patch('Faces', f{fi}, 'Vertices', n, sf{:});
            end
        else
            % process faces as an Nf*N array. Nf is the number of faces,
            % and all faces have the same number of vertices (nodes).
            hf = patch('Faces', f, 'Vertices', n, sf{:});
        end
    end

    % Draw 2D Edges ----------------------
    if ~strcmp(se{1}, 'none') && size(e, 1) > 0
        he = plot([n(e(:,1),1) n(e(:,2),1)]', [n(e(:,1),2) n(e(:,2),2)]', se{:});
    end

    % Draw 2D nodes ----------------------
    if ~strcmp(sn{1}, 'none')
        hn = plot(n(:,1), n(:,2), sn{:});
    end

elseif size(n, 2)==3
    % Draw a 3 dimensional graph ----------------------

    % use a zbuffer to avoid display pbms.
    set(gcf, 'renderer', 'zbuffer');

    % Draw 3D Faces ----------------------
    if ~strcmp(sf{1}, 'none')
        if iscell(f)
            % each face is contained in a cell.
            hf = zeros(size(f));
            for fi = 1:length(f)
                hf(fi) = patch('Faces', f{fi}, 'Vertices', n, sf{:});
            end
        else
            % process faces as an Nf*N array. Nf i the number of faces,
            % and all faces have the same number of vertices (nodes).
            hf = patch('Faces', f, 'Vertices', n, sf{:});
        end
    end

    % Draw 3D edges ----------------------
    if ~strcmp(se{1}, 'none') && size(e, 1) > 0
        he = line(...
            [n(e(:,1),1) n(e(:,2),1)]', ...
            [n(e(:,1),2) n(e(:,2),2)]', ...
            [n(e(:,1),3) n(e(:,2),3)]', ...
            se{:});
    end

    % Draw 3D nodes ----------------------
    if ~strcmp(sn{1}, 'none')
        hn = plot3(n(:,1), n(:,2), n(:,3), sn{:});
    end

end

%% Format output arguments

% return handle to edges
if nargout==1
    varargout{1} = he;
end

% return handle to nodes and edges
if nargout==2
    varargout{1} = hn;
    varargout{2} = he;
end

% return handle to nodes, edges and faces
if nargout==3
    varargout{1} = hn;
    varargout{2} = he;
    varargout{3} = hf;
end

end

function res = concatArguments(in1, in2)
% in1 is a cell array already initialized
% in2 is an argument that can be:
%   - empty
%   - the string 'none'
%   - another cell array

if isempty(in2)
    res = in1;
    return;
end

if ischar(in2)
    if strcmp('none', in2)
        res = {'none'};
        return;
    end
end

if iscell(in1)
    res = [in1(:)' in2(:)'];
else
    res = [{in1} in2(:)];
end

end


function F = conj(F)
%CONJ   Complex conjugate of a CHEBFUN.
%   CONJ(F) is the complex conjugate of F.
%
% See also REAL, IMAG.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Loop over the columns of F:
for j = 1:numel(F)

    % Conjugate the point values:
    F(j).pointValues = conj(F(j).pointValues);

    % Conjugate the FUNs:
    for k = 1:numel(F(j).funs)
        F(j).funs{k} = conj(F(j).funs{k});
    end
end

end


function [coord,R]=calcgrip(Tr,Tc)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%EN FUNCION DE DONDE ESTE LA CAJA Y DONDE ESTE EL ROBOT,
%CALCULA QUE LADO DE LA CAJA ESTA A MENOS DISTANCIA DE LA
%POSICION ACTUAL, Y DEVUELVE POR DONDE TIENE QUE COGER LA CAJA
%Y CON QUE ORIENTACION

    Tc(3,4)=Tc(3,4)+0.05;
    Tc1=Tc(1:3,4);
    Tc2=Tc(1:3,4);
    Tc3=Tc(1:3,4);
    Tc4=Tc(1:3,4);
    Tc1(1,4)=Tc(1,4)+0.14;
    dist(1,1)=norm(Tc1-Tr(1:3,4));
    Tc2(1,4)=Tc(1,4)-0.14;
    dist(2,1)=norm(Tc2-Tr(1:3,4));
    Tc3(1,4)=Tc(2,4)+0.10;
    dist(3,1)=norm(Tc3-Tr(1:3,4));
    Tc4(1,4)=Tc(2,4)-0.10;
    dist(4,1)=norm(Tc4-Tr(1:3,4));
    [M,I]=min(dist);
    if I==1
        coord=[0.14;0;0.05];
        R=[0 0 1 ;0 1 0;1 0 0];
    end
    if I==2
        coord=[-0.14;0;0.05];

        R=[0 0 -1 ;0 1 0;-1 0 0];
    end
    if I==3
        coord=[0;0.10;0.05];
        R=[0 -1 0 ;0 0 1; -1 0 0];

    end
    if I==4
        coord=[0;-0.10;0.05];
        R=[0 1 0 ;0 0 -1; 1 0 0];
    end

end