function [AA, aa, pp, rankA, p] = rowReduce(A, a, mode, printLevel)
% Eliminates dependent rows from `A` & `a` where :math:`A x = a`
%
% USAGE:
%
%    [AA, aa, pp, rankA, p] = rowReduce(A, a)
%
% INPUT:
%    A:        from :math:`A x = a`
%
% OPTIONAL INPUT:
%    a:        from :math:`A x = a`
%
%    mode:     If mode=1, LUSOL operates on A itself.
%              If mode=2, LUSOL operates on A'.
%
%    printLevel
%
% OUTPUT:
%    AA:       row reduced `A`
%    aa:       row reduced `a` i.e. `aa = a(pp)`
%    pp:       1:rankA indices of independent rows
%    rankA:    rank of `A`
%    p:        row permutation which leaves first `1:rankA` rows independent and
%              last rows dependent
%
% .. Author: - Ronan Fleming, with linear algebra advice from Michael Saunders
%            Dept of Management Science and Engineering (MS&E) Stanford University

if ~exist('a','var') %create a if not provided
    a=sparse(size(A,1),1);
else
    if size(A,1)~=length(a)
        error('Dimensions of A and a are inconsistent');
    end
end

if ~exist('mode','var')
    mode=1;
end

if ~exist('printLevel','var')
    printLevel=1;
end

[mlt,nlt]=size(A);
archstr = computer('arch');
archstr = lower(archstr);
%archstr='';%bypass until issue with lusol tolerance sorted.
switch archstr
    case {'glnx86','glnxa64','maci64'}
        %Eliminate dependent rows
        [AA,aa,p,rankA] = lusolCondense(A,a,mode,printLevel-1);
        if ~(nnz(A)>0 && rankA==0) && printLevel>0
            fprintf('%s',['Eliminated ' int2str(mlt-rankA) ' dependent rows, using lusol.']);
        end
        pp=p(1:rankA);
        %case {'PCWIN','PCWIN64'}
    otherwise
        [AA, aa, pp, rankA, p] = qrRowReduce(A,a, printLevel);
end

if nnz(A)>0 && rankA==0
    %backup in case something has gone wrong with lusolCondense
    [AA, aa, pp, rankA, p] = qrRowReduce(A, a, printLevel);
end

fprintf('\n')
end

function [AA, aa, pp, rankA, p] = qrRowReduce(A, a,  printLevel)
    [mlt,nlt]=size(A);
    A=full(A);
    %Eliminate dependent rows
    [Q,R,P] = qr(A');
    [p,q,s] = find(P);
    s      = diag(R);
    tol    = 1e-8;
    rankA  = length(find(abs(s) > tol));
    % nnz(abs(diag(R))>1.e-15)
    AA = sparse(A(p(1:rankA),:));
    pp = p(1:rankA);
    aa = sparse(a(pp));
    if  printLevel>0
             fprintf('%s',['Eliminated ' int2str(mlt-rankA) ' dependent rows, using qr factorisation.']);
    end
end


%% Trend-Cycle tutorial: Predicting recessions
% Authors:   Filippo Ferroni and  Fabio Canova
% Date:     27/05/2020, revised  15/12/2020

close all; clc; clear all;

addpath ../../cmintools/
addpath ../../bvartools/

% predicting  recession and the start of  a recession in the  euro  area.
% use recession indicator constructed with example_2_dating.m; regressors
% labor  productivity, commodity prices, long term  rates  or spread.

% Euro area AWM DATABASE: Quarterly
    [a,b,~] = xlsread('awm19up18.xlsx');
    % names of variables
    varnames = b(1,2:end);

    % time convention: Q1 = .00 and Q4 =0.75
    time = 1970 : .25 : 2017.75;
    time_start = find(time==1970.50);
    time_start1 = find(time==1999.50);
    time_end   = find(time==2017.75);
    time_break   = find(time==2007.75);

    % The  CREDIT DATA: Quartely
    [e,f,~]=xlsread('ECB_Credit_gaps.xlsx');
    varnames2= f(1,2:end);

    % real GDP, real consumption, real  investment, GDP defl, HICP,
    % short and long term interest rate, commodity  prices,
    % labor  productivity, total  employment, credit to NFC to  GDP
    yy = [a(:,1) a(:,2) a(:,4) a(:,7) a(:,19) a(:,33) a(:,34) ...
        a(:,35) a(:,42) a(:,29) e(time_start-2:time_end,25)];
    % names: YER PCR ITR YED HICP STN LTN COMPR LPROD LNN

    % data transformations:
    % 1. the log of output, consumption,investment
    ddata(:,1:3) = log(yy(time_start+1 : time_end,1:3));
    % 2. the log/log difference of GDP deflator index and CPI
    %ddata(:,4:5) = log(yy(time_start+1  : time_end,4:5));
    ddata(:,4:5) = diff(log(yy(time_start : time_end,4:5)))*400;
    % 3. the level of short and long term interest rate
    ddata(:,6:7) = yy(time_start +1 : time_end,6:7);
    % 4. the log/log difference of commodity prices
    ddata(:,8) = log(yy(time_start+1  : time_end,8));
    %ddata(:,8) = diff(log(yy(time_start : time_end,8)))*400;
    % 5. the taking the log of labor  productivity and  employment
    ddata(:,9:10) = log(yy(time_start+1 : time_end,9:10));
    %ddata(:,9) = diff(log(yy(time_start+1 : time_end,9)));
    %ddata(:,10) = log(yy(time_start+1 : time_end,10));

    % 6. spread
    ddata(:,11)= ddata(:,7)-ddata(:,6);
    %  data for  credit  to  GDP starts  only  at  time_start1 (1999:25)
    ddata(:,12) = yy(time_start+1:time_end,11);
    endd=length(ddata);

 % cc=ones(time_end-time_start,1);  % constant
 % pick log labor productivity, log commodity prices, long term rate
 % z=ddata(:,[9 8 7]);
 % pick log labor productivity, log commodity prices, spread
 z=ddata(:, [9 8 11]);

load  Eurorec
% recind has  the  recession indicator  created  with  dating_exa.m
x=recind;

% use contemporanous  values
% [estimator,cov_Hessian,ME1,ME2,ME_std] = probit(x,z);

% use  one  period lagged  values
zz=zeros(size(z,1), size(z,2));
zz(2:length(z),:)=z(1:length(z)-1,:);
timeplot = time(time_start:time_end-1);

% Estimate Probit model and the marginal effects
[estimator,cov_Hessian,ME1,ME2,ME1_std] = probit(x,zz);

% prediction
pz=[ones(length(zz),1) zz];
px=normpdf(pz*estimator);
half=0.5*ones(length(z),1);

plot(timeplot,px,'r-.','Linewidth',2); hold on;
plot(timeplot,half,'b:','Linewidth',2); hold  on;
plot(timeplot,x,'k-','Linewidth',2); hold  off; axis  tight;
legend('predition','halfline', 'actual')
pause

close  all;

% do  estimation recursively: predicting  the  beginning  of  a  recession
% which is  the  same  as  predicting  a  peak
% recession dates
%rec1b = find(time==1974.00);
%rec1e = find(time==1975.25);
%[estimator1,cov_Hessian1,ME11,ME21,ME11_std] = probit(x(1:rec1b),zz(1:rec1b,:));

rec2b = find(time==1980.00);
rec2e = find(time==1984.25);
disp('Predicting 1980 recession')
[estimator2,cov_Hessian2,ME12,ME22,ME12_std] = probit(x(1:rec2b),zz(1:rec2b,:));
pz2=[ones(rec2b,1) zz(1:rec2b,:)];
px2=normpdf(pz2*estimator2);
half2=0.5*ones(rec2b,1);

plot(timeplot(1:rec2b),x(1:rec2b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec2b),half2(1:rec2b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec2b),px2(1:rec2b),'r-.','Linewidth',2); hold  off;axis tight;
legend('actual', 'halfline', 'predition')
pause

close all;

rec3b = find(time==1992.00);
rec3e = find(time==1993.75);
disp('Predicting  1992 recession')
[estimator3,cov_Hessian3,ME13,ME23,ME13_std] = probit(x(1:rec3b),zz(1:rec3b,:));
pz3=[ones(rec3b,1) zz(1:rec3b,:)];
px3=normpdf(pz3*estimator3);
half3=0.5*ones(rec3b,1);

plot(timeplot(1:rec3b),x(1:rec3b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec3b),half3(1:rec3b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec3b),px3(1:rec3b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline','predition')
pause
close all;

rec4b = find(time==2001.00);
rec4e = find(time==2002.75);
disp('Predicting  2001 recession')
[estimator4,cov_Hessian4,ME14,ME24,ME14_std] = probit(x(1:rec4b),zz(1:rec4b,:));
pz4=[ones(rec4b,1) zz(1:rec4b,:)];
px4=normpdf(pz4*estimator4);
half4=0.5*ones(rec4b,1);

plot(timeplot(1:rec4b),x(1:rec4b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec4b),half4(1:rec4b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec4b),px4(1:rec4b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline','predition')
pause
close all;

rec5b = find(time==2008.00);
rec5e = find(time==2009.50);
disp('Predicting 2008 recession')
[estimator5,cov_Hessian5,ME15,ME25,ME15_std] = probit(x(1:rec5b),zz(1:rec5b,:));
pz5=[ones(rec5b,1) zz(1:rec5b,:)];
px5=normpdf(pz5*estimator5);
half5=0.5*ones(rec5b,1);

plot(timeplot(1:rec5b),x(1:rec5b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec5b),half5(1:rec5b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec5b),px5(1:rec5b),'r-.','Linewidth',2); hold  off; axis tight;
legend('actual', 'halfline', 'predition')
pause
close all;

rec6b = find(time==2011.25);
rec6e = find(time==2013.00);
disp('Predicting  2011 recession')
[estimator6,cov_Hessian6,ME16,ME26,ME16_std] = probit(x(1:rec6b),zz(1:rec6b,:));
pz6=[ones(rec6b,1) zz(1:rec6b,:)];
px6=normpdf(pz6*estimator6);
half6=0.5*ones(rec6b,1);

plot(timeplot(1:rec6b),x(1:rec6b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec6b),half6(1:rec6b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec6b),px6(1:rec6b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline', 'predition')

return


% By Roger Aarenstrup, roger.aarenstrup@mathworks.com
% 2006-08-18
%
% This is a state-space DC motor model of a
% Maxon RE25 10 Watt, precious metal brushes, 118743
%
% This model also have a weak connection to a load.
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%            The full dc motor model                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vin is the input voltage to the motor
% i is the motor current
% th_m is the rotor angle, theta
% dth_m is the rotor angular velocity sometimes called omega
% th_l is the load angle
% dth_l is the load angular velocity

% Controller Sample Time
Ts = 100e-6;

% PARAMETERS DC MOTOR
Rm  = 2.06;             % Motor resistance (ohm)
Lm  = 0.000238;         % motor inductance (Henrys)
Kb = 1/((406*2*pi)/60); % Back EMF constant (Volt-sec/Rad)
Kt = 0.0235;            % Torque constand (Nm/A)
Jm = 1.07e-6;           % Rotor inertia (Kg m^2)
bm = 12e-7;             % MEchanical damping (linear model of
                        % friction: bm * dth)
% PARAMETERS LOAD
Jl = 10.07e-6;   % Load inertia (10 times the rotor)
bl = 12e-6;      % Load damping (friction)
Ks = 100;        % Spring constant for connection rotor/load
b = 0.0001;      % Spring damping for connection rotor/load

% SYSTEM MATRICES
%
% States:  [i dth_m th_m dth_l th_l]'
% Input:   Vin the motor voltage
% Outputs: same as states
%
Afull = [-Rm/Lm    -Kb/Lm       0       0       0;
         Kt/Jm   -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
           0          1         0       0        0;
           0         b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
           0          0         0       1        0];

Bfull = [1/Lm 0 0 0 0]';

Cfull = [0 1 0 0 0;
         0 0 1 0 0;
         0 0 0 1 0;
         0 0 0 0 1];

Dfull = [0 0 0 0]';

sys_full = ss(Afull, Bfull, Cfull, Dfull);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     The reduced dc motor model for current control      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% SYSTEM MATRICES
%
% States:  [dth_m th_m dth_l th_l]'
% Input:   I The current to the dc motor
% Outputs: same as states
%
Ared = [ -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
              1         0       0        0;
             b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
              0         0       1        0];

Bred = [Kt/Jm 0 0 0]';

Cred = eye(4);

Dred = [0 0 0 0]';

sys_red = ss(Ared, Bred, Cred, Dred);

% Discrete version of the model (as seen from the controller)
sys_red_d = c2d(sys_red, Ts, 'zoh');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        State-space controller design attempt 1          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole1 = exp(-2*pi*Ts*[20 22 24 26]);

% Calculate the control parameters
L1 = place(sys_red_d.a, sys_red_d.b, dpole1);

% Keep the static gain of the closed loop system to 1
F=feedback(sys_red_d, L1);   % The closed loop system, F.
Kstatic = freqresp(F, 0);    % Get the static gain of F.
Kstat = 1/Kstatic(4);        % It is the fourth output (load position)

% The above commands requires toolboxes, incase you don't have them
% you can simulate the system with a unit step is see the output static
% gain. Kstat will be the inverse of that.

% to verify the pole placement
%pzmap(F);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 2 - Integrator   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole2 = exp(-2*pi*Ts*[20 22 24 26 5]);

% We only consider one output here
Cred_one = [0 1 0 0];

% Calculate the control parameters
Aint = [sys_red_d.a [0 0 0 0]';
        -Cred_one 1];

Bint= [sys_red_d.b' 0]';

Cint = [Cred_one 0];

Dint = [0]';

sys_int_d2 = ss(Aint, Bint, Cint, Dint, Ts);

% Controllability VS Observability analysis
%ob = obsv(sys_int_d2.a, sys_int_d2.c);
%cr = ctrb(sys_int_d2.a, sys_int_d2.b);
%rank(ob)
%rank(cr)

[L2, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole2);

% Feed Forward gain
Kff = L2(5)/(dpole2(5) - 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 3 - Observer     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% These should be about twice as fast as the state feedback
% for the controller.
dpole3 = exp(-2*pi*Ts*[1400 1450 1500 1550]);
%dpole3 = [-0.3 -0.33 -0.35 -0.37];

% Recalculate the reduced model with one ouput
Cred2 = [0 1 0 0];

Dred2 = 0;

sys_red2 = ss(Ared, Bred, Cred2, Dred2);

% Discrete version of the model (as seen from the controller)
sys_red_d2 = c2d(sys_red2, Ts, 'zoh');

set(sys_red_d2, 'inputname', {'Um'}, ...
                'outputname', {'Enc_out'});

% Calculate the observer state feedback
K = place(sys_red_d2.a', sys_red_d2.c', dpole3);

% Put the observer together
Aobs = [sys_red_d2.a-K'*sys_red_d2.c];
Bobs = [sys_red_d2.b K'];
Cobs = eye(4);
Dobs = [0 0 0 0; 0 0 0 0]';

observer = ss(Aobs, Bobs, Cobs, Dobs, Ts, 'inputname', {'Ue', 'Enc_in'}, ...
              'outputname', {'dth_m', 'th_m', 'dth_l', 'th_l'});

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 4 - The Servo Case  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Kinv = (Jl+Jm)/Kt;

dpole4 = exp(-2*pi*Ts*[200 204 220 224 300]);
[L3, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole4);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function pathS = MC_VG_S(S0,r,d,T,nu,theta,sigma,NTime,NSim,NBatches)
% discretization of Variance Gamma process
% using subordination

pathS = zeros(NSim,NTime+1,NBatches);       % create the output
lnS = zeros(NSim,NTime+1);                  % used per batch
dT = T / NTime;                             % delta time
omegaT = -1/nu * log(1-theta(1)*nu ...
    - nu*sigma(1)^2/2);                     % martingale correction
lnS(:,1) = log(S0);                         % Set the starting spot price

for l = 1 : NBatches                        % batch loop
    % G = nu * gamrnd(dT/nu,1,NSim,NTime);
    % dW = randn(NSim,NTime);
    for m=2:NTime+1                         % time loop
        G = nu * gamrnd(dT/nu,1,NSim,1);      % Gamma subordinator
        dW = randn(NSim,1);                 % Gaussians
        lnS(:,m) = lnS(:,m-1) ...           % log VG
            + (r-d-omegaT) * dT ...
            + theta(1) * G + sqrt(G) * sigma .* dW;
    end

    pathS(:,:,l) = exp(lnS);                % simulated paths
end


%% demo code
% reconstruction of retrospective subsampled Shepp-Logan phantom

% set paths
addpath(genpath(pwd));
addpath(genpath(['..',filesep,'CS_LAB_matlab']));

%% ------------------------------------------
% 1.) load data: phantom (2 slices, 1 channel)
% -------------------------------------------
dImg = phantom('Modified Shepp-Logan',256);
dKSpace = fftnshift(dImg,1:2);

%% --------------------------
% 2.) create subsampling mask
% ---------------------------
if(ispc), sExt = '.exe'; else sExt = ''; end
system(['.',filesep,'sampling',filesep,'Subsample',sExt,sprintf(' %d %d %d %d', 256, 1, 4, 1)]);
iMask = readSamplingMaskFromFile(['.',filesep,'samplingPattern.txt']);

%% -------------------------
% 3.) apply subsampling mask
% --------------------------
% k-Space dimensions
% cell array: NSlice x NChannels x NRepetitions x NAverages
%             each cell: 2D: yPhase x xFreq x (nTime/nPhases)
%                        3D: yPhase x xFreq x zPhase
%                        4D: yPhase x xFreq x zPhase x nTime/nPhases
dKSpaceSub = {dKSpace .* repmat(iMask,[1 256])};

%% --------------------
% 4.) reconstruct image
% ---------------------
% set some recon parameters
para.cstype = 'FOCUSS';
para.transformation = 'fft';
para.lambda = 1e-12;
para.lambdaTV = 1e-6;
para.measPara.dim = [size(dKSpaceSub{1}), 1, 1, 1];
para.measPara.LCall = ones(1,4);
para.measPara.dimension = '2D';
para.espresso.state = false;
para.espresso.direction = 'off';
para.espresso.pfn = 1;
para.flagOversampling = logical([0 0 0]);
para.postproc.turnImage = false;
para.postproc.FreqOversamplingCorr = false;
para.prop.flagPlot = false;

% CS reconstruction
dCSRecon = CS_reconstruction(dKSpaceSub, para);

% zero-padded reconstruction
dZeropadded = abs(ifftnshift(dKSpaceSub{1}));

%% ------------------
% 5.) compare results
% -------------------
figure;
subplot(1,3,1)
imagesc(dImg); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('original')
subplot(1,3,2)
imagesc(dZeropadded); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('zero-padded recon')
subplot(1,3,3)
imagesc(dCSRecon); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('CS recon')


% compute bidirectional phase offsets from poor line scanning timing
function BiDiPhase = BiDiPhaseOffsets(data)

[Ly, Lx, nplanes, NT] = size(data);

% lines scanned one direction
yr1 = 2:2:floor(Ly/2)*2;
% lines scanned in other direction
yr2 = 1:2:floor(Ly/2)*2;

% compute phase correlation between lines in x-direction
eps0 = single(1e-6);
Nmax = min(50, NT);
d1 = fft(data(yr1,:,:,1:Nmax),[],2);
d2 = conj(fft(data(yr2,:,:,1:Nmax),[],2));
d1 = d1./(abs(d1) + eps0);
d2 = d2./(abs(d2) + eps0);

cc = ifft(d1 .* d2,[],2);
cc = fftshift(cc, 2);
cc = mean(mean(mean(cc,1),3),4);

% max shift of +/-5 pixels
[cx, ix] = max(cc(floor(Lx/2)+1 + [-5:5]));
ix       = ix - (6);

BiDiPhase = -1 * ix;


function xfm = xfmPoints(x,y)

% add third dimension
x = [x zeros(size(x,1),1)];
x(1,3) =1;
y = [y ones(size(y,1),1)];

% linear tranformation
b = pinv(y)*x;
xfm = b;

% plot results
if ~nargout,
   figure;
   subplot(2,1,1);hold on;
   plot(x(:,1),x(:,2),'bo');
   plot(y(:,1),y(:,2),'rx');
   title('original data');
   legend('x','y');

   % interpolate
   yi=y*b;
   subplot(2,1,2);hold on;
   plot(x(:,1),x(:,2),'bo');
   plot(yi(:,1),yi(:,2),'rx');
   title('original data');
   legend('x','y interpolated');
end

return


function [L1, S1, statsPCP] = fastpcp(V, lambda, loops, rank0, rankThreshold, lambdaFactor )

if( nargin < 6 )
  lambdaFactor = 1.0;
  if( nargin < 5 )
    rankThreshold = 0.01;
    if( nargin < 4 )
      rank0 = 1;                % initial rank
      if( nargin < 3 )
        loops = 2;              % total number of outer loops
      end
    end
  end
end

if isempty(rankThreshold)
	rankThreshold = 0.01;
end

if isempty(lambdaFactor)
	lambdaFactor = 1.0;
end

% Data size
%[Nrows,Ncols] = size(V);

% Set flag (increments the rank plus one at each iteration)
inc_rank = 1;

% ---------------------------------
% >>>  measure time performance <<<
% ---------------------------------
%t = tic;

% ------------------------
% --- First outer loop ---
  rank = rank0;                     % current rank
  statsPCP.rank(1) = rank;          % save current rank

  % Partial SVD
  %[Ulan Slan Vlan] = lansvd(V, rank, 'L');
  %[Ulan,Slan,Vlan] = svds(V, rank);
  [Ulan,Slan,Vlan] = svdsecon(V, rank); % fastest

  % Current low-rank approximation
  L1 = Ulan*Slan*Vlan';

  % Shrinkage
  S1 = shrink(V-L1, lambda);

% ------------------------
% ---    Outer loops   ---

for k = 2:loops,

  if(inc_rank == 1)
     lambda = lambda * lambdaFactor;         % modify Lambda at each iteration
     rank = rank + 1;                        % increase rank
  end

  % low rank (partial SVD)
  %[Ulan Slan Vlan] = lansvd(V-S1, rank, 'L');
  %[Ulan,Slan,Vlan] = svds(V-S1, rank);
  [Ulan,Slan,Vlan] = svdsecon(V-S1, rank); % fastest

  currentEvals = diag(Slan);                                    % extract current evals
  statsPCP.rank(k) = length( currentEvals );                    % save current rank
  statsPCP.rho(k) = currentEvals(end) / sum( currentEvals(1:end-1) );       % relative contribution of the last evec

  % simple rule to keep or increase the current rank's value
  if(statsPCP.rho(k) < rankThreshold )
     inc_rank = 0;
  else
     inc_rank = 1;
  end

  % Current low-rank approximation
  L1 = Ulan*Slan*Vlan';

  % Shrinkage
  S1 = shrink(V-L1, lambda);
end

% ---------------------------------
% >>>  measure time performance <<<
% ---------------------------------
%statsPCP.time = toc(t);

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

function u = shrink(v, lambda)
  u = sign(v).*max(0, abs(v) - lambda);
return


function [Y, sel] = vl_colsubset(X,n,varargin)
% VL_COLSUBSET Select a given number of columns
%   Y = VL_COLSUBSET(X, N) returns a random subset Y of N columns of
%   X. The selection is order-preserving and without replacement. If N
%   is larger or equal to the number of columns of X (e.g. N = Inf),
%   then the function returns all the columns (i.e., Y = X).
%
%   If 0 < N < 1, then the function returns a fraction N of the
%   columns (rounded to the closest integer).
%
%   [Y, SEL] = VL_COLSUBSET(...) returns the indexes SEL of the
%   selected columns.
%
%   The function accepts the following options:
%
%   Beginning::
%     Returns the fist N columns.
%
%   Ending::
%     Returns the last N columns.
%
%   Random:: [default]
%     Returns N columns selected at random (using RANDPERM()).
%
%   Uniform::
%     Returns N uniformly spaced columns.
%
%   Largest::
%     Returns the N largest columns (using SORTROWS()).
%
%   Smallest::
%     Returns the N smallest columns (using SORTROWS()).
%
%  See also: VL_HELP().

% Authors: Andrea Vedaldi

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if nargin < 2, n = 1 ; end

mode = 'random' ;
i = 1 ;
while i <= length(varargin)
  switch lower(varargin{i})
    case {'beginning', ...
          'ending', ...
          'random', ...
          'uniform', ...
          'largest', ...
          'smallest'}
      mode = lower(varargin{1}) ;
      i = i + 1 ;
    otherwise
      error('Unknown option ''%s''.', varargin{i}) ;
  end
end

m = size(X,2) ;

if n < 0, error('N must not be smaller than 0.') ; end
if n ~= round(n)
  if n > 1
    error('N must be a natural number, +inf, or a fraction in 0 and 1.') ;
  end
  n = round(m * n) ;
end

n = min(m,n) ;

switch mode
  case 'random'
    perm = randperm(m) ;
    sel  = sort(perm(1:n)) ;
  case 'beginning'
    perm = 1:m ;
    sel  = sort(perm(1:n)) ;
  case 'ending'
    perm = m:-1:1 ;
    sel  = sort(perm(1:n)) ;
  case 'uniform'
    if n < 1
      sel = [] ;
    else
      sel = round(linspace(1, m, min(m,n))) ;
    end
  case 'largest'
    [drop, perm] = sortrows(X') ;
    sel = sort(perm(end-n+1:end)) ;
  case 'smallest'
    [drop, perm] = sortrows(X') ;
    sel = sort(perm(1:n)) ;
end

Y = X(:, sel) ;


%IPROFILE Extract pixels along a line
%
% V = IPROFILE(IM, P1, P2) is a vector of pixel values extracted from the
% image IM (HxWxP) between the points P1 (2x1) and P2 (2x1).  V (NxP) has
% one row for each point along the line and the row is the pixel value
% which will be a vector for a multi-plane image.
%
% [P,UV] = IPROFILE(IM, P1, P2) as above but also returns the coordinates of
% the pixels for each point along the line.  Each row of UV is the pixel
% coordinate (u,v) for the corresponding row of P.
%
% Notes::
% - The Bresenham algorithm is used to find points along the line.
%
% See also BRESENHAM, ILINE.

% 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 [p,uv] = iprofile(c, p1, p2)

    if nargin == 2
        % p1 is a set of points, in columns
        p = [];
        for i=2:numcols(p1)
            p = [p; iprofile(c, p1(:,i-1), p1(:,i))];
        end
        return
    end
    % coordinates must be integers
    p1 = round(p1); p2 = round(p2);

    points = bresenham(p1, p2);

    p = [];
    for point = points'
        p = [p; c(point(2), point(1), :)];
    end

    if nargout > 1
        uv = squeeze(points');
    end


%% DEMO_visualization_von_mises_plasticity_01
% Below is a demonstration for:
%
% * Visualization of a Von Mises yield surface in 3D

%%

clear; close all; clc

%% Plot settings.

fontSizeAxis=25;
fontSizeText=35;
colorSet=viridis(3);
lineWidth1=1;
lineWidth2=3;
lineWidthAxis=3;
f=2;

%% Control parameters

sy=1; %Yield stress
tau_octahedral=sy*sqrt(2/3); %Octahedral shear stress = cylinder radius

%%
% Create ellipsoids
t=linspace(0,2*pi,1000); %Angular coordinates
x=tau_octahedral*cos(t);
y=tau_octahedral*sqrt(3).*sin(t);
z=zeros(size(x));
v=[x(:) y(:) z(:)]; %Ellipse coordinates

%Make cylinder "slices" ellipses by rotating above ellipse
R12=euler2DCM([0 0 (45/180)*pi]);
v12=v*R12;

R13_1=euler2DCM([0 (90/180)*pi 0]);
R13_2=euler2DCM([(135/180)*pi 0 0]);
v13=v*R13_1*R13_2;

R23_1=euler2DCM([(90/180)*pi 0 0]);
R23_2=euler2DCM([0 (-45/180)*pi 0 ]);
v23=v*R23_1*R23_2;

%%
% Creating cylinder data

% Creating input structure
inputStruct.cylRadius=tau_octahedral;
inputStruct.numRadial=250;
inputStruct.cylHeight=2*sqrt(2)*tau_octahedral;
inputStruct.numHeight=10;
inputStruct.meshType='quad';
[F,V,C]=patchcylinder(inputStruct);
Q=euler2DCM([0 pi/2 0]);
V=V*Q;

R1=euler2DCM([0 asin(1/sqrt(3)) 0]);
R2=euler2DCM([0 0 -(45/180)*pi]);
V=V*R1*R2;

[x1,y1]=meshgrid(-f*sy:1:f*sy);
z1=zeros(size(x1));

[x2,z2]=meshgrid(-f*sy:1:f*sy);
y2=zeros(size(x2));

[y3,z3]=meshgrid(-f*sy:1:f*sy);
x3=zeros(size(y3));

%%
%Visualize
hf=cFigure; hold on;

h4=gpatch(F,V,'rw','none',0.5);
surf(x1,y1,z1,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);
surf(x2,y2,z2,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);
surf(x3,y3,z3,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);

h1=quiverVec([0 0 0],[1 0 0],sy,colorSet(1,:));
h2=quiverVec([0 0 0],[0 1 0],sy,colorSet(2,:));
h3=quiverVec([0 0 0],[0 0 1],sy,colorSet(3,:));

h5=plotV([-1 -1 -1; 1 1 1],'k--','LineWidth',lineWidth2); %Hydrostatic axis

h6=plotV(v12,'r-','LineWidth',lineWidth2);
h6.Color=(colorSet(1,:)+colorSet(2,:))/2;

h7=plotV(v13,'r-','LineWidth',lineWidth2);
h7.Color=(colorSet(1,:)+colorSet(3,:))/2;

h8=plotV(v23,'r-','LineWidth',lineWidth2);
h8.Color=(colorSet(2,:)+colorSet(3,:))/2;

hAxis=gca;
hAxis.XRuler.FirstCrossoverValue  = 0; % X crossover with Y axis
hAxis.YRuler.FirstCrossoverValue  = 0; % Y crossover with X axis
hAxis.ZRuler.FirstCrossoverValue  = 0; % Z crossover with X axis
hAxis.XRuler.SecondCrossoverValue = 0; % X crossover with Z axis
hAxis.YRuler.SecondCrossoverValue = 0; % Y crossover with Z axis
hAxis.ZRuler.SecondCrossoverValue = 0; % Z crossover with Y axis

text(0.5+f*sy,0,0,'$\sigma_1$','Interpreter','Latex','FontSize',fontSizeText);
text(0,0.5+f*sy,0,'$\sigma_2$','Interpreter','Latex','FontSize',fontSizeText);
text(0,0,0.5+f*sy,'$\sigma_3$','Interpreter','Latex','FontSize',fontSizeText);

xticks(-f*sy:0.5:f*sy);
yticks(-f*sy:0.5:f*sy);
zticks(-f*sy:0.5:f*sy);

legend([h1 h2 h3 h4 h5 h6 h7 h8],{'$\sigma_1$ axis','$\sigma_2$ axis','$\sigma_3$ axis',...
                           'Von Mises yield surface',...
                           'Hydrostatic line',...
                           'yield ellipse $\sigma_3=0$','yield ellipse $\sigma_2=0$','yield ellipse $\sigma_1=0$'...
                           },'Interpreter','Latex');

axis tight; axis equal; axis vis3d; view(3); %box on;
camlight headlight;
axis(f*sy*[-1 1 -1 1 -1 1])
set(gca,'FontSize',fontSizeAxis,'LineWidth',lineWidthAxis);
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Loading 3D Datasets for Manifold Learning  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Authors:
% Alberto Arrighi   (alberto.arrighi@epfl.ch)
% Ilaria Lauzana    (ilaria.lauzana@epfl.ch)
% Ludovico Novelli  (ludovico.novelli@epfl.ch)
%
% From Spring Semester Advanced Machine Learning Mini-Project
% Ecole Polytechnique Federale de Lausanne

%% GENERATE DIFFERENT DATASETS FOR MANIFOLD LEARNING EXAMPLES
%% 1) Swissroll

% Options
dataset_options                    = [];
dataset_options.numberOfPoints     = 2000;
dataset_options.name               = 'swissroll';
dataset_options.plot               = false;

% Generate dataset
[X, labels, updated_N, cmap] = ml_generate_manifold_dataset(dataset_options);

% Plot original data
plot_options            = [];
plot_options.is_eig     = false;
plot_options.points_size = 30;
plot_options.cmap       = cmap;
plot_options.title      = 'Swiss Roll';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);
axis equal

%% 2) Swissroll with hole

% Options
dataset_options                    = [];
dataset_options.numberOfPoints     = 2000;
dataset_options.name               = 'swissroll_hole';
dataset_options.plot               = false;

% Generate dataset
[X, labels, updated_N, cmap] = ml_generate_manifold_dataset(dataset_options);

% Plot original data
plot_options            = [];
plot_options.is_eig     = false;
plot_options.points_size = 30;
plot_options.cmap       = cmap;
plot_options.title      = 'Swiss Roll w/Hole';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);
axis equal

%% 3) S-curve

% Options
dataset_options                    = [];
dataset_options.numberOfPoints     = 2000;
dataset_options.name               = 'scurve';
dataset_options.plot               = false;

% Generate dataset
[X, labels, updated_N, cmap] = ml_generate_manifold_dataset(dataset_options);

% Plot original data
plot_options            = [];
plot_options.is_eig     = false;
plot_options.points_size = 30;
plot_options.cmap       = cmap;
plot_options.title      = 'S-Curve';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);

%% 4) S-curve non-homogeneous density

% Options
dataset_options                    = [];
dataset_options.numberOfPoints     = 2000;
dataset_options.name               = 'scurve_mixed_density';
dataset_options.plot               = false;

% Generate dataset
[X, labels, updated_N, cmap] = ml_generate_manifold_dataset(dataset_options);

% Plot original data
plot_options             = [];
plot_options.is_eig      = false;
plot_options.points_size = 30;
plot_options.cmap        = cmap;
plot_options.title       = 'S-Curve Mixed';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);

%% 5) Half sphere

% Options
dataset_options                    = [];
dataset_options.numberOfPoints     = 2000;
dataset_options.name               = 'sphere_hole';
dataset_options.plot               = false;

% Generate dataset
[X, labels, updated_N, cmap] = ml_generate_manifold_dataset(dataset_options);

% Plot original data
plot_options             = [];
plot_options.is_eig      = false;
plot_options.points_size = 30;
plot_options.cmap        = cmap;
plot_options.title       = 'Half-Sphere';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);

%% 6) Broken Swissroll

% Options
options                 = [];
options.numberOfPoints  = 500;
options.name            = 'swissroll_broken';
options.plot            = false;

% Generate dataset
[X,labels,updated_N,cmap] = ml_generate_manifold_dataset(options);

% Plot original data
plot_options             = [];
plot_options.is_eig      = false;
plot_options.points_size = 30;
plot_options.labels      = labels;
plot_options.title       = 'Sphere w/Hole';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_data(X,plot_options);


function varargout = equationPoints(disc)
%EQUATIONPOINTS   Points at which collocation is enforced.
%   In CHEBCOLLOC2, functions are discretized at 2nd kind points but equations
%   are enforced at 1st kind points, to avoid duplication at boundaries.

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

pointsFun = @(n) chebtech1.chebpts(n);
[varargout{1:nargout}] = valsDiscretization.points(disc, pointsFun);

end


function x = project_consensus(varargin)
% PROJECT_CONSENSUS    Bring a set of points into consensus.
%
%   project_consensus(v1,v2,...,vn) returns the elementwise
%   average of v1, ..., vn, i.e., the projection of the vi
%   onto the consensus set.

    N = length(varargin);
    x = zeros(size(varargin{1}));
    for i = 1:N
        x = x + varargin{i};
    end
    x = x./N;
end


function [MW] = TW2MW(TW)
% Convert power from terawatts to megawatts.
% Chad A. Greene 2012
MW = TW*1000000;


%s = rand('twister');
%rand('twister', s);

len = 200;              % number of 1st level observations (time points within subjects)
sub = 20;               % number of second-level units (subjects)

fixed_slope = 0.5;      % population fixed-effect slope
fixed_int = 0.3;        % pop. fixed-effect intercept

rand_slope = 0.5;         % 'true' random effect standard dev: slope
rand_int = 0.5;           % 'true' random effect standard dev: intercept

noise_sigma = 0.5;      % measurement error/unexplained std of 1st level observations
between_sigma = 0.5;    % measurement error/unexplained std of 2nd level covariate

% Generate simulated data
% 1) create predictors
x = zeros(len,sub);
y = x;
x(11:20,:) = 1;
x(111:120,:) = 1;

% 2) Create instances of effects for each subject
c = normrnd(fixed_slope, rand_slope, sub, 1);       % slope between-subjects variations
d = normrnd(fixed_int, rand_int, sub, 1);                  % intercept between-subjects variations

% 3) Create a between-ss covariate that is related to the slope but not the
% intercept
covt = scale(c + normrnd(0, between_sigma, sub, 1), 1);

% 4) Create a between-ss covariate that is related to the intercept but not the
% intercept
covti = scale(d + normrnd(0, between_sigma, sub, 1), 1);

corrcoef([c d covt covti])

% Add between-subjects error (random effects) and measurement noise
% (within-subjects error)

for i=1:sub
    y(:,i) = d(i) + c(i) .* x(:,i) + normrnd(0, noise_sigma, len, 1);
end

%% Run the model - without a 2nd-level covariate
out = igls(y, x);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with an unrelated 2nd-level covariate
out = igls(y, x, 'covariate', (1:20)');  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with a 2nd-level covariate related to slope
out = igls(y, x, 'covariate', covt);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with a 2nd-level covariate related to intercept
out = igls(y, x, 'covariate', covti);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%%
clear *pvals* *fpr*

mytype = 'i';
iterations = 200;

len = 100; sub = 10;

fixpvals = zeros(iterations, 2);
randpvals = zeros(iterations, 2);
LRTrandpvals = zeros(iterations, 2);
chic = zeros(iterations, 1);
chid = zeros(iterations, 1);

c_fixed = 0;  % slope pop. average
d_fixed = 0;   % intercept pop. average
c_rand = 0;    % slope std across Ss
d_rand = 0;    % intercept std.
noise_std = 2.0;  % within-subjects noise std

x = zeros(len,sub);
y = x;
x(1:2:end,:) = 1;                   % create signal

% --------------------------------------------------------------------------
% This block: No true random effects, so test false positive rate for
% p_randvariance

verbstr = 'noverbose';

for i = 1:iterations

    c = normrnd(c_fixed,c_rand,sub,1);       % slope between-subjects variations
    d = normrnd(d_fixed,d_rand,sub,1);         % intercept between-subjects variations

    % Add between-subjects error (random effects) and measurement noise
    % (within-subjects error)
           y = zeros(len, sub);
    for s = 1:sub
        y(:,s) = d(s) + c(s) .* x(:,s) + normrnd(0,noise_std,len,1);
    end
    out = igls(y, x, 'type', mytype, verbstr);  % for igls

    fpr_converged(i) = out.isconverged;

    fpr_betastar(i, :) = out.betastar';

    fixpvals(i, :) = out.p;
    randpvals(i, :) = out.p_randvariance'; %[out.p_randvariance_d out.p_randvariance_c];

    LRTrandpvals(i, :) = out.pLRT_randvariance';

    if mod(i, 10) == 0, fprintf(1, '%3.0f ', i); end

    verbstr = 'noverbose';
end
fprintf('\n')

%pvals_fpr = pvals;
alph = .1;

fixfpr =  sum(fixpvals < alph) ./ iterations;
randfpr =  sum(randpvals < alph) ./ iterations;
LRTfpr =  sum(LRTrandpvals < alph) ./ iterations;

fprintf('\tfixed\t\trand fx\t\t\n');
fprintf('TPR/FPR @ alpha = %3.3f:\t%3.5f\t%3.5f\t%3.5f\t%3.5f\t%3.5f\t%3.5f\n', alph, fixfpr, randfpr, LRTfpr);


figure;
% Load dataset
load('maneuvering_robot.mat');
truth = [TrueTrack.Trajectory.Vector];
obs = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',100^2,'Mapping',[1 3]);
%measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[0.001,0.02],'Mapping',[1 3]);

%% Cartesian
% subplot(1,2,1);
hold on;
truth_100 = truth.*100;
%h1 = plot(ax(1),measurement(1,k),measurement(2,k),'k*','MarkerSize', 10);
measurement = obs.feval(truth_100, true);
meas = obs.finv([measurement(1,:);measurement(2,:)]);
h1 = plot(meas(obs.Mapping(1),:),meas(obs.Mapping(2),:),'r+','MarkerSize', 15);
h2 = plot(truth_100(1,1:end),truth_100(3,1:end),'k--','LineWidth',2);
h3 = plot(truth_100(1,1),truth_100(3,1),'ko','MarkerSize', 20, 'MarkerFaceColor','Green');
h4 = plot(truth_100(1,end),truth_100(3,end),'ko','MarkerSize', 20, 'MarkerFaceColor','Red');
h5 = plot(0,0,'rd','MarkerSize', 20, 'MarkerFaceColor','Blue');
legend([h1,h2,h3,h4,h5],'Measurements','Trajectory', 'Start', 'End','Radar','Location','southeast')
str = sprintf('Vessel trajectory');
title(str)
xlabel('X (m)')
ylabel('Y (m)')
axis([0,2500,0,1500])
box on

%% Polar
% subplot(1,2,2);
% truth2meas = obs.heval(truth_100(1:2,:));
% % [a,b]=obs.heval(truth_100(1:2,:));
% %h1 = polarplot(a(x_start),b(y_start),'ko','LineWidth',2,'MarkerSize',15,'MarkerFaceColor','w');hold on;
% polarplot(truth2meas(1,:),truth2meas(2,:),'k-','LineWidth',2,'MarkerSize',10,'MarkerFaceColor','w');hold on;
% thetalim([0 90])
% %h2 = polarplot(a(x_end),b(y_end),'k^','LineWidth',2,'MarkerSize',15,'MarkerFaceColor','w');hold on;
% h3 = polarplot(measurement(1,:),100*measurement(2,:),'+r','MarkerSize',15);
% pax = gca;
% pax.ThetaAxisUnits = 'radians';
% %rlabel('Range (m)');
% %thetalabel('Bearing (rad)');


classdef OrthogonalLeastSquares < handle
    %CSOLSAPPROX Implements OLS algorithm for sparse approximation

    properties
        % Maximum residual norm
        MaxResNorm = 1e-4
        % Indicates if we should stop on exceeding residual norm
        StopOnResidualNorm = true
        % Indicates if we should stop when residual norm stops improving
        StopOnResNormStable = true
        % Maximum number of iterations for approximation
        MaxIters
        Verbose = false
        % Minimum Sparsity
        MinK = 4
        % Ignored atom (which won't be considered in identification step)
        IgnoredAtom = -1
    end

    properties(SetAccess=private)
        % The dictionary
        Dict
        % Ambient signal dimensions
        N
        % Number of atoms in dictionary
        D
        % Sparsity level of representations (may be negative)
        K
        % Result of a solver
        result
    end

    methods
        function self  = OrthogonalLeastSquares(Dict, K)
            % We assume that all the columns in dictionary are normalized.
            if isa(Dict, 'spx.dict.Operator')
                self.Dict = Dict;
            elseif ismatrix(Dict)
                self.Dict = spx.dict.MatrixOperator(Dict);
            else
                error('Unsupported operator.');
            end
            [self.N, self.D] = size(Dict);
            if ~exist('K', 'var')
                % No sparsity level has been pre-specified.
                K = -1;
            end
            self.K = K;
            % Maximum number of iterations
            maxIter = self.N;
            if K > 0
                % We have to consider pre-specified sparsity level
                maxIter = K;
            end
            self.MaxIters = maxIter;
            if K > 0 && self.MinK >= K
                self.MinK = 0;
            end
        end

        function result  = solve(self,y)
            % Uses QR factorization to implement the OLS

            % Initialization
            % Solves approximation problem using OLS
            d = self.D;
            n = self.N;
            r = y;
            min_k  = self.MinK;
            dict = self.Dict;
            % Active indices
            omega = [];
            if self.StopOnResNormStable
                oldResNorm = norm(r);
            end
            maxIter = self.MaxIters;
            % Create space for storing the Q R factors
            Q = zeros(n, maxIter);
            R = zeros(maxIter);
            % zr stores projections of y along the directions
            % in Q.
            % Since vectors in Q are orthonormal,
            % hence computing projections is as easy as
            % taking inner product.
            % This also simplifies the process of updating
            % the residual as the residual is orthogonal
            % to the vectors selected in Q so far.
            zr = [];

            ignored_atom = self.IgnoredAtom;
            % Convert the dictionary into a matrix
            dict = double(dict);

            for iter=1:maxIter
                % The Q part of previous iteration
                QLast = Q(:, 1:iter-1); % n * (k - 1)
                % The Orthogonal Projector
                OrthoProjector = eye(n) -  QLast * QLast';
                % Orthogonalize the remaining atoms of the dictionary
                qdict = OrthoProjector * dict;
                % Normalize the columns
                qdict = spx.norm.normalize_l2(qdict);
                % Compute inner products
                innerProducts = qdict' * r;
                % Mark the inner products of already selected columns as 0.
                innerProducts(omega) = 0;
                innerProducts = abs(innerProducts);
                if ignored_atom > 0
                    % forcefully ignore this atom
                    innerProducts(ignored_atom) = 0;
                end
                % Find the highest inner product
                [~, index] = max(innerProducts);
                % Add this index to support
                omega = [omega, index];
                % Pick the new atom from the dictionary
                new_atom = dict(:, index);
                % Orthogonalize the new atom
                %Compute projections to previously selected vectors in Q
                projections = QLast'* new_atom;
                % Remove the projection
                new_q = new_atom - QLast * projections;
                % Normalize
                norm_q = norm(new_q);
                new_q = new_q / norm_q;
                % Place it
                Q(:, iter) = new_q;
                % Update R
                R(1:iter-1, iter) = projections;
                R(iter, iter) = norm_q;
                % Compute the projection of y on new q.
                zr(iter) = new_q' * y;
                % Let us update the residual.
                r = r - zr(iter) * new_q;
                if self.StopOnResidualNorm || self.StopOnResNormStable
                    resNorm = norm(r);
                    if resNorm < self.MaxResNorm && iter > min_k
                        break;
                    end
                    if self.StopOnResNormStable
                        change = abs(oldResNorm  - resNorm);
                        if change/oldResNorm < .01  && iter > min_k
                            % No improvement
                            break;
                        end
                    end
                end
            end
            % Estimate
            z = zeros(d, 1);
            % We need to use back-substitution  with R to get z from zr.
            opts.UT = true;
            tmp = linsolve(R(1:iter,1:iter), zr(1:iter)', opts);
            z(omega)= tmp;
            % Solution vector
            result.z = z;
            % Residual obtained
            result.r = r;
            % residual norm
            result.rnorm = resNorm;
            % Number of iterations
            result.iterations = iter;
            % Solution support
            result.support = omega;
            self.result = result;
        end

    end

end


function [error,Reallignedsource,transform]=ICPmanu2weigthed(target,source,d2)

% This function rotates, translates and scales a 3D pointcloud "source" of N*3 size (N points in N rows, 3 collumns for XYZ)
% to fit a similar shaped point cloud "target" again of N by 3 size with a
% specifc weigth given to a surface are on the source mesh defined by d2
%
% The output shows the minimized value of dissimilarity measure in "error", the transformed source data set and the
% transformation, rotation, scaling and translation in transform.T, transform.b and transform.c such that
% Reallignedsource = b*source*T + c;

index=1;
[errortemp(index,:),Reallignedsourcetemp,transform]=ICPmanu_allignweigthed(target,source,d2);
d=errortemp(index,:);

[errortemp(index+1,:),Reallignedsourcetemp,transform]=ICPmanu_allignweigthed(target,Reallignedsourcetemp,d2);
index=index+1;
d=errortemp(index,:);

while ((errortemp(index-1,:)-errortemp(index,:)))>0.0000001
[errortemp(index+1,:),Reallignedsourcetemp,transform]=ICPmanu_allignweigthed(target,Reallignedsourcetemp,d2);
index=index+1;
d=errortemp(index,:);

end

error=errortemp(index,:);
[d,Reallignedsource,transform] = procrustes(Reallignedsourcetemp,source);


function [mi3] = cc2mi3(cc)
% Convert volume from cubic centimeters to cubic miles.
% Chad Greene 2012
mi3 = cc*2.3991275858e-16;


function Phi = dipole (z,z0,S,Beta)
Phi = S*exp(i*Beta)./(2*pi*(z-z0));


%  Figure 10.26      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_26.m is a script to generate Fig. 10.26,
%  root locus of the colocated
%  design for the satellite with PD compensation

% parameter values
m=[1, .1]; k0=[0, .091] ; d0=[0, .0036]; k1=[0, .4];

% call function
[f,g,h,j]=twomass(m,k0,d0); [f1,g,h,j] = twomass(m,k1,d0);
h1=[0, 0, 1, 0];

nc1=0.25*[2, 1];
dc1=[1/40, 1];

[ac,bc,cc,dc]=tf2ss(nc1, dc1);
sysc=ss(ac, bc,cc,dc);
sysp1=ss(f,g,h1,j);
sysol= series(sysc,sysp1);
[aol,bol,col,dol]=ssdata(sysol);
[acl]=aol-bol*col;
ccl2=[0*cc h];
sysp11=ss(f1,g,h1,j);
sysol1= series(sysc,sysp11);
[aol1,bol1,col1,dol1] =ssdata(sysol1);
acl1=aol1-bol1*col1;
hold off ; clf
rlocus(aol,bol,col,dol);
v =[-3,2,-1.5,1.5];
axis(v);
title('Fig. 10.26 Root locus for D_5(s)G_{co}(s).')
grid


% created: Zoya Bylinskii, Aug 2014

% This finds the normalized scanpath saliency between two different
% saliency maps as the mean value of the normalized saliency map at
% fixation locations.

function score = NSS(saliencyMap, fixationMap)
% saliencyMap is the saliency map
% fixationMap is the human fixation map (binary matrix)
map = double(imresize(saliencyMap,size(fixationMap)));

% normalize saliency map
map = (map - mean(map(:)))/std(map(:));

% mean value at fixation locations
score = mean(map(logical(fixationMap)));


function [ score ] = findNearbyObject( all_xyz, obj_xyz )
%FINDNEARBYOBJECT Summary of this function goes here
%   Detailed explanation goes here
N = size(all_xyz,1);

lowxyz = max(all_xyz(:,1:3), repmat(obj_xyz(1:3),N,1));
topxyz = min(all_xyz(:,4:6), repmat(obj_xyz(4:6),N,1));
inter = prod(max(topxyz-lowxyz, 0.01), 2);
area1 = prod(max(all_xyz(:,4:6)-all_xyz(:,1:3), 0.01), 2);
area2 = repmat(prod(max(obj_xyz(4:6)-obj_xyz(1:3), 0.01), 2), N, 1);

score = inter./(area1+area2-inter);
valid = any(topxyz-lowxyz<=0,2);
score(valid) = 0;

end


function [ posiout ] = labposition( posiin )
posiout(1)=236+158*posiin(2);
posiout(2)=5444-158*posiin(1);

end


function fem1d_bvp_linear_test02 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_LINEAR_TEST02 carries out test case #2.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/fem1d_bvp_linear/fem1d_bvp_linear_test02.m
%
%  Discussion:
%
%    Use A2, C2, F2, EXACT2, EXACT_UX2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_LINEAR_TEST02\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A2(X)  = 1.0\n' );
  fprintf ( 1, '  C2(X)  = 2.0\n' );
  fprintf ( 1, '  F2(X)  = X * ( 5 - X ) * exp ( X )\n' );
  fprintf ( 1, '  U2(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_linear ( n, @a2, @c2, @f2, x );

  uexact = exact2 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8e\n', ...
      i, x(i), u(i), uexact(i), abs ( u(i) - uexact(i) ) );
  end

  e1 = l1_error ( n, x, u, @exact2 );
  e2 = l2_error_linear ( n, x, u, @exact2 );
  h1s = h1s_error_linear ( n, x, u, @exact_ux2 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s  );

  return
end
function value = a2 ( x )

%*****************************************************************************80
%
%% A2 evaluates A function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c2 ( x )

%*****************************************************************************80
%
%% C2 evaluates C function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 2.0;

  return
end
function value = exact2 ( x )

%*****************************************************************************80
%
%% EXACT2 evaluates exact solution #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux2 ( x )

%*****************************************************************************80
%
%% EXACT_UX2 evaluates the derivative of exact solution #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f2 ( x )

%*****************************************************************************80
%
%% F2 evaluates right hand side function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = x .* ( 5.0 - x ) .* exp ( x );

  return
end


function fval = f1(x)
% Unimodal function f_1

Bound=[-100 100];

if nargin == 0
    fval = Bound;
else
    fval = sum(x.^2);
end


function varargout = projPointOnPolyline(point, poly, varargin)
%PROJPOINTONPOLYLINE Compute position of a point projected on a polyline.
%
%   POS = projPointOnPolyline(POINT, POLYLINE)
%   Compute the position of the orthogonal projection of a point on a
%   polyline.
%   POINT is a 1-by-2 row vector containing point coordinates
%   POLYLINE is a N-by-2 array containing coordinates of polyline vertices
%   POS is the position of the point on the polyline, between 0 and the
%   number of vertices of the polyline. POS can be a non-integer value, in
%   this case, the integer part corresponds to the polyline edge index
%   (between 0 and Nv-1), and the floating-point part corresponds to the
%   relative position on i-th edge (between 0 and 1, 0: edge start, 1: edge
%   end).
%
%   When POINT is an array of points, returns a column vector with as many
%   rows as the number of points.
%
%   POS = projPointOnPolyline(POINT, POLYLINE, CLOSED)
%   Specifies if the polyline is closed or not. CLOSED can be one of:
%     'closed' -> the polyline is closed
%     'open' -> the polyline is open
%     a column vector of logical with the same number of elements as the
%       number of points -> specify individually if each polyline is
%       closed (true=closed).
%
%   [POS, DIST] = projPointOnPolyline(...)
%   Also returns the distance between POINT and POLYLINE.
%
%   Example
%     poly = [10 10; 20 10;20 20;10 20];
%     projPointOnPolyline([15 0], poly)
%     ans =
%         0.5000
%     projPointOnPolyline([0 16], poly)
%     ans =
%         3.0000
%
%   See also
%   points2d, polygons2d, polylinePoint, projPointOnPolygon
%   distancePointPolyline
%

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

% check if input polyline is closed or not
closed = false;
if ~isempty(varargin)
    var = varargin{1};
    if strcmp('closed', var)
        closed = true;
    elseif strcmp('open', var)
        closed = false;
    elseif islogical(var)
        closed = var;
    end
end

% closes the polyline if necessary
if closed
    poly = [poly ; poly(1,:)];
end

% number of points
Np = size(point, 1);

% allocate memory results
pos     = zeros(Np, 1);
minDist = inf*ones(Np, 1);

% iterate on points
for p = 1:Np
    % build set of edges
    edges = [poly(1:end-1, :) poly(2:end, :)];

    % compute distance between current point and all edges
    [dist, edgePos] = distancePointEdge(point(p, :), edges);

    % update distance and position if necessary
    [minDist(p), edgeIndex] = min(dist);
    pos(p) = edgeIndex - 1 + edgePos(edgeIndex);
end

% process output arguments
if nargout <= 1
    varargout{1} = pos;
elseif nargout == 2
    varargout{1} = pos;
    varargout{2} = minDist;
end


%GIKINE Shoulder inverse kinematics of HAL-like right shoulder
%
% Computes the inverse kinematics of the right shoulder which is the
% same kinematically as a HAL object. This function is mainly useful
% for procedures which require many, many calls, so time can be saved
% by not referencing a HAL object.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Syntax:
%  (1) [q1, q2] = gikine(Tg, Tu)
%
% Outputs:
%  q1 : First family of solutions (mx3 matrix where m = size(Tu,3))
%  q2 : Second family of solutions (mx3 matrix where m = size(Tu,3))
%
% Inputs:
%  Tg : Transformation matrix of the shoulder frame. x, y, z point to
%        the right, above, and behind the person. Translation is
%        shoulder center of rotation
%  Tu : Transformation matrix of the upper arm frame. May be a 4x4xm
%        series of frames (or higher order, which is compressed to 3D)
%
% See also HAL HAL.gikine HAL.ikine wikine

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
%% pHRIWARE 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.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 199q3-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license,
% Modified 16/7/2014 (HAL is a subclass of SerialLink)

function [q1, q2] = gikine(Tg, Tu)

Rg = Tg(1:3,1:3);
gRu0 = [0 0 -1; 0 1 0; 1 0 0]';
u0R = (Rg*gRu0)';

u0Ru = reshape(u0R*reshape(Tu(1:3,1:3,:),3,[]),3,3,[]);

%-X,Z,Y Tait-Bryan angles

% Family 1:
q1(:,1) = squeeze(atan2(-u0Ru(3,2,:),u0Ru(2,2,:)));
q1(:,2) = squeeze(asin(-u0Ru(1,2,:)));
q1(:,3) = squeeze(atan2(-u0Ru(1,3,:),u0Ru(1,1,:)));

% Make -pi values pi so that they are deemed within joint limits
fix = q1 <= (-pi+1e-4);
q1(fix) = pi;

% Family 2:
q2(:,1) = squeeze(atan2(u0Ru(3,2,:),-u0Ru(2,2,:)));
q2(:,2) = pi-q1(:,2);
q2(:,3) = squeeze(atan2(u0Ru(1,3,:),-u0Ru(1,1,:)));
end


function [azimuth, dist]=indirectRhumbProblem(latLonStart,latLonEnd,height,useHeightApprox,a,f,numSteps4Circ)
%INDIRECTRHUMBPROBLEM Given a starting and ending latitude and longitude on
%                     a reference ellipsoid, determine the heading (in
%                     radians East of North) and the distance one must
%                     travel on the shortest constant-heading course to go
%                     from the starting point to the stopping point. A
%                     constant heading course follows a rhumb line
%                     (loxodrome) and is usually not the shortest path
%                     between two points.
%
%INPUTS: latLonStart A 2X1 vector of the starting ellipsoidal latitude
%                    (North) and longitude (East) in radians. This cannot
%                    be a pole.
%          latLonEnd A 2X1 vector of the ending ellipsoidal latitude
%                    (North) and longitude (East) in radians.
%             height The height above the reference ellipsoid at which the
%                    trajectory should be determined. This changes the
%                    distance traveled, but not the azimuthal angle of
%                    departure. If this parameter is omitted, then the
%                    default value of 0 is used.
%    useHeightApprox If true, and the height is not zero, then an
%                    approximation is made for how dist scales with
%                    altitude. Specifically, an equiatorial trajectory will
%                    scale as (a+height)/a. Thus, this scaling factor is
%                    applied to any trajectory to scale dist with altitude.
%                    If height is false, a significantly slower iterative
%                    optimization technique is used. The default value is
%                    true. The difference made when useHeightApprox=false is
%                    can generally be assumed to be less than 80m. This
%                    parameter is ignored if height=0.
%                  a The semi-major axis of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84SemiMajorAxis is used.
%                  f The flattening factor of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84Flattening is used.
%      numSteps4Circ If height!=0 then an algorithm propagating a state
%                    in ECEF coordinates around the curved Earth is used to
%                    solve the direct geodetic problem as a step in solving
%                    the indirect geodetic problem. This parameter
%                    determines the number of steps that would be needed in
%                    the direct geodetic problem for a target that
%                    circumnavigates the globe around the equator. The
%                    default value if this parameter is not provided is
%                    2000. A value of 6000 appears to be about the best
%                    number for overall precision. Reducing the number of
%                    steps will speed up the function. This parameter is not
%                    used if height=0.
%
%OUTPUTS: azimuth The constant heading in radians East of North that one
%                 must travel to go on a constant-heading course from
%                 latLonStart to latLonEnd.
%            dist The distance that one must travel on a constant-heading
%                 course to go from latLonStart to latLonEnd.
%
%If height=0, the algorithm is mostly taken from [1]. However, a formula
%using isometric latitudes, which are described in Chapter 3 of [2] to get
%the azimuth angle was used, because it is simpler. The formula is also
%explicitly mentioned in Equation 3 of [3]. However, the expression for
%computing the distance from that paper is only for a sphere, not for an
%ellipsoid, which is why the Carlton-Wippern distance computation using an
%incomplete elliptic integral of the second kind is preferred.
%
%When the azimuth found by the technique is very close to +/-pi/2 (when one
%is traveling at nearly a constant latitude), the distance computation
%switches to assume that the latitude is indeed constant even if
%latLonStart(1)!=latLonStart(2). This avoid precision problems that arise
%as a very small number is multiplied by a very large number. However, this
%reduces the accuracy of the method.
%
%Generally, calling directRhumbProblem or directRhumbProbGen with the
%azimuth and dist returned by this function will return latLonStart.
%However, if the stopping point is at a pole, then directRhumbProblem will
%correctly return a polar location, but the longitude will generally be
%wrong.
%
%When height!=0, the algorithm can be significantly slower if no
%approximation is used. Around the equator, the distance scales as
%(a+height)/a*dist as one changes the ellipsoidal height. This scaling
%applied to dist in  non-equatorial trajectories is the approximation if
%useHeightApprox=true. When useHeightApprox=false, the approximate value
%is used to determine the bounds around which the fminbnd function
%searches. The maximum error in the approximation is expected to be less
%than 80m. The search region for the value of dist used in the fminbnd
%function was set to 0.9*dist to 1.1*dist, where dist is the distance
%obtained after scaling the distance from the zero-altitude solution.
%
%EXAMPLE:
%A trajectory that crosses the international date line and and goes from
%the Northern hemisphere to the southern hemisphere. We also compute the
%reverse path and show that the azimuth angles in each direction are
%consistent with each other. We then plot the trajectory on an image of the
%spherical Earth.
% N=100;
% latStart=degMinSec2Rad(37,47.5);
% lonStart=degMinSec2Rad(-122,-27.8);
% latEnd=degMinSec2Rad(-33,-51.7);
% lonEnd=degMinSec2Rad(151,12.7);
%
% latLonStart=[latStart;lonStart];
% latLonEnd=[latEnd;lonEnd];
% [azimuth,dist]=indirectRhumbProblem(latLonStart,latLonEnd);
% [azEnd,distRev]=indirectRhumbProblem(latLonEnd,latLonStart);
%
% %If the forward and reverse estimates agree, then these values will
% %ideally be zero.
% azimuth-(-azEnd)
% dist-distRev
%
% distVals=linspace(0,dist,N);
% latLonWayPoints=directRhumbProblem(latLonStart,azimuth,distVals);
%
% %Show that the approximate direct algorithm reaches nearly the same
% %endpoint as the indirect algorithm.
% xEndWay=ellips2Cart([latLonWayPoints(:,end);0]);
% xEnd=ellips2Cart([latLonEnd;0]);
% max(abs(xEndWay-xEnd))
% max(abs(wrapRange(latLonWayPoints(:,end)-latLonEnd,-pi,pi)))
%
% xStartCart=ellips2Cart([latLonStart;0]);
% xEndCart=ellips2Cart([latLonEnd;0]);
% %The path is displayed slightly above the Earth's surface to make it
% %easier to see.
% pathPoints=ellips2Cart([latLonWayPoints;0.02*ones(1,N)]);
%
% figure(1)
% clf
% hold on
% plotMapOnEllipsoid([]);
% scatter3(xStartCart(1),xStartCart(2),xStartCart(3),100,'filled')
% scatter3(xEndCart(1),xEndCart(2),xEndCart(3),100,'filled')
% plot3(pathPoints(1,:),pathPoints(2,:),pathPoints(3,:),'-r','linewidth',4)
%
%REFERENCES:
%[1] K. C. Carlton-Wippern, "On loxodromic navigation," Journal of
%    Navigation, vol. 45, no. 2, pp. 292-297, May 1992.
%[2] J. P. Snyder, "Map projections- a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%[3] J. Alexander, "Loxodromes: A rhumb way to go," Mathematics Magazine,
%    vol. 77, no. 5, pp. 349-356, Dec. 2004.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(numSteps4Circ))
   numSteps4Circ=2000;
end

if(nargin<6||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<5||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<4||isempty(useHeightApprox))
   useHeightApprox=true;
end

if(nargin<3||isempty(height))
   height=0;
end

%Extract the components
latStart=latLonStart(1);
lonStart=latLonStart(2);
latEnd=latLonEnd(1);
lonEnd=latLonEnd(2);

%The first numerical eccentricity of the ellipsoid.
e=sqrt(2*f-f^2);

%Convert the ellipsoidal latitudes to reduced co-latitudes. A co-latitude
%is pi/2 minus the latitude. Also,
nu1=pi/2-ellipsLat2ReducedLat(latStart,f);
nu2=pi/2-ellipsLat2ReducedLat(latEnd,f);

%Though not mentioned in the above papers, the difference in the longitudes
%must be wrapped to the range of -pi/pi or else one will get useless
%results when crossing the -pi/pi boundary.
num=wrapRange(lonEnd-lonStart,-pi,pi,false);
%Equation 11 in the paper provides an expression to get the azimuth.
%however, it is simpler if one just uses isometric latitudes.
val1=ellipsLat2IsoLat(latStart,f);
val2=ellipsLat2IsoLat(latEnd,f);

if(~isfinite(val1))
    warning('The starting point is located at a geographic pole. Azimuth values will be inaccurate.')
end

if(isfinite(val1)||isfinite(val2))
    %This will properly return 0 and pi for infinite values of val1 when
    %val2 is finite and vice versa, which corresponds to headings to or
    %from a pole.
    denom=val2-val1;
    azimuth=atan2(num,denom);
else%If neither is finite, then that is the case where one is going from
    %pole to pole. In such an instance, just set the heading to 0, if going
    %North, and pi, if going South.

    if(latStart>latEnd)
        azimuth=pi;
    else
        azimuth=0;
    end
end

%The distance
if(abs(abs(azimuth)-pi/2)>2e-8)
    %Equation 12 in the paper.
    dist=a*abs(sec(azimuth))*abs(ellipIntInc2Kind(nu2,e^2)-ellipIntInc2Kind(nu1,e^2));
else
    %Equation 14b in the paper.
    dist=a*abs(sin(nu1))*abs(wrapRange(lonEnd-lonStart,-pi,pi));
end

%If a non-zero height is given, then iterate over the direct rhumb
%problem at height to determine the solution.
if(height~=0)
    endCart=ellips2Cart([latLonEnd;height],a,f);

    %The approximate scaling for the height.
    dist=((a+height)/a)*dist;

    %If a computationally-intensive but more precise algorithm to search
    %for the true distance at altitude should be used instead of a
    %simple approximation of scaling the distance.
    if(useHeightApprox==false)
        %Assume that the correct height-adjusted distance is within 10% of the
        %scaled distance value.
        distFun=@(distCur)distCostFunc(distCur,endCart,latLonStart,azimuth,height,a,f,numSteps4Circ);
        dist=fminbnd(distFun,0.9*dist,1.1*dist);
    end
end
end

function cost=distCostFunc(distCur,endCart,latLonStart,azStart,height,a,f,numSteps4Circ)
    latLonCalc=directRhumbProbGen(latLonStart,azStart,distCur,height,false,a,f,numSteps4Circ);
    cost=norm(ellips2Cart([latLonCalc;height],a,f)-endCart);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function g = SigmaNoisy(Reference,Target,Clean)

%ASSUMES THAT TARGET IMAGE IS NOT REFERENCE IMAGE OR CLEAN IMAGE

sumcn=0;

[m1,n1] = size(Reference);
[m2,n2] = size(Target);
[m3,n3] = size(Clean);

r=[m1;m2;m3;];
c=[n1/3;n2/3;n3/3;];

rows=min(r);
cols=min(c);
for i=1:3

    x1=(Reference(1:rows,1:cols,i));
    y=(Target(1:rows,1:cols,i));
    x2=(Clean(1:rows,1:cols,i));

    avg = (x1 + x2)/2;
    diffNoisy=y-avg;
    diffFull=x1-x2;

    varx1 = var(diffFull(:))/4;

    %The sigma values of Target image RGB channels
    vary= max(var(diffNoisy(:)) - varx1,0);

    g(:,i) = sqrt(vary);
    sumcn= sumcn+vary;

end
sigmaNoisy = sqrt(sumcn/3);

g(:,4)=sigmaNoisy;


% Fast Inter-Harmonic Reconstruction as a pre-process for LPC-based
% spectral envelope estimation.
%
%
% Description
%  This technique reconstructs inter-harmonics of the voice signal,
% as a pre-process for an efficient spectral envelope estimation in
% high-pitched voices. The technique is fully described in [1].

%
% Inputs
%  wave             : [samples] [Nx1] input signal (speech signal)
%  Fs               : [Hz]      [1x1] sampling frequency
%  f0               : F0 estimate (a 0 value is for an unvoiced frame)
%  order            : Order used for the LP analysis
%
%
% Outputs
%  LP               : LP coefficients
%  Energy           : energy of the frame
%
%
% Example
%  Please see the HOWTO_envelope.m example file.
%  Please see http://tcts.fpms.ac.be/~drugman/Toolbox/ for more details.
%
% References
%  [1] T.Drugman, Y. Stylianou, "Fast Inter-Harmonic Reconstruction for
%      Spectral Envelope Estimation in High-Pitched Voices", vol. 21, pp.
%      1418-1422, IEEE Signal Processing Letters, 2014.
%      http://tcts.fpms.ac.be/~drugman/files/SPL-FIHR.pdf
%
% Copyright (c) 2014 Toshiba Cambridge Research Laboratory
%
% License
%  This code will be part of the GLOAT toolbox with the following
%  licence:
%  This program is free software: you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation, either version 3 of the License, or
%  (at your option) any later version.
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
% This function will also be part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Thomas Drugman thomas.drugman@umons.ac.be

function [LP,Energy] = env_fihr(Seg,Fs,F0_local,order)

F0_target=100;
% F0* in the paper. Here set to 100 Hz.

Energy=sum(Seg.^2);

if F0_local>0

    TmpSignal=ones(1,length(Seg));
    t_tmp=1:length(TmpSignal);

    I=ceil(F0_local/F0_target);
    for k=1:I-1
        W=(I-k)/I;
        % We use here a linear weighting function. Other functions are
        % possible, as long as they meet the properties mentioned in
        % the paper.
        TmpSignal=TmpSignal+2*W*cos(2*pi*(k/I)*F0_local/Fs*t_tmp);

        % Equation (2) in the paper
    end

    Seg2=Seg.*TmpSignal';
else
    Seg2=Seg;
end

LP=lpc(Seg2,order);


function [ a_lu, info ] = r8pbu_fa ( n, mu, a )

%*****************************************************************************80
%
%% R8PBU_FA factors a R8PBU matrix.
%
%  Discussion:
%
%    The R8PBU storage format is for a symmetric positive definite band matrix.
%
%    To save storage, only the diagonal and upper triangle of A is stored,
%    in a compact diagonal format that preserves columns.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%    The matrix A must be a positive definite symmetric band matrix.
%
%    Once factored, linear systems A*x=b involving the matrix can be solved
%    by calling R8PBU_SL.  No pivoting is performed.  Pivoting is not necessary
%    for positive definite symmetric matrices.  If the matrix is not positive
%    definite, the algorithm may behave correctly, but it is also possible
%    that an illegal divide by zero will occur.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 1998
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Bunch, Moler, Stewart,
%    LINPACK User's Guide,
%    SIAM, Philadelphia, 1979.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer MU, the number of superdiagonals of the matrix.
%    MU must be at least 0, and no more than N-1.
%
%    Input, real A(MU+1,N), the N by N matrix, stored in LINPACK
%    positive definite symmetric band matrix storage.
%
%    Output, real A_LU(MU+1,N), information describing a factored form
%    of the matrix, that can be used to solve linear systems
%    A*x=b, using R8PBU_SL.
%
%    Output, integer INFO, singularity flag.
%    0, the matrix is nonsingular.
%    nonzero, the matrix is singular.
%
  info = 0;
  a_lu(1:mu+1,1:n) = a(1:mu+1,1:n);

  for j = 1 : n

    ik = mu + 1;
    jk = max ( j - mu, 1 );
    mm = max ( mu + 2 - j, 1 );

    s = 0.0;

    for k = mm : mu

      a_lu(k,j) = ( a_lu(k,j) - a_lu(ik:ik+k-mm-1,jk)' * a_lu(mm:k-1,j) )...
        / a_lu(mu+1,jk);

      s = s + a_lu(k,j) * a_lu(k,j);

      ik = ik - 1;
      jk = jk + 1;

    end

    s = a_lu(mu+1,j) - s;

    if ( s <= 0.0 )
      info = j;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8PBU_FA - Fatal error!\n' );
      fprintf ( 1, '  Nonpositive pivot on step %d\n', info );
      return;
    end

    a_lu(mu+1,j) = sqrt ( s );

  end

  return
end


function [ U, V, W ] = vals2coeffs( X,Y,Z )
% VALS2COEFFS  componentwise conversion of matrices of values to
% matrices of 2D Fourier coefficients.
%
% U, V, W = VALS2COEFFS( X,Y, Z ) converts matrices X, Y and Z of values
% sampled from doubly periodic functions on equally-spaced tensor grids of
% the domain [-pi, pi) x [-pi, pi) to matrices U, V and W,
% containing 2D Fourier coefficients for the corresponding interpolants.
%
%
% See also COEFFS2VALS

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

U = spherefun.vals2coeffs(X);
V = spherefun.vals2coeffs(Y);
W = spherefun.vals2coeffs(Z);
end


function model = ml_trainrvm(varargin)
% Learn a probabilistic (non-)linear model, via the Relevance Vector Machine.
% Model = ml_trainrvm(Trials, Targets, Options...)
%
% The Relevance Vector Machine [1] is a Bayesian equivalent to the popular Support Vector Machines
% (SVMs) [2]. RVMs can be used for general-purpose linear or non-linear (kernelized) classification
% or regression, and produce state-of-the-art results in most cases. Various kernels are supplied,
% where the rbf kernel is usually the best initial choice. In the non-linear case, the kernel
% scaling parameter should be found via parameter search, which can be time-consuming. In contrast
% to SVMs, Relevance Vector Machines give probablistic outputs, which can be practical when multiple
% uncertain predictions are to be fused, etc. In the future, an implementation of RVMs using convex
% optimization will be provided, which is assumed to give better optimality guarantees than the
% current implementation [3,4,5].
%
% RVMs (as well as kernel SVMs) are the most versatile general-purpose classifiers currently
% available in the toolbox, and are a good default choice if very little is known about the data.
% For best results, care must be taken to search over the respective regularization parameter(s)
% appropriately. If more is known about the structure of the data (e.g. that it should be linearly
% separable, or that that sparsity can be exploited across features), specialized methods may be
% more appropriate (and give similar results faster or reach better performance by overfitting less
% strongly).
%
% In:
%   Trials       : training data, as in ml_train
%
%   Targets      : target variable, as in ml_train
%
%   Options  : optional name-value parameters to control the training details:
%              'ptype': problem type: 'classification' (default) or 'regression'
%
%              'kernel': one of several kernel types:
%                         * 'linear':   Linear
%                         * 'rbf':      Gaussian / radial basis functions (default)
%                         * 'laplace':  Laplacian
%                         * 'poly':		Polynomial
%                         * 'cauchy':	Cauchy
%
%              'gamma': scaling parameter of the kernel (for regularization); if multiple values
%                       are given, the optimal gamma will be searched via evidence maximization
%                       default: 2.^(-16:0.2:10)
%
%              'degree': degree of the polynomial kernel, if used (default: 3)
%
%              'bias': whether to add a bias to the data (default: 1)
%
%              misc options:
%              'iterations': Number of interations to run for
%              'time':       time limit to run for, e.g. '1.5 hours', '30 minutes', '1 second'
%              'fixednoise': whether the gaussian noise is to be fixed 0/1
%              'beta':       (Gaussian) noise precision (inverse variance)
%              'noisestd':   (Gaussian) noise standard deviation
%              'scaling':    pre-scaling of the data (see hlp_findscaling for options) (default: 'std')
%              'diagnosticlevel':   verbosity level, 0-4
%
% Out:
%   Model   : the computed model...
%             classes indicates the class labels which the model predicts
%             additional parameters determine a posterior distribution over the weights
%
% Examples:
%   % learn a standard Relevance Vector Machine classifier
%   model = ml_trainrvm(trials,targets)
%
%   % as before, but this time use a regression approach
%   model = ml_trainrvm(trials,targets,'ptype','regression')
%
%   % use a Laplacian kernel
%   model = ml_trainrvm(trials,targets,'kernel','laplace')
%
%   % find the optimal kernel scale using parameter search
%   model = utl_searchmodel({trials,targets},'args',{{'rvm','gamma',seach(2.^(-16:2:4)))
%
%
% See also:
%   ml_predictrvm, SparseBayes
%
% References:
%  [1] Vladimir Vapnik. "The Nature of Statistical Learning Theory."
%      Springer-Verlag, 1995
%  [2] Michael E. Tipping and Alex Smola, "Sparse Bayesian Learning and the Relevance Vector Machine".
%      Journal of Machine Learning Research 1: 211?244. (2001)
%  [3] Michael E. Tipping and A. C. Faul. "Fast marginal likelihood maximisation for sparse Bayesian models."
%      In C. M. Bishop and B. J. Frey (Eds.), Proceedings of the Ninth International Workshop on Artificial Intelligence and Statistics, Key West, FL, Jan 3-6 (2003)
%  [4] David P. Wipf and Srikantan Nagarajan, "A New View of Automatic Relevance Determination,"
%      In J.C. Platt, D. Koller, Y. Singer, and S. Roweis, editors, Advances in Neural Information Processing Systems 20, MIT Press, 2008.
%  [5] David P. Wipf and Srikantan Nagarajan, "Sparse Estimation Using General Likelihoods and Non-Factorial Priors,"
%      In Advances in Neural Information Processing Systems 22, 2009.
%
%                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                           2010-04-06
dp;

opts = arg_define([0 2],varargin, ...
    arg_norep('trials'), ...
    arg_norep('targets'), ...
    arg({'ptype','Type'}, 'classification', {'classification','regression'}, 'Type of problem to solve.','cat','Core Parameters'), ...
    arg({'kernel','Kernel'}, 'rbf', {'linear','rbf','laplace','poly','cauchy'}, 'Kernel type. Linear, or Non-linear kernel types: Radial Basis Functions (general-purpose), Laplace (sparse), Polynomial (rarely preferred), and Cauchy (slightly experimental).','cat','Core Parameters'), ...
    arg({'gammap','KernelScale','gamma'}, 2.^(-16:0.5:10), [0 2^-20 2^10 Inf], 'Scaling of the kernel functions. Should match the size of structures in the data. A reasonable range is 2.^(-16:2:4).','cat','Core Parameters','shape','row'), ...
    arg({'polydegree','PolyDegree','degree'}, 3, uint32([1 100]), 'Degree of the polynomial kernel, if chosen.','cat','Core Parameters'), ...
    arg({'bias','Bias'}, true, [], 'Include a bias term in the model.','cat','Core Parameters'), ...
    arg({'scaling','Scaling'}, 'std', {'none','center','std','minmax','whiten'}, 'Pre-scaling of the data. For the regulariation to work best, the features should either be naturally scaled well, or be artificially scaled.','cat','Core Parameters'), ...
    ...
    arg({'iterations','MaxIterations'}, 100, uint32([1 100000]), 'Number of iterations to run.','cat','Miscellaneous'), ...
    arg({'time','MaxTime'}, '1000 seconds', [], 'Maximum time to run. Can use ''seconds'', ''minutes'', ''hours'' in the string.','cat','Miscellaneous'), ...
    arg({'fixednoise','NoiseFixed'}, false, [], 'Keep the Gaussian noise estimate fixed.','cat','Miscellaneous'), ...
    arg({'noiseinvvar','NoiseInvVariance','beta'}, [], [], 'Inverse variance of the Gaussian noise term.','cat','Miscellaneous','shape','scalar'), ...
    arg({'noisestd','NoiseVariance'}, [], [], 'Variance of the Gaussian noise term.','cat','Miscellaneous','shape','scalar'), ...
    arg({'diagnosticlevel','Verbosity'}, 'none', {'none','minimal','low','medium','high','ultra'}, 'Verbosity level.','cat','Miscellaneous'),...
    arg({'votingScheme','VotingScheme'},'1vR',{'1v1','1vR'},'Voting scheme. If multi-class classification is used, this determine how binary classifiers are arranged to solve the multi-class problem. 1v1 gets slow for large numbers of classes (as all pairs are tested), but can be more accurate than 1vR.'), ...
    arg({'monitor','DisplayInterval'}, uint32(0), [], 'Iterations between diagnostic outputs.','cat','Miscellaneous'));

arg_toworkspace(opts);

if is_search(gammap)
    gammap = 0.3; end

% pre-process arguments
ptype = hlp_rewrite(ptype,'classification','c','regression','r'); %#ok<*NODEF>
likelihood = hlp_rewrite(ptype,'c','bernoulli','r','gaussian');
args1 = [hlp_struct2varargin(opts,'restrict',{'iterations','time','monitor','fixednoise','freebasis','callback','callbackdata'}),{'diagnosticlevel',hlp_rewrite(opts.diagnosticlevel,'minimal','none')}];
args2 = hlp_struct2varargin(opts,'restrict',{'beta','noisestd','relevant','weights','alpha'},'rewrite',{'beta','noiseinvvar'});

% remap targets for classification
if strcmp(ptype,'c')
    classes = unique(targets);
    if length(classes) > 2
        % multiclass case: use the voter
        model = ml_trainvote(trials,targets,votingScheme,@ml_trainrvm,@ml_predictrvm,varargin{:});
        return;
    elseif length(classes) == 1
        error('BCILAB:only_one_class','Your training data set has no trials for one of your classes; you need at least two classes to train a classifier.\n\nThe most likely reasons are that one of your target markers does not occur in the data, or that all your trials of a particular class are concentrated in a single short segment of your data (10 or 20 percent). The latter would be a problem with the experiment design.');
    end
    % remap target labels to 0/1
    targets(targets==classes(1)) = 0;
    targets(targets==classes(2)) = 1;
else
    classes = [];
end

% prescale the data
sc_info = hlp_findscaling(trials,scaling);
trials = hlp_applyscaling(trials,sc_info);
basis = trials;

if length(gammap)>1
    if ~strcmp(opts.diagnosticlevel,'none')
        disp('Now optimizing gamma parameter using evidence maximization...'); end
    % optimize gamma parameter using marginal log-likelihood
    bestgam = NaN;      % best gamma parameter so far
    likelihoods = nan(length(gammap),1);
    bestlike = -Inf;    % best likelihood so far
    for k=1:length(gammap)
        gam = gammap(k);
        if ~strcmp(opts.diagnosticlevel,'none')
            fprintf('gamma=%.3f\n',gam); end
        % kernelize the data and add bias
        ktrials = utl_kernelize(trials,basis,kernel,gam,polydegree);
        ktrials = quickif(bias,[ones(size(ktrials,1),1) ktrials],ktrials);
        % run the RVM
        [param, hyperparam, diag] = hlp_diskcache('predictivemodels',@SparseBayes,likelihood,ktrials,targets,SB2_UserOptions(args1{:}),SB2_ParameterSettings(args2{:})); %#ok<ASGLU>
        if ~isempty(diag.Likelihood)
            likelihoods(k) = diag.Likelihood(end);
            if diag.Likelihood(end) > bestlike
                bestlike = diag.Likelihood(end);
                bestgam = gam;
            end
        end
    end
    gammap = bestgam;
else
    likelihoods = [];
end

% kernelize the data and add bias
ktrials = utl_kernelize(trials,basis,kernel,gammap,polydegree);
ktrials = quickif(bias,[ones(size(ktrials,1),1) ktrials],ktrials);
% run the RVM
[param, hyperparam, diag] = hlp_diskcache('predictivemodels',@SparseBayes,likelihood,ktrials,targets,SB2_UserOptions(args1{:}),SB2_ParameterSettings(args2{:}));

% preselect relevant basis vectors
if ~strcmp(kernel,'linear')
    if bias
        feature_sel = param.Relevant-1;
        if feature_sel(1) == 0
            % bias was relevant, remove it from the basis vector selection (it will be added after kernelization)
            feature_sel = feature_sel(2:end);
        else
            % bias was not relevant, forget about it
            bias = 0;
        end
    else
        feature_sel = param.Relevant;
    end
    basis = basis(feature_sel,:);
else
    feature_sel = param.Relevant;
end

model = struct('sc_info',{sc_info},'classes',{classes},'basis',{basis},'param',{param},'hyperparam',{hyperparam},...
               'diag',{diag},'ptype',{ptype},'feature_sel',{feature_sel},'bias',{bias}, ...
               'kernel',{kernel},'gamma',{gammap},'gamma_likelihoods',likelihoods,'degree',{polydegree});


function L = log_marg_prob_node_case(CPD, y, x)
% LOG_MARG_PROB_NODE_CASE Compute prod_m log P(x(i,m)| x(pi_i,m)) for node i (tabular)
% L = log_marg_prob_node_case(CPD, self_ev, parent_ev)
%
% This is a slightly optimised version of log_marg_prob_node.
% We assume we have exactly 1 case, i.e., y is a scalar and x is a vector (not a cell array).

sz = CPD.sizes;
nparents = length(sz)-1;

% We assume the CPTs are already set to the mean of the posterior (due to update_params)

switch nparents
 case 0, p = CPD.CPT(y);
 case 1, p = CPD.CPT(x(1), y);
 case 2, p = CPD.CPT(x(1), x(2), y);
 case 3, p = CPD.CPT(x(1), x(2), x(3), y);
 otherwise,
  ind = subv2ind(sz, [x y]);
  p = CPD.CPT(ind);
end
L = log(p);


function [U,sm,X,V,W] = cgsvd(A,L)
%CGSVD Compact generalized SVD of a matrix pair in regularization problems.
%
% sm = cgsvd(A,L)
% [U,sm,X,V] = cgsvd(A,L) ,  sm = [sigma,mu]
% [U,sm,X,V,W] = cgsvd(A,L) ,  sm = [sigma,mu]
%
% Computes the generalized SVD of the matrix pair (A,L). The dimensions of
% A and L must be such that [A;L] does not have fewer rows than columns.
%
% If m >= n >= p then the GSVD has the form:
%    [ A ] = [ U  0 ]*[ diag(sigma)      0    ]*inv(X)
%    [ L ]   [ 0  V ] [      0       eye(n-p) ]
%                     [  diag(mu)        0    ]
% where
%    U  is  m-by-n ,    sigma  is  p-by-1
%    V  is  p-by-p ,    mu     is  p-by-1
%    X  is  n-by-n .
%
% Otherwise the GSVD has a more complicated form (see manual for details).
%
% A possible fifth output argument returns W = inv(X).

% Reference: C. F. Van Loan, "Computing the CS and the generalized
% singular value decomposition", Numer. Math. 46 (1985), 479-491.

% Per Christian Hansen, IMM, March 17, 2008.

% Initialization.
[m,n] = size(A); [p,n1] = size(L);
if (n1 ~= n)
  error('No. columns in A and L must be the same')
end
if (m+p < n)
  error('Dimensions must satisfy m+p >= n')
end

% Call Matlab's GSVD routine.
[U,V,W,C,S] = gsvd(full(A),full(L),0);

if (m >= n)
  % The overdetermined or square case.
  sm = [diag(C(1:p,1:p)),diag(S(1:p,1:p))];
  if (nargout < 2)
    U = sm;
  else
    % Full decomposition.
    X = inv(W');
  end
else
  % The underdetermined case.
  sm = [diag(C(1:m+p-n,n-m+1:p)),diag(S(n-m+1:p,n-m+1:p))];
  if (nargout < 2)
    U = sm;
  else
    % Full decomposition.
    X = inv(W');
    X = X(:,n-m+1:n);
  end
end

if (nargout==5), W = W'; end


function [img,nrg] = rayCubeAnalytic(L,mat,air,Xsrc,Xmic,Rmax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of 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                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayCubeAnalytic.m                             |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Analytical source image method for a cube     |
%|  `---'  |                                                              |
%+========================================================================+

% Indices
i = -30:30;
a = i + 0.5 - 0.5*(-1).^i;
b = (-1).^i;

% Position relatives des sources images
x       = b*Xsrc(1) + a*L(1) - Xmic(1);
y       = b*Xsrc(2) + a*L(2) - Xmic(2);
z       = b*Xsrc(3) + a*L(3) - Xmic(3);
[x,y,z] = meshgrid(x,y,z);
img     = [x(:),y(:),z(:)];

% Dissipation de l'energie par propagation spherique
dst = sqrt(sum(img.^2,2));
nrg = 1./(dst.^2);

% Dissipation de l'energie par les parois
nrg   = nrg * ones(1,size(mat,2));
rfl   = (1-mat);
i0    = abs(0.5*i - 0.25 + 0.25*(-1).^i);
i1    = abs(0.5*i + 0.25 - 0.25*(-1).^i);
for j = 1:size(nrg,2)
    [rx,ry,rz] = meshgrid(...
        (rfl(1,j).^i0).*(rfl(2,j).^i1),...
        (rfl(3,j).^i0).*(rfl(4,j).^i1),...
        (rfl(5,j).^i0).*(rfl(6,j).^i1));
    nrg(:,j) = (rx(:).*ry(:).*rz(:)) .* nrg(:,j);
end

% Energie dissipee par l'air en fonction de la distance
dst = sqrt(sum(img.^2,2));
nrg = nrg .* exp(-dst*air);

% Selections des images selon l'ordre initial
ind = find(dst<Rmax);
dst = dst(ind);
img = img(ind,:);
nrg = nrg(ind,:);

% Sort in phase
[~,ind] = sort(dst);
img       = img(ind,:);
nrg       = nrg(ind,:);

% Normalisation de l'energie
nrg = nrg./max(max(nrg));

end


function predictions = predictOneVsAll(all_theta, X)
    %% PREDICT Predict the label for a trained one-vs-all classifier. The labels
    %are in the range 1..K, where K = size(all_theta, 1).
    %  predictions = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
    %  for each example in the matrix X. Note that X contains the examples in
    %  rows. all_theta is a matrix where the i-th row is a trained logistic
    %  regression theta vector for the i-th class. You should set p to a vector
    %  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
    %  for 4 examples)

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

    % ====================== YOUR CODE HERE ======================
    % Instructions: Complete the following code to make predictions using
    %               your learned logistic regression parameters (one-vs-all).
    %               You should set p to a vector of predictions (from 1 to
    %               num_labels).
    %
    % Hint: This code can be done all vectorized using the max function.
    %       In particular, the max function can also return the index of the
    %       max element, for more information see 'help max'. If your examples
    %       are in rows, then, you can use max(A, [], 2) to obtain the max
    %       for each row.
    %

    % try every example against every classifier
    activations = sigmoid(X * all_theta');

    % return both probabilities, predictions
    % but throw away probabilities for now
    [~, predictions] = max(activations, [], 2);

end


% Using Chebyshev Polynomials
clear all; close all; clc;

% Define:
f = @(x) x.^4;
%f = @(x) 4*(x.^2-x.^4).*exp(-x./2);

x = (-1:0.1:1)'; % for -1 <= x <= 1