function R = SeparateAngles(X,deep,is_real)
% SeparateAngles: Frequency angular partitioning
%  Usage:
%     R = SeparateAngles(X,deep)
%  Inputs:
%    X     m by m matrix (m = 2*2^(j+1)).
%          The input is the Fourier transform of an object obtained
%          after windowing with a bandpass Meyer window which
%          isolates frequencies in the range 2^(j-1) <= |k| <=
%          2^j. We interpret the array as samples at the frequency
%          points (k1,k2), -2^(j+1) <= k1,k2 < 2^(j+1)
%
%   deep   2^deep is the number of angular wedges in each of the
%          directional  panels NESW (North, East, South, West).
%  Outputs:
%    R      Array of smoothly localized Fourier samples
%           size = 4 * m * 2^j *  2*2^(j+1)/m, m = 2^deep
%
%     (i)   first index -- cardinal point 1 = W, 2 = E, 3 = N, 4 = S.
%     (ii)  second index -- indexes orientations according to the
%              organization below; e.g. (1,3) would correspond to
%              the wedge marked with a star.
%
%                                j
%             ------------------------------------->
%            |
%            |  1                                4
%            |
%            |
%            |
%            |  2                                3
%            |
%       i    |
%            |
%            |  3*                                2
%            |
%            |
%            |
%            |  4                                1
%            \/
%
%
%                     West                      East
%
%              North and South are obtained by transposition.
%
%     (iii) third and fourth indices-- frequency points, regular
%           samples along parallel slanted lines (shape of a parallelogram)
%
% Description
%   Smoothly localizes the FT near angular wedges
%   Operates by interpolating the FT along vertical and horizontal
%   directions
% See Also
%   Adj_SeparateAngles, AtA, AtA_Toeplitz
%
% By Emmanuel Candes, 2003-2004

  if nargin < 3
    is_real = 0;
  end

  n  = size(X,1);
  n2 = n/2;
  boxlen = n/2^deep;
  boxcnt = 2^deep;

  R = zeros(4,boxcnt,n2/2,2*boxlen);

  [ix,w] = DetailMeyerWindow([n2/4 n2/2],3);
  lx = reverse(-ix);

  m      =  0:(boxcnt - 1);
  ym     =  (m-boxcnt/2).*boxlen + boxlen/2;
  slope  =  -ym./n2;

  % recall that fft_mid0 operates along columns
  % Columns: West and East

  F = ifft_mid0(X).*sqrt(n);

  for r = 1:(n2/2),
    k = lx(r); t = n2 + k + 1;
    shift  =  ym + slope.*(k+n2);
    alpha =  -k./n2;
    w = MakeSineWindow(boxlen,alpha);
    y = Evaluate_FT(F(:,t),shift,boxlen,0,w);
    R(1,:,r,:)  =  reshape(y,2*boxlen,boxcnt).';
    rsym = n2/2+1-r;
    if (is_real)
      R(2,:,rsym,:) = conj(fliplr(squeeze(R(1,:,r,:))));
    else
      t = n2 - k + 1;
      shift = -shift + 1;
      y = Evaluate_FT(F(:,t),shift,boxlen,0,w);
      R(2,:,rsym,:) = reshape(y,2*boxlen,boxcnt).';
    end
  end

  % Rows: North and South
  F = ifft_mid0(X.').*sqrt(n);
  for r = 1:(n2/2),
    k = lx(r);  t = n2 + k + 1;
    shift  =  ym + slope.*(k+n2);
    alpha =  -k./n2;
    w = MakeSineWindow(boxlen,alpha);
    y = Evaluate_FT(F(:,t),shift,boxlen,0,w);
    R(3,:,r,:) = reshape(y,2*boxlen,boxcnt).';

    rsym = n2/2+1-r;
    if (is_real)
      R(4,:,rsym,:) = conj(fliplr(squeeze(R(3,:,r,:))));
    else
      t = n2 - k + 1;
      shift = -shift + 1;
      y = Evaluate_FT(F(:,t),shift,boxlen,0,w);
      R(4,:,rsym,:) = reshape(y,2*boxlen,boxcnt).';
    end
  end


function x = At_fft2( b, N, mask, P, m, n, bComplex )
% 2D IFFT
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

if( nargin < 4 ), P = 1:N; end

K = length(b);
fx = complex(zeros(m,n),zeros(m,n));
fx(mask) = 1/sqrt(2)*b(1:K/2) + 1i *1/sqrt(2)* b(K/2+1:K);
x = zeros(N,1);

if(bComplex)
    fct = sqrt(N/2); % complex input image
else
    fct = sqrt(N); % real input image
end

tmp = fct*fftshift(ifft2(ifftshift(fx)));

if(~bComplex)
    % real input image
    x(P) = abs(tmp(:));
else
    % complex input image
    x(P) = [1/sqrt(2)*real(tmp(:)); 1/sqrt(2)*imag(tmp(:))];
end

% x(P) = real(tmp(:)); % phantom

end


function [V,Beta,R] = qr_left (A)
%QR_LEFT left-looking Householder QR factorization.
% Example:
%  [V,Beta,R] = qr_left (A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;
V = zeros (m,n) ;
Beta = zeros (1,n) ;
R = zeros (m,n) ;
for k = 1:n
    x = A (:,k) ;
    for i = 1:k-1
        v = V (i:m,i) ;
        beta = Beta (i) ;
        x (i:m) = x (i:m) - v * (beta * (v' * x (i:m))) ;
    end
    [v,beta,s] = gallery ('house', x (k:m), 2) ;
    V (k:m,k) = v ;
    Beta (k) = beta ;
    R (1:(k-1),k) = x (1:(k-1)) ;
    R (k,k) = s ;
end


% Jason Joseph Rebello
% Carnegie Mellon University (Jan 2012 - May 2013)
% Masters in Electrical & Computer Engineering
% Logistic Regression with Regularization

% Run LogisitcRegression.m
% This program uses Logistic regression to classify handwritten digits. The
% data is comprised of a part of the MNIST dataset. 5000 examples are used.

clear all;
clc;
close all;

%% Load Training Data
t = cputime;
fprintf('Loading and Visualizing Data')

load('data.mat');

% Randomly select 100 data points to display
sel = randperm(size(X, 1));
sel = sel(1:100);

displayData(X(sel, :)); % function taken from Andrew Ng machine learning course
fprintf('...done\n');

%% Split data into training and testing
fprintf('Splitting Data into training and testing sets');
[XTrain XTest yTrain yTest] = splitData(X, y);
fprintf('...done\n\n');

%% Initialize parameters
fprintf('Initializing parameters');
m = size(X, 1); % number of examples
lambda = 0.1; % regularization parameter
numLabels = size(unique(y),1); % number of labels
fprintf('...done\n');

%% Training Logistic Regression classifier
fprintf('Training One-vs-All Logistic Regression');

theta = LRClassifier(XTrain, yTrain, numLabels, lambda);
fprintf('...done\n');

%% Predict numbers
prediction = predict(theta, XTest);

%% Calculate Accuracy over the training data
fprintf('\nTest Set Accuracy: %f\n', mean(double(prediction == yTest)) * 100);

fprintf('Program executed in %f seconds or %f minutes\n', cputime-t, (cputime-t)/60);


function [y,K] = spm_rand_power_law(csd,Hz,dt,N)
% generates random variates with a power law spectral density
% FORMAT [y,K] = spm_rand_power_law(csd,Hz,dt,N)
% csd - spectral densities (one per column)
% Hz  - frequencies
% dt  - sampling interval
% N   - number of time bins
%
% y   - random variate
% K   - convolution (kernel) operator: y(:,i) = K*randn(N,1)
%
% see also: spm_rand_mar; spm_Q
%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_rand_power_law.m 6759 2016-03-27 19:45:17Z karl $

% create random process
%==========================================================================

% create AR representation and associated convolution kernels
%--------------------------------------------------------------------------
p     = fix(length(Hz) - 1);
for i = 1:size(csd,2);
    ccf    = spm_csd2ccf(csd(:,i),Hz,dt);
    mar    = spm_ccf2mar(ccf,p);
    A      = [1 -mar.a'];
    P      = spdiags(ones(N,1)*A,-(0:mar.p),N,N);
    y(:,i) = P\randn(N,1)*sqrt(max(ccf));
end

% convolution kernel
%--------------------------------------------------------------------------
if nargout > 1
    K = inv(P)*sqrt(max(ccf));
end

return

% NB: alternative scheme - create random process
%==========================================================================
[m,n] = size(csd);
w     = (0:(N - 1))/dt/N;
dHz   = Hz(2) - Hz(1);
g     = zeros(N,n);
for i = 1:m
    j      = find(w > Hz(i),1);
    s      = sqrt(csd(i,:)).*(randn(1,n) + 1j*randn(1,n));
    g(j,:) = s;
    j      = N - j + 2;
    g(j,:) = conj(s);

end
y     = real(ifft(g));
y     = y*sqrt(mean(sum(G)*dHz)/mean(var(y)));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc;
clear;
close all;

%% Problem Definition

TestProblem=1;   % Set to 1, 2, or 3

switch TestProblem
    case 1
        CostFunction=@(x) MyCost1(x);
        nVar=5;
        VarMin=-4;
        VarMax=4;

    case 2
        CostFunction=@(x) MyCost2(x);
        nVar=3;
        VarMin=-5;
        VarMax=5;

    case 3
        CostFunction=@(x) MyCost3(x);
        nVar=2;
        VarMin=0;
        VarMax=1;
end

VarSize=[1 nVar];

VelMax=(VarMax-VarMin)/10;

%% MOPSO Settings

nPop=100;   % Population Size

nRep=100;   % Repository Size

MaxIt=100;  % Maximum Number of Iterations

phi1=2.05;
phi2=2.05;
phi=phi1+phi2;
chi=2/(phi-2+sqrt(phi^2-4*phi));

w=chi;              % Inertia Weight
wdamp=1;            % Inertia Weight Damping Ratio
c1=chi*phi1;        % Personal Learning Coefficient
c2=chi*phi2;        % Global Learning Coefficient

alpha=0.1;  % Grid Inflation Parameter

nGrid=10;   % Number of Grids per each Dimension

beta=4;     % Leader Selection Pressure Parameter

gamma=2;    % Extra (to be deleted) Repository Member Selection Pressure

%% Initialization

particle=CreateEmptyParticle(nPop);

for i=1:nPop
    particle(i).Velocity=0;
    particle(i).Position=unifrnd(VarMin,VarMax,VarSize);

    particle(i).Cost=CostFunction(particle(i).Position);

    particle(i).Best.Position=particle(i).Position;
    particle(i).Best.Cost=particle(i).Cost;
end

particle=DetermineDomination(particle);

rep=GetNonDominatedParticles(particle);

rep_costs=GetCosts(rep);
G=CreateHypercubes(rep_costs,nGrid,alpha);

for i=1:numel(rep)
    [rep(i).GridIndex rep(i).GridSubIndex]=GetGridIndex(rep(i),G);
end

%% MOPSO Main Loop

for it=1:MaxIt
    for i=1:nPop
        rep_h=SelectLeader(rep,beta);

        particle(i).Velocity=w*particle(i).Velocity ...
                             +c1*rand*(particle(i).Best.Position - particle(i).Position) ...
                             +c2*rand*(rep_h.Position -  particle(i).Position);

        particle(i).Velocity=min(max(particle(i).Velocity,-VelMax),+VelMax);

        particle(i).Position=particle(i).Position + particle(i).Velocity;

        flag=(particle(i).Position<VarMin | particle(i).Position>VarMax);
        particle(i).Velocity(flag)=-particle(i).Velocity(flag);

        particle(i).Position=min(max(particle(i).Position,VarMin),VarMax);

        particle(i).Cost=CostFunction(particle(i).Position);

        if Dominates(particle(i),particle(i).Best)
            particle(i).Best.Position=particle(i).Position;
            particle(i).Best.Cost=particle(i).Cost;

        elseif ~Dominates(particle(i).Best,particle(i))
            if rand<0.5
                particle(i).Best.Position=particle(i).Position;
                particle(i).Best.Cost=particle(i).Cost;
            end
        end

    end

    particle=DetermineDomination(particle);
    nd_particle=GetNonDominatedParticles(particle);

    rep=[rep
         nd_particle];

    rep=DetermineDomination(rep);
    rep=GetNonDominatedParticles(rep);

    for i=1:numel(rep)
        [rep(i).GridIndex rep(i).GridSubIndex]=GetGridIndex(rep(i),G);
    end

    if numel(rep)>nRep
        EXTRA=numel(rep)-nRep;
        rep=DeleteFromRep(rep,EXTRA,gamma);

        rep_costs=GetCosts(rep);
        G=CreateHypercubes(rep_costs,nGrid,alpha);

    end

    disp(['Iteration ' num2str(it) ': Number of Repository Particles = ' num2str(numel(rep))]);

    w=w*wdamp;
end

%% Results

costs=GetCosts(particle);
rep_costs=GetCosts(rep);

figure;

plot(costs(1,:),costs(2,:),'b.');
hold on;
plot(rep_costs(1,:),rep_costs(2,:),'rx');
legend('Main Population','Repository');


% [Sol] = homography2Motion(H)

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

% Homography estimation between views of a plane
% decompose it into rotation and translation and plane normal
% p - retinal (calibrated) coordinates of the first view
% q - retinal (calibrated) coordinate in the second view
% Sol = [rotation, translation*1/d, plane normal] four possible solutions
% H - two view homography
%
% Algorith, 5.2 in Chapter 5, "An introduction to 3-D Vision"
% by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS)
%
% Code distributed free for non-commercial use
% Copyright (c) MASKS, 2003
%
% Last modified 5/5/2003

function [Sol]  = homography2Motion(Hest);

 % decomposition of H into motion and structure in case of calibration case
 [u,s,v] = svd(Hest);
 H = Hest/s(2,2);
 [u,s,v] = svd(H'*H);

 if det(u) < 0 u = -u; end;

 s1 = s(1,1); s2 = s(2,2); s3 = s(3,3);
 v1 = u(:,1); v2 = u(:,2); v3 = u(:,3);
 u1 = (v1*sqrt(1-s3) + v3*sqrt(s1 -1))/sqrt(s1 - s3);
 u2 = (v1*sqrt(1-s3) - v3*sqrt(s1 -1))/sqrt(s1 - s3);

 U1 = [v2, u1, skew(v2)*u1];
 U2 = [v2, u2, skew(v2)*u2];
 W1 = [H*v2, H*u1, skew(H*v2)*H*u1];
 W2 = [H*v2, H*u2, skew(H*v2)*H*u2];

 N1 = skew(v2)*u1;
 N2 = skew(v2)*u2;

 R1 = W1*U1'
 R2 = W2*U2'

 Sol(:,:,1) = [W1*U1', (H - W1*U1')*N1, N1];
 Sol(:,:,2) = [W2*U2', (H - W2*U2')*N2, N2];
 Sol(:,:,3) = [W1*U1', -(H - W1*U1')*N1, -N1];
 Sol(:,:,4) = [W2*U2', -(H - W2*U2')*N2, -N2];


function omega = angle_outer(q1,q2,varargin)
% calcualtes the rotational angle between all rotations q1 and q2
%
% Syntax
%   omega = angle_outer(q1,q2)
%
% Input
%  q1, q2 - @quaternion
%
% Output
%  omega  - double [q1 x q2]

if nargin == 1, q2 = q1; end

omega = real(2*acos(abs(dot_outer(q1,q2,varargin{:}))));


%  INTERNAL FUNCTION: Kalman filter for non-regime-switching models
%
%  ::
%
%    f=missing_observations_kalman_filter(y,T,R,Z,H,Q,init,dy,ca,level)
%
%  Args:
%
%       y (p x n matrix): data
%       T (m x m x nT): state matrix, see below
%       R (m x r x nR): state matrix, see below
%       Z (p x m | logical): state matrix or selector, see below
%       H (p x p x nH): Covariances of measurement errors
%       Q (r x r x nQ): Covariances of shocks
%       init (struct):
%       dy (p x ndy):
%       ca (m x nca):
%       level (0 | 1 | 2 | {3}): controls the level output produced.
%
%           - if level==0, only the likelihood is returned
%           - if level==1, filters/forecasts are returned in addition to level 0
%           - if level==2, updates are returned in addition to level 1
%           - if level==3, smoothed values are returned in addition to level 2
%
%       nstep (integer | {1}): Number of forecast steps
%
%  Returns:
%     :
%
%       - **f** [struct]: output structure
%
%           - **retcode** [0|305]: 305 if computation of the likelihood breaks
%             downs
%           - **incr** [vector]: likelihood in each period
%           - **log_lik** [scalar]: log likelihood
%           - **a** [m x n+1 x nstep]: filtered stated (available if level >0)
%           - **P** [m x m x n+1 x nstep]: Covariance matrix of filtered state
%             (available if level >0)
%           - **att** [m x n]: Updated state vector  (available if level >1)
%           - **Ptt** [m x m x n]: Covariance matrix for updates (available if
%             level >1)
%           - **iF** [p x p x n]: Inverses of covariance matrix of forecast
%             errors
%           - **v** [p x n]: Forecast errors
%           - **K** [m x p x n]: Kalman gains
%           - **alpha** [m x n]: smoothed state vector
%           - **V** [m x m x n]: Covariance matrix of smoothed state vector
%           - **r** [m x n+1]: quantity useful for the efficient computation of
%             the state vector
%           - **eta** [r x n]: smoothed shocks
%           - **epsilon** [p x n]: smoothed measurement errors
%
%  Note:
%
%     The system is of the form
%     .. math::
%
%        alpha(t+1) = T*alpha(t) + ca(t) + R(t)*eta(t),  eta ~ N(0, Q)
%        y(t) =   Z*alpha(t) + dy(t) + epsilon(t),  epsilon ~ N(0,H)
%
%


%########################################################################
%
%	- PPGI Toolbox -
%   A MATLAB toolbox for Photoplethysmography Imaging (PPGI)
%
% Author   : Christian S. Pilz
% Company  : The Nature of Space of Time
% Date     : 07.05.2019
%
% Contact  : cpi@partofthestars.com
% Web Page : www.partofthestars.com
%
% Version  : beta0.1
%
%########################################################################
%
%	separate.m:
%
% Description:
%
%   Implements the Kalman based separation of frequencies based upon the diffusion process algorithm.
%
% References:
%
%	Christian S. Pilz, Jarek Krajewski, Vladimir Blazek.
%   On the Diffusion Process for Heart Rate Estimation from Face Videos under Realistic Conditions.
%   Pattern Recognition: 39th German Conference, GCPR 2017, Basel, Switzerland.
%   Proceedings (Lecture Notes in Computer Science), Springer, 2017
%

function [S,CS,QCS,SMM,SPP,FMM,FPP] = separate(Y,dt,FF,nharm,BF,BQ,BL,BH,R,qr)
% - Separate periodic signal from other signals
%
% Syntax:
%   [S,CS,QCS,SMM,SPP,FMM,FPP] = separate(Y,dt,FF,nharm,BF,BQ,BL,BH,R,qr)
%
% In:
%   Y  - Signal as 1xD vector
%   dt - Sampling period in seconds (e.g. 0.1)
%   FF - Fundamental frequencies as 1xD vector in BPM (from the IMM)
%   nharm - Number of harmonics to be estimated (e.g. 3)
%   BF - Feedback matrix for the bias model (e.g. [0 1; 0 0])
%   BQ - Process spectral density for the bias model (e.g. 0.01)
%   BL - Noise multiplier matrix for the bias model (e.g. [0;1])
%   BH - Measurement matrix for the bias model (e.g. [1 0])
%   R  - Measurement variance (e.g. 0.1^2)
%   qr - Resonator's process noise spectral density (e.g. 0.1)
%
% Out:
%   S   - Cleaned signal as 1xD vector
%   CS  - Fundamental signal and its harmonics as (nharm)xD vector
%   QCS - Quadrature periodic signals as (nharm)xD vector
%   SMM - The smoother means NxD
%   SPP - The smoother covariances NxNxD
%   FMM - The filter means NxD
%   FPP - The filter covariances NxNxD
%
% Description:
%   Separate given signal into true signal and periodic
%   signal by using the known frequency of the periodic signal.
%   See TRACK for the model used.

    % Number of harmonics (including fundamental)
    N = nharm;

    % Prior means and covariances
    m0 = zeros(2*N+size(BF,1),1);  % XXX: Could be function parameter
    P0 = 10*eye(size(m0,1));       % XXX: Could be function parameter

    % Mean to measurement at step 1
    m0(1) = Y(1);

    %
    % Form the constant matrices
    %
    F  = zeros(size(m0,1));
    H  = zeros(1,size(m0,1));
    L  = zeros(size(m0,1),N+size(BQ,1));
    Qc = zeros(N+size(BQ,1));

    for j=1:N
        i1 = 1+2*(j-1);
        i2 = 2+2*(j-1);
        L(i2,j) = 1;
        Qc(j,j) = qr;
        H(1,i1) = 1;
    end

    %
    % Form the bias model
    %
    i1 = 1+2*N;
    i2 = 2+2*N;
    j1 = N+2-size(BQ,1);
    j2 = N+1;
    F(i1:i2,i1:i2)  = BF;
    L(i1:i2,j1:j2)  = BL;
    H(1,i1:i2)      = BH;
    Qc(j1:j2,j1:j2) = BQ;

%% Run filter

    m = m0;
    P = P0;

    FMM = zeros(size(m,1),length(Y));
    FPP = zeros(size(P,1),size(P,2),length(Y));

    for k=1:length(Y)

        %
        % Update the resonators
        %
        w  = 2*pi*FF(k)/60;
        for j=1:N
            i1 = 1+2*(j-1);
            i2 = 2+2*(j-1);
            F(i1:i2,i1:i2) = [0 j*w; -j*w 0];
        end

        %
        % Discretize
        %
        [A,Q] = lti_disc(F,L,Qc,dt);

        %
        % Estimate with KF
        %
        [m,P] = kf_predict(m,P,A,Q);
        [m,P] = kf_update(m,P,Y(k),H,R);

        FMM(:,k) = m;
        FPP(:,:,k) = P;
    end

%% Run smoother

    % Allocate and set initial
    SMM = zeros(size(FMM));
    SPP = zeros(size(FPP));
    SMM(:,end)   = m;
    SPP(:,:,end) = P;

    for k=length(Y)-1:-1:1

        %
        % Update each resonator
        %
        w  = 2*pi*FF(k+1)/60;
        for j=1:N
            i1 = 1+2*(j-1);
            i2 = 2+2*(j-1);
            F(i1:i2,i1:i2) = [0 j*w; -j*w 0];
        end

        %
        % Discretize
        %
        [A,Q] = lti_disc(F,L,Qc,dt);

        % Smoother
        m_pred = A * FMM(:,k);
        P_pred = A * FPP(:,:,k) * A' + Q;
        D  = FPP(:,:,k) * A' / P_pred;
        m  = FMM(:,k) + D * (m - m_pred);
        P  = FPP(:,:,k) + D * (P - P_pred) * D';

        % Store results
        SMM(:,k)   = m;
        SPP(:,:,k) = P;

    end

    %
    % Extract the signals
    %
    S   = SMM(end-size(BF,1)+1,:);
    CS  = SMM(1:2:(2*nharm),:);
    QCS = SMM(2:2:(2*nharm+1),:);

function [A,Q] = lti_disc(F,L,Q,dt)
% LTI_DISC - Discretize LTI ODE with Gaussian Noise
%
% Syntax:
%   [A,Q] = lti_disc(F,L,Qc,dt)
%
% In:
%   F  - NxN Feedback matrix
%   L  - NxL Noise effect matrix        (optional, default identity)
%   Qc - LxL Diagonal Spectral Density  (optional, default zeros)
%   dt - Time Step                      (optional, default 1)
%
% Out:
%   A - Transition matrix
%   Q - Discrete Process Covariance
%
% Description:
%   Discretize LTI ODE with Gaussian Noise. The original
%   ODE model is in form
%
%     dx/dt = F x + L w,  w ~ N(0,Qc)
%
%   Result of discretization is the model
%
%     x[k] = A x[k-1] + q, q ~ N(0,Q)
%
%   Which can be used for integrating the model
%   exactly over time steps, which are multiples
%   of dt.
%

  %
  % Check number of arguments
  %
  if nargin < 1
    error('Too few arguments');
  end
  if nargin < 2
    L = [];
  end
  if nargin < 3
    Q = [];
  end
  if nargin < 4
    dt = [];
  end

  if isempty(L)
    L = eye(size(F,1));
  end
  if isempty(Q)
    Q = zeros(size(F,1),size(F,1));
  end
  if isempty(dt)
    dt = 1;
  end

  %
  % Closed form integration of transition matrix
  %
  A = expm(F*dt);

  %
  % Closed form integration of covariance
  % by matrix fraction decomposition
  %
  n   = size(F,1);
  Phi = [F L*Q*L'; zeros(n,n) -F'];
  AB  = expm(Phi*dt)*[zeros(n,n);eye(n)];
  Q   = AB(1:n,:)/AB((n+1):(2*n),:);

 function [x,P] = kf_predict(x,P,A,Q,B,u)
% KF_PREDICT - Perform Kalman Filter prediction step
%
% Syntax:
%   [X,P] = KF_PREDICT(X,P,A,Q,B,U)
%
% In:
%   X - Nx1 mean state estimate of previous step
%   P - NxN state covariance of previous step
%   A - Transition matrix of discrete model (optional, default identity)
%   Q - Process noise of discrete model     (optional, default zero)
%   B - Input effect matrix                 (optional, default identity)
%   U - Constant input                      (optional, default empty)
%
% Out:
%   X - Predicted state mean
%   P - Predicted state covariance
%
% Description:
%   Perform Kalman Filter prediction step. The model is
%
%     x[k] = A*x[k-1] + B*u[k-1] + q,  q ~ N(0,Q).
%
%   The predicted state is distributed as follows:
%
%     p(x[k] | x[k-1]) = N(x[k] | A*x[k-1] + B*u[k-1], Q[k-1])
%
%   The predicted mean x-[k] and covariance P-[k] are calculated
%   with the following equations:
%
%     m-[k] = A*x[k-1] + B*u[k-1]
%     P-[k] = A*P[k-1]*A' + Q.
%
%   If there is no input u present then the first equation reduces to
%     m-[k] = A*x[k-1]
%

  %
  % Check arguments
  %
  if nargin < 3
    A = [];
  end
  if nargin < 4
    Q = [];
  end
  if nargin < 5
    B = [];
  end
  if nargin < 6
    u = [];
  end

  %
  % Apply defaults
  %
  if isempty(A)
    A = eye(size(x,1));
  end
  if isempty(Q)
    Q = zeros(size(x,1));
  end
  if isempty(B) & ~isempty(u)
    B = eye(size(x,1),size(u,1));
  end

  %
  % Perform prediction
  %
  if isempty(u)
    x = A * x;
    P = A * P * A' + Q;
  else
    x = A * x + B * u;
    P = A * P * A' + Q;
  end

 function [X,P,K,IM,IS,LH] = kf_update(X,P,y,H,R)
% KF_UPDATE - Kalman Filter update step
%
% Syntax:
%   [X,P,K,IM,IS,LH] = KF_UPDATE(X,P,Y,H,R)
%
% In:
%   X - Nx1 mean state estimate after prediction step
%   P - NxN state covariance after prediction step
%   Y - Dx1 measurement vector.
%   H - Measurement matrix.
%   R - Measurement noise covariance.
%
% Out:
%   X  - Updated state mean
%   P  - Updated state covariance
%   K  - Computed Kalman gain
%   IM - Mean of predictive distribution of Y
%   IS - Covariance or predictive mean of Y
%   LH - Predictive probability (likelihood) of measurement.
%
% Description:
%   Kalman filter measurement update step. Kalman Filter
%   model is
%
%     x[k] = A*x[k-1] + B*u[k-1] + q,  q ~ N(0,Q)
%     y[k] = H*x[k]   + r,             r ~ N(0,R)
%
%   Prediction step of Kalman filter computes predicted
%   mean m-[k] and covariance P-[k] of state:
%
%     p(x[k] | y[1:k-1]) = N(x[k] | m-[k], P-[k])
%
%   See for instance KF_PREDICT how m-[k] and P-[k] are
%   calculated.
%
%   Update step computes the posterior mean m[k] and
%   covariance P[k]  of state given new measurement:
%
%     p(x[k] |�y[1:k]) = N(x[k] |�m[k], P[k])
%
%   Innovation distribution is defined as
%
%     p(y[k] |�y[1:k-1]) = N(y[k] | IM[k], IS[k])
%
%   Updated mean x[k] and covarience P[k] are given by
%   the following equations (not the only possible ones):
%
%     v[k] = y[k] - H[k]*m-[k]
%     S[k] = H[k]*P-[k]*H[k]' + R[k]
%     K[k] = P-[k]*H[k]'*[S[k]]^(-1)
%     m[k] = m-[k] + K[k]*v[k]
%     P[k] = P-[k] - K[k]*S[k]*K[k]'
%
% Example:
%   m = m0;
%   P = P0;
%   M = m0;
%   for i=1:size(Y,2)
%     [m,P] = kf_predict(m,P,A,Q);
%     [m,P] = kf_update(m,P,Y(:,i),H,R);
%     M = [M m];
%   end
%

  %
  % Check which arguments are there
  %
  if nargin < 5
    error('Too few arguments');
  end

  %
  % update step
  %
  IM = H*X;
  IS = (R + H*P*H');
  K = P*H'/IS;
  X = X + K * (y-IM);
  P = P - K*IS*K';
  if nargout > 5
    LH = gauss_pdf(y,IM,IS);
  end

  function [P,E] = gauss_pdf(X,M,S)
%GAUSS_PDF  Multivariate Gaussian PDF
%
% Syntax:
%   [P,E] = GAUSS_PDF(X,M,S)
%
% In:
%   X - Dx1 value or N values as DxN matrix
%   M - Dx1 mean of distibution or N values as DxN matrix.
%   S - DxD covariance matrix
%
% Out:
%   P - Probability of X.
%   E - Negative logarithm of P
%
% Description:
%   Calculate values of PDF (Probability Density
%   Function) of multivariate Gaussian distribution
%
%    N(X | M, S)
%
%   Function returns probability of X in PDF. If multiple
%   X's or M's are given (as multiple columns), function
%   returns probabilities for each of them. X's and M's are
%   repeated to match each other, S must be the same for all.
%

  if size(M,2) == 1
    DX = X-repmat(M,1,size(X,2));
    E = 0.5*sum(DX.*(S\DX),1);
    d = size(M,1);
    E = E + 0.5 * d * log(2*pi) + 0.5 * log(det(S));
    P = exp(-E);
  elseif size(X,2) == 1
    DX = repmat(X,1,size(M,2))-M;
    E = 0.5*sum(DX.*(S\DX),1);
    d = size(M,1);
    E = E + 0.5 * d * log(2*pi) + 0.5 * log(det(S));
    P = exp(-E);
  else
    DX = X-M;
    E = 0.5*DX'*(S\DX);
    d = size(M,1);
    E = E + 0.5 * d * log(2*pi) + 0.5 * log(det(S));
    P = exp(-E);
  end


function polygon_properties_test03 ( )

%*****************************************************************************80
%
%% POLYGON_PROPERTIES_TEST03 tests POLYGON_AREA_2;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
  test_num = 2;

  area_exact_test = [ 2.0, 6.0 ];
  n_test = [ 4, 8 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_TEST03\n' );
  fprintf ( 1, '  For a polygon:\n' );
  fprintf ( 1, '  POLYGON_AREA_2 computes the area.\n' );

  for test = 1 : test_num

    n = n_test(test);
    area_exact = area_exact_test(test);

    if ( test == 1 )

      v = [ ...
        1.0, 0.0; ...
        2.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 1.0 ]';

    elseif ( test == 2 )

      v = [ ...
        0.0, 0.0; ...
        3.0, 0.0; ...
        3.0, 3.0; ...
        2.0, 3.0; ...
        2.0, 1.0; ...
        1.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 2.0 ]';

    end

    r8mat_transpose_print ( 2, n, v, '  The polygon vertices:' );

    area = polygon_area_2 ( n, v );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Exact area is        %g\n', area_exact );
    fprintf ( 1, '  The computed area is %g\n', area );

  end

  return
end


function [ yval, ypval, yppval, left_out ] = spline_cubic_val2 ( ...
  n, t, y, ypp, left, tval )

%*****************************************************************************80
%
%% SPLINE_CUBIC_VAL2 evaluates a piecewise cubic spline at a point.
%
%  Discussion:
%
%    This routine is a modification of SPLINE_CUBIC_VAL; it allows the
%    user to speed up the code by suggesting the appropriate T interval
%    to search first.
%
%    SPLINE_CUBIC_SET must have already been called to define the
%    values of YPP.
%
%    In the LEFT interval, let RIGHT = LEFT+1.  The form of the spline is
%
%      SPL(T) =
%          A
%        + B * ( T - T(LEFT) )
%        + C * ( T - T(LEFT) )**2
%        + D * ( T - T(LEFT) )**3
%
%    Here:
%      A = Y(LEFT)
%      B = ( Y(RIGHT) - Y(LEFT) ) / ( T(RIGHT) - T(LEFT) )
%        - ( YPP(RIGHT) + 2 * YPP(LEFT) ) * ( T(RIGHT) - T(LEFT) ) / 6
%      C = YPP(LEFT) / 2
%      D = ( YPP(RIGHT) - YPP(LEFT) ) / ( 6 * ( T(RIGHT) - T(LEFT) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl de Boor,
%    A Practical Guide to Splines,
%    Springer Verlag, 1978.
%
%  Parameters:
%
%    Input, integer N, the number of knots.
%
%    Input, real T(N), the knot values.
%
%    Input, real Y(N), the data values at the knots.
%
%    Input, real YPP(N), the second derivatives of the spline at
%    the knots.
%
%    Input, integer LEFT, the suggested T interval to search.
%    LEFT should be between 1 and N-1.  If LEFT is not in this range,
%    then its value will be ignored.
%
%    Input, real TVAL, a point, typically between T(1) and T(N), at
%    which the spline is to be evalulated.  If TVAL lies outside
%    this range, extrapolation is used.
%
%    Output, real YVAL, YPVAL, YPPVAL, the value of the spline, and
%    its first two derivatives at TVAL.
%
%    Output, integer LEFT_OUT, the actual interval in which TVAL lies.
%
  left_out = left;
%
%  Determine the interval [T(LEFT), T(RIGHT)] that contains TVAL.
%
%  What you want from R8VEC_BRACKET3 is that TVAL is to be computed
%  by the data in interval {T(LEFT), T(RIGHT)].
%
  left_out = r8vec_bracket3 ( n, t, tval, left_out );
  right = left_out + 1;
%
%  In the interval LEFT_OUT, the polynomial is in terms of a normalized
%  coordinate  ( DT / H ) between 0 and 1.
%
  dt = tval - t(left_out);
  h = t(right) - t(left_out);

  yval = y(left_out) + dt * ( ( y(right) - y(left_out) ) / h ...
              - ( ypp(right) / 6.0 + ypp(left_out) / 3.0 ) * h ...
       + dt * ( 0.5 * ypp(left_out) ...
       + dt * ( ( ypp(right) - ypp(left_out) ) / ( 6.0 * h ) ) ) );

  ypval = ( y(right) - y(left_out) ) / h ...
      - ( ypp(right) / 6.0 + ypp(left_out) / 3.0 ) * h ...
      + dt * ( ypp(left_out) ...
      + dt * ( 0.5 * ( ypp(right) - ypp(left_out) ) / h ) );

  yppval = ypp(left_out) + dt * ( ypp(right) - ypp(left_out) ) / h;

  return
end


function b = r83_np_ml ( n, a_lu, x, job )

%*****************************************************************************80
%
%% R83_NP_ML computes A * x or x * A, where A has been factored by R83_NP_FA.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 2.
%
%    Input, real A_LU(3,N), the LU factors from R83_FA.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Input, integer JOB, specifies the product to find.
%    0, compute A * x.
%    nonzero, compute A' * x.
%
%    Output, real B(N), the product A*x or A'*x.
%
  b(1:n) = x(1:n);

  if ( job == 0 )
%
%  Compute X := U * X
%
    for i = 1 : n

      b(i) = a_lu(2,i) * b(i);

      if ( i < n )
        b(i) = b(i) + a_lu(1,i+1) * b(i+1);
      end

    end
%
%  Compute X: = L * X.
%
    for i = n : -1 : 2
      b(i) = b(i) + a_lu(3,i-1) * b(i-1);
    end

  else
%
%  Compute X: = L' * X.
%
    for i = 1 : n-1
      b(i) = b(i) + a_lu(3,i) * b(i+1);
    end
%
%  Compute X: = U' * X.
%
    for i = n : -1 : 2
      b(i) = a_lu(2,i) * b(i);
      b(i) = b(i) + a_lu(1,i) * b(i-1);
    end
    b(1) = a_lu(2,1) * b(1);

  end

  return
end


% Make some synthetic quaternions
syms   q0 q1 q2 q3   p0 p1 p2 p3   real

q = [q0 q1 q2 q3]';
p = [p0 p1 p2 p3]';

vgg_assert_equal('vgg_quat_mul(p,q)', 'vgg_quat_matrix(p) * q', 1e-8,1);

% Generate a random unit quaternion.
% This represents a 3D rotation.
q = randn(4,1);
q = q / norm(q);

% Now convert it to a rotation matrix,
% and convert back to ensure these routines
% are self-inverse
R = vgg_quat_rotation_matrix(q);
qback = vgg_quat_from_rotation_matrix(R);

vgg_assert_equal(q, qback, -1e-8,1);


%input is defined in "b", ref is defined in "n" (e.g inp=[g*sin(45) g*cos(45) 0], ref=[0 0 1])
%Cbn_est=(I-S(e))Cbn, e=E[inp_err]

function [Cnb E]=align_wander(inp, ref)

%Compute the dcm matrix
Cnb=align_vector(inp,ref);

if (nargout==2) %Compute the error matrix
    E=zeros(3);
    ep=0.001;

    for i=1:3
        %ith perturbation
        inp_err=inp;
        inp_err(i)=inp_err(i)+ep;
        Cnb_err=align_vector(inp_err,ref);
        err=Cnb_err*Cnb'-eye(3);
        E(:,i)=[-err(2,3);err(1,3);-err(1,2)]/ep;
    end

end

function Cnb=align_vector(inp, ref)
%normalize
inp=inp/norm(inp);
ref=ref/norm(ref);

%angles
dotp=dot(ref,inp);
crosv=cross(ref,inp);
crosp=norm(crosv);

if (crosp==0) %inp and ref is already aligned
    if (dotp>=0) %no rotation is necessary assert(dotp==1)
        Cnb=eye(3);
    else %assert(dotp==-1)
        %Find a vector that is ortogonal to ref
        if (ref(1)^2>ref(2)^2)
            vr_a=[-ref(3);0;ref(1)];
        else
            vr_a=[0;-ref(3);ref(2)];
        end
        vr_a=vr_a/norm(vr_a)*pi;

        %rotate around computed orthogonal vector
        Cnb=rot2dcm_v000(vr_a);
    end
else
    Cnb=eye(3)+skew(crosv)+((1-dotp)/crosp^2)*skew(crosv)*skew(crosv);
end

return;


function [rss,bic,aic ] = ml_kmeans_eval(X,labels,C,dist,lambda)
%ML_KMEANS_EVAL
%
%   input ----------------------------------------------------------------
%
%       o   X    :   (N x D), dataset of N samples of dimension D.
%
%       o labels :   (N x 1), class labels
%
%       o   C    :   (K x D), K centroids of dimension D.
%
%       o dist   :   string,  distance metric used.
%
%       o lambda :   (1 x 1), regulariser for AIC
%
%   output ----------------------------------------------------------------
%
%       o rss   :   (1 x 1), residual sum of squares
%
%       o aic   :   (1 x 1), Akaike information criteria
%
%       o bic   :   (1 x 1), Bayesian information criterion
%
%   info ------------------------------------------------------------------
%
%   http://nlp.stanford.edu/IR-book/html/htmledition/cluster-cardinality-in-k-means-1.html
%

N       = size(X,1);
[K,D]   = size(C);

if ~exist('lambda','var'),lambda = 2; end

% compute RSS
rss       = 0;
for k=1:K
    idx     = k==labels;
    rss     = rss + sum(sqrt(ml_distfunc(X(idx,:), C(k,:), dist)).^2);
end

aic = ml_aic(-0.5*rss,D * K,lambda);
bic = ml_bic(-0.5*rss,D * K,N);

end


function weight = olmar1_expert(data, weight_o, epsilon, W)
% This program generates portfolio for a specified parameter setting.
% OLMAR-1 expert
%
% function weight = olmar1_expert(data, weight_o, epsilon, W)
%
% weight: experts portfolio, used for next rebalance/combination
%
% data: market sequence vectors
% weight_o: last portfolio
% epsilon: mean reversion threshold
% W: window size for calculating moving average
%
% Example: weight = olmar1_expert(data, weight_o, epsilon, W)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[T, N] = size(data);

if (T < W+1)
    data_phi = data(T, :);
else
    data_phi = zeros(1, N);
    tmp_x = ones(1, N);
    for i = 1:W,
        data_phi = data_phi + 1./tmp_x;
        tmp_x = tmp_x.*data(T-i+1, :);
    end

    data_phi = data_phi*(1/W);
end

% Step 3: Suffer loss
ell = max([0 epsilon - data_phi*weight_o]);

% Step 4: Set parameter
x_bar = mean(data_phi);
denominator = (data_phi - x_bar)*(data_phi - x_bar)';
if (~eq(denominator, 0.0)),
    lambda = ell / denominator;
else  % Zero volatility
    lambda = 0;
end

% Step 5: Update portfolio
weight = weight_o + lambda*(data_phi' - x_bar);

% Step 6: Normalize portfolio
weight = simplex_projection(weight, 1);

end


%% INCREMENTAL TENSOR LEARNING
%%% Incremental tensor analysis: Theory and applications (2008)
%%% http://dl.acm.org/citation.cfm?id=1409621
close all; clear all; clc;

%% LIBRARIES
addpath('libs/poblano_toolbox_1.1');
addpath('libs/tensor_toolbox_2.5');
addpath('libs/nway331');
addpath('libs/itl');

%% TENSORS
A = full(ttensor(tenrand([2,3,4]),{rand(10,2),rand(30,3), rand(40,4)}));
B = full(ttensor(tenrand([2,3,4]),{rand(10,2),rand(30,3), rand(40,4)}));
D = {A,A,A,A,A,B,B,B,B,B};

%% Dynamic Tensor Decomposition (DTA)
%%% Compute DTA with no forgetting factor
[T,C] = DTA(D{1},[2,3,4]);
for i = 2:10
  [T,C] = DTA(D{i},[2,3,4],C);
  err = norm(full(T)-D{i})/norm(D{i});
  fprintf('tensor #%d has error %f\n',i,err);
end
%%% Compute DTA with forgetting factor alpha = 0.1
[T,C] = DTA(D{1},[2,3,4]);
alpha = 0.1;
for i = 2:10
  [T,C] = DTA(D{i},[2,3,4],C, alpha);
  err = norm(full(T)-D{i})/norm(D{i});
  fprintf('tensor #%d has error %f\n',i,err);
end

%% Streaming Tensor Decomposition (STA)
%%% Compute STA with no forgetting factor
[T,S] = STA(D{1},[2,3,4]);
for i = 2:10
  [T,S] = STA(D{i},[2,3,4],T,S);
  err = norm(full(T)-D{i})/norm(D{i});
  fprintf('tensor #%d has error %f\n',i,err);
end
%%% Compute STA with forgetting factor alpha = 0.9
[T,S] = STA(D{1},[2,3,4]);
alpha = 0.9;
for i = 2:10
  [T,S] = STA(D{i},[2,3,4],T,S, alpha);
  err = norm(full(T)-D{i})/norm(D{i});
  fprintf('tensor #%d has error %f\n',i,err);
end

%% Window-based Tensor Decomposition (WTA)
[T,C] = WTA(D{1},[2,3,4]);
for i = 2:10
  [T,C] = WTA(D{i},[2,3,4],tensor(T));
  err = norm(full(T)-D{i})/norm(D{i});
  fprintf('tensor #%d has error %e\n',i,err);
end


% test_surface_param.m

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013-2014 University of Oxford
% Version: 0.6.3
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Open surface for 4 cardiac valves
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

load('data/valve-annula-points-009.mat');

% plot points
hold off
subplot(2, 2, 1)
plot3(x(:, 1), x(:, 2), x(:, 3), '*')
axis equal

% compute simple XY parametrization
uv0 = surface_param(x, 'xy');
d0 = dmatrix(uv0', uv0', 'euclidean');

% plot parametrization
subplot(2, 2, 2)
hold off
plot(uv0(:, 1), uv0(:, 2), '.')
axis equal
title('XY')

% compute PCA parametrization
uv = surface_param(x, 'pca');

% plot parametrization, aligned with the XY case
[~, uv] = procrustes(uv0, uv, 'Scaling', false);
subplot(2, 2, 3)
hold off
plot(uv(:, 1), uv(:, 2), '.')
axis equal
title('PCA')

% compute Isomap parametrization
param.type = 'isomap';
param.neigh = 'epsilon';
param.size = .0020;
param.d = dmatrix(x', x', 'euclidean');
uv = surface_param(x, param);

% plot parametrization
[~, uv] = procrustes(uv0, uv, 'Scaling', false);
subplot(2, 2, 4)
hold off
plot(uv(:, 1), uv(:, 2), '.')
axis equal
title('Isomap')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Open planar triangular mesh to compare isomap and MDS map
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% quadrangular mesh split into asymmetric triangles

% create rectangle with triangular mesh
[tri, x] = surface_tridomain('rect', 'step', 1/10, [0 0], [1 1/5*3]);
x(:, end+1) = 0;

% plot original mesh and parametrization
con = dmatrix_mesh(tri);
hold off
gplot(con, x(:, 1:2))

%% Isomap on a triangular mesh
% note that we need to compute the local distances on the mesh, and then
% apply Dijkstra. If we don't provide the distance matrix, by default
% surface_param() will compute distances between all pairs of vertices
clear param
param.type = 'isomap';
param.d = dmatrix_mesh(tri, x);
param.d = dijkstra(param.d, 1:length(param.d));
uv = surface_param(x, param);

% rigid registration to overlap the solution with the original data
[~, uv] = procrustes(x, uv, 'Scaling',false);

% add parametrization to the plot
hold on
gplot(con, uv, 'r')

%% Open classic MDSmap on the same triangular mesh
clear param
param.type = 'cmdsmap';
param.options.constraint_map = 0.4 * ones(size(x, 1), 1); % trigger warning
param.options.end_points = [1 2 3]; % trigger warning
param.options.nb_iter_max = 4; % trigger warning
param.tri = tri;
uv = surface_param(x, param);

% rigid registration to overlap the solution with the original data
[~, uv] = procrustes(x, uv, 'Scaling',false);

% add parametrization to the plot
hold on
gplot(con, uv, 'g')

%% check that MDSmap with Dijkstra is the same as Isomap
clear param
param.type = 'cmdsmap';
param.options.constraint_map = 0.4 * ones(1, size(x, 1)); % trigger warning
param.options.end_points = [1 2 3]; % trigger warning
param.options.nb_iter_max = 4; % trigger warning
[~, param.d] = dmatrix_mesh(tri, x, 'dijkstra');
uv = surface_param(x, param);

% rigid registration to overlap the solution with the original data
[~, uv] = procrustes(x, uv, 'Scaling',false);

% add parametrization to the plot
% it is expected that this perfectly overlaps the Isomap result computed
% above
hold on
gplot(con, uv, 'k')

%% lmdscale (MDSmap with local neighbourhood)
clear param
param.type = 'lmdscale';
param.tri = tri;
param.options.constraint_map = 0.4 * ones(size(x, 1), 1);
param.options2.MaxIter = 100;
param.options2.MaxInc = .01;
[uv, out] = surface_param(x, param);
out.stopCondition

% rigid registration to overlap the solution with the original data
[~, uv] = procrustes(x, uv, 'Scaling',false);

% add parametrization to the plot
% it is expected that this perfectly overlaps the Isomap result computed
% above
hold on
gplot(con, uv, 'c')

% plot error
figure
plot(out.err.maxinc)
hold on
plot(out.err.medinc)

% plot stress
hold off
plot(out.err.stress1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Closed surface from scattered point set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% create a closed surface triangular mesh of a left ventricle

% seed the random generator so that we always get the same points
rng(0)

% load low resolution segmentation of LV, with detail removed
scimat = scimat_load('data/008-lv-resampled-0_31.mha');

% keep perimeter only
scimat.data = bwperim(scimat.data);

% coordinates of perimeter points
idx = find(scimat.data);
[r, c, s] = ind2sub(size(scimat.data), idx);
x = scimat_index2world([r c s], scimat);

% subsample boundary points
x = x(randi(size(x, 1), round(size(x, 1)/5), 1), :);

% order a bit the vertices, so that nearby vertices are close
x = sortrows(x);

% plot point set
subplot(2, 2, 1)
hold off
plot3(x(:, 1), x(:, 2), x(:, 3), '.')
axis equal

% mesh the points using an alpha-shape
[~, s] = alphavol(x, scimat.axis(1).spacing * 40);
tri = s.bnd;

% check that we don't have non-manifold vertices
idx = tri_find_nonmanifold_vertex(tri, x, scimat);
if (nnz(idx))
    error('Assertion fail: mesh has non-manifold vertices')
end

% remove points not connected to any triangle
[tri, x] = tri_squeeze(tri, x);

% plot mesh
subplot(2, 2, 2)
hold off
trisurf(tri, x(:, 1), x(:, 2), x(:, 3));
axis equal

% PCA-normalization of mesh
x = pca_normalize(x);

% plot mesh
subplot(2, 2, 3)
hold off
trisurf(tri, x(:, 1), x(:, 2), x(:, 3));
axis equal

% median radius of the points
sphrad = median(sqrt(sum(x.^2, 2)))

%% sphproj: simple projection on a sphere around the centroid
clear param
param.type = 'sphproj';
[latlon, out] = surface_param(x, param);
out.medrad

% rigid registration to overlap the solution with the original data
[xx, yy, zz] = sph2cart(latlon(:, 2), latlon(:, 1), sphrad);
[~, x1] = procrustes(x, [xx, yy, zz], 'Scaling',false);

% plot result
subplot(1, 2, 1)
hold off
trisurf(tri, x(:, 1), x(:, 2), x(:, 3));
axis equal
subplot(1, 2, 2)
hold off
trisurf(tri, x1(:, 1), x1(:, 2), x1(:, 3))
title('sphproj')
axis equal

%% CALD

% % using the SPHARM-MAT toolbox GUI
% faces = tri;
% vertices = x;
% save('/tmp/bar_obj.mat', 'faces', 'vertices')
% SPHARM_MAT
% load('/tmp/bar_CALD_smo.mat')

clear param
param.type = 'cald';
param.tri = tri;
param.options.MeshGridSize = 50;
param.options.MaxSPHARMDegree = 6;
param.options.Tolerance = 2;
param.options.Smoothing = 2;
param.options.Iteration = 100;
param.options.LocalIteration = 10;
latlon = surface_param(x, param);

% check for self-intersections
bad = cgal_check_self_intersect(param.tri, x);
nnz(bad>0)

% rigid registration to overlap the solution with the original data
[xx, yy, zz] = sph2cart(latlon(:, 2), latlon(:, 1), sphrad);
[~, x1] = procrustes(x, [xx, yy, zz], 'Scaling',false);

% plot parametrization
subplot(1, 2, 2)
hold off
trisurf(tri, x1(:, 1), x1(:, 2), x1(:, 3))
title('CALD')
axis equal

%% smdscale

% compute spherical MDSmap parametrization, no need the constrain the
% distance matrix more
clear param
param.type = 'smdscale';
param.tri = tri;
param.dmethod = 'fastmarching';
% param.options.constraint_map = .2 * sphrad * ones(size(x, 1), 1);
param.sphrad = sqrt(sum(cgal_trifacet_area(tri, x))/4/pi);
param.init = 'random';
param.options2.MaxIter = 15;
param.options2.MaxAlpha = .01 / 180 * pi;
[uv, out] = surface_param(x, param);
out.stopCondition
lat = uv(:, 1);
lon = uv(:, 2);

% plot parametrization
subplot(1, 2, 2)
hold off
[xx, yy, zz] = sph2cart(lon, lat, param.sphrad);
[~, x1] = procrustes(x, [xx, yy, zz], 'Scaling', false);
trisurf(param.tri, x1(:, 1), x1(:, 2), x1(:, 3));
title('smdscale')
axis equal

% check for self-intersections
bad = cgal_check_self_intersect(param.tri, x1);
nnz(bad>0)

% plot error
figure
subplot(2, 2, 1)
hold off
plot(out.err.rawstress)
title('Raw stress')
subplot(2, 2, 2)
hold off
plot(out.err.stress1)
title('Stress1')
subplot(2, 1, 2)
hold off
plot(out.err.maxalpha)
hold on
plot(out.err.medalpha)
title('Alpha')

% TODO:
% %% compare spherical Isomap and CALD
%
% % compute great circle distances for CALD
% dsphCALD = zeros(size(out.dsph));
% for I = 1:length(dsphCALD)
%     dsphCALD(:, I) = distance(latCALD(I), lonCALD(I), latCALD, lonCALD, ...
%         [out.sphrad 0], 'radians');
% end
%
% % plot distances to optimize vs spherical distances for CALD
% subplot(2, 1, 1)
% hold off
% plot(d(:), dsphCALD(:), '.')
% hold on
% plot([0 max(d(:))], [0 max(d(:))], 'r', 'LineWidth', 2)
% title('CALD')
% xlabel('Distance on the mesh')
% ylabel('Distance on the spherical parametrization')
% axis equal
%
%
% % plot boxplots with the distance ratios
% N = numel(d);
% subplot(1, 1, 1)
% hold off
% boxplot([d(:)-dsphCALD(:) ; d(:)-out.dsph(:)], ...
%     [ones(N, 1); 2*ones(N, 1)], 'labels', {'CALD', 'Sph Isomap'}, ...
%     'notch', 'on')
% title('Target distance - distance on spherical parametrization')


function x = Inv_GUSFT_CG(b,lambda,MaxIts,ErrTol,Pre,x0,mu)
% Inv_UtU_CG: Invert GUSFT_Toeplitz.
%  Usage:
%    x = Inv_GUSFT_CG(b,lambda,MaxIts,ErrTol,Pre,x0,mu);
%  Inputs:
%    b       vector of length n
%    lambda  vector of Fourier multipliers; length 2*n-1
%    MaxIts  Maximum number of CG iterations. Default 6.
%    ErrTol  Error tolerance. Default 1.e-9.
%    Pre     Boolean variable. Pre = 1 if we apply a preconditioner
%    x0      Initial guess
%    mu      Variable for use with the preconditioner
%  Outputs:
%    x       Vector of length n
%  Description
%    Performs the inverse UtU where U is the nouniform FT. This is
%    an approximate inverse which uses a conjugate gradient
%    solver. Each iteration uses GUSFT_Toeplitz (which can be
%    applied by merely using only 2 FFTs).
% See Also
%   GUSFT_Toeplitz
%
% By Emmanuel candes, 2003-2004

if (nargin < 2)
   error('Not enough input arguments.');
end

if (nargin < 3) | isempty(MaxIts)
   MaxIts = 6;
end

if (nargin < 4) | isempty(ErrTol)
   ErrTol = 1.e-9;
end

if (nargin < 5) | isempty(Pre)
	Pre = 0;
end

if (nargin < 6) | isempty(x0)
	x0 = b;
end

if (nargin < 7) | isempty(mu)
	mu = ones(size(b));
end

% Initialization
xk = x0;

if MaxIts > 0,
	 temp = GUSFT_Toeplitz(xk,lambda);

	 rk=b-temp;
	 if Pre == 0,
	   dk = rk;
	   else
	     dk=ApplyCirculantPrecond(rk,mu);
	 end

	% Conjugate gradient iteration
	for j=1:MaxIts,
		 perr=max(abs(rk));
		 fprintf(['Iteration ',int2str(j-1),': Residual error=']);
		 fprintf('%f\n',perr);

		 if perr>ErrTol,
			  temp = GUSFT_Toeplitz(dk,lambda);
			  a0 = sum(conj(rk).*dk);
			  a1 = sum(conj(dk).*temp);

			  % Update x and residual
			  a=a0/a1;
			  xk=xk+a*dk;
			  rk=rk-a*temp;

			  % Update search direction
			  if Pre == 0,
			    b = sum(abs(rk.^2))/a0;
			    dk = rk+b*dk;
			  else
			    rkp = ApplyCirculantPrecond(rk,mu);
			    b = sum(conj(rkp).*rk)/a0;
			    dk = rkp+b*dk;
			  end
		 end
	end
end

x = xk;

%  References: Jonathan R. Shewchuk "An introduction to the Conjugate
%  Gradient Method Without the Agonizing Pain"
%
%  Amir Averbuch and David L. Donoho "BeamLab 200"


function determ = r8mat_determinant ( n, a )

%*****************************************************************************80
%
%% R8MAT_DETERMINANT computes the determinant of a matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the orderof the matrix A.
%
%    Input, real A(N,N), the matrix.
%
%    Output, real DETERM, the determinant.
%
  b(1:n,1:n) = a(1:n,1:n);

  [ b, pivot, info ] = r8mat_gefa ( b, n );

  determ = r8mat_gedet ( b, n, pivot );

  return
end


function Z = inormal(varargin)
% Applies a rank-based inverse normal transformation.
%
% Usage: Z = inormal(X)
%            inormal(X,c)
%            inormal(X,method)
%            inormal(X,...,quanti)
%
% Inputs:
% X      : Original data. Can be a vector or an array.
% c      : Constant to be used in the transformation.
%          Default c=3/8 (Blom).
% method : Method to choose c. Accepted values are:
%              'Blom'   (c=3/8),
%              'Tukey'  (c=1/3),
%              'Bliss', (c=1/2)  and
%              'Waerden' or 'SOLAR' (c=0).
% quanti : All data guaranteed to be quantitative and
%          without NaN?
%          This can be a true/false. If true, the function
%          runs much faster if X is an array.
%          Default is false.
%
% Outputs:
% Z      : Transformed data.
%
% References:
% * Van der Waerden BL. Order tests for the two-sample
%   problem and their power. Proc Koninklijke Nederlandse
%   Akademie van Wetenschappen. Ser A. 1952; 55:453-458
% * Blom G. Statistical estimates and transformed
%   beta-variables. Wiley, New York, 1958.
% * Tukey JW. The future of data analysis.
%   Ann Math Stat. 1962; 33:1-67.
% * Bliss CI. Statistics in biology. McGraw-Hill,
%   New York, 1967.
%
% _____________________________________
% Anderson M. Winkler
% Yale University / Institute of Living
% Aug/2011 (first version)
% Jun/2014 (this version)
% http://brainder.org

% Accept inputs & defaults
c0 = 3/8;  % Default (Blom, 1958)
quanti = false;
if nargin == 1,
    c = c0;
elseif nargin > 1 && ischar(varargin{2}),
    switch lower(varargin{2}),
        case 'blom'
            c = 3/8;
        case 'tukey'
            c = 1/3;
        case 'bliss'
            c = 1/2;
        case 'waerden'
            c = 0;
        case 'solar'
            c = 0; % SOLAR is the same as Van der Waerden
        otherwise
            error('Method %s unknown. Use either ''Blom'', ''Tukey'', ''Bliss'', ''Waerden'' or ''SOLAR''.',varargin{2});
    end
elseif nargin > 1 && isscalar(varargin{2}),
    c = varargin{2};  % For a user-specified value for c
end
if nargin == 3,
    quanti = varargin{3};
    if isempty(varargin{2}),
        c = c0;
    end
end
X = varargin{1};

% If the trait is quantitative, avoid the loop
if quanti,
    % Get the rank for each value
    [~,iX] = sort(X);
    [~,ri] = sort(iX);

    % Do the actual transformation
    N = size(X,1);
    p = ((ri-c)/(N-2*c+1));
    Z = sqrt(2)*erfinv(2*p-1);

else
    Z = nan(size(X));
    for x = 1:size(X,2),

        % Remove NaNs
        XX = X(:,x);
        ynan = ~isnan(XX);
        XX = XX(ynan);

        % Get the rank for each value
        [~,iX] = sort(XX);
        [~,ri] = sort(iX);

        % Do the actual transformation
        N = size(XX,1);
        p = ((ri-c)/(N-2*c+1));
        Y = sqrt(2)*erfinv(2*p-1);

        % Check for repeated values
        [U,~,IC] = unique(XX);
        if numel(U) < N,
            sIC = sort(IC);
            dIC = diff(vertcat(sIC,1));
            U = unique(sIC(~dIC));
            for u = 1:numel(U),
                Y(IC == U(u)) = mean(Y(IC == U(u)));
            end
        end

        % Put the NaNs back
        Z(ynan,x) = Y;
    end
end


function xa = binom ( x, xx, npl, m, nt )

%*****************************************************************************80
%
%% BINOM: binomial expansion series for the (-1/M) power of a Chebyshev series.
%
%  Discussion:
%
%    This routine uses a certain number of terms of the binomial expansion
%    series to estimate the (-1/M) power of a given Chebyshev series.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Roger Broucke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Broucke,
%    Algorithm 446:
%    Ten Subroutines for the Manipulation of Chebyshev Series,
%    Communications of the ACM,
%    October 1973, Volume 16, Number 4, pages 254-256.
%
%  Parameters:
%
%    Input, real X(NPL), the given Chebyshev series.
%
%    Input, real XX(NPL), an initial estimate for
%    the Chebyshev series for the input function raised to the (-1/M) power.
%
%    Input, integer NPL, the number of terms in the
%    Chebyshev series.
%
%    Input, integer M, defines the exponent, (-1/M).
%    0 < M.
%
%    Input, integer NT, the number of terms of the binomial
%    series to be used.
%
%    Output, real XA(NPL), the estimated Chebyshev series
%    for the input function raised to the (-1/M) power.
%
  xa = zeros ( npl, 1 );

  dm = m;
  alfa = - 1.0 / dm;

  ww(1:npl) = x(1:npl);

  for k = 1 : m

    w2 = mltply ( ww, xx, npl );

    ww(1:npl) = w2(1:npl);

  end

  ww(1) = ww(1) - 2.0;

  xa(1) = 2.0;
  xa(2:npl) = 0.0;

  w3(1) = 2.0;
  w3(2:npl) = 0.0;

  for k = 2 : nt

    dkmm = k - 1;
    dkm2 = k - 2;
    coef = ( alfa - dkm2 ) / dkmm;

    w2 = mltply ( w3, ww, npl );

    w3(1:npl) = w2(1:npl) * coef;

    xa(1:npl) = xa(1:npl) + w3(1:npl);

  end

  w2 = mltply ( xa, xx, npl );

  xa(1:npl) = w2(1:npl);

  return
end


function [count edges mid loc] = histcn(X, varargin)
% function [count edges mid loc] = histcn(X, edge1, edge2, ..., edgeN)
%
% Purpose: compute n-dimensional histogram
%
% INPUT
%   - X: is (M x N) array, represents M data points in R^N
%   - edgek: are the bin vectors on dimension k, k=1...N.
%     If it is a scalar (Nk), the bins will be the linear subdivision of
%     the data on the range [min(X(:,k)), max(X(:,k))] into Nk
%     sub-intervals
%     If it's empty, a default of 32 subdivions will be used
%
% OUTPUT
%   - count: n-dimensional array count of X on the bins, i.e.,
%         count(i1,i2,...,iN) = cardinal of X such that
%                  edge1(i1) <= X(:,i1) < edge1(i1)+1 and
%                       ...
%                  edgeN(iN) <= X(:,iN) < edgeN(iN)+1
%   - edges: (1 x N) cell, each provides the effective edges used in the
%     respective dimension
%   - mid: (1 x N) cell, provides the mid points of the cellpatch used in
%     the respective dimension
%   - loc: (M x N) array, index location of X in the bins. Points have out
%     of range coordinates will have zero at the corresponding dimension.
%
% DATA ACCUMULATE SYNTAX:
%   [ ... ] = histcn(..., 'AccumData', VAL);
%   where VAL is M x 1 array. Each VAL(k) corresponds to position X(k,:)
%   will be accumulated in the cell containing X. The accumulate result
%   is returned in COUNT.
%   NOTE: Calling without 'AccumData' is similar to having VAL = ones(M,1)
%
%   [ ... ] = histcn(..., 'AccumData', VAL, 'FUN', FUN);
%     applies the function FUN to each subset of elements of VAL.  FUN is
%     a function that accepts a column vector and returns
%     a numeric, logical, or char scalar, or a scalar cell.  A has the same class
%     as the values returned by FUN.  FUN is @SUM by default.  Specify FUN as []
%     for the default behavior.
%
% Usage examples:
%   M = 1e5;
%   N = 3;
%   X = randn(M,N);
%   [N edges mid loc] = histcn(X);
%   imagesc(mid{1:2},N(:,:,ceil(end/2)))
%
% % Compute the mean on rectangular patch from scattered data
%   DataSize = 1e5;
%   Lat = rand(1,DataSize)*180;
%   Lon = rand(1,DataSize)*360;
%   Data = randn(1,DataSize);
%   lat_edge = 0:1:180;
%   lon_edge = 0:1:360;
%   meanData = histcn([Lat(:) Lon(:)], lat_edge, lon_edge, 'AccumData', Data, 'Fun', @mean);
%
% See also: HIST, ACCUMARRAY
%
% Bruno Luong: <brunoluong@yahoo.com>
% Last update: 25/August/2011

if ndims(X)>2
    error('histcn: X requires to be an (M x N) array of M points in R^N');
end
DEFAULT_NBINS = 32;

AccumData = [];
Fun = {};

% Looks for where optional parameters start
% For now only 'AccumData' is valid
split = find(cellfun('isclass', varargin, 'char'), 1, 'first');
if ~isempty(split)
    for k = split:2:length(varargin)
        if strcmpi(varargin{k},'AccumData')
            AccumData = varargin{k+1}(:);
        elseif strcmpi(varargin{k},'Fun')
            Fun = varargin(k+1); % 1x1 cell
        end
    end
    varargin = varargin(1:split-1);
end

% Get the dimension
nd = size(X,2);
edges = varargin;
if nd<length(edges)
    nd = length(edges); % wasting CPU time warranty
else
    edges(end+1:nd) = {DEFAULT_NBINS};
end

% Allocation of array loc: index location of X in the bins
loc = zeros(size(X));
sz = zeros(1,nd);
% Loop in the dimension
for d=1:nd
    ed = edges{d};
    Xd = X(:,d);
    if isempty(ed)
        ed = DEFAULT_NBINS;
    end
    if isscalar(ed) % automatic linear subdivision
        ed = linspace(min(Xd),max(Xd),ed+1);
    end
    edges{d} = ed;
    % Call histc on this dimension
    [dummy loc(:,d)] = histc(Xd, ed, 1);
    % Use sz(d) = length(ed); to create consistent number of bins
    sz(d) = length(ed)-1;
end % for-loop

% Clean
clear dummy

% This is need for seldome points that hit the right border
sz = max([sz; max(loc,[],1)]);

% Compute the mid points
mid = cellfun(@(e) 0.5*(e(1:end-1)+e(2:end)), edges, ...
              'UniformOutput', false);

% Count for points where all coordinates are falling in a corresponding
% bins
if nd==1
    sz = [sz 1]; % Matlab doesn't know what is one-dimensional array!
end

hasdata = all(loc>0, 2);
if ~isempty(AccumData)
    count = accumarray(loc(hasdata,:), AccumData(hasdata), sz, Fun{:});
else
    count = accumarray(loc(hasdata,:), 1, sz);
end

return

end % histcn


function [Fn,Ln] = fibs3(n)
% fibs: vpi tool to efficiently compute the n'th Fibonacci number and the n'th Lucas number
% usage: [Fn,Ln] = fibs3(n)
%
% For efficiency, fibs3 uses a variety of tricks to
% maximize speed. While computation of fibonacci numbers
% is commonly done recursively, fibs3 does so using a
% direct iterative scheme given the binary representation
% of n. In addition, several Fibonacci and Lucas number
% identities are employed to maximize throughput.
%
% The methods employed by fibs3 will be O(log2(n)) in time.

if (nargin~=1) || (numel(n) ~= 1) || (n~=round(n)) || (abs(n) > 2^53)
  error('n must be scalar and an integer, <= 2^53 in absolute value')
end

% much faster if n is not a vpi but a double.
% also, we don't need to worry about n being
% larger than 2^53, as this would have a vast
% number of digits.
n = double(n);

% ensure that n is positive, handle the negative
% cases later.
nsign = sign(n);
n = abs(n);

% get the binary representation of n. Thus
% nbin is a character vector, of length
% ceil(log2(n)).
nbin = dec2bin(n);

% get the 4 highest order bits from nbin
k = min(numel(nbin),4);

% zero is a special case to stop at.
if n == 0
  Fn = vpi(0);
  Ln = vpi(2);
  return
else
  % start the sequence from the top
  % few bits of n.
  nhigh = bin2dec(nbin(1:k));
  Fseq = [1 1 2 3  5  8 13 21 34  55  89 144 233 377  610];
  Lseq = [1 3 4 7 11 18 29 47 76 123 199 322 521 843 1364];

  Fn = vpi(Fseq(nhigh));
  Ln = vpi(Lseq(nhigh));
end

% We need to loop forwards. Essentially, we started
% with the highest order bit(s) of the binary representation
% for n. Look at each successively lower order bit.
% If the next bit is 0, then we are essentially doubling
% the index at this step. If the next bit is odd, then
% we are moving to 2*n+1.
for k = 5:numel(nbin)
  bit = (nbin(k) == '1');
  if bit
    % the next bit was odd. Use
    % a 2*n+1 rule to step up.
    F2n = Fn.*Ln;
    % we want to do this...
    %   L2n = (5 .*Fn.*Fn + Ln.*Ln)./2;
    % Instead use the identity that
    %   5F(n)^2 + L(n)^2 = 2*L(n)^2 + 4*(-1)^(n+1)
    % to make that expression more efficiently
    % computed. See that the form used below
    % has only a single multiplication between a
    % pair of large integers, whereas the prior
    % form for L2n would have had several multiples
    % as well as a divide.
    L2n = Ln.*Ln + 2*(-1)^(nhigh+1);

    Fn = (L2n + F2n)./2;
    Ln = (5 .*F2n + L2n)./2;
  else
    % the next bit was even. Use the 2*n
    % rule to step up.
    F2n = Fn.*Ln;
    Ln = Ln.*Ln + 2*(-1)^(nhigh+1);
    Fn = F2n;
  end

  % update the top bits of n
  nhigh = 2*nhigh + bit;
end

% if n was negative, then we may need to apply a
% sign change to Fn and Ln.
if nsign < 0
  if iseven(n)
    Fn = (-nsign).*Fn;
    Ln = nsign.*Ln;
  else
    Fn = nsign.*Fn;
    Ln = (-nsign).*Ln;
  end
end

% ==================================
% End mainline, begin subfunctions.
% ==================================

function result = iseven(n)
% tests if a scalar value is an even integer
if isnumeric(n)
  result = (mod(n,2) == 0);
elseif isa(n,'vpi')
  % must have been a vpi
  result = (mod(trailingdigit(n,1),2) == 0);
else
  error('n must be either numeric or vpi')
end


function [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = cf_exp_to_ss(magnSigma2, lengthScale)
% CF_EXP_TO_SS - Exponential covariance functions to state space
%
% Syntax:
%   [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = cf_exp_to_ss(magnSigma2, lengthScale)
%
% In:
%   magnSigma2  - Magnitude scale parameter (default: 1)
%   lengthScale - Length scale parameter (default: 1)
%
% Out:
%   F           - Feedback matrix
%   L           - Noise effect matrix
%   Qc          - Spectral density of white noise process w(t)
%   H           - Observation model matrix
%   Pinf        - Covariance of the stationary process
%   dF          - Derivatives of F w.r.t. parameters
%   dQc         - Derivatives of Qc w.r.t. parameters
%   dPinf       - Derivatives of Pinf w.r.t. parameters
%   params      - Input and output parameter information
%
% Description:
%   This function converts the exponential (Ornstein-Uhlenbeck) covariance
%   function to state space models. The covariance function
%   parametrization is as follows
%
%     k(tau) = magnSigma2 exp(-|tau|/lengthScale),
%
%   where magnSigma2 is the magnitude scale parameter, lengthScale the
%   distance scale parameter, and tau the time difference between states,
%   tau = t-t'.
%     This function takes the covariance function parameters as inputs and
%   outputs the corresponding state space model matrices. The state space
%   model is given as follows in terms of a stochastic differential
%   equation
%
%      df(t)/dt = F f(t) + L w(t),
%
%   where w(t) is a white noise process with spectral denisty Qc. The
%   observation model for discrete observation y_k of f(t_k) at step k,
%   is as follows
%
%      y_k = H f(t_k) + r_k, r_k ~ N(0, R),
%
%   where r_k is the Gaussian measurement noise with covariance R.
%     Pinf is the stationary covariance, where the value of Pinf(i,j),
%   is defined as follows
%
%      Pinf(i,j) = E[(f_i(t)-E[f_i(t)])(f_j(t)-E[f_j(t)])],
%
%   where f_i(t) is component i of state vector f(t).
%     Derivatives: All have same form. For example, dF has the following
%   form:
%
%       dF(:,:,1) = dF/d(magnSigma2 = input parameter_1),
%       dF(:,:,i) = dF/d(input parameter_i).
%
% References:
%
%   [1] Simo Sarkka, Arno Solin, Jouni Hartikainen (2013).
%       Spatiotemporal learning via infinite-dimensional Bayesian
%       filtering and smoothing. IEEE Signal Processing Magazine,
%       30(4):51-61.
%
%   [2] Jouni Hartikainen and Simo Sarkka (2010). Kalman filtering and
%       smoothing solutions to temporal Gaussian process regression
%       models. Proceedings of IEEE International Workshop on Machine
%       Learning for Signal Processing (MLSP).
%
% See also:
%   COV_EXP, SPEC_EXP
%
% Copyright:
%   2012-2014   Arno Solin
%   2013-2014   Jukka Koskenranta
%
%  This software is distributed under the GNU General Public
%  License (version 3 or later); please refer to the file
%  License.txt, included with the software, for details.

%% Apply defaults

  % Check if magnSigm2 is given
  if nargin < 1 || isempty(magnSigma2), magnSigma2 = 1; end

  % Check if lengthScale is given
  if nargin < 2 || isempty(lengthScale), lengthScale = 1; end

%% Form state space model

  % Feedback matrix
  F  = -1/lengthScale;

  % Noise effect matrix
  L  = 1;

  % Spectral density
  Qc = 2*magnSigma2/lengthScale;

  % Observation model
  H  = 1;

%% Stationary covariance

  % Calculate Pinf only if requsted
  if nargout > 4,
    Pinf = magnSigma2;
  end

%% Calculate derivatives

  % Calculate derivatives only if requested
  if nargout > 5

    % Derivative of F w.r.t. parameter magnSigma2
    dFmagnSigma2 = 0;

    % Derivative of F w.r.t parameter lengthScale
    dFlengthScale = 1/lengthScale^2;

    % Derivative of Qc w.r.t. parameter magnSigma2
    dQcmagnSigma2 = 2/lengthScale;

    % Derivative of Qc w.r.t. parameter lengthScale
    dQclengthScale = -2*magnSigma2/lengthScale^2;

    % Derivative of Pinf w.r.t. parameter magnSigma2
    dPinfmagnSigma2 = 1;

    % Derivative of Pinf w.r.t. parameter lengthScale
    dPinflengthScale = 0;

    % Stack all derivatives
    dF = zeros(1,1,2);
    dQc = zeros(1,1,2);
    dPinf = zeros(1,1,2);

    dF(:,:,1) = dFmagnSigma2;
    dF(:,:,2) = dFlengthScale;
    dQc(:,:,1) = dQcmagnSigma2;
    dQc(:,:,2) = dQclengthScale;
    dPinf(:,:,1) = dPinfmagnSigma2;
    dPinf(:,:,2) = dPinflengthScale;

  end

%% Return parameter names

  % Only return if requested
  if nargout > 8

    % Stationarity
    p.stationary = true;

    % Input parameter information
    p.in{1}.name = 'magnSigma2';  p.in{1}.default = 1; p.in{1}.opt = true;
    p.in{2}.name = 'lengthScale'; p.in{2}.default = 1; p.in{2}.opt = true;

    % Return parameter setup
    params = p;

  end


function [ranset,seed]=randomset(tot,nel,seed)

%RANDOMSET draws randomly a subsample of nel cases out of tot.
%(It is called if not all (p+1)-subsets out of n will be considered.)
%
% Required input arguments:
%     tot : The total number of observations to consider
%     nel : The number of observations that the subsample must contain
% Optional input arguments:
%     seed : To define the state of the generator (default=0)
%            (0 sets the generator to its default initial state)
%
% Output arguments:
%   ranset : Random subset of nel cases out of tot.
%   seed   : The corresponding state.
%
%
% I/O:
%    [ranset,seed]=randomset(n,n/2,0)
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html

if nargin==2
    seed=0;
end

for j=1:nel
   [random,seed]=uniran(seed);
   num=floor(random*tot)+1;
   if j > 1
      while any(ranset==num)
         [random,seed]=uniran(seed);
         num=floor(random*tot)+1;
      end
   end
   ranset(j)=num;
end


% This example shows how to use the stretchmesh function to expand the
% size of the computation window at the edges without increasing the
% number of gridpoints.  It uses the same example waveguide
% structure considered in 'basic-semivector.m'

% Refractive indices:
n1 = 3.34;          % Lower cladding
n2 = 3.44;          % Core
n3 = 1.00;          % Upper cladding (air)

% Vertical dimensions:
h1 = 2.0;           % Lower cladding
h2 = 1.3;           % Core thickness
h3 = 0.5;           % Upper cladding
rh = 1.1;           % Ridge height

% Horizontal dimensions:
rw = 1.0;           % Ridge half-width
side = 1.5;         % Space on side

% Grid size:
dx = 0.0125;        % grid size (horizontal)
dy = 0.0125;        % grid size (vertical)

lambda = 1.55;      % vacuum wavelength
nmodes = 1;         % number of modes to compute

fprintf (1,'generating index mesh...\n');
[x,y,xc,yc,nx,ny,eps,edges] = waveguidemesh([n1,n2,n3],[h1,h2,h3], ...
                                            rh,rw,side,dx,dy);

% Stretch the 40 gridpoints on the south and east edges of the
% computational window by factors of 4 and 3, respectively.

[x,y,xc,yc,dx,dy] = stretchmesh(x,y,[0,40,40,0],[1,4,3,1]);

[Ex,neff] = svmodes(lambda,n2,nmodes,dx,dy,eps,'000S','EX');

fprintf(1,'neff = %.6f\n',neff);

figure(1);
contourmode(x,y,Ex);
title('Ex (TE Mode)'); xlabel('x'); ylabel('y');
for v = edges, line(v{:}); end


%[2012]-"Flower pollination algorithm for global optimization"

% (9/12/2020)

function FPA = jFlowerPollinationAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
beta  = 1.5;    % levy component
P     = 0.8;    % switch probability

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'P'), P = opts.P; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'beta'), beta = opts.beta; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Compute fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) =fun(feat,label,(X(i,:) > thres),opts);
  % Best flower
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
Xnew = zeros(N,dim);

curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iterations
while t <= max_Iter
	for i = 1:N
    % Global pollination
    if rand() < P
      % Levy distribution (2)
      L = jLevyDistribution(beta,dim);
      for d = 1:dim
        % Global pollination (1)
        Xnew(i,d) = X(i,d) + L(d) * (X(i,d) - Xgb(d));
      end
    % Local pollination
    else
      % Different flower j, k in same species
      R   = randperm(N);
      J   = R(1);
      K   = R(2);
      % Epsilon [0 to 1]
      eps = rand();
      for d = 1:dim
        % Local pollination (3)
        Xnew(i,d) = X(i,d) + eps * (X(J,d) - X(K,d));
      end
    end
    % Check boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Fitness
  for i = 1:N
    % Compute fitness
    Fnew = fun(feat,label,(Xnew(i,:) > thres),opts);
    % Update if there is better solution
    if Fnew <= fit(i)
      X(i,:) = Xnew(i,:);
      fit(i) = Fnew;
    end
    % Best flower
    if fit(i) < fitG
      Xgb  = X(i,:);
      fitG = fit(i);
    end
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (FPA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
FPA.sf = Sf;
FPA.ff = sFeat;
FPA.nf = length(Sf);
FPA.c  = curve;
FPA.f  = feat;
FPA.l  = label;
end

%// Levy Flight //
function LF = jLevyDistribution(beta,dim)
% Sigma
nume  = gamma(1 + beta) * sin(pi * beta / 2);
deno  = gamma((1 + beta) / 2) * beta * 2 ^ ((beta - 1) / 2);
sigma = (nume / deno) ^ (1 / beta);
% Parameter u & v
u = randn(1,dim) * sigma;
v = randn(1,dim);
% Step
step = u ./ abs(v) .^ (1 / beta);
LF   = 0.01 * step;
end


function [y,zf,u]=randfilt(pb,pa,ny,zi)
%RANDFILT Generate filtered gaussian noise without initial transient
%
% Generates noise by passing gaussian noise through a filter pb/pa
% output signal y is a column vector of length ny
%
% zf and zi are the output and optional input state as defined in filter()
% If zi is not specified, random numbers with the correct covariance will be used.
% output u*u' is the state covariance matrix for filter().

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: randfilt.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% check to see if we must generate zi

if pa(1)~=1
   pb=pb/pa(1); pa=pa/pa(1);
end
if nargin<4 | nargout>2
   lb=length(pb);
   la=length(pa);

   k=max(la,lb)-1;
   l=la-1;
   ii=k+1-l:k;

   % form controllability matrix

   q=zeros(k,k);
   [z,q(:,1)]=filter(pb,pa,1);
   for i=2:k [z,q(:,i)]=filter(pb,pa,0,q(:,i-1)); end

   % we generate m through the step-down procedure
   s=randn(k,1);
   if l
      m=zeros(l,l);
      g=pa;
      for i=1:l
         g=(g(1)*g(1:end-1)-g(end)*g(end:-1:2))/sqrt((g(1)-g(end))*(g(1)+g(end)));
         m(i,i:l)=g;
      end
      s(ii)=triu(toeplitz(pa(1:l)))*(m\s(ii));
      if nargout>2
         u=q;
         u(:,ii)=q(:,ii)*triu(toeplitz(pa(1:l)))/m;
      end
   else
      if nargout>2
         u=q;
      end
   end
   if nargin < 4
      if k
         zi=q*s;
      else
         zi=[];
      end
   end
end
if ny
   [y,zf]=filter(pb,pa,randn(ny,1),zi);
else
   zf=zi;
   y=[];
   end


% edgeProcessing processes edges from given image following steps below.
%
%   * Step 1. Edge detection
% Extracts Edge Map from given image and preprocess it to reduce complexity.
%   * Step 2. Edge Contours extraction
% Extracts Edge Contours from given Edge Map.
%   * Step 3. Line segment fitting
% Extracts Line Segments fitting on the Edge Contour.
%   * Step 4. Detecting edge portions with Smooth Curvatures.
% Classify edges using Curvatures so that every edge doesn't have a sudden
% change which is called sharp turn and a inflexion point.
%
% Input     - im : 'jpg' image which taken by normal smart phone.
% Output    - ResizedIm : (possibly) resized image.
%             SmoothCurvatures = The list of edges with Smooth Curvature.
%             Each of them doesn't have any sharp turn and inflexion point.

function [ResizedIm, SmoothCurvatures] = edgeProcessing(im, showFigures)

%% Initialize

% Set default parameters
if nargin < 2
    showFigures = true;
end

% For edge detection.
T_low = 0.2;
T_high = 0.4;
sigma = 1;

% For Line segment fitting.
DeviationThreshold = 2;

% For Smooth Curvatures.
SharpTurnThreshold = 90;

%% Preprocess image

% Resize if input image is too high resolution.
larger_side = max(size(im,1), size(im,2));
max_side = 800;
if larger_side > max_side
    im = imresize(im, max_side/larger_side);
end
ResizedIm = im;

% Convert RGB to gray.
im = rgb2gray(im);
im = histeq(im);

%% Step 1. Edge detection

% Implement Canny edge detection.
EdgeMap = edge(im, 'canny', [T_low, T_high], sigma);

% Apply 'thin' operation to ignore unnecessary parts.
EdgeMap = bwmorph(EdgeMap,'thin',Inf);

% Remove junctions.
[ROW, COL] = size(EdgeMap);
for i = 2:ROW-1
    for j = 2:COL-1
        if EdgeMap(i,j) == 1 && sum(sum(EdgeMap(i-1:i+1,j-1:j+1))) >= 4
            EdgeMap(i-1:i+1,j-1:j+1) = 0;
        end
    end
end

% Remove isolated pixels.
EdgeMap = bwmorph(EdgeMap,'clean');

%% Step 2. Edge Contours extraction

% Extract Edge Contours using preprocessed EdgeMap.
EdgeContours = extractEdgeContours(EdgeMap);

% Draw Edge Contours
if showFigures
    drawLineSegments(size(EdgeMap), EdgeContours);
end

%% Step 3. Line segment fitting

% Extract Line Segments fitting on the Edge Contour.
SegmentList = lineSegmentFitting(EdgeContours, DeviationThreshold);

% Draw Segment Lines
if showFigures
    drawLineSegments(size(EdgeMap), SegmentList);
end

%% Step 4. Detecting edge portions with Smooth Curvatures.

% Classify Smooth Curvatures.
SmoothCurvatures = classifySmoothCurvatures(SegmentList, SharpTurnThreshold);

% Draw Smooth Curvatures.
if showFigures
    drawLineSegments(size(EdgeMap), SmoothCurvatures);
end

end


function [ Y ] = ATMAV( X, h )
% ATMED  - The fuzzy filter with Triangular function with MEDian center
% x     - input (noisy image)
% y     - output (de-noised image)
% F     - window function, weighting function

if nargin < 2
    h=3;    % local search window size: h=3; h=5; h=7; h=9; h=11;
    f=1;    % padding value for window: f=1; f=2; f=3; f=4; f=5;
else
    f=(h-1)/2;    % padding value for window: f=1; f=2; f=3; f=4; f=5;
end
F = zeros(h*h,1);
Y  = zeros(size(X));

X = double(padarray(X,[f f],'symmetric'));
[m n ~] = size(X);

for i=1+f:1:m-f
   for j=1+f:1:n-f
       x=reshape(X(i-f:i+f, j-f:j+f),[],1);
       xmin = min(x);
       xmav = mean(x);
       xmax = max(x);

       F(:,:) = 0;
       if (xmav-xmin==0)||(xmax-xmav==0)
           F(:,:) = 1;
       else
           ind1 = find((x>=xmin)&(x<=xmav));
           F(ind1) = 1-(xmav-x(ind1))/(xmav-xmin);

           ind2 = find((x>=xmav)&(x<=xmax));
           F(ind2) = 1-(x(ind2)-xmav)/(xmax-xmav);
       end

       Y(i-f,j-f) = sum(sum(F.*x))/sum(sum(F));
       clear xmax xmin xmav ind1 ind2;
   end
end
end


%% New features in MatlabBGL version 4.0
% MatlabBGL 3.0 was only released as a beta test.  Version 4.0 is a
% full release with quite a few new features.

%% Improved reweighted graphs
% In MatlabBGL 3.0, reweighted graphs were a pain to use.   Now they are
% simple!  We combine a structural matrix with a weight matrix.  As(i,j)=1
% if there is an edge between vertex i and j and A(i,j)=wij where wij is
% the weight of the edge between i and j.

As = cycle_graph(6,'directed',0); % compute a 6 node cycle graph
A = As; % set all the weights to be one initially
A(2,3) = 0; A(3,2) = 0; % make one edge have zero weight
fprintf('Edges\n');
full(As)
fprintf('Weights\n');
full(A)

%%
% Note that As is given as the graph in the following call, not A!
[d pred] = shortest_paths(As,1,'edge_weight',edge_weight_vector(As,A));
d(3) % distance from vertex 1 to vertex 3 should be just 1!

%% Graph layout algorithms
% Sometimes, it's really nice to see a picture of your graph.  The BGL
% implements a few graph layout algorithms and so these are now in
% MatlabBGL 4.0!

%%
G = grid_graph(6,5);
X = kamada_kawai_spring_layout(G);
gplot(G,X,'.-');

%%
G = grid_graph(6,5);
X = fruchterman_reingold_force_directed_layout(G);
gplot(G,X,'.-');

%%
G = grid_graph(6,5);
X = gursoy_atun_layout(G);
gplot(G,X,'.-');

%% Planar graph algorithms
% The Boost Graph Library received a new suite of planar graph algorithms.
% These are now in MatlabBGL too.

%%
% A grid in the xy plane is a planar graph.
G = grid_graph(6,5);
is_planar = boyer_myrvold_planarity_test(G)

%%
% Recall that K_5 (the clique on 5 vertices) is not a planar graph.  Let's
% see what happens.
K5 = clique_graph(5);
is_planar = test_planar_graph(K5) % helpful wrapper

%%
% We can also draw planar graphs
G = grid_graph(6,5);
X = chrobak_payne_straight_line_drawing(G);
gplot(G,X,'.-'); % it looks a little different!

%% New option syntax
% You probably noticed that the "struct" command that permeated MatlabBGL
% calls before is gone in these examples.  We've moved to a new option
% syntax that gives you the _choice_ between the MatlabBGL struct style
% arguments and a list of key-value pairs

%%
% We'll look at spanning trees on the clique graph with 5 vertices.
% Using Prim's algorithm, the spanning tree we get depends on the root.  We
% always get a star graph rooted at the vertex we pick as the root.
G = clique_graph(5);

%%
% Old style
full(mst(G,struct('root',5,'algname','prim')))

%%
% New style

%%
% Just to make sure it works
full(mst(G,'root',1,'algname','prim'))


    function [yM] = model(xm, a)
    %
    % Model (linear and nonlinear) definitions
    % for the function numerFminS() as a 'fun'
    %
    % Linear (dataLRM.txt):
    %yM = zeros(2,1);
    %yM=a(1)*xm+a(2);
    % Second degree polynomial (dataNRM.txt):
    yM = zeros(3,1);
    yM=a(1)*xm.^2+a(2)*xm+a(3);
    % Power exponet (dataERM.txt):
    %yM = zeros(2,1);
    %yM=a(1)*xm.^a(2);


%% function Xhat = triangulate(x1,x2,P2,homogeneous)
% triangulates the points Xhat, x1,x2 using P1(assumed to be [I|0]),P2
% Assumes the points x1,x2 are normalized so their last dimension is 1
% Xhat is the homogeneous coordinates of the reconstructed X
function Xhat = triangulate(x1,x2,P2,homogeneous)
Xhat = ones(4,size(x1,2));
P1 = [eye(3) zeros(3,1)];
for i = 1 : size(x1,2)
    if homogeneous
        A =[x1(1,i)*P1(3,:) - P1(1,:);
            x1(2,i)*P1(3,:) - P1(2,:);
            x2(1,i)*P2(3,:) - P2(1,:);
            x2(2,i)*P2(3,:) - P2(2,:)];
        [u,s,v] = svd(A);
        Xhat(:,i) = v(:,end);
    else
        A =[x1(1,i)*P1(3,1:3) - P1(1,1:3);
            x1(2,i)*P1(3,1:3) - P1(2,1:3);
            x2(1,i)*P2(3,1:3) - P2(1,1:3);
            x2(2,i)*P2(3,1:3) - P2(2,1:3)];
        b = -[x1(1,i)*P1(3,4) - P1(1,4);
            x1(2,i)*P1(3,4) - P1(2,4);
            x2(1,i)*P2(3,4) - P2(1,4);
            x2(2,i)*P2(3,4) - P2(2,4)];
        [u,s,v] = svd(A);
        bprime = u'*b;
        y = bprime(1:3) ./ (diag(s));
        Xhat(1:3,i) = v*y;
    end
end
Xhat = Xhat./repmat(Xhat(4,:),4,1);
end


function expected_scale =  spm_expected_scale(min_img_sz, boxes, spm_params)
% expected_scale =  spm_expected_scale(min_img_sz, boxes, spm_params)
%
%   min_img_sz      min(size(im, 1), size(im, 2))
%   boxes           in rows with (l, t, r, b)
%
% AUTORIGHTS
% ---------------------------------------------------------
% Copyright (c) 2014, Shaoqing Ren
%
% This file is part of the SPP code and is available
% under the terms of the Simplified BSD License provided in
% LICENSE. Please retain this notice and LICENSE if you use
% this file (or any portion of it) in your project.
% ---------------------------------------------------------

area = (boxes(:, 3) - boxes(:, 1) + 1) .* (boxes(:, 4) - boxes(:, 2) + 1);

expected_scale = spm_params.sz_conv_standard * spm_params.step_standard * min_img_sz ./ sqrt(area);
%     scale_expected = standard_img_size * min_img_sz ./ sqrt(area);
expected_scale = round(expected_scale(:));

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% angular resolution
dt=pi/40;
t=0:dt:2*pi-dt;

%% parameters of a side-cut fiber
h=100;
r1=20;
r2=25;
a=-1;
b=0;
c=1;
d=-h;

%% key nodes of a side-cut fiber

n1=[r1*sin(t(:)) r1*cos(t(:)) zeros(size(t(:)))];
n2=[r2*sin(t(:)) r2*cos(t(:)) zeros(size(t(:)))];

n3=[r1*sin(t(:)) r1*cos(t(:)) -d-(a*r1*sin(t(:))+b*r1*cos(t(:)))/c];
n4=[r2*sin(t(:)) r2*cos(t(:)) -d-(a*r2*sin(t(:))+b*r2*cos(t(:)))/c];

no=[n1;n2;n3;n4];

%% PLCs of the side-cut fiber

clear fc;
count=1;
for i=1:length(t)-1
   % the last number in each cell is the fc id
   fc{count}={[i+length(t) i+3*length(t) i+3*length(t)+1 i+length(t)+1],1}; count=count+1;
   fc{count}={[i i+2*length(t) i+2*length(t)+1 i+1],2}; count=count+1;
end
i=length(t);
fc{count}={[i+length(t) i+3*length(t) 1+3*length(t) 1+length(t)],1}; count=count+1;
fc{count}={[i i+2*length(t) 1+2*length(t) 1],2}; count=count+1;

fc{count}={1:1+length(t)-1,3};count=count+1;  % bottom inner circle
fc{count}={[1+length(t):1+length(t)*2-1 nan fliplr(1:1+length(t)-1)],4};count=count+1; % button outter circle
fc{count}={1+length(t)*2:1+length(t)*3-1,5};count=count+1;  % top inner circle
fc{count}={[1+length(t)*3:1+length(t)*4-1 nan fliplr(1+length(t)*2:1+length(t)*3-1)],6}; % top outter circle

%% mesh generation of the cladding for the side-cut fiber
%[node,elem,face]=s2m(no,face,1,50);
[node,elem,face]=surf2mesh(no,fc,min(no),max(no),1,50,[0 0 1],[],0);

plotmesh(no,fc,'y>-0.1');
figure
plotmesh(node,elem,'x>0 | y>0');


%vgg_F_from_7pts_2img  Computes fundamental matrix from 7 points across 2 images.
%
%   [P,X] = vgg_F_from_7pts_2img(x), where
%      x ... double(3,7,2) or cell{2} of double(3,7), 7 homogeneous points in 2 images
%      F ... double(3,3), fundamental matrix
%   There are 0 to 3 solutions for F. Solutions are pruned by requirement that
%   scalars s in all equations s*cross(e1,x1)==F*x2 are positive.
%   In case of multiple solutions, F has one dimension
%   more such that F(:,:,n) is the n-th solution.
%
%   Also the form F = vgg_F_from_7pts_2img(x1,x2) is accepted.

function F = vgg_F_from_7pts_2img(x1,x2)

if nargin==1
  if iscell(x1)
    x2 = x1{2};
    x1 = x1{1};
  else
    x2 = x1(:,:,2);
    x1 = x1(:,:,1);
  end
end
if any(size(x1)~=[3 7]) | any(size(x2)~=[3 7])
  error('Wrong size of input points.');
end

% Linear step
A = vgg_vec_swap(x1,x2)';
[u,s,v] = svd(A,0);
FF{1} = reshape(v(:,end-1),[3 3]);
FF{2} = reshape(v(:,end  ),[3 3]);

% Solving cubic equation and getting 1 or 3 solutions for F
a = vgg_singF_from_FF(FF);
F = [];
for i = 1:length(a)
  Fi = a(i)*FF{1} + (1-a(i))*FF{2};
  %for n = 1:7, disp(norm(x(:,n,1)'*Fi*x(:,n,2))), end  % test code
  if signs_OK(Fi,x1,x2)
    F = cat(3, F, Fi);
  end
end

return

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

% Checks sign consistence of F and x
function OK = signs_OK(F,x1,x2)
[u,s,v] = svd(F');
e1 = v(:,3);
l1 = vgg_contreps(e1)*x1;
s = sum( (F*x2) .* l1 );
OK = all(s>0) | all(s<0);
return


function x = poisson_solution ( nrow, ncol, rhs_num )

%*****************************************************************************80
%
%% POISSON_SOLUTION returns the solution of a Poisson linear system.
%
%  Discussion:
%
%    The Poisson matrix is associated with an NROW by NCOL rectangular
%    grid of points.
%
%    Assume that the points are numbered from left to right, bottom to top.
%
%    If the K-th point is in row I and column J, set X = I + J.
%
%    This will be the solution to the linear system.
%
%  Example:
%
%    NROW = 3, NCOL = 3
%
%    ^
%    |  7  8  9
%    J  4  5  6
%    |  1  2  3
%    |
%    +-----I---->
%
%    Solution vector X = ( 2, 3, 4, 3, 4, 5, 4, 5, 6 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, Charles Van Loan,
%    Matrix Computations, second edition,
%    Johns Hopkins University Press, Baltimore, Maryland, 1989
%    (Section 4.5.4).
%
%  Parameters:
%
%    Input, integer NROW, NCOL, the number of rows and columns
%    in the grid.
%
%    Input, integer RHS_NUM, the number of right hand sides.
%
%    Output, real X(NROW*NCOL,RHS_NUM), the solution.
%
  n = nrow * ncol;

  x = zeros ( n, rhs_num );

  k = 0;
  for j = 1 : nrow
    for i = 1 : ncol
      k = k + 1;
      x(k,1) = i + j;
    end
  end

  return
end


function f = solharm(l, m, varargin)
% SOLHARM Complex-valued, solid harmonic of degree L and order M.
%
%   F = SOLHARM(L, M) returns the degree L, order M real-valued
%   solid harmonic on the ball.  F is normalized so that its two-norm
%   over the sphere is 1.
%
%   F = SOLHARM(L, M, 'complex') returns the degree L, order M complex-valued
%   solid harmonic on the ball.  F is normalized so that its two-norm
%   over the sphere is 1.
%
% See also spherefun.sphharm.

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

if nargin == 2
    % Complex solid harmonics
    F = complex_solharm(l,abs(m));
    % Determine if the cos or sin term should be added:
    pos = abs(max(0, sign(m+1)));
    % Compute the real spherical harmonic:
    % Real part of pos = 1, imaginary part if pos = 0
    F(:,1,:) = (-1)^(1-pos)*F(:,2*abs(m)+1,:);
    F = (-1i)^(1-pos)*F/(1+(m~=0));
    f = ballfun(F,'coeffs');

elseif nargin > 2 && strcmp(varargin{1},'complex')
    % Complex solid harmonics
    F = complex_solharm(l,m)/sqrt(1+(abs(m)>0));
    f = ballfun(F,'coeffs');

else
    % Don't know what to do.
    error('CHEBFUN:BALLFUN:solharm:input', ...
                'Unrecognized inputs.')
end
end

function F = complex_solharm(l,m)
% SOLHARM Complex-valued, solid harmonic of degree L and order M.
%
%   F = SOLHARM(L, M) returns the degree L, order M complex-valued
%   solid harmonic on the ball.  F is normalized so that its two-norm
%   over the sphere is 1.

if ( l < abs(m) )
    error('CHEBFUN:BALLFUN:solHarm', 'Degree must be >= order for solid harmonic ');
end

% abs(m) <= l
% Compute P^abs(m)_l
S = [l+1,2*abs(m)+1,2*l+1];
Plm = normalized_legendre(l,abs(m));
if m < 0
   Plm = (-1)^m*Plm;
end
% Normalize the solid harmonic so that its two-norm over the ball is 1
% This is the normalization constant so that the integral of r^l is 1
Plm = Plm*sqrt(2*l+3);

% Compute the chebyshev coefficients of r^l
r = chebpts(l+1, [-1 1]);
monomial = r.^l;
monomial = chebtech2.vals2coeffs(monomial);

% Return the Spherical Harmonic Y^m_l
F = zeros(S);
F(:,abs(m)+m+1,:) = reshape(monomial*Plm.',l+1,1,2*l+1);
end

function Pold = normalized_legendre(l_max,m_max)
%NORMALIZED_LEGENDRE   Generate the fully normalized associated Legendre
% polynomials P^m_max_l_max
%
% Normalization for the spherical harmonics is \tilde{P}^m_l =
% P^m_l/(2*sqrt(pi)).

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

%% Below is the implementation the Modified Forward Column (MFC) method
% described in the Holmes and Featherstones paper (2002)
% It computes P^m_n/u^m by a stable recurrence to avoid numerical errors
% near the poles

% Discretization number of the polynomial
p = 2*l_max+1;

% Interpolation points
th = pi*trigpts(p);

% Precompute vector cos(th)
CosTh = cos(th);

%% Compute P_m_max^m_max / u^m_max

% Initialize P^0_0 / u^0
Pold = ones(p, 1);

% Compute P^m_m/u^m
for m = 1:m_max
    % Compute Pm^m with Pm-1^m-1
    Pold = sqrt((2*m+1)/(2*m-(m==1)))*Pold;
end

% Initialize the recurrence (Pm^m-1 does not exist)
Poldold = zeros(p, 1);

%% Compute P^m_l / u^m with the recurrence formula, m_max+1 <= l <= l_max
for l = m_max+1:l_max
    anm = sqrt((4*l^2-1)/((l-m_max)*(l+m_max)));
    bnm = sqrt((2*l+1)*(l+m_max-1)*(l-m_max-1)/((l-m_max)*(l+m_max)*(2*l-3)));
    % Compute the normalized associated legendre polynomial P^m_l/u^m
    Pl = anm*CosTh.*Pold - bnm*Poldold;

    % Update the polynomials for the recurrence
    Poldold = Pold;
    Pold = Pl;
end

% Normalize the polynomial and recover associated Legendre polynomials
% Divide by sqrt(2) if m_max > 0
Pold = (-1)^m_max*sin(th).^m_max.*Pold/sqrt(4*pi);

% FFT to recover the coefficients
Pold = trigtech.vals2coeffs(Pold);
end


function [ states ] = BuildStateList
%BuildStateList builds a state list from a state matrix

% state discretization for the mountain car problem
xdiv  = (0.55-(-1.5))   / 50.0;
xpdiv = (0.07-(-0.07)) / 50.0;

x = -1.5:xdiv:0.5;
xp= -0.07:xpdiv:0.07;

N=size(x,2);
M=size(xp,2);

states=[];
index=1;
for i=1:N
    for j=1:M
        states(index,1)=x(i);
        states(index,2)=xp(j);
        index=index+1;
    end
end