%%
%An example to RMA from the book: Carrara:"Spotlight Synthetic Aperture
%Radar: Signal Processing Algortithms" Artech House 1995.
%Signal Data from: Tab10_1
%%
clear all,close all; clc
Nf=2^10; %number of points for freq. range

%array parameter
dx=0.6;L=760.8; %interelement space in synthetic array (length L)
Na=L/dx+1; %number of array elements
X=linspace(-L/2,L/2,Na).'; %position of array elements
Kx=linspace(-pi/dx,pi/dx,Na).';%azimuthal spacial wavenumber (page 410)
Kx=repmat(Kx,1,Nf);
Rs=1000; %distance from origin to the array plane
X=X+1j*Rs; % XY antenna position as complex number. Here, image processing
%in XY plane only, therefore, complex description for cartesian coordinates
%is convinient.

%frequency range
co=299792458; %free space wave velocity
fc=242.4*1E6; %last frequency in the Band of width B (in the book it should
%be center freq., but the Range Freq. in Fig. 10.11 doesn't fit then.)
B=133.5*1E6; %bandwidth
freq=linspace(fc-B,fc,Nf);
Kr=4*pi*freq/co;%Frequency wavenumber (page 410)
Kr=repmat(Kr,Na,1);

%target position:
tar=[-200+1j*100, 200+1j*100, 0+1j*100,...
    -200, 0, 200,...
    -200-1j*100, 200-1j*100, 0-1j*100]; %3x3 grid
%tar=[0, 200, -1j*100]; %(table 10.1)

%simulated data, dechirped (compressed) and deskewed in range:
data=my_sinthSISO_planresp( X,freq,tar );
%phase reference acc. to image origin
%(see also: Sanchez "3-D Radar Imaging Using Range Migration Techniques",
%IEEE; or:
%http://www.lluisvives.com/servlet/SirveObras/01604307547816095212257/003402_12.pdf):
data=data.*exp(1j*Rs*Kr);
%
%fig10.7 and 10.8
data_compr=fftshift(ifft(data,[],2),2);
imagesc(abs(data_compr));
%fig10.7

%azimuth fft
data=fftshift(data,1); %due to -Xmin..Xmax
data=fft(data,[],1);%azimuth fft
data=fftshift(data,1); %due to -Kx_min..Kx_max
%azimuth fft

%Kx accord to fig10.11
plot(Kx,Kr,'.')
Xax=-5:1:5;Yax=5:1:10;
axis([ min(Xax) max(Xax) min(Yax) max(Yax)])
set(gca,'XTick',Xax,'YTick',Yax)
grid on
%Kx accord to fig10.11

%fig10.11
imagesc(Kr(1,:),Kx(:,1),abs(data))
%fig10.11

%range compressed signal in Fig10.12
data_compr=fftshift(ifft(data,[],2),2);
%fig10.12
imagesc(abs(data_compr));
%fig10.12

%Ky wavenumber, squared, accordingly to (10.9)
Ky_sq=Kr.^2-Kx.^2;
%only positive Ky is physical
Neg=logical(Ky_sq < 0); %
Ky_sq(Neg)=NaN; data(Neg)=NaN; Kr(Neg)=NaN; %only positive Ky is acceptable
%matched filter, phase acc. to (10.8)
Fmf=-Rs*Kr+Rs*sqrt(Ky_sq);
%figure 10.13
mesh(Kr,Kx,Fmf)
axis([4 10 -2.8 2.8 -2000 0])
%figure 10.13

data=data.*exp(1j*Fmf); %data matching by matched filter
%fig. 10.14 middle
data_compr=fftshift(ifft(data,[],2),2);
imagesc(abs(data_compr));
%fig. 10.14 middle

%stolt interpolation
%Fig 10.15 (a)
plot(Kx,Kr,'.')
%Fig 10.15 (a)
Ky=sqrt(Ky_sq); %cross-range wavenumber by (10.9)
%Fig 10.15 (b)
plot(Kx,Ky,'.')
%Fig 10.15 (b)
Ky_int=Ky(Na/2-0.5,:);%data interpolation to Ky_int cross-range wavenumber
data=my_spline(Ky,data,Ky_int);%1D data interpolation by spline function
%stolt interpolation

%processing window in Ky dimension
Ny=750;
data=data(:,1:Ny);
Ky_int=Ky_int(1:Ny);

%range compression
data=fftshift(ifft(data,[],2),2);
%fig10.20 (a)
imagesc(abs(data));
%fig10.20 (a)

%azimuth compression
data=fftshift(data,1);%compare to azimuth fft above
data=ifft(data,[],1);
data=fftshift(data,1);
%%
dy=2*pi/(Ky_int(end)-Ky_int(1)); %step in cross-range dimension
Y=-Ny/2*dy:dy:(Ny/2-1)*dy;%cross-range axis
%fig10.20 (b)
imagesc(Y,X,abs(data));
ylabel('Cross-Range, m')
xlabel('Range, m')
axis([ -300 300 -300 300])
colormap('pink')
%fig10.20 (b)


function S1 = my_conv2(S1, sig, varargin)
% S1 is the matrix to be filtered along a choice of axes
% sig is either a scalar or a sequence of scalars, one for each axis to be filtered
% varargin can be the dimensions to do filtering, if len(sig) != x.shape
% if sig is scalar and no axes are provided, the default axis is 2

if sig>.25
    idims = 2;
    if ~isempty(varargin)
        idims = varargin{1};
    end
    if numel(idims)>1 && numel(sig)>1
        sigall = sig;
    else
        sigall = repmat(sig, numel(idims), 1);
    end

    for i = 1:length(idims)
        sig = sigall(i);

        idim = idims(i);
        Nd = ndims(S1);

        S1 = permute(S1, [idim 1:idim-1 idim+1:Nd]);

        dsnew = size(S1);

        S1 = reshape(S1, size(S1,1), []);
        dsnew2 = size(S1);

        % NN = size(S1,1);
        % NT = size(S1,2);

        tmax = ceil(4*sig);
        dt = -tmax:1:tmax;
        gaus = exp( - dt.^2/(2*sig^2));
        gaus = gaus'/sum(gaus);

        % Norms = conv(ones(NT,1), gaus, 'same');
        % Smooth = zeros(NN, NT);
        % for n = 1:NN
        %    Smooth(n,:) = (conv(S1(n,:)', gaus, 'same')./Norms)';
        % end

        cNorm = filter(gaus, 1, cat(1, ones(dsnew2(1), 1), zeros(tmax,1)));
        cNorm = cNorm(1+tmax:end, :);
        S1 = filter(gaus, 1, cat(1, S1, zeros([tmax, dsnew2(2)])));
        S1(1:tmax, :) = [];
        S1 = reshape(S1, dsnew);

        S1 = bsxfun(@rdivide, S1, cNorm);

        S1 = permute(S1, [2:idim 1 idim+1:Nd]);
    end
end


% relperm and capillary pressure curves
% to be used with FVTool package
% Written by Ali A. Eftekhari
% This is what this function does:
% res=zeros(size(sw));
% for i=1:numel(sw)
%   if swc(i)<=sw(i) && sw(i)<=1-sor(i)
%     res(i)=kro0(i)*((1-sw(i)-sor(i))/(1-sor(i)-swc(i)))^no(i);
%   elseif 0.0<sw(i) && sw(i)<swc(i)
%     res(i)=1+(kro0(i)-1)/swc(i)*sw(i);
%   elseif sw(i)>1-sor(i)
%     res(i)=0.0;
%   elseif sw(i)<=0.0
%     res(i)=1.0;
%   end
% end
%
% However, it is slow in this form. Therefore, I have rewritten it in
% vectorized from. This one is kept here for reference. You can accelerate
% it by @parfor loop, but the vectorized form is substantially faster
function res=kro_loop(sw, kro0, sor, swc, no)
res=zeros(size(sw));
for i=1:numel(sw)
  if swc(i)<=sw(i) && sw(i)<=1-sor(i)
    res(i)=kro0(i)*((1-sw(i)-sor(i))/(1-sor(i)-swc(i)))^no(i);
  elseif 0.0<sw(i) && sw(i)<swc(i)
    res(i)=1+(kro0(i)-1)/swc(i)*sw(i);
  elseif sw(i)>1-sor(i)
    res(i)=0.0;
  elseif sw(i)<=0.0
    res(i)=1.0;
  end
end


function p=v_lin2pcma(x,m,s)
%V_LIN2PCMA Convert linear PCM to A-law P=(X,M,S)
%	pcma = v_lin2pcma(lin) where lin contains a vector
%	or matrix of signal values.
%	The input values will be converted to integer
%	A-law pcm vlues in the range 0 to 255 and the XORed with m
%	(default m=85).
%
%	Input values are multiplied by the scale factor s:
%
%		   s		Input Range
%
%		   1		+-4096
%		2017.396342	+-2.03033976 (default)
%		4096		+-1
%
%	Input values outside the selected range will be clipped.
%
%	The default value of the scale factor is 2017.396342 which equals
%	sqrt((1120^2 + 2624^2)/2). This factor follows ITU standard G.711 and
%	the sine wave with PCM-A values [225 244 244 225 97 116 116 97]
%	has a mean square value of unity corresponding to 0 dBm0.
%
%	See also PCMA2LIN, LIN2PCMA, LIN2PCMU

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_lin2pcma.m 10865 2018-09-21 17:22:45Z dmb $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
  s=2017.396342;
  if nargin<2 m=85; end
end

y=x*pow2(s,-6);
y=(abs(y+63)-abs(y-63))/2;
q=floor((y+64)/64);
[a,e]=log2(abs(y));
d = (e+abs(e))/2;
p=128*q+16*d+floor(pow2(a,e-d+5));
if m p=bitxor(p,m); end;


function P = jacobiPD(n,a,b,x)
    P = 0;
    for  s = 0:n
        P = P + ...
            1/(factorial(s)*factorial(n+a-s)*factorial(b+s)*factorial(n-s)) * ...
            ((x-1)/2).^(n-s).*((x+1)/2).^s;
    end
    P = P*factorial(n+a) * factorial(n+b);
end


function [distance] = d_gaussian_wasserstein( ellipse1, ellipse2)
% D_GAUSSIAN_WASSERSTEIN gives the wasserstein distance between two ellipse
% which are interpreted as Gaussians
%
% Input:
%        ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2]
%        ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2]
%
% Output:
%       distance, scalar, gaussian wasserstein distance
%
% Written by Shishan Yang

m1 = ellipse1(1:2);
alpha1 = ellipse1(3);
eigen_val1 = [ellipse1(4), ellipse1(5)];
eigen_vec1 = [cos(alpha1), -sin(alpha1); sin(alpha1), cos(alpha1)];
sigma1 = eigen_vec1*diag(eigen_val1.^2)*eigen_vec1';
sigma1 = (sigma1+sigma1')/2; % make covariance symmetric

m2 = ellipse2(1:2);
alpha2 = ellipse2(3);
eigen_val2 = [ellipse2(4),ellipse2(5)];
eigen_vec2 = [cos(alpha2), -sin(alpha2); sin(alpha2), cos(alpha2)];
sigma2 = eigen_vec2*diag(eigen_val2.^2)*eigen_vec2';
sigma2 = (sigma2+sigma2')/2; % make covariance symmetric

error_sq = norm(m1-m2)^2 + trace(sigma1 + sigma2 -2*sqrtm((sqrtm(sigma1)*sigma2*sqrtm(sigma1))));
distance = sqrt(error_sq);

end


function out = CO_Embed2_Basic(y,tau)
% CO_Embed2_Basic Point density statistics in a 2-d embedding space.
%
% Computes a set of point-density statistics in a plot of y_i against y_{i-tau}.
%
% INPUTS:
% y, the input time series.
%
% tau, the time lag (can be set to 'tau' to set the time lag the first zero
%                       crossing of the autocorrelation function).
%
% Outputs include the number of points near the diagonal, and similarly, the
% number of points that are close to certain geometric shapes in the y_{i-tau},
% y_{tau} plot, including parabolas, rings, and circles.

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

if nargin < 2
    tau = 1;
end

doPlot = false; % plot outputs to a figure

if strcmp(tau,'tau')
	% Make tau the first zero crossing of the autocorrelation function
    tau = CO_FirstCrossing(y,'ac',0,'discrete');
end

xt = y(1:end-tau); % part of the time series
xtp = y(1+tau:end); % time-lagged time series
N = length(y) - tau; % Length of each time series subsegment

% Points in a thick bottom-left -- top-right diagonal
out.updiag01 = sum(abs(xtp-xt) < 0.1)/N;
out.updiag05 = sum(abs(xtp-xt) < 0.5)/N;

% Points in a thick bottom-right -- top-left diagonal
out.downdiag01 = sum(abs(xtp+xt) < 0.1)/N;
out.downdiag05 = sum(abs(xtp+xt) < 0.5)/N;

% Ratio of these
out.ratdiag01 = out.updiag01/out.downdiag01;
out.ratdiag05 = out.updiag05/out.downdiag05;

% In a thick parabola concave up
out.parabup01 = sum(abs(xtp-xt.^2) < 0.1)/N;
out.parabup05 = sum(abs(xtp-xt.^2) < 0.5)/N;

% In a thick parabola concave down
out.parabdown01 = sum(abs(xtp+xt.^2) < 0.1)/N;
out.parabdown05 = sum(abs(xtp+xt.^2) < 0.5)/N;

% In a thick parabola concave up, shifted up 1
out.parabup01_1 = sum(abs(xtp-(xt.^2+1)) < 0.1)/N;
out.parabup05_1 = sum(abs(xtp-(xt.^2+1)) < 0.5)/N;

% In a thick parabola concave down, shifted up 1
out.parabdown01_1 = sum(abs(xtp+(xt.^2-1)) < 0.1)/N;
out.parabdown05_1 = sum(abs(xtp+(xt.^2-1)) < 0.5)/N;

% In a thick parabola concave up, shifted down 1
out.parabup01_n1 = sum(abs(xtp-(xt.^2-1)) < 0.1)/N;
out.parabup05_n1 = sum(abs(xtp-(xt.^2-1)) < 0.5)/N;

% In a thick parabola concave down, shifted down 1
out.parabdown01_n1 = sum(abs(xtp+(xt.^2+1)) < 0.1)/N;
out.parabdown05_n1 = sum(abs(xtp+(xt.^2+1)) < 0.5)/N;

% RINGS (points within a radius range)
out.ring1_01 = sum(abs(xtp.^2+xt.^2-1) < 0.1)/N;
out.ring1_02 = sum(abs(xtp.^2+xt.^2-1) < 0.2)/N;
out.ring1_05 = sum(abs(xtp.^2+xt.^2-1) < 0.5)/N;

% CIRCLES (points inside a given circular boundary)
out.incircle_01 = sum(xtp.^2+xt.^2 < 0.1)/N;
out.incircle_02 = sum(xtp.^2+xt.^2 < 0.2)/N;
out.incircle_05 = sum(xtp.^2+xt.^2 < 0.5)/N;
out.incircle_1 = sum(xtp.^2+xt.^2 < 1)/N;
out.incircle_2 = sum(xtp.^2+xt.^2 < 2)/N;
out.incircle_3 = sum(xtp.^2+xt.^2 < 3)/N;
out.medianincircle = median([out.incircle_01, out.incircle_02, out.incircle_05 ...
                            out.incircle_1, out.incircle_2, out.incircle_3]);
out.stdincircle = std([out.incircle_01, out.incircle_02, out.incircle_05 ...
                        out.incircle_1, out.incircle_2, out.incircle_3]);

if doPlot
    figure('color','w'); box('on');
    plot(xt,xtp,'.k');
    hold on
    r = (xtp.^2+xt.^2 < 0.2);
    plot(xt(r),xtp(r),'.g')
end

end


function F = cumprod(F)
%CUMPROD   Indefinite product integral.
%   CUMPROD(F) is the indefinite product integral of the CHEBFUN F, which is
%   defined as exp(cumsum(log(F))).
%
% See also PROD.

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

% Loop over the columns:
for k = 1:numel(F)
    %[TODO]: Is this right? What about row CHEBFUNS?
    F(k) = exp(cumsum(log(F(k))));
end

end


function Xhat = gsp_jft(G,X)
%GSP_JFT Compute the Joint time-vertex Fourier Transform
%   Usage:  Xhat = gsp_jft(G,X)
%
%   Input parameters:
%         G          : Time-Vertex graph structure
%         X          : Time-Vertex signal
%   Output parameters:
%         Xhat       : Joint Time-Vertex Fourier Transform of X
%
%
%   'gsp_jft(G,X)' computes the joint time-vertex Fourier transform of the time-vertex
%    signal $X$ with respect to the Fourier basis of the graph G: U_G and the DFT basis U_T.
%
%   .. X_hat = U_G' * X * conj(U_T)
%
%
%   To compute the Fourier basis of a graph G, you can use the function::
%
%           G = gsp_compute_fourier_basis(G);
%
%   Example:::
%
%           N = 50; T=200;
%           G = gsp_sensor(N);
%           G = gsp_jtv_graph(G,T);
%           G = gsp_compute_fourier_basis(G);
%           X = sin((1:N)'*(1:T)*pi/(4*N));
%           Xhat = gsp_jft(G,X);
%           imagesc(fftshift(abs(Xhat),2));
%
%   See also: gsp_ijft, gsp_compute_fourier_basis
%

% Author : Francesco Grassi
% Date   : September 2016

if isempty(G.jtv.NFFT)
    NFFT = size(X,2);
else
    NFFT = G.jtv.NFFT;
end

normalize = 1/sqrt(NFFT);

switch G.jtv.transform
    case 'dft'
        Xhat = fft(gsp_gft(G,X),NFFT,2)*normalize;
    case 'dct'
        if size(X,3)>1
            Xhat = ipermute(dct(permute(gsp_gft(G,X),[2 1 3]),NFFT),[2 1 3]);
        else
            Xhat = dct(gsp_gft(G,X).',NFFT).';
        end

    otherwise
        error('Unknown transform');
end

end


function asa047_test02 ( )

%*****************************************************************************80
%
%% ASA047_TEST02 demonstrates the use of NELMIN on POWELL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2008
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA047_TEST02\n' );
  fprintf ( 1, '  Apply NELMIN to POWELL quartic function.\n' );

  start(1:n) =  [ 3.0; - 1.0; 0.0; 1.0 ];
  reqmin = 1.0E-08;
  step(1:4) = 1.0;
  konvge = 10;
  kcount = 500;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting point X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', start(i) );
  end

  ynewlo = powell ( start );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X) = %f\n', ynewlo );

  [ xmin, ynewlo, icount, numres, ifault ] = nelmin ( @powell, n, start, ...
  reqmin, step, konvge, kcount );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Return code IFAULT = %d\n', ifault );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate of minimizing value X*:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', xmin(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %f\n', ynewlo );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', icount );
  fprintf ( 1, '  Number of restarts =   %d\n', numres );

  return
end


function sftpack_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests R8VEC_SHT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2010
%
%  Author:
%
%    John Burkardt
%
  n = 17;
  alo = 0.0;
  ahi = 5.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For real slow Hartley transforms,\n' );
  fprintf ( 1, '  R8VEC_SHT does a forward or backward transform.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of data items is N = %d\n', n );
%
%  Set the data values.
%
  seed = 123456789;

  [ c, seed ] = r8vec_uniform ( n, alo, ahi, seed );

  r8vec_print_part ( n, c, 10, '  The original data:' );
%
%  Compute the coefficients.
%
  d = r8vec_sht ( n, c );

  r8vec_print_part ( n, d, 10, '  The Hartley coefficients:' );
%
%  Now compute inverse transform of coefficients.  Should get back the
%  original data.

  e = r8vec_sht ( n, d );

  r8vec_print_part ( n, e, 10, '  The retrieved data:' );

  return
end


function Out = QualityIndices(I_HS,I_REF,ratio)
%--------------------------------------------------------------------------
% Quality Indices
%
% USAGE
%   Out = QualityIndices(I_HS,I_REF,ratio)
%
% INPUT
%   I_HS  : target HS data (rows,cols,bands)
%   I_REF : reference HS data (rows,cols,bands)
%   ratio : GSD ratio between HS and MS imagers
%
% OUTPUT
%   Out.cc   : CC
%   Out.sam  : SAM
%   Out.rmse : RMSE
%   Out.ergas: ERGAS
%
%--------------------------------------------------------------------------
[rows,cols,bands] = size(I_REF);

%Remove border from the analysis
I_HS  = I_HS(ratio+1:rows-ratio,ratio+1:cols-ratio,:);
I_REF = I_REF(ratio+1:rows-ratio,ratio+1:cols-ratio,:);

cc = CC(I_HS,I_REF);
Out.cc = mean(cc);
Out.cc_std = std(cc);
[angle_SAM,map] = SAM(I_HS,I_REF);
Out.sam = angle_SAM;
Out.rmse = RMSE(I_HS,I_REF);
Out.ergas = ERGAS(I_HS,I_REF,ratio);

disp(['CC   : ', num2str(Out.cc), ' (std: ',num2str(Out.cc_std),')']);
disp(['SAM  : ' num2str(Out.sam)]);
disp(['RMSE : ' num2str(Out.rmse)]);
disp(['ERGAS: ' num2str(Out.ergas)]);


function phiFaceAverage = linearMean(phi)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the arithmetic average on
% the cell faces, for a uniform mesh.
%
% SYNOPSIS:
%   phiFaceAverage = arithmeticMean(phi)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract data from the mesh structure

d = phi.domain.dimension;
if (d ==1) || (d==1.5) || (d==1.8)
    dx = phi.domain.cellsize.x;
    xvalue=(dx(2:end).*phi.value(1:end-1)+dx(1:end-1).*phi.value(2:end))./(dx(2:end)+dx(1:end-1));
    yvalue=[];
    zvalue=[];
elseif (d == 2) || (d == 2.5) || (d == 2.8)
    Nx = phi.domain.dims(1);
    Ny = phi.domain.dims(2);
    dx = repmat(phi.domain.cellsize.x, 1, Ny);
    dy = repmat(phi.domain.cellsize.y', Nx, 1);
	xvalue=(dx(2:end,:).*phi.value(1:end-1,2:end-1)+...
        dx(1:end-1,:).*phi.value(2:end,2:end-1))./(dx(2:end,:)+dx(1:end-1,:));
    yvalue=(dy(:,2:end).*phi.value(2:end-1,1:end-1)+...
        dy(:,1:end-1).*phi.value(2:end-1,2:end))./(dy(:,2:end)+dy(:,1:end-1));
    zvalue=[];
elseif (d == 3) || (d==3.2)
    Nx = phi.domain.dims(1);
    Ny = phi.domain.dims(2);
    Nz = phi.domain.dims(3);
    dx = repmat(phi.domain.cellsize.x, 1, Ny, Nz);
    dy = repmat(phi.domain.cellsize.y', Nx, 1, Nz);
    DZ = zeros(1,1,Nz+2);
    DZ(1,1,:) = phi.domain.cellsize.z;
    dz=repmat(DZ, Nx, Ny, 1);
    xvalue=(dx(2:end,:,:).*phi.value(1:end-1,2:end-1,2:end-1)+...
        dx(1:end-1,:,:).*phi.value(2:end,2:end-1,2:end-1))./(dx(2:end,:,:)+dx(1:end-1,:,:));
    yvalue=(dy(:,2:end,:).*phi.value(2:end-1,1:end-1,2:end-1)+...
        dy(:,1:end-1,:).*phi.value(2:end-1,2:end,2:end-1))./(dy(:,1:end-1,:)+dy(:,2:end,:));
    zvalue=(dz(:,:,2:end).*phi.value(2:end-1,2:end-1,1:end-1)+...
        dz(:,:,1:end-1).*phi.value(2:end-1,2:end-1,2:end))./(dz(:,:,1:end-1)+dz(:,:,2:end));
end
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, zvalue);


% Graph of Buckley-Leverett flux function

% Theory in Section 9.4 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% 	See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program generates Fig. 9.10 in the book

		% ...............Input.........................................
a = 0.5;	% Parameter in flux function on page 371
		% ............End of input.....................................
x = 0.0:0.05:1.0; den = x.*x + 0.5*(1-x).^2; f = (x.*x)./den;
figure(1), clf, plot(x,f,'-')


function kf = linear_correlation(xf, yf)
%LINEAR_CORRELATION Linear Kernel at all shifts, i.e. correlation.
%   Computes the dot-product for all relative shifts between input images
%   X and Y, which must both be MxN. They must also be periodic (ie.,
%   pre-processed with a cosine window). The result is an MxN map of
%   responses.
%
%   Inputs and output are all in the Fourier domain.
%
%   Joao F. Henriques, 2014
%   http://www.isr.uc.pt/~henriques/

	%cross-correlation term in Fourier domain
	kf = sum(xf .* conj(yf), 3) / numel(xf);

end


function [pred,model] = mdlTrans_lapridge(Xl,Yl,Xu,param)
%Laplacian ridge regression
%
%	This is a model-level semi-supervised (transductive) learning
% algorithm. It may be used as a baseline to other domain adaptation
% methods.
% Application scope:
%	+ partially labeled data
%	+ label type: regression

% Xl:	labeled X
% Yl:	labels of X
% Xu:	unlabeled X
%
% param: Struct of hyper-parameters, please see the first cell of this
%	program ("default parameters") for details. You can set parameter p to
%	x by setting param.p = x. For parameters that are not set, default
%	values will be used.
%
% pred : predicted labels for Xu
%
% ref: M. Belkin, P. Niyogi, and V. Sindhwani, "Manifold regularization: A
% geometric framework for learning from labeled and unlabeled examples,"
% J. Mach. Learn. Res., 2006.
% Copyright 2016 Ke YAN, Tsinghua Univ. http://yanke23.com , xjed09@gmail.com

addpath(genpath('lapsvmp_v02'))

%% default parameters
% please find the details and other parameters in functions calckernel and laplacian
t = 2;
g = .001;
gamma_I = 1;
gamma_A = 1e-5;
knn = 5;

defParam

%% kernels
if t==0, Kernel = 'linear'; KernelParam = 0;
elseif t==1, Kernel = 'poly'; KernelParam = g;
elseif t==2, Kernel = 'rbf'; KernelParam = sqrt(1/2/g);
else error('wrong ker'); end

%% make options
options = make_options('gamma_I',gamma_I,'gamma_A',gamma_A,'NN',knn,...
	'Kernel',Kernel,'KernelParam',KernelParam);
options.LaplacianNormalize = 0;
if t==1,options.polyDegree = 2;end

%% sort data
nTrain = size(Xl,1);
nTest = size(Xu,1);
Xl = [ones(nTrain,1),Xl];
Xu = [ones(nTest,1),Xu];

%% compute
X = [Xl;Xu];
K = calckernel(options,X,X);
L = laplacian(options,X);

J = diag([ones(1,nTrain),zeros(1,nTest)]);
I = eye(nTrain+nTest);
Yl = [Yl;zeros(nTest,1)];
alpha = (J*K + gamma_A*nTrain*I + gamma_I*nTrain/(nTrain+nTest)^2*L*K) \ Yl;

%% predict
out = K*alpha;
pred = out(nTrain+1:nTrain+nTest);
model.K = K;
model.alpha = alpha;

end


function [R,C,B] = circumradius(V,T)
  % CIRCUMRADIUS Return the circumradius of each triangle/tet element
  %
  % R = circumradius(V,T)
  % [R,C,B] = circumradius(V,T)
  %
  % Input:
  %   V  #V by dim list of vertex positions
  %   T  #T by simplex-size list of tet indices
  % Output:
  %   R  #T by 1 list of simplex circumradii
  %   C  #T by dim list of simplex circumcenters
  %   B  #T by simplex-size list of barycentric coordinates so that:
  %     C(t,:) = B(t,:) * V(T(t,:),:)
  %
  % Known issues:
  %   B output only supported for triangles
  %

  switch size(T,2)
  case 4
    d_14 = sqrt(sum((V(T(:,1),:)-V(T(:,4),:)).^2,2));
    d_23 = sqrt(sum((V(T(:,2),:)-V(T(:,3),:)).^2,2));
    d_24 = sqrt(sum((V(T(:,2),:)-V(T(:,4),:)).^2,2));
    d_34 = sqrt(sum((V(T(:,3),:)-V(T(:,4),:)).^2,2));
    d_12 = sqrt(sum((V(T(:,1),:)-V(T(:,2),:)).^2,2));
    d_13 = sqrt(sum((V(T(:,1),:)-V(T(:,3),:)).^2,2));
    R = sqrt((d_14.*d_23+d_13.*d_24-d_12.*d_34).*(d_14.*d_23-d_13.*d_24+d_12.*d_34).*(-d_14.*d_23+d_13.*d_24+d_12.*d_34).*(d_14.*d_23+d_13.*d_24+d_12.*d_34))./(sqrt(2)*sqrt(-2.*d_34.^2.*d_12.^4-2.*d_34.^4.*d_12.^2-2.*d_13.^2.*d_23.^2.*d_12.^2+2.*d_14.^2.*d_23.^2.*d_12.^2+2.*d_13.^2.*d_24.^2.*d_12.^2-2.*d_14.^2.*d_24.^2.*d_12.^2+2.*d_13.^2.*d_34.^2.*d_12.^2+2.*d_14.^2.*d_34.^2.*d_12.^2+2.*d_23.^2.*d_34.^2.*d_12.^2+2.*d_24.^2.*d_34.^2.*d_12.^2-2.*d_14.^2.*d_23.^4-2.*d_13.^2.*d_24.^4-2.*d_14.^4.*d_23.^2+2.*d_13.^2.*d_14.^2.*d_23.^2-2.*d_13.^4.*d_24.^2+2.*d_13.^2.*d_14.^2.*d_24.^2+2.*d_13.^2.*d_23.^2.*d_24.^2+2.*d_14.^2.*d_23.^2.*d_24.^2-2.*d_13.^2.*d_14.^2.*d_34.^2+2.*d_14.^2.*d_23.^2.*d_34.^2+2.*d_13.^2.*d_24.^2.*d_34.^2-2.*d_23.^2.*d_24.^2.*d_34.^2));
    assert(nargout == 1);
  case 3
    % http://www.mathworks.com/matlabcentral/fileexchange/17300-circumcircle-of-a-triangle/content/circumcircle.m

    %compute the length of sides (AB, BC and CA) of the triangle
    l = [ ...
      sqrt(sum((V(T(:,2),:)-V(T(:,3),:)).^2,2)) ...
      sqrt(sum((V(T(:,3),:)-V(T(:,1),:)).^2,2)) ...
      sqrt(sum((V(T(:,1),:)-V(T(:,2),:)).^2,2)) ...
      ];
    dblA = doublearea(V,T);
    %use formula: R=abc/(4*area) to compute the circum radius
    R = prod(l,2)./(2*dblA);
    %compute the barycentric coordinates of the circum center
    B= [ ...
      l(:,1).^2.*(-l(:,1).^2+l(:,2).^2+l(:,3).^2) ...
      l(:,2).^2.*( l(:,1).^2-l(:,2).^2+l(:,3).^2) ...
      l(:,3).^2.*( l(:,1).^2+l(:,2).^2-l(:,3).^2)];
    % normalize
    B = bsxfun(@rdivide,B,sum(B,2));
    %convert to the real coordinates
    C = zeros(size(T,1),size(V,2));
    for d = 1:size(V,2);
      C(:,d) = sum(B.*[V(T(:,1),d) V(T(:,2),d) V(T(:,3),d)],2);
    end

end


function triangleDemo(varargin)
%TRIANGLEDEMO Demo file of geom2d lib: lines and circles of a triangle
%
%   Usage:
%   triangleDemo
%
%   The macro run automatically, and draw several liens and circles
%   associated with a basic triangle.
%
%   Example
%   triangleDemo
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-11-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Triangle

% defines vertices
p1 = [2 4];
p2 = [18 6];
p3 = [4 16];

% concatenates vertices to form the polygon
triangle = [p1; p2; p3];

% draw the triangle
figure(1); clf;
hold on;
axis([0 20 0 20]);
axis equal;
drawPolygon(triangle, 'linewidth', 3);
drawPoint(triangle, 'marker', 'o', 'markersize', 10, 'linewidth', 2, ...
    'markerFaceColor', 'w');

%% altitudes

% create lines associated with each triangle edge
edge1 = createLine(p2, p3);
edge2 = createLine(p1, p3);
edge3 = createLine(p1, p2);

% altitudes of the triangle
alt1 = orthogonalLine(edge1, p1);
alt2 = orthogonalLine(edge2, p2);
alt3 = orthogonalLine(edge3, p3);

% compute also feet
foot1 = intersectLines(edge1, alt1);
foot2 = intersectLines(edge2, alt2);
foot3 = intersectLines(edge3, alt3);

% draw altitudes
drawLine(alt1, 'color', [0 0 .8]);
drawLine(alt2, 'color', [0 0 .8]);
drawLine(alt3, 'color', [0 0 .8]);

% draw feet
feet = [foot1; foot2 ;foot3];
drawPoint(feet, 'marker', 's', 'color', [0 0 .8], 'linewidth', 2, 'markerFaceColor', 'w');

orthoCenter = intersectLines(alt1, alt2);
drawPoint(orthoCenter, ...
    'marker', 'o', 'color', [0 0 .8], 'linewidth', 2, 'markerfacecolor', 'w');

%% Median rays and inscribed circle

% compute rays emanating from each vertex
ray1 = bisector(p2, p1, p3);
ray2 = bisector(p3, p2, p1);
ray3 = bisector(p1, p3, p2);

% draw rays (all in one call)
drawRay([ray1 ; ray2 ; ray3], 'color', [0 .5 0]);

% center of inscribed circle (assimilates rays to lines)
innerCircleCenter = intersectLines(ray1, ray2);

% radius of iscribed circle is computed as the distance to one of the sides
innerRadius = distancePointLine(innerCircleCenter, edge1);

% create the circle
innerCircle = [innerCircleCenter innerRadius];

% draw the inner circle
drawCircle(innerCircle, 'color', [0 .5 0], 'linewidth', 2);
drawPoint(innerCircleCenter, ...
    'color',  [0 .5 0], 'markerFaceColor', 'w', 'linewidth', 2);

%% Circumscribed circle

% edges midpoints
mid12 = midPoint(p1, p2);
mid13 = midPoint(p1, p3);
mid23 = midPoint(p2, p3);

% draw midpoints
midPoints = [mid12 ; mid13; mid23];
drawPoint(midPoints, 'marker', 's', 'color', 'r', 'linewidth', 2, 'markerFaceColor', 'w');

% perpendicular bisectors associated with each side
perp1 = orthogonalLine(edge1, mid23);
perp2 = orthogonalLine(edge2, mid13);
perp3 = orthogonalLine(edge3, mid12);

% draw perpendicular bisectors
drawLine([perp1 ; perp2 ; perp3], 'color', 'r');

% compute orthogonal circle
orthoCenter = intersectLines(perp1, perp2);
orthoRadius = distancePoints(orthoCenter, p1);

% draw Orthogonal center
drawCircle([orthoCenter orthoRadius], 'color', 'r', 'linewidth', 2);
drawPoint(orthoCenter, 'color', 'r', 'markerFaceColor', 'w', 'linewidth', 2);


function [test_failed_s, test_failed_b, test_failed_l]=test_waveletfilters
%TEST_WAVELETFILTERS  Test the erbfilters filter generator

[f, fs] = gspi;
f=f(1:1000);
Ls = length(f);
scales = linspace(10,0.1,100);
scales = flip(scales);
fmin = 250;
fmax = 20000;
bins = 8;
M=100;
alpha = 1-2/(1+sqrt(5)); % 1-1/(goldenratio) delay sequence
delays = @(n,a) a*(mod(n*alpha+.5,1)-.5);

wavelettypes = {{'cauchy', 300},{'fbsp', 4, 3}, {'morse'} };

for ii = 1:numel(wavelettypes)
    %first call includes the option to set a starting frequency for the wavelet
    %frequency range (can be applied to others too)
    [g_scales,a_scales,~,L_scales, info_scales]=waveletfilters(Ls,scales, 'delay', delays, wavelettypes{ii}, 'single', 'uniform', 'redtar', 4, 'complex');
    [g_bins,a_bins,~,L_bins,info_bins] = waveletfilters(Ls,'bins', fs,fmin, fmax, bins,'delay', delays, wavelettypes{ii}, 'repeat', 'uniform', 'startfreq', 800);
    [g_linear,a_linear,~,L_linear,info_linear] = waveletfilters(Ls,'linear', fs,fmin, fmax, M,'delay', delays, wavelettypes{ii}, 'uniform');

    Lscales = filterbanklength(L_scales, a_scales);
    Lbins = filterbanklength(L_bins, a_bins);
    Llinear = filterbanklength(L_linear, a_linear);

    gd_scales=filterbankdual(g_scales,a_scales,Lscales, 'asfreqfilter');
    gd_bins_asf=filterbankrealdual(g_bins,a_bins,Lbins, 'asfreqfilter');
    %gd_bins_e=filterbankrealdual(g_bins,a_bins,Lbins, 'econ');
    gd_linear=filterbankrealdual(g_linear,a_linear,Llinear, 'asfreqfilter');

    if 0
        % Inspect it: Dual windows, frame bounds and the response
        disp('Frame bounds scales:')
        [A,B]=filterbankbounds(g_scales,a_scales,Lscales);
        A
        B
        B/A
        filterbankresponse(g_scales,a_scales,Lscales,'real','plot');
        disp('Frame bounds bins:')
        [A,B]=filterbankrealbounds(g_bins,a_bins,Lbins);
        A
        B
        B/A
        filterbankresponse(g_bins,a_bins,Lbins,'real','plot');
        disp('Frame bounds linear:')
        [A,B]=filterbankrealbounds(g_linear,a_linear,Llinear);
        A
        B
        B/A
        filterbankresponse(g_scales,a_scales,Lscales,'real','plot');
        hold on
        filterbankresponse(g_bins,a_bins,Lbins,'real','plot');
        filterbankresponse(g_linear,a_linear,Llinear,'real','plot');
        figure; filterbankfreqz(g_scales,a_scales,Ls,fs,'plot','linabs','posfreq');
        figure; filterbankfreqz(g_bins,a_bins,Ls,fs,'plot','linabs','posfreq');
        figure; filterbankfreqz(g_linear,a_linear,Ls,fs,'plot','linabs','posfreq');
    end

    %reconstruct them all and calculate the norm to input
    c_scales = filterbank(f, g_scales, a_scales);
    fhat_scales = 2*real(ifilterbank(c_scales, gd_scales, a_scales));

    c_bins = filterbank(f, g_bins, a_bins);
    fhat_bins = 2*real(ifilterbank(c_bins, gd_bins_asf, a_bins));

    c_linear = filterbank(f, g_linear, a_linear);
    fhat_linear = 2*real(ifilterbank(c_linear, gd_linear, a_linear));

    if length(fhat_scales) > length(f)
        res_scales=norm(fhat_scales(1:length(f)) - f);
    else
        res_scales=norm(f(1:length(fhat_scales)) - fhat_scales);
    end
    if length(fhat_bins) > length(f)
        res_bins=norm(fhat_bins(1:length(f)) - f);
    else
        res_bins=norm(f(1:length(fhat_bins)) - fhat_bins);
    end
    if length(fhat_linear) > length(f)
        res_linear=norm(fhat_linear(1:length(f)) - f);
    else
        res_linear=norm(f(1:length(fhat_linear)) - fhat_linear);
    end

    %[test_failed_s,fail_s]=ltfatdiditfail(res_scales,0, 400);
    test_failed_s = 0;
    [test_failed_b,fail_b]=ltfatdiditfail(res_bins,0, 0.00001);
    [test_failed_l,fail_l]=ltfatdiditfail(res_linear,0, 0.00001);
    %s_scales=sprintf(['WAVELETFILTER DUAL SCALES:%3i %0.5g %s'],res_scales,fail_s);
    s_bins=sprintf(['WAVELETFILTER DUAL BINS:%3i %0.5g %s'],res_bins,fail_b);
    s_linear=sprintf(['WAVELETFILTER DUAL LINEAR:%3i %0.5g %s'],res_linear,fail_l);
    %disp(s_scales);
    disp(s_bins);
    disp(s_linear);
end


function v = basis_serene ( xq, yq, xw, ys, xe, yn, xx, yy )

%*****************************************************************************80
%
%% BASIS_SERENE evaluates the serendipity basis functions.
%
%  Discussion:
%
%    This procedure assumes that a serendipity element has been defined,
%    whose sides are parallel to coordinate axes.
%
%    The local element numbering is
%
%      YN  3--2--1
%       |  |     |
%       |  4     8
%       |  |     |
%      YS  5--6--7
%       |
%       +--XW---XE--
%
%    We note that each basis function can be written as the product of
%    three linear terms, which never result in an x^2y^2 term.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XQ, YQ, the evaluation point.
%
%    Input, real XW, YS, the coordinates of the lower left corner.
%
%    Input, real XE, YN, the coordinates of the upper right corner.
%
%    Input, real XX(8), YY(8), the coordinates of the 8 nodes.
%
%    Output, real V(8), the value of the basis functions at (XQ,YQ).
%
  v = zeros(8,1);

  v(1) = not1 ( xq, xw, xx(1) ) ...
       * not1 ( yq, ys, yy(1) ) ...
       * not2 ( xq, yq, xx(8), yy(8), xx(2), yy(2), xx(1), yy(1) );

  v(2) = not1 ( xq, xw, xx(2) ) ...
       * not1 ( xq, xe, xx(2) ) ...
       * not1 ( yq, ys, yy(2) );

  v(3) = not1 ( xq, xe, xx(3) ) ...
       * not1 ( yq, ys, yy(3) ) ...
       * not2 ( xq, yq, xx(2), yy(2), xx(4), yy(4), xx(3), yy(3) );

  v(4) = not1 ( xq, xe, xx(4) ) ...
       * not1 ( yq, yn, yy(4) ) ...
       * not1 ( yq, ys, yy(4) );

  v(5) = not1 ( xq, xe, xx(5) ) ...
       * not1 ( yq, yn, yy(5) ) ...
       * not2 ( xq, yq, xx(4), yy(4), xx(6), yy(6), xx(5), yy(5) );

  v(6) = not1 ( xq, xe, xx(6) ) ...
       * not1 ( xq, xw, xx(6) ) ...
       * not1 ( yq, yn, yy(6) );

  v(7) = not1 ( xq, xw, xx(7) ) ...
       * not1 ( yq, yn, yy(7) ) ...
       * not2 ( xq, yq, xx(6), yy(6), xx(8), yy(8), xx(7), yy(7) );

  v(8) = not1 ( yq, ys, yy(8) ) ...
       * not1 ( yq, yn, yy(8) ) ...
       * not1 ( xq, xw, xx(8) );

  return
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomIntSing2D.m                             |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Singular integration over a triangle          |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Edge definition
a = 0;
b = 1;
N = 100;

% Reference mesh (edge)
vtx  = [a 0 0 ; b 0 0];
elt  = [1 2];
mesh = msh(vtx,elt);

% Observation points (100 random & 11 segments points)
X = [-2 + 5*rand(N,3);
    [(-2:0.2:3)' zeros(26,2)] ];
X(:,3) = 0;

% Graphical representation
plot(mesh,1)
hold on
plotNrm(mesh,'b')
plot3(X(:,1),X(:,2),X(:,3),'or')
axis equal
view(0,90)
grid on

% Analytical integration (exact)
tic
S     = mesh.vtx;
n     = mesh.nrm;
tau   = mesh.tgt;
[logRa,rlogRa,gradlogRa] = domSemiAnalyticInt2D(X,S,n,tau);
toc

% 1D simpson integration ("exact")
tic
logRs     = zeros(size(logRa));
rlogRs    = zeros(size(rlogRa));
gradlogRs = zeros(size(gradlogRa));
for i = 1:size(X,1)
    % Function |r| in the 2D plane
    fun = @(x1) sqrt( (x1-X(i,1)).^2 + X(i,2).^2 );

    % Scalar integration log(|r|)
    logRs(i) = integral(@(x1) log(fun(x1)),a,b,'Reltol',1e-12);

    % Vectorial integration rlog(|r|)
    rlogRs(i,1) = integral(@(x1) (x1-X(i,1)).*log(fun(x1)),a,b,'Reltol',1e-12);
    rlogRs(i,2) = -X(i,2) .*  logRs(i);
    rlogRs(i,3) = 0;

    % Vectoral integration gradlog(|r|)
    if (X(i,2) == 0) && (X(i,1)>=a && X(i,1)<=b)
        gradlogRs(i,:) = 0;
    else
        gradlogRs(i,1) = integral(@(x1) (x1-X(i,1))./fun(x1).^2,a,b);
        gradlogRs(i,2) = integral(@(x1) -X(i,2)./fun(x1).^2,a,b);
        gradlogRs(i,3) = 0;
    end
end
toc

% Relative errors log(|r|)
figure
semilogy( abs(logRa-logRs)./abs(logRs) )
title('Relative error (log) : \int log(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int log(|r|)')
norm(logRa(I)-logRs(I),'inf')./norm(logRa(I),'inf')
norm(logRa-logRs,'inf')./norm(logRa,'inf')

% Relative errors rlog(|r|)
figure
semilogy( abs(rlogRa-rlogRs)./abs(rlogRs) )
title('Relative error (log) : \int rlog(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int rlog(|r|)')
norm(rlogRa(I)-rlogRs(I),'inf')./norm(rlogRa(I),'inf')
norm(rlogRa-rlogRs,'inf')./norm(rlogRa,'inf')

% Relative errors gradlog(|r|)
figure
semilogy( abs(gradlogRa-gradlogRs)./abs(gradlogRs) )
title('Relative error (log) : \int gradlog(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int gradlog(|r|)')
norm(gradlogRa(I)-gradlogRs(I),'inf')./norm(gradlogRa(I),'inf')
norm(gradlogRa-gradlogRs,'inf')./norm(gradlogRa,'inf')

disp('~~> Michto gypsilab !')


function recon_im = SENSE_recon(smap, reduced_fft, reduced_dim, np, regularizer, figs_on)
% function recon_im = SENSE_recon(smap, reduced_fft, reduced_dim, np,
%       regularizer, figs_on)
% in:
%   smap          [N M nc]                complex sensitivity maps
%   reduced_fft   [N/np M] or [N M/np]    undersampled fft
%   reduced_dim   1 or 2                  dimension of undersampling
%   np            degree of undersampling
%   regularizer   string, 'none' or 'tikhonov'
%   figs_on       boolean that shows intermediate images for debugging
% out:
%   recon_im      [N M]                   SENSE reconstructed image
% This implementation assumes that the fft will be reduced in such a way
% that the DC term is still included (i.e. even DFT coefficients retained)
% 2012-06-07 Mai Le, University of Michigan

% dimensions of original image
dims = [size(smap,1) size(smap,2)];

% number of coils
nc = size(smap,3);

% create aliased intermediate images
aliased_im = zeros(size(reduced_fft,1), size(reduced_fft,2), nc);
for ii = 1:nc
     aliased_im(:,:,ii) = ifft2(reduced_fft(:,:,ii));
end

% plot aliased images
if (figs_on)
    figure;
    for ii = 1:nc
        subplot(2,2,ii); imshow(abs(aliased_im(:,:,ii)),[]); colorbar;
    end
end

% calculate regularization parameter
% I found empirically that somewhere between 0.5% and 1% of the max SVD
% value works well for beta
C = Cdiff1(np);
[U, SIG, V] = svd(C'*C);
beta = 0.01*max(SIG(:));
beta = 0.005*max(SIG(:));

% reconstruct image by finding (regularized) LS solution for each set of pixels
recon_im = zeros(dims);
if (reduced_dim == 1)
    for ii = 1:dims(1)/np
        for jj = 1:dims(2)
            S = constructS(reduced_dim, smap, ii, jj, np);
            a = squeeze([aliased_im(ii,jj,1:nc)]);
            v = recon_pixels(S,a,regularizer, beta);
            recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np);
        end
    end
else
    for ii = 1:dims(1)
        for jj = 1:dims(2)/np
            S = constructS(reduced_dim, smap, ii, jj, np);
            a = squeeze([aliased_im(ii,jj,1:nc)]);
            v = recon_pixels(S,a,regularizer, beta);
            recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np);
        end
    end
end

% selects values from sensitivity map relevant for specific pixels
function S = constructS(reduced_dim, smap, ii, jj, np)
dims = size(smap);
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    S = squeeze([smap(ii+indeces,jj,:)]).';
else
    S = squeeze([smap(ii,jj+indeces,:)]).';
end

% places each of the reconstructed pixels in the reconstructed image
function recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np)
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    for kk = 1:np
        recon_im(ii+indeces(kk),jj) = v(kk);
    end
else
    for kk = 1:np
        recon_im(ii,jj+indeces(kk)) = v(kk);
    end
end

% reconstruct pixels with or without Tikhonov regularization
function pixels = recon_pixels(S,a,regularizer, beta)
switch regularizer
    case 'none'
        pixels = S\a;
    case 'tikhonov'
        M = min(size(S));
        pixels = inv(S'*S+(beta^2)*eye(M))*S'*a;
    otherwise
        pixels = S\a;
end


function p=Phi(x)
   p=0.5*(1.+erf(x/sqrt(2)));
end


function gf=gradmaxdiagnoExp(x,varargin)
% Maximizaton of the diagonality of hte matrix.
covmat = varargin{1};   % #pix X #pix X #taus
% sizevec = varargin{2};
sizevec = [size(covmat,1), length(x)/size(covmat,1)];
W = pinv(exp(reshape(x, sizevec(1), sizevec(2))))';
ntau = size(covmat,3); % #taus (different time delays)
gftmp = zeros(ntau,prod(sizevec));
for tau=1:ntau
    C = covmat(:,:,tau);
    P = W'*C*W;
    Q = diag(1./diag(P));
    R = inv(P);
%     gfr = (Q'*W' - (W*R)')*0.5*(C+C');
    gfr = Q'*W'*0.5*(C+C') - 0.5*((C*W*R)'+R*W'*C);
    gftmp(tau,:) = reshape(gfr',1,prod(sizevec));
end

% gf1 = sum(gftmp,1).*exp(x);
gf1 = sum(gftmp,1);
gf1r = reshape(gf1,sizevec(1), sizevec(2));

% gf2 = -gf1r.*(pinv(W)*exp(reshape(x, sizevec(1)*sizevec(2), sizevec(3)))*pinv(W))';
% gf2 = -(W'*pinv(gf1r)*W');
% gf2 = -(W*pinv(gf1r)*W);
% gf2 = -gf1r.*W;
% S pinv(W) to nejak nefunguje...
gf2 = -gf1r.*W;

gf=reshape(gf2,1,prod(sizevec));


clear;

addpath('../PDM_helpers/');

Reconstruct_Torresani;

%% Create the PDM using PCA, from the recovered 3D data
clear
load('Torr_menpo_wild.mat');

% need to still perform procrustes though

x = P3(1:end/3,:);
y = P3(end/3+1:2*end/3,:);

% To make sure that PDM faces the right way (positive Z towards the screen)
z = P3(2*end/3+1:end,:);

[ normX, normY, normZ, meanShape, Transform ] = ProcrustesAnalysis3D(x,y,z, true);
observations = [normX normY normZ];

[princComp, score, eigenVals] = princomp(observations,'econ');
% Keeping most variation
totalSum = sum(eigenVals);
count = numel(eigenVals);
for i=1:numel(eigenVals)
   if ((sum(eigenVals(1:i)) / totalSum) >= 0.999)
      count = i;
      break;
   end
end

V = princComp(:,1:count);
E = eigenVals(1:count);
M = meanShape(:);

% Now normalise it to have actual world scale
lPupil = [(M(37) + M(40))/2; (M(37 + 68) + M(40 + 68))/2];
rPupil = [(M(43) + M(46))/2; (M(43 + 68) + M(46 + 68))/2];

dist = norm(lPupil - rPupil,2);

% average human interocular distance is 65mm
scaling = 65 / dist;

% normalise the mean values as well
M(1:end/3) = M(1:end/3) - mean(M(1:end/3));
M(end/3+1:2*end/3) = M(end/3+1:2*end/3) - mean(M(end/3+1:2*end/3));
M(2*end/3+1:end) = M(2*end/3+1:end) - mean(M(2*end/3+1:end));

M = M * scaling;
E = E * scaling .^ 2;

% orthonormalise V
scalingV = sqrt(sum(V.^2));
V = V ./ repmat(scalingV, numel(M),1);

E = E .* (scalingV') .^ 2;

%% Also align the model to a Multi-PIE one for accurate head orientation
% also align the mean shape model (an aligned 68 point PDM from Multi-PIE dataset)
M_wild = M;

load('pdm_68_multi_pie.mat', 'M');
M_align = M;

n_points = numel(M)/3;

M_wild = reshape(M_wild, n_points, 3);

% work out the translation and rotation needed
[ R, T ] = AlignShapesKabsch(M_wild, reshape(M_align, n_points,3));

% Transform the wild one to be in the same reference as the Multi-PIE one
M_aligned = (R * M_wild')';

M_aligned(:,1) = M_aligned(:,1) + T(1);
M_aligned(:,2) = M_aligned(:,2) + T(2);
M_aligned(:,3) = M_aligned(:,3) + T(3);

M_aligned = M_aligned(:);
V_aligned = V;

% need to align the principal components as well
for i=1:size(V,2)

    V_aligned_pie_curr = (R * reshape(V(:,i), n_points, 3)')';
    V_aligned(:,i) = V_aligned_pie_curr(:);

end

V = V_aligned;
M = M_aligned;

save('pdm_68_aligned_menpo.mat', 'E', 'M', 'V');
writePDM(V, E, M, 'pdm_68_aligned_menpo.txt');

% also save this to model location
if(exist('../../models/pdm/', 'file'))
    save('../../models/pdm/pdm_68_aligned_menpo.mat', 'E', 'M', 'V');
end


function [c,ceq] = heart(x)
% For nonlinear constraint demonstration.

c = (x(1)^2 + x(2)^2 - 1)^3 - x(1)^2*x(2)^3 ;
% c(2) = -x(1) ; % Half-heart
% ceq = x(1) - 1 ;
ceq = [] ;


function plot_lc(rho,eta,marker,ps,reg_param)
%PLOT_LC Plot the L-curve.
%
% plot_lc(rho,eta,marker,ps,reg_param)
%
% Plots the L-shaped curve of the solution norm
%    eta = || x ||      if   ps = 1
%    eta = || L x ||    if   ps = 2
% as a function of the residual norm rho = || A x - b ||.  If ps is
% not specified, the value ps = 1 is assumed.
%
% The text string marker is used as marker.  If marker is not
% specified, the marker '-' is used.
%
% If a fifth argument reg_param is present, holding the regularization
% parameters corresponding to rho and eta, then some points on the
% L-curve are identified by their corresponding parameter.

% Per Christian Hansen, IMM, 12/29/97.

% Set defaults.
if (nargin==2), marker = '-'; end  % Default marker.
if (nargin < 4), ps = 1; end       % Std. form is default.
np = 10;                           % Number of identified points.

% Initialization.
if (ps < 1 | ps > 2), error('Illegal value of ps'), end
n = length(rho); ni = round(n/np);

% Make plot.
loglog(rho(2:end-1),eta(2:end-1)), ax = axis;
if (max(eta)/min(eta) > 10 | max(rho)/min(rho) > 10)
  if (nargin < 5)
    loglog(rho,eta,marker), axis(ax)
  else
    loglog(rho,eta,marker,rho(ni:ni:n),eta(ni:ni:n),'x'), axis(ax)
    HoldState = ishold; hold on;
    for k = ni:ni:n
      text(rho(k),eta(k),num2str(reg_param(k)));
    end
    if (~HoldState), hold off; end
  end
else
  if (nargin < 5)
    plot(rho,eta,marker), axis(ax)
  else
    plot(rho,eta,marker,rho(ni:ni:n),eta(ni:ni:n),'x'), axis(ax)
    HoldState = ishold; hold on;
    for k = ni:ni:n
      text(rho(k),eta(k),num2str(reg_param(k)));
    end
    if (~HoldState), hold off; end
  end
end
xlabel('residual norm || A x - b ||_2')
if (ps==1)
  ylabel('solution norm || x ||_2')
else
  ylabel('solution semi-norm || L x ||_2')
end
title('L-curve')


function [f,x,u]=ksdensityw(y,w,varargin)
%KSDENSITY Compute density estimate
%   [F,XI]=KSDENSITY(X) computes a probability density estimate of the sample
%   in the vector X.  F is the vector of density values evaluated at the
%   points in XI.  The estimate is based on a normal kernel function, using a
%   window parameter (bandwidth) that is a function of the number of points
%   in X.  The density is evaluated at 100 equally-spaced points covering
%   the range of the data in X.
%
%   F=KSDENSITY(X,XI) specifies the vector XI of values where the density
%   estimate is to be evaluated.
%
%   [F,XI,U]=KSDENSITY(...) also returns the bandwidth of the kernel smoothing
%   window.
%
%   DWH: w are the weights for y (w = 1/n for ksdenstiy)
%
%   [...]=KSDENSITY(...,'PARAM1',val1,'PARAM2',val2,...) specifies parameter
%   name/value pairs to control the density estimation.  Valid parameters
%   are the following:
%
%      Parameter    Value
%      'kernel'     The type of kernel smoother to use, chosen from among
%                   'normal' (default), 'box', 'triangle', and
%                   'epanechinikov'.
%      'npoints'    The number of equally-spaced points in XI.
%      'width'      The bandwidth of the kernel smoothing window.  The default
%                   is optimal for estimating normal densities, but you
%                   may want to choose a smaller value to reveal features
%                   such as multiple modes.
%
%   In place of the kernel functions listed above, you can specify another
%   function by using @ (such as @normpdf) or quotes (such as 'normpdf').
%   The function must take a single argument that is an array of distances
%   between data values and places where the density is evaluated, and
%   return an array of the same size containing corresponding values of
%   the kernel function.
%
%   Example:
%      x = [randn(30,1); 5+randn(30,1)];
%      [f,xi] = ksdensity(x);
%      plot(xi,f);
%   This example generates a mixture of two normal distributions, and
%   plots the estimated density.
%
%   See also HIST, @.

% Reference:
%   A.W. Bowman and A. Azzalini (1997), "Applied Smoothing
%      Techniques for Data Analysis," Oxford University Press.

%   Copyright 1993-2002 The MathWorks, Inc.
%   $Revision: 1.7 $  $Date: 2002/03/21 20:36:29 $

% Get y vector and its dimensions
if (prod(size(y)) > length(y)), error('X must be a vector'); end
y = y(:);
y(isnan(y)) = [];
n = length(y);
ymin = min(y);
ymax = max(y);

% Maybe x was specified, or maybe not
if ~isempty(varargin)
   if ~ischar(varargin{1})
      x = varargin{1};
      varargin(1) = [];
   end
end

% Process additional name/value pair arguments
okargs = {'width' 'npoints' 'kernel'};
defaults = {[] [] 'normal'};
[emsg,u,m,kernel] = statgetargs(okargs, defaults, varargin{:});
error(emsg);

% Default window parameter is optimal for normal distribution
if (isempty(u)),
   med = median(y);
   sig = median(abs(y-med)) / 0.6745;
   if sig<=0, sig = ymax-ymin; end
   if sig>0
      u = sig * (4/(3*n))^(1/5);
   else
      u = 1;
   end
end

% Check other arguments or get defaults.
if ~exist('x','var')
   if isempty(m), m=100; end
   x = linspace(ymin-2*u, ymax+2*u, m);
elseif (prod(size(x)) > length(x))
   error('XI must be a vector');
end
xsize = size(x);
x = x(:);
m = length(x);

okkernels = {'normal' 'epanechinikov' 'box' 'triangle'};
if isempty(kernel)
   kernel = okkernels{1};
elseif ~(isa(kernel,'function_handle') | isa(kernel,'inline'))
   if ~ischar(kernel)
      error('Smoothing kernel must be a function.');
   end
   knum = strmatch(lower(kernel), okkernels);
   if (length(knum) == 1)
      kernel = okkernels{knum};
   end
end

blocksize = 1e6;
if n*m<=blocksize
   % Compute kernel density estimate in one operation
   z = (repmat(x',n,1)-repmat(y,1,m))/u;
   w2 = repmat(w, 1, m);
   f = sum(feval(kernel, z).*w2,1);
else
   % For large vectors, process blocks of elements as a group
   M = max(1,ceil(blocksize/n));
   mrem = rem(m,M);
   if mrem==0, mrem = min(M,m); end
   x = x';

   f = zeros(1,m);
   ii = 1:mrem;
   z = (repmat(x(ii),n,1)-repmat(y,1,mrem))/u;
   w2 = repmat(w, 1, mrem);
   f(ii) = sum(feval(kernel, z).*w2,1);
   z = zeros(n,M);
   w2 = zeros(n,M);
   for j=mrem+1:M:m
      ii = j:j+M-1;
      z(:) = (repmat(x(ii),n,1)-repmat(y,1,M))/u;
      w2(:) = repmat(w, 1, M);
      f(ii) = sum(feval(kernel, z).*w2,1);
   end
end

f = reshape(f./u, xsize);

% -----------------------------
% The following are functions that define smoothing kernels k(z).
% Each function takes a single input Z and returns the value of
% the smoothing kernel.  These sample kernels are designed to
% produce outputs that are somewhat comparable (differences due
% to shape rather than scale), so they are all probability
% density functions with unit variance.
%
% The density estimate has the form
%    f(x;k,h) = mean over i=1:n of k((x-y(i))/h) / h

function f = normal(z)
%NORMAL Normal density kernel.
%f = normpdf(z);
f = exp(-0.5 * z .^2) ./ sqrt(2*pi);

function f = epanechinikov(z)
%EPANECHINIKOV Epanechinikov's asymptotically optimal kernel.
a = sqrt(5);
z = max(-a, min(z,a));
f = .75 * (1 - .2*z.^2) / a;

function f = box(z)
%BOX    Box-shaped kernel
a = sqrt(3);
f = (abs(z)<=a) ./ (2 * a);

function f = triangle(z)
%TRIANGLE Triangular kernel.
a = sqrt(6);
z = abs(z);
f = (z<=a) .* (1 - z/a) / a;


function pass = test_functionForm

%% Building blocks
dom = [-2 2];
I = operatorBlock.eye(dom);
D = operatorBlock.diff(dom);
Z = operatorBlock.zeros(dom);
x = chebfun('x', dom);
u = sin(x.^2);
U = operatorBlock.mult(u);

%% Operator instantiations
eyeop = toFunction(I);
err(1) = norm( eyeop(u) - u );
diffop = toFunction(D);
TwoX = diffop(x.*x);   % should be the chebfun 2*x
TwoXAgain = D*(x.*x);
err(2) = norm( 2*x - TwoX );
multop = toFunction(U);
err(3) = norm( multop(x+1) - u.*(x+1) );   % should be zero
% shorthand notation, U*chebfun
err(4) = norm( multop(x+1) - U*(x+1) );

%%

pass = err < 1e-14;

end


%% Recover function p from P0 to P1
% [rp,nodenew,elemnew] = recoverP02P1(node,elem,p,recoverMethod)
% The recover methods
% 1) 'LS': least square
% 2) 'LA': solve the Laplacian problem, only this method
%           the new node and new elem will be constructed.
%
% Created by Lin Zhong April, 2013.

%% Node and Elem
clear all; close all;
[node,elem] = squaremesh([0,1,0,1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');
figure(1)
showmesh(node,elem);
findnode(node);
findelem(node,elem);
display(elem);

NT = size(elem,1);
p = ones(NT,1);
[rp,nodenew,elemnew] = recoverP02P1(node,elem,p,'LA');
figure(2)
showmesh(nodenew,elemnew);
findnode(nodenew);
findelem(nodenew,elemnew);
display(elemnew);

%% LS and LA
% For LS, the value at the nodes are constructed by least square linear
% fitting of the barycenters of the adjacent elemts.
%
% For LA, we construct new nodes and new elems. The new nodes are the
% vertices of the triangles and the barycenters of the elemenst. The new
% elems are obtained by connecting two barycenters of the interior edges,
% and each end point of that edge. Please check the graph.
% We get the values of the interior vertices by solving the Laplacian
% problem. To improve the efficiency, we only construct part of the
% stiffness matrix.
% $$ A_{N,N+NT}(rp;p)^t = 0$$
% we solve $$rp = -A_{N,N}\A(N,N+NT)*p$$
% We notice that $A_{N,N}$ is diagonal, so this process should be fast.
% For the boundary nodes, we use least square.
%
% Both methods achieve the same accuracy.
% Because of the large size for loop, the LS method is slower than LA.
[node,elem] = squaremesh([0,1,0,1],0.05);
NT = size(elem,1);
p = ones(NT,1);
profile on
recoverP02P1(node,elem,p,'LA');
profile viewer


function gd=gabprojdual(gm,g,a,M,varargin);
%GABPROJDUAL   Gabor Dual window by projection
%   Usage:  gd=gabprojdual(gm,g,a,M)
%           gd=gabprojdual(gm,g,a,M,L)
%
%   Input parameters:
%         gm    : Window to project.
%         g     : Window function.
%         a     : Length of time shift.
%         M     : Number of modulations.
%         L     : Length of transform to consider
%   Output parameters:
%         gd    : Dual window.
%
%   `gabprojdual(gm,g,a,M)` calculates the dual window of the Gabor frame given
%   by *g*, *a* and *M* closest to *gm* measured in the $l^2$ norm. The
%   function projects the suggested window *gm* onto the subspace of
%   admissable dual windows, hence the name of the function.
%
%   `gabprojdual(gm,g,a,M,L)` first extends the windows *g* and *gm* to
%   length *L*.
%
%   `gabprojdual(...,'lt',lt)` does the same for a non-separable lattice
%   specified by *lt*. Please see the help of |matrix2latticetype| for a
%   precise description of the parameter *lt*.
%
%   See also:  gabdual, gabtight, gabdualnorm, fir2long

%   AUTHOR : Peter L. Søndergaard

if nargin<4
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
definput.flags.phase={'freqinv','timeinv'};
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    % Minimum transform length by default.
    Ls=1;

    % Use the window lengths, if any of them are numerical
    if isnumeric(g)
        Ls=max(length(g),Ls);
    end;

    if isnumeric(gm)
        Ls=max(length(gm),Ls);
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser)
    end;

end;

[g, info_g]  = gabwin(g, a,M,L,kv.lt,'callfun',upper(mfilename));
[gm,info_gm] = gabwin(gm,a,M,L,kv.lt,'callfun',upper(mfilename));

% gm must have the correct length, otherwise dgt will zero-extend it
% incorrectly using postpad instead of fir2long
gm=fir2long(gm,L);

% Calculate the canonical dual.
gamma0=gabdual(g,a,M,'lt',kv.lt);

% Get the residual
gres=gm-gamma0;

% Calculate parts that lives in span of adjoint lattice.
if isreal(gres) && isreal(gamma0) && isreal(g) && kv.lt(2)<=2
    gk=idgtreal(dgtreal(gres,gamma0,M,a,'lt',kv.lt),g,M,a,'lt',kv.lt)*M/a;
else
    gk=idgt(dgt(gres,gamma0,M,a,'lt',kv.lt),g,M,'lt',kv.lt)*M/a;
end;

% Construct dual window
gd=gamma0+(gres-gk);


function test96()
% function test96()
% Low-rank Euclidena distance matrix completion
%
% We use the tuned geometry, symfixedrankNewYYquotientfactory to solve the
% EDMCP. This test file is different from test file 'test97'
%

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:

clear all; clc; close all;

m = 500;
r = 5;
Yo = randn(m,r); % True embedding
C = squareform(pdist(Yo)'.^2); % True distances

% Create the problem structure
% quotient YY (tuned for least square problems) geometry
problem.M = symfixedrankNewYYquotientfactory(m,  r);

df = problem.M.dim();
p = 5*df/(m*m);
symm = @(M) .5*(M+M');

% mask = symm(rand(m, m)) <= p;
mask = spones(sprandsym(m, p));

prop_known = sum(sum(mask == 1))/(m*m);
fprintf('Low-rank EDM completion... \n');
fprintf('Fraction of entries given: %f \n', full(prop_known));

J = eye(m) - (1/m)*ones(m,1)*ones(m,1)'; % Centering matrix

problem.cost = @cost;
    function f = cost(X)
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        f = 0.5*(norm(mask.*(KvX - C), 'fro')^2);
    end

problem.grad = @grad;
    function g = grad(X)
        Y = X.Y;
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        J= eye(m) - (1/m)*ones(m,1)*ones(m,1)' ;
        mat = mask.*(KvX - C);
        R = J*(diag(mat*ones(m,1)) - mat)*J;
        R = 4*R;
        r = size(Y, 2);
        YtY = Y'*Y;
        invYtY = eye(r) / YtY;

        g = struct('Y', R*Y*invYtY);
    end

problem.hess = @hess;
    function Hess = hess(X, eta)

        Y = X.Y;
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        J = eye(m) - (1/m)*ones(m,1)*ones(m,1)' ;
        mat = mask.*(KvX - C);
        R = J*(diag(mat*ones(m,1)) - mat)*J;
        R = 4*R;

        Xdot = eta.Y*Y' + Y*eta.Y';
        XdotV = Xdot;
        KvXdot = diag(XdotV)*ones(1, m) + ones(m, 1)*diag(XdotV)' - 2*XdotV;
        mat2 = mask.*KvXdot;
        KvstarMatdot = J*(diag(mat2*ones(m,1)) - mat2)*J;
        KvstarMatdot = 4*KvstarMatdot;

        r = size(Y, 2);
        YtY = Y'*Y;
        invYtY = eye(r) / YtY;

        Hess.Y = R*eta.Y*invYtY  +  KvstarMatdot*Y*invYtY;
        Hess.Y = Hess.Y - 2*R*Y*(invYtY * symm(eta.Y'*X.Y) * invYtY);

        gradY = R*Y*invYtY;

        % I still need a correction factor for the non-constant metric
        Hess.Y = Hess.Y + gradY*symm(eta.Y'*X.Y)*invYtY + eta.Y*symm(gradY'*X.Y)*invYtY - X.Y*symm(eta.Y'*gradY)*invYtY;

        Hess = problem.M.proj(X, Hess);
    end

% % Check numerically whether gradient and Hessian are correct
%     checkgradient(problem);
%     drawnow;
%     pause;
%     checkhessian(problem);
%     drawnow;
%     pause;

% Initialization
[U, S, ~ ] = svds(-0.5*J*(mask.*C)*J, r); % This initialization is based on the strain error minimization of EDM
Y0 = U*(S.^0.5);
X0 = struct('Y', Y0);

% Options (not mandatory)
options.maxiter = inf;
options.maxinner = 30;
options.maxtime = 120;
options.tolgradnorm = 1e-9;
options.Delta_bar = m * r;
options.Delta0 = options.Delta_bar / 8;

% Pick an algorithm to solve the problem
   [Xopt, costopt, info] = trustregions(problem, X0, options);
%  [Xopt costopt info] = steepestdescent(problem, X0, options);
% [Xopt costopt info] = conjugategradient(problem, X0, options);

end


% Predict the label for a trained one-vs-all classifier. The labels
% are in the range 1..K, where K = size(all_theta, 1)
function p = one_vs_all_predict(all_theta, X)
    m = size(X, 1);
    num_labels = size(all_theta, 1);

    % We need to return the following variables correctly.
    p = zeros(size(X, 1), 1);

    % Add ones to the X data matrix
    X = [ones(m, 1) X];

    % Calculate probabilities of each number for each input example.
    % Each row relates to the input image and each column is a probability that this example is 1 or 2 or 3 etc.
    h = sigmoid(X * all_theta');

    % Now let's find the highest predicted probability for each row.
    % Also find out the row index with highest probability since the index is the number we're trying to predict.
    [p_vals, p] = max(h, [], 2);
end


function [ x, y, z, w ] = ld0050 ( )

%*****************************************************************************80
%
%% LD0050 computes the 50 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(50,1);
  y = zeros(50,1);
  z = zeros(50,1);
  w = zeros(50,1);
  a = 0.0;
  b = 0.0;
  v = 0.1269841269841270E-01;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.2257495590828924E-01;
  [ n, x, y, z, w ] = gen_oh ( 2, n, a, b, v, x, y, z, w );
  v = 0.2109375000000000E-01;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.3015113445777636;
  v = 0.2017333553791887E-01;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );

  return
end


% Example 8.7: Floorplan generation test script
% Section 8.8.1/2, Boyd & Vandenberghe "Convex Optimization"
% Joelle Skaf 12/04/05
%
% Rectangles aligned with the axes need to be place in the smallest
% possible bounding box. No overlap is allowed. Each rectangle to be placed
% can be reconfigured, within some limits.
% In the current problem, 60 rectangles are to be place. We are given 2
% acyclic graphs H and V (for horizontal and vertical) that specify the
% relative positioning constraints of those rectangles.
% We are also given minimal areas for the rectangles and aspect ratio
% constraints

% input data
load data_floorplan_60;
rho = 1;
Amin = 100*ones(1,n);

[W, H, w, h, x, y] = floorplan(adj_H, adj_V, rho, Amin,ones(60,1)*0.5,ones(60,1)*2);
fill([0; W; W; 0],[0;0;H;H],[1 1 1]);           % bounding box
hold on
for i=1:n
    fill([x(i); x(i)+w(i); x(i)+w(i); x(i)],[y(i);y(i);y(i)+h(i);y(i)+h(i)],0.90*[1 1 1]);
    hold on;
    text(x(i)+w(i)/2, y(i)+h(i)/2,int2str(i));
end
axis([0 W 0 H]);
axis equal; axis off;


function y = negbinztr_lpdf(x,l,r)
%NEGBINZTR_LPDF Zero trunc. negative binomial log probability density function
%
%  Description
%    Y = NEGBINZTR_LPDF(X,L,R) returns the log of zero truncated
%    Negative binomial probability density function with location
%    parameter L and dispersion parameter R (0<R<infty).
%
%    Negative binomial has different parameterizations and we use the form
%      p(x|l,r) = (r/(r+l))^r * gamma(r+y)/(gamma(r)*gamma(y+1))
%                             * (l/(r+l))^y
%    which approaches Poisson distribution when R goes to infinity.
%    Zero truncated Negative Binomial has p(x==0|l,r)=0.
%
%    The size of Y is the common size of X, L and R. A scalar input
%    functions as a constant matrix of the same size as the other
%    input.
%
%     Note that the density function is zero unless X is an
%     integer.
%
%  See also
%    NEGBINZTR_PDF, NEGBIN_LPDF, NEGBIN_PDF
%
% Copyright (c) 2010 Jarno Vanhatalo
% Copyright (c) 2010-2011  Aki Vehtari

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

if isscalar(r) && isscalar(l)
  if l<0
    y = repmat(NaN,size(x));
  else
    y = repmat(-Inf,size(x));
  end
  if l==0
    y(x==1) = 0;
  elseif l>0
    k = (x >= 1 & x == round(x));
    if (any(k))
      lp0=r.*(log(r) - log(r+l));
      y(k)=lp0 + gammaln(r+x(k)) - gammaln(r) - gammaln(x(k)+1) + x(k).*(log(l) - log(r+l)) -log(1-exp(lp0));
    end
  end
else
  y = repmat(-Inf,size(x));
  y(l < 0) = NaN;
  y(x==1 & l==0) = 0;

  k = (x >= 1 & x == round(x) & l > 0);
  if (any(k))
    lp0=r(k).*(log(r(k)) - log(r(k)+l(k)));
    y(k)=lp0 + gammaln(r(k)+x(k)) - gammaln(r(k)) - gammaln(x(k)+1) + x(k).*(log(l(k)) - log(r(k)+l(k))) -log(1-exp(lp0));
  end
end


%MPQ_POLY Polygon moments
%
% M = MPQ_POLY(V, P, Q) is the PQ'th moment of the polygon with vertices
% described by the columns of V.
%
% Notes::
% - The points must be sorted such that they follow the perimeter in
%   sequence (counter-clockwise).
% - If the points are clockwise the moments will all be negated, so centroids
%   will be still be correct.
% - If the first and last point in the list are the same, they are considered
%   to be a single vertex.
%
% See also MPQ, NPQ_POLY, UPQ_POLY, Polygon.

% 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/>.

function m = mpq(iv, p, q)
    if ~all(iv(:,1) == iv(:,end))
        %disp('closing the polygon')
        iv = [iv iv(:,1)];
    end
    [nr,n] = size(iv);
    if nr < 2,
        error('must be at least two rows of data')
    end
    x = iv(1,:);
    y = iv(2,:);

    m = 0.0;
    for l=1:n
        if l == 1
            dxl = x(l) - x(n);
            dyl = y(l) - y(n);
        else
            dxl = x(l) - x(l-1);
            dyl = y(l) - y(l-1);
        end
        Al = x(l)*dyl - y(l)*dxl;

        s = 0.0;
        for i=0:p
            for j=0:q
                s = s + (-1)^(i+j) * combin(p,i) * combin(q,j)/(i+j+1) * x(l)^(p-i)*y(l)^(q-j) * dxl^i * dyl^j;
            end
        end
        m = m + Al * s;
    end
    m = m / (p+q+2);

function c = combin(n, r)
%
% COMBIN(n,r)
%   compute number of combinations of size r from set n
%
    c = prod((n-r+1):n) / prod(1:r);


function [p,ll]=parametersfbs(xf,xs,y,wstilda,wstilda2,wstilda3,p)
T=length(xf(1,:));

for t=2:T
ep1(t)=p(7)*normpdf(xs(t),p(1)*xs(t-1)+p(5),sqrt((1+p(6)))*p(2))/(p(7)*normpdf(xs(t),p(1)*xs(t-1)+p(5),sqrt((1+p(6)))*p(2))+(1-p(7))*normpdf(xs(t),p(1)*xs(t-1),p(2)));
ep2(t)=1-ep1(t);
end

e11(T)=(xf(:,T).*xf(:,T))'*wstilda3(:,T);
for t=1:T-1
e21(t)=(xf(:,t+1).*xf(:,t))'*wstilda2(:,t);
e11(t)=(xf(:,t).*xf(:,t))'*wstilda3(:,t);
eyf(t+1)=exp(xf(:,t+1))'*wstilda(:,t+1);
eff(t+1)=exp(2*xf(:,t+1))'*wstilda(:,t+1);
end

esumx2x2p1=sum(e11(2:end).*ep1(2:end));
esumx2x1p1=sum(e21.*ep1(2:end));
esumx1x1p1=sum(e11(1:end-1).*ep1(2:end));
esumx1p1=sum(xs(1:end-1).*ep1(2:end));
esumx2p1=sum(xs(2:end).*ep1(2:end));
esump1=sum(ep1(2:end));

esumx2x2p2=sum(e11(2:end).*ep2(2:end));
esumx2x1p2=sum(e21.*ep2(2:end));
esumx1x1p2=sum(e11(1:end-1).*ep2(2:end));

top=(1/((p(6)+1)*p(2)^2))*(esumx2x1p1-p(5)*esumx1p1)+(1/p(2)^2)*esumx2x1p2;
bot=(1/((p(6)+1)*p(2)^2))*esumx1x1p1+(1/p(2)^2)*esumx1x1p2;
p(1)=top/bot;

first=esumx2x2p1-2*p(5)*esumx2p1+sum(p(5)^2*ep1(2:end))-2*p(1)*esumx2x1p1+2*p(1)*p(5)*esumx1p1+p(1)^2*esumx1x1p1;
second=esumx2x2p2-2*p(1)*esumx2x1p2+p(1)^2*esumx1x1p2;
p(2)=abs(((1/(p(6)+1))*first+second)/(T-1))^.5;

sumyy=sum(y.*y);
sumyfx=sum(y.*eyf);
sumfxfx=sum(eff(2:end));
p(3)=sumyfx/sumfxfx;

p(4)=sqrt(abs(sumyy-2*p(3)*sumyfx+p(3)^2*sumfxfx)/T);

p(5)=abs((esumx2p1-p(1)*esumx1p1)/sum(ep1(2:end)));

p(6)=abs(first/(p(2)^2*esump1)-1);
p(7)=esump1/T;

ll=likelihoodj(y,xs,p(1),p(2),p(3),p(4),p(5),p(6),p(7));


function [x,P]=Kfilter(v,H,R,x_pre,P_pre)

nstate=size(x_pre,1);
K=P_pre*H'*(H*P_pre*H'+R)^-1;
x=x_pre+K*v;
P=(eye(nstate)-K*H)*P_pre;

return


function X = vgg_intersect_quadrics(A, B, C);
%
% function X = vgg_intersect_quadrics(A, B, C);
%
% Purpose:
%   Compute intersections of three quadrics in projective 3-space.
%
% Input:
%   A, B, C are symmetric 4x4 matrices.
%
% Output:
%   X is 4-by-8 complex. Each column is a root.
%
% The function works by converting the problem into an 8x8 generalized
% eigen-problem.
%
% fsm@robots.ox.ac.uk
%

if any(size(A) ~= [4 4])
  error('bad A')
end
if any(size(B) ~= [4 4])
  error('bad B')
end
if any(size(C) ~= [4 4])
  error('bad C')
end

% symmetrize and normalize.
A = A + A.'; A = A / norm(A, 'fro'); a = [A(1, 1) 2*A(1, 2) 2*A(1, 3) 2*A(1, 4) A(2, 2) 2*A(2, 3) 2*A(2, 4) A(3, 3) 2*A(3, 4) A(4, 4)].';
B = B + B.'; B = B / norm(B, 'fro'); b = [B(1, 1) 2*B(1, 2) 2*B(1, 3) 2*B(1, 4) B(2, 2) 2*B(2, 3) 2*B(2, 4) B(3, 3) 2*B(3, 4) B(4, 4)].';
C = C + C.'; C = C / norm(C, 'fro'); c = [C(1, 1) 2*C(1, 2) 2*C(1, 3) 2*C(1, 4) C(2, 2) 2*C(2, 3) 2*C(2, 4) C(3, 3) 2*C(3, 4) C(4, 4)].';

% make multiplication tables (for degrees 1+2=3, 2+2=4 and 3+1=4)
if 1
  ijk = [
    1
    22
    43
    64
    82
    105
    126
    147
    163
    186
    208
    229
    244
    267
    289
    310
    325
    351
    372
    393
    406
    432
    454
    475
    487
    513
    535
    556
    568
    594
    617
    638
    649
    675
    698
    719
    730
    756
    779
    800
    ];
  X12 = zeros(20, 4, 10); X12(ijk) = 1;

  ijk = [
    1
    37
    73
    109
    145
    181
    217
    253
    289
    325
    352
    390
    426
    462
    501
    537
    573
    609
    645
    681
    703
    741
    778
    814
    852
    889
    925
    962
    998
    1034
    1054
    1092
    1129
    1165
    1203
    1240
    1276
    1313
    1349
    1385
    1405
    1446
    1482
    1518
    1561
    1597
    1633
    1669
    1705
    1741
    1756
    1797
    1834
    1870
    1912
    1949
    1985
    2022
    2058
    2094
    2107
    2148
    2185
    2221
    2263
    2300
    2336
    2373
    2409
    2445
    2458
    2499
    2537
    2573
    2614
    2652
    2688
    2726
    2762
    2798
    2809
    2850
    2888
    2924
    2965
    3003
    3039
    3077
    3113
    3149
    3160
    3201
    3239
    3275
    3316
    3354
    3390
    3428
    3464
    3500
    ];
  X22 = zeros(35, 10, 10); X22(ijk) = 1;

  ijk = [
    1
    37
    73
    109
    145
    181
    217
    253
    289
    325
    361
    397
    433
    469
    505
    541
    577
    613
    649
    685
    702
    740
    776
    812
    851
    887
    923
    959
    995
    1031
    1071
    1107
    1143
    1179
    1215
    1251
    1287
    1323
    1359
    1395
    1403
    1441
    1478
    1514
    1552
    1589
    1625
    1662
    1698
    1734
    1772
    1809
    1845
    1882
    1918
    1954
    1991
    2027
    2063
    2099
    2104
    2142
    2179
    2215
    2253
    2290
    2326
    2363
    2399
    2435
    2473
    2510
    2546
    2583
    2619
    2655
    2692
    2728
    2764
    2800
    ];
  X31 = zeros(35, 20, 4); X31(ijk) = 1;

  clear ijk
end

% compose ideal in degree 2.
I2 = [a b c];

% compute ideal in degrees 3 and 4.
I3 = reshape(reshape(X12, [20* 4 10]) * I2, [20 3* 4]);
I4 = reshape(reshape(X22, [35*10 10]) * I2, [35 3*10]);

% the columns of C3 and C4 span complements of I3 and I4, respectively.
[U,S,V] = svd(I3.'); C3 = V(:, 13:20); %W3 = diag(S).'
[U,S,V] = svd(I4.'); C4 = V(:, 28:35); %W4 = diag(S).'

% make the four multiplication operators.
M = cell(1, 4);
for k=1:4
  M{k} = C4.' * X31(:, :, k) * C3;
end

% FIXME: here we only use M{1} and M{2} to compute the Schur
% decomposition although we use all four multiplication operators to
% extract the root coordinates.
[AA, BB, Q, Z] = qz(M{1}, M{2});
for k=1:4
  M{k} = Q*M{k}*Z;
end
%M{:}

X = zeros(4, 8);
X(1, :) = diag(M{1}).';
X(2, :) = diag(M{2}).';
X(3, :) = diag(M{3}).';
X(4, :) = diag(M{4}).';

% normalize
X = X ./ repmat(max(abs(X), [], 1), [4 1]);

return;


function [delta,err] = diffTracker( I0, I1, sig, ss )
% Fast, robust estimation of translation/scale change between two images.
%
% Approximates the translational offset between two images by assuming the
% images lie on linear manifold. Specifically, assumes that if I0 and I1
% are a pair of images related by a translation [dx dy], then (I0+I1)/2 is
% the image exactly halfway between I0 and I1 (ie I0 translated by [dx/2
% dy/2]). The above only holds for small translations and spatially smooth
% images. As such the input images typically need to be spatially smoothed
% first, the amount of necessary smoothing will increase as the size of
% translation increases (experiment for best results). The code is quite
% fast, the bottleneck is the spatial smoothing.
%
% The actual computation is performed as follows. First we generate an
% artificial translation of I0 by 1 pixel in x and y, and store the results
% in Tx and Ty. Also, if ss>1, we generate an artificial scaling Ts of I0
% by upsampling by a factor of ss. The linearity assumption tells us that:
%  I1 = I0 + (I0-Tx) * dx + (I0-Ty) * dy +  (I0-Ts) * ds
% Only dx, dy and possibly ds are unknown in the resulting overcomplete set
% of linear equations, least squares is then used. The error of the
% estimate can be used as a measure of the quality of the linear fit.
%
% This function was inspired by the beautiful work ok Yang et al.:
%   H. Yang, M. Pollefeys, G. Welch, J. Frahm, and A. Ilie. Differential
%   camera tracking through linearizing the local appearance manifold.
%   CVPR, 2007.
%
% USAGE
%  [delta,err] = diffTracker( I0, I1, [sig], [ss] )
%
% INPUTS
%  I0       - reference grayscale double image
%  I1       - translated version of I0
%  sig      - [0] amount of Gaussian spatial smoothing to apply
%  ss       - [0] scale step for artificial scaling (if >1)
%
% OUTPUTS
%  delta    - estimated dx/dy/ds
%  err      - squared error of estimate
%
% EXAMPLE - translation only
%  I = double(imread('cameraman.tif'))/255; dx=3; dy=5;
%  I0=I(1+dy:end,1+dx:end); figure(1); im(I0);
%  I1=I(1:end-dy,1:end-dx); figure(2); im(I1);
%  tic, [delta,err] = diffTracker( I0, I1, 10 ), toc
%
% EXAMPLE - translation and scale
%  I0 = double(imread('coins.png'))/255; dx=9; dy=-2; ds=1.10;
%  H1 = [eye(2)*ds -[dy; dx]; 0 0 1];
%  I1 = imtransform2(I0,H1);
%  tic, [ds,err] = diffTracker( I0, I1, 25, 1.05 ), toc
%  H2 = [eye(2)*ds(3) -[ds(2); ds(1)]; 0 0 1];
%  I2 = imtransform2(I0,H2);
%  figure(1); im(I0); figure(2); im(I1); figure(3); im(I2);
%
% See also
%
% Piotr's Image&Video Toolbox      Version 2.61
% Copyright 2010 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

% get inputs
if(nargin<3 || isempty(sig)), sig=0; end
if(nargin<4 || isempty(ss)), ss=0; end

% smooth images, keep only valid region
if( sig>0 ), f=filterGauss(2*ceil(sig*2.25)+1,[],sig^2);
  I0 = conv2(conv2(I0,f','valid'),f,'valid');
  I1 = conv2(conv2(I1,f','valid'),f,'valid');
end

% I0 translated by 1 pixel both in x and y, crop I0/I1 so dims match
if(ss>1), Ts=arrayToDims(imResample(I0,ss),size(I0)); end
Ty = I0(2:end,1:end-1); Tx = I0(1:end-1,2:end);
I0 = I0(1:end-1,1:end-1); I1 = I1(1:end-1,1:end-1);

% I1 = I0 + (I0-Tx)*dx + (I0-Ty)*dy + (I0-Ts)*ds, recover delta accordingly
dI1=I1(:)-I0(:); dIy=I0(:)-Ty(:); dIx=I0(:)-Tx(:);
if(ss>1), Ts=Ts(1:end-1,1:end-1); dIs=I0(:)-Ts(:); else dIs=[]; end
delta = -[dIx dIy dIs] \ dI1;

% compute squared error (if over certain threshold may wish to discard)
if(nargout>1), err=sum((-[dIx dIy dIs]*delta - dI1).^2) / length(dI1); end

% put scale delta into units independent of ss
if(ss>1), delta(3)=ss^delta(3); end

end


function [e1, e2, E1_l, E2_l] = hmgLinEndpoints(l,t1,t2)

% HMGLINENDPOINTS  HMG line endpoints.
%   HMGLINENDPOINTS(L,T) returns the 3D Euclidean endpoint of the
%   HMG line L at abscissa T.
%
%   [E1,E2] = HMGLINENDPOINTS(L,T1,T2) returns two 3D Euclidean endpoints
%   at abscissas T1 and T2.
%
%   [e1,e2,E1_l,E2_l] = HMGLINENDPINTS(...) returns the Jacobians of the
%   endopints wrt L.
%
%   See also HMGLINSEGMENT, HMGLIN2SEG, HMGLIN2IDPPNTS.

%   Copyright 2009 Teresa Vidal.

if nargout <= 2

    % support segment
    s  = hmgLin2seg(l);

    % support points
    p1 = s(1:3);
    p2 = s(4:6);

    % endpoints
    e1 = (1-t1)*p1 + t1*p2;
    e2 = (1-t2)*p1 + t2*p2;

else

    [s, S_l] = hmgLin2seg(l);       % support segment

    p1 = s(1:3);                    % support points
    p2 = s(4:6);

    P1_l = S_l(1:3,:);              % jacobians of sup points
    P2_l = S_l(4:6,:);

    e1 = (1-t1)*p1 + t1*p2;         % endpoints
    e2 = (1-t2)*p1 + t2*p2;

    E1_p1 = 1-t1;                   % jacobians of endpoints
    E1_p2 = t1;
    E2_p1 = 1-t2;
    E2_p2 = t2;

    E1_l = E1_p1*P1_l + E1_p2*P2_l; % chain rule
    E2_l = E2_p1*P1_l + E2_p2*P2_l;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function bernoulli_number_test ( )

%*****************************************************************************80
%
%% BERNOULLI_NUMBER_TEST tests BERNOULLI_NUMBER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BERNOULLI_NUMBER_TEST\n' );
  fprintf ( 1, '  BERNOULLI_NUMBER computes Bernoulli numbers;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I      Exact        Bernoulli\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, c0 ] = bernoulli_number_values ( n_data );

    if ( n_data == 0 )
      break
    end

    c1(1:n+1) = bernoulli_number ( n );

    fprintf ( 1, '  %2d  %14e  %14e\n', n, c0, c1(n+1) );

  end

  return
end


function lambda = rodman_eigenvalues ( n, alpha )

%*****************************************************************************80
%
%% RODMAN_EIGENVALUES returns the eigenvalues of the RODMAN matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real ALPHA, the parameter.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1:n-1,1) = 1.0 - alpha;

  lambda(n,1) = 1.0 + alpha * ( n - 1 );

  return
end


function g = p35_g ( n, x )

%*****************************************************************************80
%
%% P35_G evaluates the gradient for problem 35.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  factor1 = 0.0;
  df1dx1 = 0.0;
  for i = 1 : 5
    y = i;
    factor1 = factor1 + y * cos ( ( y + 1.0 ) * x(1) + y );
    df1dx1 = df1dx1 - y * ( y + 1.0 ) * sin ( ( y + 1.0 ) * x(1) + y );
  end

  factor2 = 0.0;
  df2dx2 = 0.0;
  for i = 1 : 5
    y = i;
    factor2 = factor2 + y * cos ( ( y + 1.0 ) * x(2) + y );
    df2dx2 = df2dx2 - y * ( y + 1.0 ) * sin ( ( y + 1.0 ) * x(2) + y );
  end

  g(1) = df1dx1 * factor2;
  g(2) = factor1 * df2dx2;

  return
end


function [U]=routh(R1,R2)
%these function make next row of routh hurwits
%R2 , R1 must be vector array or cell vector but both format must be same
%length R2 should be same with R1
%these function use match.m for same dimension
%output is a vector
nu=0;
ce=0;
if isnumeric(R1) && isnumeric(R2)
    a=[];
    nu=1;
elseif iscell(R1) && iscell(R2)
    a=sym;
    ce=1;
else
    nu=0;
    ce=0;
end
m=length(R1);
n=length(R2);
if m<n
    R1=match(R1,R2);
end

if n<m
    R2=match(R2,R1);
end
k=2;

if nu==1
    a(1,1)=R1(1,1);
    a(2,1)=R2(1,1);
elseif ce==1
    a(1,1)=R1{1,1};
    a(2,1)=R2{1,1};
end

for f=1:m
    if k~=f
        if nu==1
            a(1,2)=R1(1,k);
            a(2,2)=R2(1,k);
            U(1,f)=(-det(a))/R2(1,1);
        elseif ce==1
            a(1,2)=R1{1,k};
            a(2,2)=R2{1,k};
            U(1,f)=(-det(a))/R2{1,1};
        end
    end
    if k~=m
        k=k+1;
    end
end

if length(U)<m
    U=match(U,R1);
end


% mean functions to be use by Gaussian process functions. There are two
% different kinds of mean functions: simple and composite:
%
% simple mean functions:
%
%   meanZero      - zero mean function
%   meanOne       - one mean function
%   meanConst     - constant mean function
%   meanLinear    - linear mean function
%   meanPoly      - polynomial mean function
%   meanDiscrete  - precomputed mean for discrete data
%   meanGP        - predictive mean of another GP
%   meanGPexact   - predictive mean of a regression GP
%   meanNN        - nearest neighbor mean function
%
% composite covariance functions (see explanation at the bottom):
%
%   meanScale     - scaled version of a mean function
%   meanPow       - power of a mean function
%   meanProd      - products of mean functions
%   meanSum       - sums of mean functions
%   meanMask      - mask some dimensions of the data
%   meanPref      - difference mean for preference learning
%
% Naming convention: all mean functions are named "mean/mean*.m".
%
%
% 1) With no or only a single input argument:
%
%    s = meanNAME  or  s = meanNAME(hyp)
%
% The mean function returns a string s telling how many hyperparameters hyp it
% expects, using the convention that "D" is the dimension of the input space.
% For example, calling "meanLinear" returns the string 'D'.
%
% 2) With two input arguments:
%
%    m = meanNAME(hyp, x)
%
% The function computes and returns the mean vector where hyp are the
% hyperparameters and x is an n by D matrix of cases, where D is the dimension
% of the input space. The returned mean vector is of size n by 1.
%
% 3) With three input arguments:
%
%    dm = meanNAME(hyp, x, i)
%
% The function computes and returns the n by 1 vector of partial derivatives
% of the mean vector w.r.t. hyp(i) i.e. hyperparameter number i.
%
% See also doc/usageMean.m.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2014-12-08.
%                                      File automatically generated using noweb.


function kern = rbfKernParamInit(kern)

% RBFKERNPARAMINIT RBF kernel parameter initialisation.
% The radial basis function kernel (RBF) is sometimes also known as
% the squared exponential kernel. It is a very smooth non-linear
% kernel and is a popular choice for generic use.
%
% k(x_i, x_j) = sigma2 * exp(-gamma/2 *(x_i - x_j)'*(x_i - x_j))
%
% The parameters are sigma2, the process variance (kern.variance)
% and gamma, the inverse width (kern.inverseWidth). The inverse
% width controls how wide the basis functions are, the larger
% gamma, the smaller the basis functions are.
%
% There is also an automatic relevance determination version of
% this kernel provided.
%
% SEEALSO : rbfardKernParamInit
%
% FORMAT
% DESC initialises the radial basis function
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006
%
% COPYRIGHT : Antti Honkela, 2012

% KERN

kern.inverseWidth = 1;
kern.variance = 1;
kern.nParams = 2;

% Constrains parameters positive for optimisation.
if (isfield(kern,'options')) && ...
   (isfield(kern.options,'boundedParam')) && kern.options.boundedParam,
  for k=1:2,
    kern.transforms(k).index = k;
    kern.transforms(k).type = optimiDefaultConstraint('bounded');
    kern.transforms(k).transformsettings = [0 1e6];
  end
else
  kern.transforms.index = [1 2];
  kern.transforms.type = optimiDefaultConstraint('positive');
end
kern.isStationary = true;
kern.isNormalised = false;


function geometry_test0368 ( )

%*****************************************************************************80
%
%% TEST0368 tests SEGMENT_POINT_DIST_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  test_num = 3;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0368\n' );
  fprintf ( 1, '  SEGMENT_POINT_NEAR_3D computes the nearest point\n' );
  fprintf ( 1, '    from a line segment to a point in 3D.\n' );

  for test = 1 : test_num

    [ p1, seed ] = r8vec_uniform_01 ( dim_num, seed );
    [ p2, seed ] = r8vec_uniform_01 ( dim_num, seed );
    [ p, seed ] = r8vec_uniform_01 ( dim_num, seed );

    [ pn, dist, t ] = segment_point_near_3d ( p1, p2, p );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  TEST = %d', test );
    fprintf ( 1, '  P1 =   %12f  %12f  %12f\n', p1(1:dim_num) );
    fprintf ( 1, '  P2 =   %12f  %12f  %12f\n', p2(1:dim_num) );
    fprintf ( 1, '  P =    %12f  %12f  %12f\n', p(1:dim_num) );
    fprintf ( 1, '  PN =   %12f  %12f  %12f\n', pn(1:dim_num) );
    fprintf ( 1, '  DIST = %12f\n', dist );
    fprintf ( 1, '  T =    %12f\n', t );

  end

  return
end


function lambda = fourier_eigenvalues ( n )

%*****************************************************************************80
%
%% FOURIER_EIGENVALUES returns the eigenvalues of the FOURIER matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, complex LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1,1) = 1.0;

  lambda(2:4:n,1) = - 1.0;
  lambda(3:4:n,1) =   i;
  lambda(4:4:n,1) =   1.0;
  lambda(5:4:n,1) = - i;

  return
end


close all
clear all
clc

x = -pi:.1:pi;
y = sin(x);
p = plot(x,y)
set(gca,'XTick',-pi:pi/2:pi)
set(gca,'XTickLabel',{'-pi','-pi/2','0','pi/2','pi'})
xlabel('-\pi \leq \Theta \leq \pi')
ylabel('sin(\Theta)')
title('Simulation Results')
text(-pi/4,sin(-pi/4),'\leftarrow sin(-\pi\div4)',...
     'HorizontalAlignment','left')
set(p,'Color','red','LineWidth',2)

latex_fig(10, 3, 1.5)


function minDist = distancePointPolygon(point, poly)
%DISTANCEPOINTPOLYGON Shortest distance between a point and a polygon
%
%   DIST = distancePointPolygon(POINT, POLYGON)
%   Computes the shortest distance between the point POINT and the polygon
%   given by POLYGON. POINT is a 1-by-2 row vector, and POLYGON is a N-by-2
%   array containing vertex coordinates.
%   The distance is computed as the minimal distance to the boundary edges.
%
%   Example
%     % Computes the distance between a point and a square
%     square = [0 0; 10 0;10 10;0 10];
%     p0 = [16 3];
%     distancePointPolygon(p0, square)
%     ans =
%          6
%
%   See also
%   polygons2d, points2d, distancePointPolyline, distancePointEdge,
%   projPointOnPolyline
%

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

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

% call to distancePointPolyline
minDist = distancePointPolyline(point, poly);


function cvx_optpnt = complex_lorentz( sx, dim )

%COMPLEX_LORENTZ   Complex second-order cone.
%   COMPLEX_LORENTZ(N), where N is a positive integer, creates a column
%   variable of length N and a scalar variable, and constrains them
%   to lie in a second-order cone. That is, given the declaration
%       variable x(n) complex
%       variable y
%   the constraint
%       {x,y} == complex_lorentz(n)
%   is equivalent to
%       norm(x,2) <= y
%   The inequality form is more natural, and preferred in most cases. But
%   in fact, the COMPLEX_LORENTZ set form is used by CVX itself to convert
%   complex NORM()-based constraints to solvable form.
%
%   COMPLEX_LORENTZ(SX,DIM), where SX is a valid size vector and DIM is a
%   positive integer, creates an array variable of size SX and an array
%   variable of size SY (see below) and applies the second-order cone
%   constraint along dimension DIM. That is, given the declarations
%       sy = sx; sy(min(dim,length(sx)+1))=1;
%       variable x(sx) complex
%       variable y(sy)
%   the constraint
%       {x,y} == complex_lorentz(sx,dim)
%   is equivalent to
%       norms(x,2,dim) <= y
%   Again, the inequality form is preferred, but CVX uses the set form
%   internally. DIM is optional; if it is omitted, the first non-singleton
%   dimension is used.
%
%   LORENTZ(SX,DIM,CPLX) creates real second-order cones if CPLX is FALSE,
%   and complex second-order cones if CPLX is TRUE. The latter case is
%   equivalent to COMPLEX_LORENTZ(SX,DIM).
%
%   Disciplined convex programming information:
%       LORENTZ is a cvx set specification. See the user guide for
%       details on how to use sets.

error( nargchk( 1, 2, nargin ) );
if nargin == 1,
    cvx_optpnt = lorentz( sx, [], true );
else
    cvx_optpnt = lorentz( sx, dim, true );
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Some examples using the tools in this distribution.
%%% Eero Simoncelli, 2/97.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Load an image, and downsample to a size appropriate for the machine speed.
oim = pgmRead('einstein.pgm');
tic; corrDn(oim,[1 1; 1 1]/4,'reflect1',[2 2]); time = toc;
imSubSample = min(max(floor(log2(time)/2+3),0),2);
im = blurDn(oim, imSubSample,'qmf9');
clear oim;

%%% ShowIm:
%% 3 types of automatic graylevel scaling, 2 types of automatic
%% sizing, with or without title and Range information.
help showIm
clf; showIm(im,'auto1','auto','Al')
clf; showIm('im','auto2')
clf; showIm(im,'auto3',2)

%%% Statistics:
mean2(im)
var2(im)
skew2(im)
kurt2(im)
entropy2(im)
imStats(im)

%%% Synthetic images.  First pick some parameters:
sz = 200;
dir = 2*pi*rand(1)
slope = 10*rand(1)-5
int = 10*rand(1)-5;
orig = round(1+(sz-1)*rand(2,1));
expt = 0.8+rand(1)
ampl = 1+5*rand(1)
ph = 2*pi*rand(1)
per = 20
twidth = 7

clf;
showIm(mkRamp(sz,dir,slope,int,orig));
showIm(mkImpulse(sz,orig,ampl));
showIm(mkR(sz,expt,orig));
showIm(mkAngle(sz,dir));
showIm(mkDisc(sz,sz/4,orig,twidth));
showIm(mkGaussian(sz,(sz/6)^2,orig,ampl));
showIm(mkZonePlate(sz,ampl,ph));
showIm(mkAngularSine(sz,3,ampl,ph,orig));
showIm(mkSine(sz,per,dir,ampl,ph,orig));
showIm(mkSquare(sz,per,dir,ampl,ph,orig,twidth));
showIm(mkFract(sz,expt));

%%% Point operations (lookup tables):
[Xtbl,Ytbl] = rcosFn(20, 25, [-1 1]);
plot(Xtbl,Ytbl);
showIm(pointOp(mkR(100,1,[70,30]), Ytbl, Xtbl(1), Xtbl(2)-Xtbl(1), 0));

%%% histogram Modification/matching:
[N,X] = histo(im, 150);
[mn, mx] = range2(im);
matched = histoMatch(rand(size(im)), N, X);
showIm(im + sqrt(-1)*matched);
[Nm,Xm] = histo(matched,150);
nextFig(2,1);
  subplot(1,2,1); plot(X,N); axis([mn mx 0 max(N)]);
  subplot(1,2,2);  plot(Xm,Nm); axis([mn mx 0 max(N)]);
nextFig(2,-1);

%%% Convolution routines:

%% Compare speed of convolution/downsampling routines:
noise = rand(400); filt = rand(10);
tic; res1 = corrDn(noise,filt(10:-1:1,10:-1:1),'reflect1',[2 2]); toc;
tic; ires = rconv2(noise,filt); res2 = ires(1:2:400,1:2:400); toc;
imStats(res1,res2)

%% Display image and extension of left and top boundaries:
fsz = [9 9];
fmid = ceil((fsz+1)/2);
imsz = [16 16];

% pick one:
im = eye(imsz);
im = mkRamp(imsz,pi/6);
im = mkSquare(imsz,6,pi/6);

% pick one:
edges='reflect1';
edges='reflect2';
edges='repeat';
edges='extend';
edges='zero';
edges='circular';
edges='dont-compute';

filt = mkImpulse(fsz,[1 1]);
showIm(corrDn(im,filt,edges));
line([0,0,imsz(2),imsz(2),0]+fmid(2)-0.5, ...
     [0,imsz(1),imsz(1),0,0]+fmid(1)-0.5);
title(sprintf('Edges = %s',edges));

%%% Multi-scale pyramids (see pyramids.m for more examples,
%%% and explanations):

%% A Laplacian pyramid:
[pyr,pind] = buildLpyr(im);
showLpyr(pyr,pind);

res = reconLpyr(pyr, pind); 		% full reconstruction
imStats(im,res);			% essentially perfect

res = reconLpyr(pyr, pind, [2 3]);  %reconstruct 2nd and 3rd levels only
showIm(res);

%% Wavelet/QMF pyramids:
filt = 'qmf9'; edges = 'reflect1';
filt = 'haar'; edges = 'qreflect2';
filt = 'qmf12'; edges = 'qreflect2';
filt = 'daub3'; edges = 'circular';

%[pyr,pind] = buildWpyr(im, 5-imSubSample, filt, edges);
[pyr,pind] = buildWpyr(im, 'auto', filt, edges);
showWpyr(pyr,pind,'auto2');

res = reconWpyr(pyr, pind, filt, edges);
clf;
showIm(im + i*res);
imStats(im,res);

res = reconWpyr(pyr, pind, filt, edges, 'all', [2]);  %vertical only
clf;
showIm(res);

%% Steerable pyramid:
%[pyr,pind] = buildSpyr(im,4-imSubSample,'sp3Filters');
[pyr,pind] = buildSpyr(im,'auto','sp3Filters');
showSpyr(pyr,pind);

%% Steerable pyramid, constructed in frequency domain:
%[pyr,pind] = buildSFpyr(im,5-imSubSample,4);  %5 orientation bands
[pyr,pind] = buildSFpyr(im);  %5 orientation bands
showSpyr(pyr,pind);
res = reconSFpyr(pyr,pind);
imStats(im,res);


% The following vectorized Matlab code implements Algorithm 1 from the
% paper
% Projection onto the probability simplex: An efficient algorithm with a
% simple proof, and an application, by W. Wang and M.A. Carreira-Perpinan,
% see https://arxiv.org/abs/1309.1541
%
% It projects each column vector in the D N matrix Y onto the probability
% simplex in D dimensions.

function X = SimplexColProj(Y)

Y = Y';
[N,D] = size(Y);
X = sort(Y,2,'descend');
Xtmp = (cumsum(X,2)-1)*diag(sparse(1./(1:D)));
X = max(bsxfun(@minus,Y,Xtmp(sub2ind([N,D],(1:N)',sum(X>Xtmp,2)))),0);
X = X';