function tyinfo = slgausstype(GS)
%SLGAUSSTYPE Judges the type of a Gaussian model struct
%
% $ Syntax $
%   - tyinfo = slgausstype(GS)
%
% $ Arguments $
%   - GS:       the Gaussian model struct
%   - tyinfo:   the type information structure with following fields
%               - varform:  the form of variance: 'univar'|'diagvar'|'covar'
%               - sharevar: whether the variance(covariance) is shared
%               - hasinv:   whether is invvars or invcovs exists
%               - hasmw:    whether the mixture weights exist
%
% $ Remarks $
%   - The function will check the validity of the struct and will raise
%     an error for invalid models. So it can be used to check validity
%     even you don't need to know the type of the model.
%
% $ History $
%   - Created by Dahua Lin, on Aug 23rd, 2006
%

%% verify basic fields

if ~isstruct(GS)
    gs_argerror('The Gaussian model should be a struct');
end

if ~all(slisfields(GS, {'dim', 'nmodels', 'means'}))
    gs_argerror('The Gaussian model struct should have all of the fields: dim, nmodels and means');
end

d = GS.dim;
k = GS.nmodels;

if ~isequal(size(GS.means), [d k])
    gs_sizerror('The means field should be an array of size d x k');
end

%% verify variance/covariance field

if isfield(GS, 'vars')
    sizvf = size(GS.vars);
    if isequal(sizvf, [1 1])
        varform = 'univar';
        sharevar = true;
    elseif isequal(sizvf, [1 k])
        varform = 'univar';
        sharevar = false;
    elseif isequal(sizvf, [d 1])
        varform = 'diagvar';
        sharevar = true;
    elseif isequal(sizvf, [d k])
        varform = 'diagvar';
        sharevar = false;
    else
        gs_arrerror('The size of vars is illegal');
    end

    hasinv = isfield(GS, 'invvars');

elseif isfield(GS, 'covs')

    sizcvf = size(GS.covs);
    if isequal(sizcvf, [d d])
        varform = 'covar';
        sharevar = true;
    elseif isequal(sizcvf, [d d k])
        varform = 'covar';
        sharevar = false;
    else
        gs_arrerror('The size of covariance is illegal');
    end

    hasinv = isfield(GS, 'invcovs');

else
    gs_arrerror('The Gaussian struct lacks a field for variance/covariance');
end

hasmw = isfield(GS, 'mixweights');

if nargout >= 1
    tyinfo = struct(...
        'varform', varform, ...
        'sharevar', sharevar, ...
        'hasinv', hasinv, ...
        'hasmw', hasmw);
end

function gs_argerror(errmsg)

error('sltoolbox:invalidarg', errmsg);

function gs_sizerror(errmsg)

error('sltoolbox:sizmismatch', errmsg);


%MDL_BAXTER Kinematic model of Baxter dual-arm robot
%
% MDL_BAXTER is a script that creates the workspace variables left and
% right which describes the kinematic characteristics of the two 7-joint
% arms of a Rethink Robotics Baxter robot using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         vertical 'READY' configuration
%   qd         lower arm horizontal as per data sheet
%
% Notes::
% - SI units of metres are used.
%
% References::
% "Kinematics Modeling and Experimental Verification of Baxter Robot"
% Z. Ju, C. Yang, H. Ma, Chinese Control Conf, 2015.
%
% See also mdl_nao, SerialLink.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% MODEL: Baxter, Rethink Robotics, 7DOF, standard_DH

%     th  d  a  alpha

links = [
        Revolute('d', 0.27,        'a', 0.069, 'alpha', -pi/2)
        Revolute('d', 0,           'a', 0, 'alpha', pi/2, 'offset', pi/2)
        Revolute('d', 0.102+0.262, 'a', 0.069, 'alpha', -pi/2)
        Revolute('d', 0,           'a', 0, 'alpha', pi/2)
        Revolute('d', 0.103+0.271, 'a', 0.010, 'alpha', -pi/2)
        Revolute('d', 0,           'a', 0, 'alpha', pi/2)
        Revolute('d', 0.28,        'a', 0, 'alpha', 0)
];

left =  SerialLink(links, 'name', 'Baxter LEFT', 'manufacturer', 'Rethink Robotics');
right = SerialLink(links, 'name', 'Baxter RIGHT', 'manufacturer', 'Rethink Robotics');

left.base = transl(0.064614, 0.25858, 0.119)*rpy2tr(0, 0, pi/4, 'xyz');
right.base = transl(0.063534, -0.25966, 0.119)*rpy2tr(0, 0, -pi/4, 'xyz');

% define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         vertical 'READY' configuration
%   qstretch   arm is stretched out in the X direction
%   qn         arm is at a nominal non-singular configuration
%
qz = [0 0 0 0 0 0 0]; % zero angles, L shaped pose
qr = [0 -pi/2 -pi/2 0 0 0 0]; % ready pose, arm up
qs = [0 0 -pi/2 0 0 0 0];
qn = [0 pi/4 pi/2 0 pi/4  0 0];


function rs = db(s, dbmin)

%tstoolbox/@signal/db
%   Syntax:
%     * db(s, dbmin)
%
%   Compute decibel values of signal relative to a reference value that is
%   determined by the signal's yunit values below dbmin are set to dbmin.
%   If dbmin is ommited it is set to -120.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt
narginchk(1,2);
if nargin < 2, dbmin = -120; end

yu = yunit(s);

ref = dbref(yu);
scf = dbscale(yu);

c = db(s.core, ref, scf, dbmin); 	% call real working routine for parent core object
rs = signal(c, s);				% special constructor calling syntax for working routines

rs = setyunit(rs, unit(['dB' label(yu)]));
rs = addhistory(rs,  ['Calculated decibel values (stretch=' num2str(scf) ',ref=' num2str(ref) ')'] );
rs = addcommandlines(rs, 's = db(s', dbmin);


%% How to load the data, deconvolve it
clear all;
DataFolder = '\\feevault\data0-shared\shared\EmilyShijieShared\CaExtraction\IsolatePreTut\6991';
cnmfeFilePath = fullfile('\\feevault\data0-shared\shared\EmilyShijieShared\CaExtraction\IsolatePreTut\6991', 'CNMFE_BatchVer.mat');
load(fullfile(DataFolder, 'singinginfo.mat'))
load(cnmfeFilePath)
%% compile
L =1; % units of seconds
SONGfs = 1/diff(singinginfo(1).SpecTime(1:2));
VIDEOfs = singinginfo(1).VIDEOfs;
Lsong = L*SONGfs;
Lneural = L*VIDEOfs;

% for each file
% drop long gaps from spectrogram and neural
% concatenate song and neural, and keep track of new seg times
% do extraction

Fnums = 1:length(neuron_batch);
compSONG = [];
compNEURO = [];
FirstOfFileNeuro = [];
FirstOfFileSong = [];
for fi = 1:length(Fnums)
    filei = Fnums(fi);
    RawSong = singinginfo(filei).SongSpec;
    RawNeural =  neuron_batch(filei).DeconvSpiketrain(:,:);
    RawNeural(isnan(RawNeural)) = 0; % deconv sometimes makes nans
    [N,T] = size(RawNeural);

    if length(singinginfo(filei).segs)>0
        % take out long gaps in neural
        bouts = SegsToBouts(singinginfo(filei).segs,.05*singinginfo(filei).SOUNDfs);
        onsetTimes = floor(bouts(:,1)*singinginfo(filei).VIDEOfs/singinginfo(filei).SOUNDfs);
        offsetTimes = ceil(bouts(:,2)*singinginfo(filei).VIDEOfs/singinginfo(filei).SOUNDfs);
        % BUFFER = nan(size(NEURAL,1),Lneural);%.*mean(SPEC,2);
        NEURAL1 = [];
        for segi = 1:size(bouts,1)
            onsetTime =max(onsetTimes(segi),1);
            offsetTime =min(offsetTimes(segi),size(RawNeural,2));
            % expand spectrogram at syll onset
            NEURAL1 = [NEURAL1 RawNeural(:,onsetTime:offsetTime)];
        end
        NEURAL = NEURAL1;
%         NEURAL(NEURAL>=1) = 1; % binarize, and scale for plotting
%         NEURAL(:,end-L:end) = 0; % can't fit last L timepoints, so pad with zero
        FirstOfFileNeuro(fi) = size(compNEURO,2)+1;
        compNEURO = [compNEURO NEURAL];

        % take out long gaps in song
        onsetTimes = ceil((onsetTimes)/singinginfo(filei).VIDEOfs/diff(singinginfo(filei).SpecTime(1:2)));
        offsetTimes = floor((offsetTimes+1)/singinginfo(filei).VIDEOfs/diff(singinginfo(filei).SpecTime(1:2)));
        SONG1 = [];
        for segi = 1:size(bouts,1)
            onsetTime =max(onsetTimes(segi),1);
            offsetTime =min(offsetTimes(segi), size(RawSong,2));
            % expand spectrogram at syll onset
            SONG1 = [SONG1  RawSong(:,onsetTime:offsetTime)];
        end
        SONG = SONG1;
        FirstOfFileSong(fi) = size(compSONG,2)+1;
        compSONG = [compSONG SONG];
        figure(5); imagesc(compSONG); drawnow; shg
    end
end
NEURAL = compNEURO;
%
NEURAL = NEURAL./(prctile(NEURAL,100,2)+prctile(NEURAL(:),95));
NEURAL(max(NEURAL,[],2)<.5,:) = [];
SONG = compSONG;
SONG = SONG-prctile(SONG(:),70); SONG(SONG<0)=0;
SONG = SONG/max(SONG(:));
%% break data into training set and test set
splitN = floor(size(NEURAL,2)*.75);
splitS = floor(size(SONG,2)*.75);
trainNEURAL = NEURAL(:,1:splitN);
trainSONG = SONG(:,1:splitS);
testNEURAL = NEURAL(:,(splitN+1):end);
testSONG = SONG(:,(splitS+1):end);

%% choosing lambda
K = 10;
X = trainNEURAL;
lambdas = sort([logspace(-1,-5,100)], 'ascend');
loadings = [];
regularization = [];
cost = [];
for li = 1:length(lambdas)
    X = trainNEURAL;
    [N,T] = size(X);
    [W, H, ~,loadings(li,:),power]= seqNMF(X,'K',K,'L',Lneural,...
        'lambdaL1W', .1, 'lambda', lambdas(li), 'maxiter', 100, 'showPlot', 0);
    [cost(li),regularization(li),~] = helper.get_seqNMF_cost(X,W,H);
    li
end
%% plot costs as a function of lambda
R = (regularization-min(regularization)); R = R./mean(R);
C = (cost -min(cost)); C = C./mean(C);

clf; hold on
scatter(lambdas, R, 'b', 'markerfacecolor', 'flat');
scatter(lambdas, C, 'r', 'markerfacecolor', 'flat');
xlabel('Lambda'); ylabel('Cost (au)')
set(legend('Regularization cost', 'Reconstruction cost'), 'Box', 'on')
set(gca, 'xscale', 'log', 'ytick', [], 'color', 'none')

savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'IsolateSequences_Feb13';
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [4 3];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_raw_costvslambda.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end
readytosave = 0
%% chose lambda=.004; run multiple times, see loadings
loadings = [];
pvals = [];
is_significant = [];
X = trainNEURAL;
for iteri = 1:100
    [W, H, ~,loadings(iteri,:),power]= seqNMF(X,'K',K,'L',Lneural,...
            'lambdaL1W', .1, 'lambda', .004, 'maxiter', 100, 'showPlot', 0);
    p = .01;
    [pvals(iteri,:),is_significant(iteri,:)] = test_significance(testNEURAL,W,p, 5000)
    W = W(:,is_significant(iteri,:)==1,:);
    H = H(is_significant(iteri,:)==1,:);
    [max_factor, L_sort, max_sort, hybrid] = helper.ClusterByFactor(W(:,:,:),1);
    indSort = hybrid(:,3);
    tstart = 300;
    clf; WHPlot(W(indSort,:,:),H(:,tstart:end), X(indSort,tstart:end), 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end))
    iteri
end
%% plot significant loadings
clf; hold on
h = histogram(sum(is_significant,2), 'edgecolor', 'w', 'facecolor', .7*[1 1 1]);
h.BinCounts = h.BinCounts/sum(h.BinCounts)*100;
xlim([0 10]);
xlabel('# significant factors')
ylabel('% seqNMF runs')
% %%

% XX = repmat(1:size(loadings,1),size(loadings,1),1)+.25*rand(size(loadings,1))-.125;
% XX = XX(:);
% LL = loadings(:);
%
% scatter(XX(is_significant(:)==1),LL(is_significant(:)==1), 'k*')
% scatter(XX(is_significant(:)==0),LL(is_significant(:)==0), 'ko')
% xlabel('Sorted factor #'); ylabel('Loading')

savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'IsolateSequences_Feb13';
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [3 2];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_NumSigFacs.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end
readytosave = 0

%% plot one example factorization
rng(122); %
X = trainNEURAL;
[W, H, ~,loadings(iteri,:),power]= seqNMF(X,'K',K,'L',Lneural,...
            'lambdaL1W', .1, 'lambda', .004, 'maxiter', 100, 'showPlot', 1);
p = .01;
[pvals,is_significant] = test_significance(testNEURAL,W,p, 5000)
W = W(:,is_significant,:);
H = H(is_significant,:);
[max_factor, L_sort, max_sort, hybrid] = helper.ClusterByFactor(W(:,:,:),1);
indSort = hybrid(:,3);
tstart = 300;
clf; WHPlot(W(indSort,:,:),H(:,tstart:end), X(indSort,tstart:end), 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end))
%
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'IsolateSequences_Feb13';
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_RawSorted.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end

clf; WHPlot(W(indSort,:,:),H(:,tstart:end), [], 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end))
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'IsolateSequences_Feb13';
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_Reconstruction.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end

indperm = indSort(randperm(N));
clf; WHPlot(W(indperm,:,:),H(:,tstart:end), X(indperm,tstart:end), 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end))
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'IsolateSequences_Feb13';
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_RawUnsorted.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end
readytosave = 0
% %% run seqNMF on neural data training set
% rng(111)
% X = trainNEURAL;
% [N,T] = size(X);
% K = 3;
% clf; shg
% [W,H,errneural] = seqNMF(X,'K',K,'L',Lneural,...
%     'lambdaL1W', .1, 'lambda', .01, 'maxiter', 150);
% [max_factor, L_sort, max_sort, hybrid] = helper.ClusterByFactor(W(:,:,:),1);
% indSort = hybrid(:,3);
%
% tstart = 300;
% clf; WHPlot(W(indSort,:,:),H(:,tstart:end), X(indSort,tstart:end), 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end))
% % clf; WHPlot(W(indSort,:,:),H, X(indSort,:), 0,trainSONG)
% savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
% saveTitle = 'IsolateSequences_Feb13';
% thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
% papersize = [6 4];
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_raw_sorted.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
% end
% readytosave = 0

% grayscaleWHPlot(WTWO,HTWO, double(NEURAL>0), SONG, VIDEOfs/SONGfs)

% %% testing significance
% p = .05;
% [pvals,is_significant] = test_significance(testNEURAL,W,p, 5000)

%% Factor-triggered song examples and rastors
HTriggeredSpec(H,trainSONG,VIDEOfs,SONGfs,Lsong);

thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [3 2];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_HtriggeredSpec.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end

HTriggeredRaster(H,trainNEURAL(indSort,:),Lneural)
thisFile = 'C:\Users\emackev\Documents\MATLAB\seqnmf\TestbedFigs_elm.m';
papersize = [3 3];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
if readytosave
    set(gcf, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_HtriggeredRaster.pdf']));
    copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
end

readytosave = 0

%
%
%
%
%
% HTriggeredRaster(HTWO,NEURAL(indSort,:),Lneural);
% papersize = [3 7]; %[185.5 116.17*3/4]/30;
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_rasters.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
% end
% clf
% HTriggeredSpec(HTWO,SONG,VIDEOfs,SONGfs,Lsong)
% papersize = [3 4]; %[185.5 116.17*3/4]/30;
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_specs.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
%     set(gcf, 'color', 'w')
% end
% readytosave = 0;
% %%
% helper.AutoSelectLambda(NEURAL, 6, Lneural)
% papersize = [4 3];
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_choosinglambda.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
%     set(gcf, 'color', 'w')
% end
% readytosave = 0;
% %% compute loadings for multiple runs
% rng(3333)
%
% [N,T] = size(NEURAL);
% K = 15;
% config = struct();
% config.ploton = 0;
% config.maxiter = 100;
% config.lambdaW = .002;
% config.competeW = 0;
% config.lambdaL1W = .25;
% config.lambdaL1H = 0;
% config.shift = 1;
% clf; shg
% for iteri = 1:10
%     config.lambdaW = .002;
%     config.lambdaL1W = .25;
% %     [Wneural,Hneural,errneural] = algorithms.seq_NMF(NEURAL,K,Lneural,config);
% %     LOADING(iteri,:) = helper.computeLoadingPercentPower(NEURAL,Wneural,Hneural);
%     config.lambdaW = 0;
%     config.lambdaL1W = 0;
%     [Wneural,Hneural,errneural] = algorithms.seq_NMF(NEURAL,K,Lneural,config);
%     LOADING_cnmf(iteri,:) = helper.computeLoadingPercentPower(NEURAL,Wneural,Hneural);
%     iteri
% end
% %%
% clf
% X = repmat(1:K,iteri,1) + .5*rand(iteri,K)-.25;
% scatter(X(:), LOADING_cnmf(:), 'k', 'markerfacecolor', 'flat')
% hold on
% scatter(X(:), LOADING(:), 'r', 'markerfacecolor', 'flat')
% legend('CNMF', 'seqNMF')
% xlabel('Factor #')
% ylabel('Loading (% power explained)');
% %
% papersize = [3 2.5];
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_Loadings.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
%     set(gcf, 'color', 'w')
% end
% readytosave = 0;
% %% compute rmse & pow explained for multiple K's
% rng(3333)
%
% [N,T] = size(NEURAL);
% K = 10;
% config = struct();
% config.ploton = 0;
% config.maxiter = 100;
% config.lambdaW = .002;
% config.competeW = 0;
% config.lambdaL1W = .25;
% config.lambdaL1H = 0;
% config.shift = 1;
% clf; shg
% RMSE = [];
% RMSE_cnmf = [];
% POWEXP = [];
% POWEXP_cnmf = [];
% W = {};
% H = {};
% W_cnmf = {};
% H_cnmf = {};
% Score = [];
% Score_cnmf = [];
%
%
% for Ki = 1:K
%     for iteri = 1:10
%         config.lambdaW = .002;
%         config.lambdaL1W = .25;
%         [W{iteri,Ki},H{iteri,Ki},RMSE(iteri,Ki),~,POWEXP(iteri,Ki)] = algorithms.seq_NMF(NEURAL,Ki,Lneural,config);
%         config.lambdaW = 0;
%         config.lambdaL1W = 0;
%         [W_cnmf{iteri,Ki},H_cnmf{iteri,Ki},RMSE_cnmf(iteri,Ki),~,POWEXP_cnmf(iteri,Ki)] = algorithms.seq_NMF(NEURAL,Ki,Lneural,config);
%         iteri
%         RMSE
%         RMSE_cnmf
%     end
%     Score(:,Ki) = helper.consistency(W(:,Ki), H(:,Ki));
%     Score_cnmf(:,Ki) = helper.consistency(W_cnmf(:,Ki), H_cnmf(:,Ki));
% end
% %% plot it
% clf
% subplot(1,3,1); cla
% X = repmat(1:K,iteri,1) + .5*rand(iteri,K)-.25;
% scatter(X(:), RMSE_cnmf(:), 5,'k', 'markerfacecolor', 'flat')
% hold on;
% scatter(X(:), RMSE(:), 5,'r', 'markerfacecolor', 'flat')
% % legend('CNMF', 'seqNMF')
% axis tight;
% ylims = ylim;
% ylim([0 ylims(2)])
% xlabel('K')
% ylabel('RMSE');
%
% subplot(1,3,2); cla
% X = repmat(1:K,iteri,1) + .5*rand(iteri,K)-.25;
% scatter(X(:), POWEXP_cnmf(:), 5,'k', 'markerfacecolor', 'flat')
% hold on
% scatter(X(:), POWEXP(:), 5,'r', 'markerfacecolor', 'flat')
% % legend('CNMF', 'seqNMF')
% axis tight;
% ylims = ylim;
% ylim([0 ylims(2)])
% xlabel('K')
% ylabel('Power explained (%)');
% %
% subplot(1,3,3); cla
% patches = plot.violin(Score_cnmf+rand(size(Score))*.00001, 'facecolor','k');
% for ki = 1:K
%     patches(ki).Vertices(:,1) = patches(ki).Vertices(:,1) -.25;
% end
% hold on
% plot.violin(Score+rand(size(Score))*.00001, 'facecolor','r');
% ylims = ylim;
% ylim([0 ylims(2)])
% % X = repmat(1:K,size(Score,1),1) + .5*rand(size(Score,1),K)-.25;
% % scatter(X(:), Score_cnmf(:), 2,'k', 'markerfacecolor', 'flat')
% % hold on
% % scatter(X(:), Score(:), 2,'r', 'markerfacecolor', 'flat')
% % legend('CNMF', 'seqNMF')
% legend off; box off
% xlabel('K')
% ylabel('Consistency');
%
% papersize = [8 2];
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
% if readytosave
%     set(gcf, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_chooseK.pdf']));
%     copyfile(thisFile, fullfile(savedir, [saveTitle '.m']));
%     set(gcf, 'color', 'w')
% end
% readytosave = 0;


  function ob = Gblock(base, nblock)
%|function ob = Gblock(base, nblock)
%|
%| Construct Gblock object, which is a 'meta' system object,
%| designed to work with ordered-subsets (aka block iterative) algorithms.
%| See Gblock_test.m for example usage.
%|
%| A Gblock system is created from another 'block-izable' base system object
%| 'base' by calling: Gb = Gblock(base, nblock, 1);
%|
%| Any projector object can be used if nblock=1, otherwise the base object
%| must have a "mtimes_block" method for block multiplications.
%|
%| The overloaded capabilities of a Gblock object include:
%|	Ab * x			full forward projection
%|	Ab' * y			full back projection
%|	Ab{i_block} * x		project one block (e.g., a set of views)
%|	Ab{i_block}' * x	back project one block
%|				i_block = 1,2,...,nubset
%|
%| Other properties are inherited from the base system object.
%|
%| Copyright 2002-2-18, Jeff Fessler, University of Michigan

if nargin < 2, nblock = 1; end

% create default object, as required by Mathworks
ob.base = [];
ob.nblock = nblock;
ob.i_block = 0;

if nargin == 0	% required by Mathworks
	ob = class(ob, 'Gblock');
return
end

% handle matrix as a special case
if isnumeric(base)
	error 'Gblock no longer supports numeric; call Gmatrix or Gsparse first'
%	r = input('do you wish to continue? [n|y]', 's');
%	if ~streq(r, 'y'), error '', end
%	base = Gsparse(base);
%	if nblock ~= 1, error 'call Gsparse first', end
end

% ensure that a "mtimes_block" method is available (if needed)
if nblock > 1
	try
		mtimes_block(base, 'exists');
	catch
		error 'this base object does not have a mtimes_block method'
	end
end

ob.base = base;
ob = class(ob, 'Gblock');


classdef TestRandMVNormal
    %TestRandMVNormal

    methods (Static)
        function test_1
            d = 3;
            mu = rand(1,d);
            sigma = eye(d);
            nsamples = 50;
            samples = cv.randMVNormal(mu, sigma, nsamples);
            validateattributes(samples, {'numeric'}, {'size',[nsamples d]});
            mean(samples);  % should be close to mu
            cov(samples);   % should be close to sigma
        end

        function test_error_argnum
            try
                cv.randMVNormal();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


% Plot P

a = getElement(logsout, 'Kdata');
t = squeeze(a.Values.time);
K = squeeze(a.Values.data);

plot(t, K(1,:), '-', ...
	 t, K(2,:), '-.',...
     t, K(3,:), ':', ...
     'LineWidth', 2)

legend('K_1', 'K_2', 'K_3')

xlabel('Time (sec)')

%%
print('Ch2_ex1_fig3_K', '-depsc')


function fig = draw_bndry(fig, bndryy, bndryx, clr)
% fig = draw_bndry(fig, bndryy, bndryx, clr)
% draws specified points (boundaries) into picture matrix (figure)

    bndryy = round(bndryy);
    bndryx = round(bndryx);
    c = size(fig, 1);
    d = size(fig, 2);
    e = size(fig, 3);
    % draw only pixel that are in the image region
    draw = bndryy >= 1 & bndryy <= c & bndryx >= 1 & bndryx <= d;

    % Manipulate Figure
    ind = sub2ind([c,d], bndryy(draw), bndryx(draw));
    for j = 1:e
        fig(ind) = clr(j);
        ind = ind + c*d;
    end
end


% [INPUT]
% chi = A float n-by-n-by-t matrix [0,1] representing the time-varying Chi coefficients.
%
% [OUTPUT]
% achi = A row vector of floats [0,1] of length t representing the Average Chi.
% adr = A row vector of floats [0,1] of length t representing the Asymptotic Dependence Rate.

function [achi,adr] = chi_metrics(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('chi',@(x)validateattributes(x,{'double'},{'real' '3d' 'nonempty'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    chi = validate_input(ipr.chi);

    nargoutchk(2,2);

    [achi,adr] = chi_metrics_internal(chi);

end

function [achi,adr] = chi_metrics_internal(chi)

    up = isempty(getCurrentTask());

    [n1,n2,t] = size(chi);
    n = min(n1,n2);

    achi = zeros(t,1);
    adr = zeros(t,1);

    if (up)
        parfor k = 1:t
            chi_k = chi(:,:,k);

            adr_num = 0;
            chi_sum = 0;
            den = 0;

            for i = 1:n
                for j = 1:n
                    if (i == j)
                        continue;
                    end

                    chi_kij = chi_k(i,j);

                    if (isnan(chi_kij))
                        continue;
                    end

                    if (chi_kij > 0)
                        adr_num = adr_num + 1;
                    end

                    chi_sum = chi_sum + chi_kij;
                    den = den + 1;
                end
            end

            achi(k) = chi_sum / den;
            adr(k) = adr_num / den;
        end
    else
        for k = 1:t
            chi_k = chi(:,:,k);

            adr_num = 0;
            chi_sum = 0;
            den = 0;

            for i = 1:n
                for j = 1:n
                    if (i == j)
                        continue;
                    end

                    chi_kij = chi_k(i,j);

                    if (isnan(chi_kij))
                        continue;
                    end

                    if (chi_kij > 0)
                        adr_num = adr_num + 1;
                    end

                    chi_sum = chi_sum + chi_kij;
                    den = den + 1;
                end
            end

            achi(k) = chi_sum / den;
            adr(k) = adr_num / den;
        end
    end

end

function chi = validate_input(chi)

    [n1,n2,t] = size(chi);

    if ((n1 ~= n2) || (min(n1,n2) < 2))
        error('The value of ''chi'' is invalid. Expected input to be a square 3d matrix with a minimum size of 2x2xt.');
    end

    if (t < 5)
        error('The value of ''chi'' is invalid. Expected input to be a square 3d matrix with at least elements on the third dimension.');
    end

    chiv = chi(:);
    chiv(isnan(chiv)) = [];

    if (any((chiv < 0) | (chiv > 1)))
        error('The value of ''chi'' is invalid. Expected input to contain non-NaN values greater than or equal to 0 and less than or equal to 1.');
    end

end


function r=interleave(x,y)
% interleave two vectors, x and y

xc=num2cell(x);
yc=num2cell(y);
r=cell(1,numel(x)+numel(y));
r(1:2:2*numel(x))=xc;
r(2:2:2*numel(y))=yc;
r=[r{:}];


%SWIVELDATA Create look-up tables of swivel angle median and range
%
% Will save the file SwivelData.mat to the same location as this
% function. The median and range look-up tables are called phi_med and
% phi_range respectively. Angles are in radians, and non-reachable
% points are NaN. They are made with 1deg resolution, and are nxnxn in
% size, where n may be set, but default 101.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Known issues:
%  - The model for the shoulder range of motion seems to give some
%     outlier results, which causes in a large swivel angle range
%  - When interpolating to find a value, when near the surface of the
%     arm's reacahble workspace, a NaN may be returned though it is
%     reachable

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

w = which('wrapToPi');
if isempty(w)
    error(pHRIWARE('error', ...
        ['The MATLAB wrapToPi function could not be detected.', ...
        'Replace it with your own version']));
end

n = 101;

hal = HAL();
AP = hal.AP;
reach = sum([AP{2} AP{3}]);

X = linspace(-reach,reach,n);
Y = linspace(-reach,reach,n);
Z = linspace(-reach,reach,n);
PHI = d2r*(-180:179);

phi_med = zeros(n,n,n,'single');
phi_range = zeros(n,n,n,'single');

Q1 = zeros(length(PHI), 7);
Q2 = zeros(length(PHI), 7);

for x = 1:n
    for y = 1:n
        for z = 1:n
            [~,~,Tu] = h2fsu(AP,[X(x),Y(y),Z(z)]',PHI);
            [Q1(:,1:3), Q2(:,1:3)] = gikine(AP{1},Tu);
            [~, u] = hal.reachable(Q1, Q2);

            valid = logical(~u');
%             v = find(valid);
%             if ~isempty(v)
%                 d = diff([v, v(end)+v(1)]);
%                 if ~all(d==1)
%                     longest = [0 0 0];
%                     count = 0;
%                     for i=1:length(d)
%                         if d(i) == 1
%                             count = count+1;
%                         else
%                             if count > longest(3)
%                                 longest = [i-count i-1 count];
%                             end
%                             count = 0;
%                         end
%                     end
%                     if ~isequal(longest,[0 0 0])
%                         valid = v(longest(1):longest(2));
%                     else
%                         valid = false(size(PHI));
%                     end
%                 else
%                     valid = v;
%                 end
%             end

                reachablePhi = PHI(valid);

                if ~isempty(reachablePhi)
                    phiM = max(reachablePhi);
                    phim = min(reachablePhi);

                    if phim == PHI(1) && phiM == PHI(end)
                        unreachablePhi = PHI(logical(~valid));
                        phiM = min(unreachablePhi) + 1*d2r + 2*pi;
                        phim = max(unreachablePhi) - 1*d2r;
                    end
                else
                    phiM = NaN;
                    phim = NaN;
                end

            phi_med(x,y,z) = (phiM+phim)/2;
            phi_range(x,y,z) = (phiM-phim)/2;
        end
    end
    fprintf('.');
    if ~mod(x,10), fprintf('\n'); end
end

phi_med = wrapToPi(phi_med);

dir = fileparts(which('swivelData.m'));
save([dir,'\SwivelData.mat'],'phi_med','phi_range');
fprintf('saved!\n');


filename='Cantileverbeam_Tetrahedra_Linear_Structured_Fine';%Cantilever_tetrahedra';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
%cost = {'compliance','perimeter'};
%weights = [1 0.1];
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
%optimizer = 'DualNestedInPrimal';
optimizer = 'AlternatingPrimalDual';

optimizerUnconstrained = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.2;
optimality_final =1e-3;
constr_final = 1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-1;
constr_initial = 1e-1;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

plotting = false;
printing = false;
monitoring = false;
monitoring_interval = 10;


function feats = compute_features(net, img, do_norm)
% feats = compute_features(net, img) - compute features from a network
%
%  Inputs: net - the neural network (same as for vl_simplenn)
%          img - the input image (H x W x 3 - RGB image)
%
%  Output: feats - the reference given as argument to invert_nn.m
%
% Author: Aravindh Mahendran
%      New College, University of Oxford

% normalize the input image

if ~exist('do_norm','var');do_norm=1;end
if do_norm
    normalize = get_cnn_normalize(net.normalization);
    x0 = normalize(img);
else
    x0 = single(img);
end
% Convert the image into a 4D matrix as required by vl_simplenn
x0 = repmat(x0, [1, 1, 1, 1]);
% Run feedforward for network

switch net.cnn_mode
    case 0;res = vl_simplenn_dw(net, x0);feats = res(end).x;
    case 1;res = net.caffe.forward({x0});feats = res{end};
end


% [p,inl,res,L] = ransacfit(x,model,maxres,smpls,smpln[,doLO,cnstr]) - Ransac fitting
%
% x         ... data - structure depends on the model, see the function
%                      implementation
% model     ... {'line','line-cnstr-seg','plane','conic','3Dpts','E5ptNister','P3PSolver','uX2PSolver'}
% maxres    ... maximal residual to vote
% smpls     ... sample size 0    = minimal sample
%                          (0,1) = percentile from the size of data
%                           >=1  = number of point in the samples
% smpln     ... number of samples
% doLO      ... {0,1} do local optimization (0 implicit)
% cnstr     ... constraint parameters
%               'line-cnstr-seg' ... cnstr = [x1 x2], such that x1 resp. x2
%                                                     is on the left resp. right
%                                                     from the line
% p         ... model parameters
% inl       ... inliers
% res       ... residuals for the model p
% L         ... fitting structure L{i} = output at the i-th improvement
%
% Example: >>ransacfit;

% T.Pajdla, pajdla@cmp.felk.cvut.cz
% 2015-07-11
function [p,inl,r,L] = at_ransac_p3p(x,maxres,smpls,smpln,doLO,bop)
if nargin>0 %% Fit
  if nargin<6, cnstr = []; end
  if nargin<5, doLO = false; end % implicitly do not use the local optimization
  %% model independent sample size
  % percentile from the data lengths
  if smpls >0 && smpls <1, smpl = max(2,ceil(length(x)*smpls)); end
  %         case 'P3PSolver'
  % x is struct containing
  % x.K = camera calibration matrix
  % x.iK = inversion of the camera calibration matris
  % x.x = [u;X] ... stacked hom coordis of image points & coords of 3D points
  if smpls == 0, smpl = 3; end % the minimal case
  if smpls>=3, smpl = smpls; end  % enforce the number of samples
  fitF = @P3PSolver; % 4pt absolute pose
  resF = @PerspRepErr; % residual function
  fitDataF = @P3PSolverFitImPoints; % original data
  resDataF = @E5ptResImPoints; % original data
  cnstr = x.K;

  % check if the sample size has been assigned
  if ~exist('smpl','var'), error('ransacfit: invalid sample size for smpls = %s',smpls); end
  %% RANSAC
  % prepare method depndent data from fitting and evaluation
  xf = fitDataF(x);
  xr = resDataF(x);
  % do ransac
  si = round((size(xf,2)-1)*rand(smpl,smpln)+1); % samples in the columns
  iN = 0;
  p = [];
  inl = [];
  r = [];
  k = 1;
  for i = si
    y = xf(:,i); % get a sample
    pf = fitF(y); % fit a model
    r  = resF(pf,xr,cnstr); % eval residuals & constraints
    il = abs(r)<=maxres; % inliers
    if size(il,1)>1 % there are more alternative models returned by fitF
      in = sum(il,2); % inlier #
      [in,ix] = max(in);
      il = il(ix,:); % select the best inliers
      r = r(ix,:); % select the residuals
      pf = pf{ix}; % select the best model
    else
      in = sum(il); % inlier #
      if ~isempty(pf)
        if iscell(pf)
          pf = pf{1};
        end
      end
    end
    if in>iN % larger support
      if doLO % local optimization
        y = xf(:,il); % get all inliers

%         po = fitF(y); % fit a model

        bop.constant_points = ones(1,sum(il));
        [uout,Kout,Rout,Cout,Xout,eout] = uPXBA_({y(1:2,:)},{pf},y(4:6,:),bop);
        po{1} = [Rout{1} -Rout{1}*Cout{1}];

        ro = resF(po,xr,cnstr); % eval residuals & constraints
        ilo = abs(ro)<=maxres; % inliers
        if size(ilo,1)>1 % there are more alternative models returned by fitF
          ino = sum(ilo,2); % inlier #
          [ino,ix] = max(ino);
          ilo = ilo(ix,:); % select the best inliers
          ro = ro(ix,:); % select the residuals
          po = po{ix}; % select the best model
        else
          ino = sum(ilo); % inlier #
        end
        if ino>in % an improvement
          p = po; r = ro; il = ilo; in = ino;
          fprintf('LO succeeded!\n');
        end
      end
      iN  = in;
      inl = il;
      p = pf;
      if nargout>3 % store the history
        L{k}.iN = iN; L{k}.p = p; L{k}.res = r; L{k}.inl = inl;
        k = k + 1;
      end
    end
  end
  % refit to all inliers (must return only one model!)
  if sum(inl)>0
    y   = xf(:,inl); % inliers
    p = fitF(y,p); % fit
    r = resF(p,xr,cnstr); % residuals
    inl = abs(r)<=maxres; % inliers
  else

  end
  if nargout>3 % store the history
    L{k}.iN = iN; L{k}.p = p; L{k}.res = r; L{k}.inl = inl;
  end

end


function [estSig, cost] = ILRMAISS(mixSig, nSrc, sampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv)
% Blind source separation with independent low-rank matrix analysis (ILRMA) based on iterative source steering update rule
%
% Coded by D. Kitamura (d-kitamura@ieee.org)
%
% Copyright 2021 Daichi Kitamura
%
% These programs are distributed only for academic research at
% universities and research institutions.
% It is not allowed to use or modify these programs for commercial or
% industrial purpose without our permission.
% When you use or modify these programs and write research articles,
% cite the following references:
%
% # Original paper (The algorithm was called "Rank-1 MNMF" in this paper)
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined
% blind source separation unifying independent vector analysis and
% nonnegative matrix factorization," IEEE/ACM Trans. ASLP, vol. 24,
% no. 9, pp. 1626-1641, September 2016.
%
% # Book chapter (The algorithm was renamed as "ILRMA")
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined
% blind source separation with independent low-rank matrix analysis,"
% Audio Source Separation. Signals and Communication Technology.,
% S. Makino, Ed. Springer, Cham, pp. 125-155, March 2018.
%
% # Original AuxIVA-ISS paper
% S. Robin and N. Ono,
% "Fast and stable blind source separation with rank-1 updates,"
% Proc. ICASSP, pp.236–240, 2020.
%
% See also:
% http://d-kitamura.net
% http://d-kitamura.net/demo-ILRMA_en.html
%
% [syntax]
%   [estSig,cost] = ILRMAISS(mixSig,nSrc,sampFreq,nBases,fftSize,shiftSize,windowType,nIter,ilrmaType,refMic,applyNormalize,applyWhitening,drawConv)
%
% [inputs]
%         mixSig: observed mixture (sigLen x nCh)
%           nSrc: number of sources in the mixture (scalar)
%       sampFreq: sampling frequency [Hz] of mixSig (scalar)
%         nBases: number of bases in NMF model (scalar, # of bases for "each" source when ilrmaType=1, and # of bases for "all" sources when ilrmaType=2, default: 4)
%        fftSize: window length [points] in STFT (scalar, default: next higher power of 2 that exceeds 0.256*sampFreq)
%      shiftSize: shift length [points] in STFT (scalar, default: fftSize/2)
%     windowType: window function used in STFT (name of window function, default: 'hamming')
%          nIter: number of iterations in the parameter update in ILRMA (scalar, default: 100)
%      ilrmaType: without or with partitioning function (1: ILRMA without partitioning function (ILRMA type1), 2: ILRMA with partitioning function (ILRMA type2), default: 1)
%         refMic: reference microphone for applying back projection (default: 1)
% applyNormalize: normalize parameters in each iteration to avoid numerical divergence (0: do not apply, 1: average-power-based normalization, 2: back projection (only for ILRMA type1), normalization may collapse monotonic decrease of the cost function, default: 0)
% applyWhitening: apply whitening to the observed multichannel spectrograms or not (true or false, default: true)
%       drawConv: plot cost function values in each iteration or not (true or false, default: false)
%
% [outputs]
%         estSig: estimated signals (sigLen x nCh x nSrc)
%           cost: convergence behavior of cost function in ILRMA (nIter+1 x 1)
%

% Arguments check and set default values
arguments
    mixSig (:,:) double
    nSrc (1,1) double {mustBeInteger(nSrc)}
    sampFreq (1,1) double
    nBases (1,1) double {mustBeInteger(nBases)} = 4
    fftSize (1,1) double {mustBeInteger(fftSize)} = 2^nextpow2(0.256*sampFreq)
    shiftSize (1,1) double {mustBeInteger(shiftSize)} = fftSize/2
    windowType char {mustBeMember(windowType,{'hamming','hann','rectangular','blackman','sine'})} = 'hamming'
    nIter (1,1) double {mustBeInteger(nIter)} = 100
    ilrmaType (1,1) double {mustBeInteger(ilrmaType)} = 1
    refMic (1,1) double {mustBeInteger(refMic)} = 1
    applyNormalize (1,1) double {mustBeInteger(applyNormalize)} = 0
    applyWhitening (1,1) logical = true
    drawConv (1,1) logical = false
end

% Error check
[sigLen, nCh] = size(mixSig); % sigLen: signal length, nCh: number of channels
if sigLen < nCh; error("The size of mixSig might be wrong.\n"); end
if nCh < nSrc || nSrc < 2; error("The number of channels must be equal to or grater than the number of sources in the mixture.\n"); end
if sampFreq <= 0; error("The sampling frequency (sampFreq) must be a positive value.\n"); end
if nBases < 1; error("The number of bases (nBases) must be a positive integer value.\n"); end
if fftSize < 1; error("The FFT length in STFT (fftSize) must be a positive integer value.\n"); end
if shiftSize < 1; error("The shift length in STFT (shiftSize) must be a positive integer value.\n"); end
if nIter < 1; error("The number of iterations (nIter) must be a positive integer value.\n"); end
if ilrmaType ~= 1 && ilrmaType ~= 2; error("The ILRMA type (ilrmaType) must be set to 1 or 2.\n"); end
if refMic < 1 || refMic > nCh; error("The reference microphone must be an integer between 1 and nCh.\n"); end
if applyNormalize ~= 0 && applyNormalize ~= 1 && applyNormalize ~= 2; error("The normalization type (applyNormalize) must be set to 0, 1, or 2.\n"); end
if applyNormalize == 2 && ilrmaType == 2; error("The back-projection-based normalization only supports ILRMA type 1.\n"); end

% Apply multichannel short-time Fourier transform (STFT)
[mixSpecgram, windowInStft] = STFT(mixSig, fftSize, shiftSize, windowType);

% Apply whitening (decorrelate X so that the correlation matrix becomes an identity matrix) based on principal component analysis
if applyWhitening
    inputMixSpecgram = whitening(mixSpecgram, nSrc); % apply whitening, where dimension is reduced from nCh to nSrc when nSrc < nCh
else
    inputMixSpecgram = mixSpecgram(:,:,1:nSrc); % when nSrc < nCh, only mixSpecgram(:,:,1:nSrc) is input to ILRMA so that the number of microphones equals to the number of sources (determined condition)
end

% Apply ILRMA
if ilrmaType == 1
    [estSpecgram, cost] = local_ILRMA1ISS(inputMixSpecgram, nIter, nBases, applyNormalize, drawConv, mixSpecgram(:,:,refMic));
else
    [estSpecgram, cost] = local_ILRMA2ISS(inputMixSpecgram, nIter, nBases, applyNormalize, drawConv);
end

% Apply back projection (fix the scale ambiguity using the reference microphone channel)
scaleFixedSepSpecgram = backProjection(estSpecgram, mixSpecgram(:,:,refMic)); % scale-fixed estimated signal

% Inverse STFT for each source
estSig = ISTFT(scaleFixedSepSpecgram, shiftSize, windowInStft, sigLen);
end

%% Local function for ILRMA type1 (without pertitioning function) based on ISS
function [Y, cost] = local_ILRMA1ISS(X, nIter, L, applyNormalize, drawConv, refMixSpecgram)
% [inputs]
%              X: observed multichannel spectrograms (I x J x M)
%          nIter: number of iterations of the parameter updates
%              L: number of bases in NMF model for each source
% applyNormalize: normalize parameters in each iteration to avoid numerical divergence (0: do not apply, 1: average-power-based normalization, 2: back projection, normalization may collapse monotonic decrease of the cost function)
%       drawConv: plot cost function values in each iteration or not (true or false)
% refMixSpecgram: observed reference spectrogram before apply whitening (I x J)
%
% [outputs]
%              Y: estimated spectrograms of sources (I x J x N)
%           cost: convergence behavior of cost function in ILRMA (nIter+1 x 1)
%
% [scalars]
%              I: number of frequency bins,
%              J: number of time frames
%              M: number of channels (microphones)
%              N: number of sources (equals to M)
%              L: number of bases in NMF model for each source
%
% [matrices]
%              X: observed multichannel spectrograms (I x J x M)
%             pX: permuted observed multichannel spectrograms (M x J x I)
%             pY: permuted separated multisource spectrograms (N x J x I)
%              W: frequency-wise demixing matrix (N x M x I)
%              Y: estimated multisource spectrograms (I x J x N)
%              P: estimated multisource power spectrograms (I x J x N)
%              T: sourcewise basis matrix in NMF (I x L x N)
%              V: sourcewise activation matrix in NMF (L x J x N)
%              R: sourcewise low-rank model spectrogram constructed by T and V (I x J x N)
%              E: identity matrix (N x N)
%              U: model-spectrogram-weighted sample covariance matrix of the mixture (M x M)
%

% Initialization
[I,J,M] = size(X); % I:frequency bins, J: time frames, M: channels
pX = permute(X, [3,2,1]); % permuted X whose dimensions are M x J x I
N = M; % N: number of sources, which equals to M in ILRMA
W = zeros(N,M,I); % frequency-wise demixing matrix
Y = zeros(I,J,N); % estimated spectrograms of sources (Y(i,:,n) =  W(n,:,i)*pX(:,:,i))
for i = 1:I
    W(:,:,i) = eye(N); % initial demixing matrices are set to identity matrices
    Y(i,:,:) = (W(:,:,i)*pX(:,:,i)).'; % initial estimated spectrograms
end
P = max(abs(Y).^2, eps); % power spectrogram of Y
T = max(rand( I, L, N ), eps); % sourcewise basis matrix in NMF
V = max(rand( L, J, N ), eps); % sourcewise activation matrix in NMF
R = zeros(I,J,N); % sourcewise low-rank model spectrogram constructed by T and V (R(:,:,n) = T(:,:,n)*V(:,:,n))
for n = 1:N
    R(:,:,n) = T(:,:,n)*V(:,:,n); % initial source model defined by T and V
end
v = zeros(N,1); % v vector for iterative source steering
cost = zeros(nIter+1, 1);

% Calculate initial cost function value
if drawConv
    cost(1,1) = local_calcCostFunction( P, R, W, I, J );
end

% Optimize parameters in ILRMA (W, T, and V)
fprintf('Iteration:    ');
for iIter = 1:nIter
    fprintf('\b\b\b\b%4d', iIter);

    %%%%% Update parameters %%%%%
    for n = 1:N
        %%%%% Update rule of T %%%%%
        T(:,:,n) = T(:,:,n) .* sqrt((P(:,:,n)./(R(:,:,n).^2))*V(:,:,n).' ./ ( (1./R(:,:,n))*V(:,:,n).' ));
        T(:,:,n) = max(T(:,:,n), eps);
        R(:,:,n) = T(:,:,n)*V(:,:,n);
        %%%%% Update rule of V %%%%%
        V(:,:,n) = V(:,:,n) .* sqrt(T(:,:,n).'*(P(:,:,n)./(R(:,:,n).^2)) ./ ( T(:,:,n).'*(1./R(:,:,n)) ));
        V(:,:,n) = max(V(:,:,n), eps);
        R(:,:,n) = T(:,:,n)*V(:,:,n);
        %%%%% Update rule of Y %%%%%
        YY = Y .* conj(Y(:,:,n)); % I x J x N, using implicit expansion (IxJxN .* IxJx1)
        for i = 1:I
            for nn = 1:N % calculate v vector BEGIN
                d = sum((1./(R(i,:,nn))).*real(YY(i,:,n)), 2) / J; % scalar
                if nn ~= n
                    u = sum((1./(R(i,:,nn))).*YY(i,:,nn), 2) / J; % scalar
                    v(nn,1) = u / d;
                else
                    v(nn,1) = 1 - 1/sqrt(d);
                end
            end % calculate v vector END
            Y(i,:,:) = Y(i,:,:) - permute(v.*Y(i,:,n), [3,2,1]); % update Y, usnig implicit expansion (Nx1 .* 1xJ = NxJ -> 1xJxN)
        end
    end
    P = max(abs(Y).^2, eps); % power spectrogram of Y

    %%%%% Normalization %%%%%
    if applyNormalize == 1 % average-power-based normalization
        lambda = sqrt(sum(sum(P,1),2)/(I*J)); % 1 x 1 x N
        Y = Y./lambda; % I x J x N (use implicit expansion)
        lambdaPow = lambda.^2; % 1 x 1 x N
        P = P./lambdaPow; % I x J x N (use implicit expansion)
        R = R./lambdaPow; % I x J x N (use implicit expansion)
        T = T./lambdaPow; % I x L x N (use implicit expansion)
    elseif applyNormalize == 2 % back projection
        lambda = local_backProjection(Y, refMixSpecgram, I, N); % N x 1 x I
        pLambda = permute(lambda, [3,2,1]); % I x 1 x N
        Y = Y.*pLambda; % I x J x N (use implicit expansion)
        lambdaPow = abs(pLambda).^2; % I x 1 x N
        P = P.*lambdaPow; % I x J x N (use implicit expansion)
        R = R.*lambdaPow; % I x J x N (use implicit expansion)
        T = T.*lambdaPow; % I x L x N (use implicit expansion)
    end

    %%%%% Calculate cost function value %%%%%
    if drawConv
        pY = permute(Y, [3,2,1]); % IxJxN -> NxJxI
        for i = 1:I
            W(:,:,i) = pY(:,:,i)*pX(:,:,i)' / (pX(:,:,i)*pX(:,:,i)'); % derived by "Y(:,:,i) = W(:,:,i)*X(:,:,i)"
        end
        cost(iIter+1,1) = local_calcCostFunction( P, R, W, I, J );
    end
end

% Draw convergence behavior
if drawConv
    figure; plot((0:nIter), cost);
    set(gca, 'FontName', 'Times', 'FontSize', 16);
    xlabel('Number of iterations', 'FontName', 'Arial', 'FontSize', 16);
    ylabel('Value of cost function', 'FontName', 'Arial', 'FontSize', 16);
end
fprintf(' ILRMA1-ISS done.\n');
end

%% Local function for ILRMA type2 (with pertitioning function) based on ISS
function [Y, cost] = local_ILRMA2ISS(X, nIter, K, applyNormalize, drawConv)
% [inputs]
%              X: observed multichannel spectrograms (I x J x M)
%          nIter: number of iterations of the parameter updates
%              K: number of bases in NMF model shared for all the source
% applyNormalize: normalize parameters in each iteration to avoid numerical divergence (0: do not apply, 1: average-power-based normalization, normalization may collapse monotonic decrease of the cost function)
%       drawConv: plot cost function values in each iteration or not (true or false)
%
% [outputs]
%              Y: estimated spectrograms of sources (I x J x N)
%           cost: convergence behavior of cost function in ILRMA (nIter+1 x 1)
%
% [scalars]
%              I: number of frequency bins,
%              J: number of time frames
%              M: number of channels (microphones)
%              N: number of sources (equals to M)
%              K: number of bases in NMF model shared for all the source
%
% [matrices]
%              X: observed multichannel spectrograms (I x J x M)
%             pX: permuted observed multichannel spectrograms (M x J x I)
%             pY: permuted separated multisource spectrograms (N x J x I)
%              W: frequency-wise demixing matrix (N x M x I)
%              Y: estimated multisource spectrograms (I x J x N)
%              P: estimated multisource power spectrograms (I x J x N)
%              T: basis matrix in NMF (I x K)
%              V: activation matrix in NMF (K x J)
%              Z: partitioning function matrix in NMF that clusters K bases into N sources (N x K)
%              R: sourcewise low-rank model spectrogram constructed by T and V (I x J x N)
%              E: identity matrix (N x N)
%              U: model-spectrogram-weighted sample covariance matrix of the mixture (M x M)
%

% Initialization
[I,J,M] = size(X); % I:frequency bins, J: time frames, M: channels
pX = permute(X, [3,2,1]); % permuted X whose dimensions are M x J x I
N = M; % N: number of sources, which equals to M in ILRMA
W = zeros(N,M,I); % frequency-wise demixing matrix
Y = zeros(I,J,N); % estimated spectrograms of sources (Y(i,:,n) =  W(n,:,i)*pX(:,:,i))
for i = 1:I
    W(:,:,i) = eye(N); % initial demixing matrices are set to identity matrices
    Y(i,:,:) = (W(:,:,i)*pX(:,:,i)).'; % initial estimated spectrograms are set to the observed mixture spectrograms
end
P = max(abs(Y).^2, eps); % power spectrogram of Y
T = max(rand( I, K ), eps); % basis matrix in NMF shared for all the sources
V = max(rand( K, J ), eps); % activation matrix in NMF shared for all the sources
Z = max(rand( N, K ), eps); % partitioning function matrix in NMF that clusters K bases into N sources
Z  = Z./sum(Z,1); % ensure sum_n z_{nk} = 1 (use implicit expansion)
tmpT = zeros(size(T)); % temporal variable used in the update rule of T
tmpV = zeros(size(V)); % temporal variable used in the update rule of V
tmpZ = zeros(size(Z)); % temporal variable used in the update rule of Z
R = zeros(I,J,N); % sourcewise low-rank model spectrogram constructed by T, V, and Z (R(:,:,n) = T(:,:,n)*V(:,:,n))
for n = 1:N
    R(:,:,n) = (Z(n,:).*T)*V; % initial source model defined by T, V, and Z (use implicit expansion)
end
v = zeros(N,1); % v vector for iterative source steering
cost = zeros(nIter+1, 1);

% Calculate initial cost function value
if drawConv
    cost(1,1) = local_calcCostFunction( P, R, W, I, J );
end

% Optimize parameters in ILRMA (W, T, and V)
fprintf('Iteration:    ');
for iIter = 1:nIter
    fprintf('\b\b\b\b%4d', iIter);

    %%%%% Update parameters %%%%%
    %%%%% Update rule of Z %%%%%
    for n = 1:N
        tmpZ(n,:) = (( sum((T.'*(P(:,:,n)./(R(:,:,n).^2))).*V, 2) )./( sum((T.'*(1./R(:,:,n))).*V, 2) )).';
    end
    Z = Z .* sqrt(tmpZ);
    Z  = max(Z./sum(Z,1), eps); % ensure sum_n z_{nk} = 1 (use implicit expansion)
    for n = 1:N
        R(:,:,n) = (Z(n,:).*T)*V; % initial source model defined by NMF (use implicit expansion)
    end
    %%%%% Update rule of T %%%%%
    for i = 1:I
        Pi = squeeze(P(i,:,:)); % J x N
        Ri = squeeze(R(i,:,:)); % J x N
        tmpT(i,:) = (( sum((V*(Pi./(Ri.^2))).*(Z.'), 2) )./( sum((V*(1./Ri)).*(Z.'), 2) )).';
    end
    T = max(T.*sqrt(tmpT), eps);
    for n = 1:N
        R(:,:,n) = (Z(n,:).*T)*V; % initial source model defined by NMF (use implicit expansion)
    end
    %%%%% Update rule of V %%%%%
    for j = 1:J
        Pj = squeeze(P(:,j,:)); % I x N
        Rj = squeeze(R(:,j,:)); % I x N
        tmpV(:,j) = ( sum((T.'*(Pj./(Rj.^2))).*(Z.'), 2) )./( sum((T.'*(1./Rj)).*(Z.'), 2) );
    end
    V = max(V.*sqrt(tmpV), eps);
    for n = 1:N
        R(:,:,n) = (Z(n,:).*T)*V; % initial source model defined by NMF (use implicit expansion)
    end
    %%%%% Update rule of Y %%%%%
    for n=1:N
        YY = Y .* conj(Y(:,:,n)); % I x J x N, using implicit expansion (IxJxN .* IxJx1)
        for i = 1:I
            for nn = 1:N % calculate v vector BEGIN
                d = sum((1./(R(i,:,nn))).*real(YY(i,:,n)), 2) / J; % scalar
                if nn ~= n
                    u = sum((1./(R(i,:,nn))).*YY(i,:,nn), 2) / J; % scalar
                    v(nn,1) = u / d;
                else
                    v(nn,1) = 1 - 1/sqrt(d);
                end
            end % calculate v vector END
            Y(i,:,:) = Y(i,:,:) - permute(v.*Y(i,:,n), [3,2,1]); % update Y, usnig implicit expansion (Nx1 .* 1xJ = NxJ -> 1xJxN)
        end
    end
    P = max(abs(Y).^2, eps); % power spectrogram of Y

    %%%%% Normalization %%%%%
    if applyNormalize == 1
        lambda = sqrt(sum(sum(P,1),2)/(I*J)); % 1 x 1 x N
        Y = Y./lambda; % I x J x N (use implicit expansion)
        P = P./lambda.^2; % I x J x N (use implicit expansion)
        R = R./lambda.^2; % I x J x N (use implicit expansion)
        Zlambda = Z./(squeeze(lambda).^2); % N x K
        ZlambdaSum = sum(Zlambda,1); % 1 x K
        T = T.*ZlambdaSum; % I x K (use implicit expansion)
        Z = Zlambda./ZlambdaSum; % N x K (use implicit expansion)
    end

    %%%%% Calculate cost function value %%%%%
    if drawConv
        pY = permute(Y, [3,2,1]); % IxJxN -> NxJxI
        for i = 1:I
            W(:,:,i) = pY(:,:,i)*pX(:,:,i)' / (pX(:,:,i)*pX(:,:,i)'); % derived by "Y(:,:,i) = W(:,:,i)*X(:,:,i)"
        end
        cost(iIter+1,1) = local_calcCostFunction( P, R, W, I, J );
    end
end

% Draw convergence behavior
if drawConv
    figure; plot((0:nIter), cost);
    set(gca, 'FontName', 'Times', 'FontSize', 16);
    xlabel('Number of iterations', 'FontName', 'Arial', 'FontSize', 16);
    ylabel('Value of cost function', 'FontName', 'Arial', 'FontSize', 16);
end
fprintf(' ILRMA2-ISS done.\n');
end

%% Local function for calculating cost function value in ILRMA
function [ cost ] = local_calcCostFunction(P, R, W, I, J)
logDetAbsW = zeros(I,1);
for i = 1:I
    logDetAbsW(i,1) = log(max(abs(det(W(:,:,i))), eps));
end
cost = sum(sum(sum(P./R+log(R),3),2),1) - 2*J*sum(logDetAbsW, 1);
end

%% Local function for applying back projection and returns frequency-wise coefficients
function [ D ] = local_backProjection(Y, X, I, N)
D = zeros(I,N);
for i = 1:I
    Yi = squeeze(Y(i,:,:)).'; % N x J
    D(i,:) = X(i,:,1)*Yi'/(Yi*Yi'); % 1 x N
end
D(isnan(D) | isinf(D)) = 0; % replace NaN and Inf to 0
D = permute(D, [2,3,1]); % N x 1 x I
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% EOF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function []=ps_scn_filt()
%PS_SCN_FILT estimate spatially correlated noise in unwrapped phase
%
%   Andy Hooper, June 2006
%
%   =======================================================================
%   11/2006 AH: Error corrected that was leaving master in temporal smoothing
%   04/2007 AH: Added 64-bit machine compatibility
%   05/2007 AH: Spatially correlated look angle error added
%   02/2010 AH: Replace unwrap_ifg_index with drop_ifg_index
%   02/2010 AH: Bug fixed that could cause a slave scn to be set to zero
%   11/2010 AH: If ramps estimated in step 7, subtract before scn estimation
%   02/2011 DB: Decreased time required for spatial filtering by factor 10
%   =======================================================================
logit;
fprintf('Estimating other spatially-correlated noise...\n')

pix_size=getparm('unwrap_grid_size',1);
time_win=getparm('scn_time_win',1);
deramp_ifg=getparm('scn_deramp_ifg',1);
scn_wavelength=getparm('scn_wavelength',1);
drop_ifg_index=getparm('drop_ifg_index',1);
small_baseline_flag=getparm('small_baseline_flag',1);

load psver
psname=['ps',num2str(psver)];
phuwname=['phuw',num2str(psver)];
sclaname=['scla',num2str(psver)];
apsname=['aps',num2str(psver)];
scnname=['scn',num2str(psver)]; % spatially-correlated noise

ps=load(psname);
uw=load(phuwname);
%aps=load(apsname);

if strcmpi(small_baseline_flag,'y')
    unwrap_ifg_index=[1:ps.n_image];
else
    unwrap_ifg_index=setdiff([1:ps.n_ifg],drop_ifg_index);
end

day=ps.day(unwrap_ifg_index);
master_ix=sum(ps.master_day>ps.day)+1;
n_ifg=length(unwrap_ifg_index);
n_ps=ps.n_ps;

ph_all=single(uw.ph_uw(:,unwrap_ifg_index));
if exist([sclaname,'.mat'],'file')
    scla=load(sclaname);
    ph_all=ph_all-single(scla.ph_scla(:,unwrap_ifg_index));
    ph_all=ph_all-repmat(single(scla.C_ps_uw),1,length(unwrap_ifg_index));
    if ~isempty(scla.ph_ramp)
        ph_all=ph_all-single(scla.ph_ramp(:,unwrap_ifg_index));
    end
end
ph_all(isnan(ph_all))=0;

disp(sprintf('   Number of points per ifg: %d',n_ps))

nodename=['scnfilt.1.node'];
fid=fopen(nodename,'w');
fprintf(fid,'%d 2 0 0\n',n_ps);

for i=1:n_ps
    fprintf(fid,'%d %f %f\n',i,ps.xy(i,2),ps.xy(i,3));
end

fclose(fid);

system('triangle -e scnfilt.1.node > triangle_scn.log');

fid=fopen('scnfilt.2.edge','r');
header=str2num(fgetl(fid));
N=header(1);
edges_nz=zeros(N,4);
for i=1:N
    edges_nz(i,:)=str2num(fgetl(fid));
end
fclose(fid);

%%% deramp end ifgs (unlike aps, orbit errors not so random and end
%%% orbit errors can pass through the low-pass filter
if strcmpi(deramp_ifg,'all')
    deramp_ifg=1:ps.n_ifg;
end
deramp_ifg=intersect(deramp_ifg,unwrap_ifg_index);
deramp_ix=zeros(size(deramp_ifg));
ph_ramp=zeros(n_ps,length(deramp_ifg));

if ~isempty(deramp_ifg)
    fprintf('   deramping selected ifgs...\n')
    G=double([ones(n_ps,1),ps.xy(:,2),ps.xy(:,3)]);
    %G=double([ones(n_ps,1),ps.xy(:,2)]); % range only

    for i=1:length(deramp_ifg)
        i3=find(unwrap_ifg_index==deramp_ifg(i))
        deramp_ix(i)=i3;
        d=(ph_all(:,i3));
        m=G\double(d(:));
        ph_this_ramp=G*m;
        ph_all(:,i3)=ph_all(:,i3)-ph_this_ramp; % subtract ramp
        ph_ramp(:,i)=ph_this_ramp;
    end
    save(scnname,'ph_ramp')
end

%%% smooth in time using gaussian moving window
isnanix=isnan(uw.ph_uw);
uw.ph_uw(isnanix)=0;
dph=ph_all(edges_nz(:,3),:)-ph_all(edges_nz(:,2),:);
dph_lpt=zeros(size(dph));
n_edges=size(dph,1);

fprintf('   low-pass filtering pixel-pairs in time...\n')

for i1=1:n_ifg
    time_diff_sq=(day(i1)-day)'.^2;
    weight_factor=exp(-time_diff_sq/2/time_win^2);
    weight_factor(master_ix)=0; % leave out master
    weight_factor=weight_factor/sum(weight_factor);
    dph_lpt(:,i1)=sum(dph.*repmat(weight_factor,n_edges,1),2);
end

dph_hpt=dph-dph_lpt;  % leaves master APS - slave APS - slave noise (+ residue master noise)

ph_hpt=zeros(n_ps-1,n_ifg);
ref_ix=1;

A=sparse([[1:n_edges]';[1:n_edges]'],[edges_nz(:,2);edges_nz(:,3)],[-ones(n_edges,1);ones(n_edges,1)]);
A=double(A(:,[1:ref_ix-1,ref_ix+1:n_ps]));

fprintf('   solving for high-frequency (in time) pixel phase...\n')

for i=1:n_ifg
    ph_hpt(:,i)=A\double(dph_hpt(:,i));
end

ph_hpt=[ph_hpt(1:ref_ix-1,:);zeros(1,n_ifg);ph_hpt(ref_ix:end,:)]; % add back ref point

ph_hpt(:,deramp_ix)=ph_hpt(:,deramp_ix)+ph_ramp;

ph_hpt=single(ph_hpt);

sigma_sq_times_2=2*scn_wavelength.^2;
ph_scn=nan(n_ps,n_ifg);
patch_dist=scn_wavelength*4;
patch_dist_sq=patch_dist*patch_dist;
ix_range=ceil(n_ps/(max(ps.xy(:,3))-min(ps.xy(:,3)))*patch_dist*0.2);
ix1=1;
ix2=ix_range;
ps.xy(:,1)=[1:n_ps]';

fprintf('   low-pass filtering in space...\n')

for i=1:n_ps
    x_min=ps.xy(i,2)-patch_dist;
    x_max=ps.xy(i,2)+patch_dist;
    y_min=ps.xy(i,3)-patch_dist;
    y_max=ps.xy(i,3)+patch_dist;

    ix1=ix1+ix_range;
    ix1(ix1>n_ps)=n_ps;
    while ix1>1 & ps.xy(ix1-1,3)>=y_min
        ix1=ix1-ix_range;
    end

    ix2=ix2-ix_range;
    ix2(ix2<1)=1;
        while ix2<n_ps & ps.xy(ix2+1,3)<=y_max
        ix2=ix2+ix_range;
    end

    ix1(ix1<1)=1;
    ix2(ix2>n_ps)=n_ps;

    xy_near=ps.xy(ix1:ix2,:);
    xy_near=xy_near(xy_near(:,2)>=x_min & xy_near(:,2)<=x_max & xy_near(:,3)>=y_min & xy_near(:,3)<=y_max,:);
    dist_sq=(xy_near(:,2)-ps.xy(i,2)).^2+(xy_near(:,3)-ps.xy(i,3)).^2;
    in_range_ix=dist_sq<patch_dist_sq; % exclude those out of range
    xy_near=xy_near(in_range_ix);
    dist_sq=dist_sq(in_range_ix);
    weight_factor=exp(-dist_sq/sigma_sq_times_2);
    weight_factor=weight_factor/sum(weight_factor); % normalize

    %ph_scn(i,:)=sum(ph_hpt(xy_near(:,1),:).*repmat(weight_factor,1,n_ifg),1); 		% slow method
    ph_scn(i,:)=weight_factor'*ph_hpt(xy_near(:,1),:);  				% speed increased by a factor 10

    if i/1000==floor(i/1000)
        disp([num2str(i),' PS processed'])
    end
end

ph_scn=ph_scn-repmat(ph_scn(1,:),n_ps,1); % re-ref to 1st PS
ph_scn_slave=zeros(size(uw.ph_uw));
ph_scn_slave(:,unwrap_ifg_index)=ph_scn;
ph_scn_slave(:,master_ix)=0;

save(scnname,'ph_scn_slave','ph_hpt','ph_ramp')
logit(1);


function [e, varargout] = neterr(w, net, x, t)
%NETERR	Evaluate network error function for generic optimizers
%
%	Description
%
%	E = NETERR(W, NET, X, T) takes a weight vector W and a network data
%	structure NET, together with the matrix X of input vectors and the
%	matrix T of target vectors, and returns the value of the error
%	function evaluated at W.
%
%	[E, VARARGOUT] = NETERR(W, NET, X, T) also returns any additional
%	return values from the error function.
%
%	See also
%	NETGRAD, NETHESS, NETOPT
%

%	Copyright (c) Ian T Nabney (1996-2001)

errstr = [net.type, 'err'];
net = netunpak(net, w);

[s{1:nargout}] = feval(errstr, net, x, t);
e = s{1};
if nargout > 1
  for i = 2:nargout
    varargout{i-1} = s{i};
  end
end


%% Setup
p = get_default_truck_trailer_params();
p.noiseLevel = 0.01;
p.trailerWheelbase = 11;
forwardTarget = [20 2 0.3];
xx = [0 -5 0 0];
opt = odeset('Events', @obstacleEvents);
dt = 0.2;

%% setup viz
figure(101)
ax(1) = subplot(121);
tp1 = truck_trailer_plot(p, ax(1));
title('No learning')
ax(2) = subplot(122);
tp2 = truck_trailer_plot(p, ax(2));
title('Learning via ADP')
%% No learning
numPullups = 5;

tsave = [0];
ysave = [xx];

for ct = 1:numPullups

%     if ct <=3
%         p.forwardTarget = [20 3 0.1];
%     else
%         p.forwardTarget = [20 0 0];
%     end
%forward
if ysave(end,2) >= -0.3
   p.forwardTarget = [20 0 0];
else
   p.forwardTarget =forwardTarget;
end

p.velocity = 1;
[t,y] = ode45(@(t, x)sys_truck_trailer_wrapper(t, x, p), ...
    tsave(end)+[0 40], [ysave(end,1:4) zeros(1,10)], opt);

tsave = [tsave; t];
ysave = [ysave; y(:,1:4)];

%backward
p.velocity = -1;
[t,y] = ode45(@(t, x)sys_truck_trailer_wrapper(t, x, p), ...
    tsave(end)+[0 40], [ysave(end,1:4) zeros(1,10)], opt);

tsave = [tsave; t];
ysave = [ysave; y(:,1:4)];

end

ts = (tsave(1):dt:tsave(end))';
[~, ix] = unique(tsave);
ys = interp1(tsave(ix), ysave(ix,:), ts, 'linear', 'extrap');
ts1 = ts;
ys1 = ys;
%%
% for jj = 1:numel(ts)
%     tp1.updateFig(ys(jj,:))
%     drawnow
% end

%% With ADP

tsave = [0];
ysave = [xx];
psave = zeros(3,3,0);
ksave = zeros(3,1,0);

for ct = 1:numPullups

%     if ct <=3
%         p.forwardTarget = [20 3 0.1];
%     else
%         p.forwardTarget = [20 0 0];
%     end

if ysave(end,2) >= -0.3
   p.forwardTarget = [20 0 0];
else
   p.forwardTarget = forwardTarget;
end
%forward
p.velocity = 1;
[t,y] = ode45(@(t, x)sys_truck_trailer_wrapper(t, x, p), ...
    tsave(end)+[0 40], [ysave(end,1:4) zeros(1,10)], opt);
tsave = [tsave; t];
ysave = [ysave; y(:,1:4)];

%backward
p.velocity = -1;
xbase = kron(xx(2:4), xx(2:4));
kbase = [0 0 0];
sbase = [0 0 0 0 0 0];
qbase = 0;

for ii = 1:30
    [t, y] = ode45(@(t, x)sys_truck_trailer_wrapper(t, x, p), ...
        tsave(end)+[0 0.5], [y(end,1:4) zeros(1,10)]);

    tsave = [tsave; t];
    ysave = [ysave; y(:,1:4)];

    xbase = [xbase;
        kron(y(end, 2:4), y(end, 2:4))];
    qbase = [qbase;
        y(end, 5)];
    kbase = [kbase;
        y(end, 6:8)];
    sbase = [sbase;
        y(end, 9:14)];
end

[t, y] = ode45(@(t, x)sys_truck_trailer_wrapper(t, x, p), ...
    tsave(end)+[0 20], [y(end,1:4) zeros(1,10)], opt);
tsave = [tsave; t];
ysave = [ysave; y(:,1:4)];

% Learning
xbase = xbase(2:end,:) - xbase(1:end-1,:);
kbase = kbase(2:end,:);
qbase = qbase(2:end,:);
sbase = sbase(2:end,:);
l = [xbase(:,[1 2 3 5 6 9]) kbase sbase] \ -qbase;
Ve = [l(1) l(2)/2 l(3)/2;
    l(2)/2 l(4) l(5)/2;
    l(3)/2 l(5)/2 l(6)]

Ke = (p.R \ l(7:9)) * 0.5

D1 = 3.0;     % trailer wheelbase
D2 = 11.0;   % tractor wheelbase
A = [  0    -1     0;
    0     0     0;
    0     0    1/p.trailerWheelbase];
B = [0;
    -1/p.truckWheelbase;
    1/p.truckWheelbase];

P = lyap((A-B*p.feedbackGain)',p.Q + p.feedbackGain'*p.R*p.feedbackGain)
Knext = p.R \ B'*P

psave(:,:,end+1) = P;
ksave(:,:,end+1) = p.feedbackGain;
p.feedbackGain = Knext;

end

%
% Resample t
ts = (tsave(1):dt:tsave(end))';
[~, ix] = unique(tsave);
ys = interp1(tsave(ix), ysave(ix,:), ts, 'linear', 'extrap');
ts2 =ts;
ys2 =ys;
% %%
% for jj = 1:numel(ts)
%     tp2.updateFig(ys(jj,:))
%     drawnow
% end

[Ps, ~, Ks] = care(A,B,p.Q, p.R);
%% Parallel plot
%v = VideoWriter('newfile.mp4','MPEG-4');
%open(v)
for jj = 1:numel(ts1)
    tp1.updateFig(ys1(jj,:))
    if jj <= numel(ts2)
        tp2.updateFig(ys2(jj,:))
    end
    drawnow
 %   frame = getframe(gcf);
 %   writeVideo(v,frame);
end
%close(v)

%%
figure(301)
plot(1:5, norm(squeeze(ksave - Ks')))


function fig=vgg_gui_H(i1,i2,H)
%
%	fig=vgg_gui_H(i1,i2,H)
%
%
% Visualizes an homography matrix of two views
%
%IN:
%	i1 - Matlab image
%	i2 - Matlab image
%	H - 3x3 Homography matrix. Assumes that image coordiantes are 1..width
%		where pixel centers are at integer locations.
%
%OUT:
%	fig - handle to the figure

% $Id: vgg_gui_H.m,v 1.3 2001/10/30 14:42:41 wexler Exp $
% Yoni, Tue Mar 27 19:31:16 2001

if nargin==3
   action='start';
else
   action=i1;
   ud = get(gcf, 'UserData');
end

if strcmp(action,'start'),
   if nargin ~= 3
      error('Must give 3 arguments... read the docs.\n');
   end

   h0 = figure('Color',[0.8 0.8 0.8], ...
	       'NumberTitle','off', ...
	       'Name','Play With Homography Matrix', ...
	       'ButtonDownFcn', 'disp(''Click on images'')',...
	       'WindowButtonUpFcn', 'vgg_gui_H(''none'');',...
	       'WindowButtonMotionFcn', 'vgg_gui_H(''move'')', ...
	       'Pointer', 'crosshair', ...
	       'DoubleBuffer', 'on',...
	       'Units','normalized');
   m0=uimenu('Label', '&Color');
   uimenu(m0, 'Label', '&Red', 'ForegroundColor', [1 0 0], ...
	  'Accelerator', 'r', 'Callback', 'vgg_gui_H(''cr'');');
   uimenu(m0, 'Label', '&Green', 'ForegroundColor', [0 1 0], ...
	  'Accelerator', 'g', 'Callback', 'vgg_gui_H(''cg'');');
   uimenu(m0, 'Label', '&Blue', 'ForegroundColor', [0 0 1], ...
	  'Accelerator', 'b', 'Callback', 'vgg_gui_H(''cb'');');
   m1=uimenu('Label', '&Size');
   uimenu(m1, 'Label', '&Increase', 'Callback', 'vgg_gui_H(''s+'');', 'Accelerator', '+');
   uimenu(m1, 'Label', '&Decrease', 'Callback', 'vgg_gui_H(''s-'');', 'Accelerator', '-');
   uimenu(m1, 'Label', '&1', 'Callback', 'vgg_gui_H(''s1'');', 'Accelerator', '1');
   uimenu(m1, 'Label', '&2', 'Callback', 'vgg_gui_H(''s2'');', 'Accelerator', '2');
   uimenu(m1, 'Label', '&3', 'Callback', 'vgg_gui_H(''s3'');', 'Accelerator', '3');
   uimenu(m1, 'Label', '&4', 'Callback', 'vgg_gui_H(''s4'');', 'Accelerator', '4');
   uimenu(m1, 'Label', '&5', 'Callback', 'vgg_gui_H(''s5'');', 'Accelerator', '5');
   uimenu(m1, 'Label', '&6', 'Callback', 'vgg_gui_H(''s6'');', 'Accelerator', '6');
   uimenu(m1, 'Label', '&7', 'Callback', 'vgg_gui_H(''s7'');', 'Accelerator', '7');
   uimenu(m1, 'Label', '&8', 'Callback', 'vgg_gui_H(''s8'');', 'Accelerator', '8');
   uimenu(m1, 'Label', '&9', 'Callback', 'vgg_gui_H(''s9'');', 'Accelerator', '9');

   ah1 = axes('Parent', h0, ...
	      'Position',[0 0 .5 1]);
   h1=imshow(i1); hold on; title('Image 1');
   set(h1, 'ButtonDownFcn','vgg_gui_H(''b1'');');

   ah2 = axes('Parent',h0, ...
	      'Position',[.5 0 .5 1], ...
	      'Tag','Axes2');
   h2=imshow(i2); hold on; title('Image 2');
   set(h2, 'ButtonDownFcn','vgg_gui_H(''b2'');');

   point=plot(-1000, -1000,'EraseMode','xor');
   point2=plot(-1000, -1000,'EraseMode','xor');

   s1=size(i1); s2=size(i2);
   t(:,:,1)=H;  t(:,:,2)=inv(H);  H=t;

   ud=struct('h0', h0, 'h',[h1 h2], 'ah', [ah1, ah2], ...
	     'sizes', [s1(1:2); s2(1:2)], ...
	     'current', -1, 'color', 'r', 'size', 1, ...
	     'p', point, 'H', H, 'p2', point2 );

   set(h0,'UserData',ud);

   if nargout > 0, fig = h0; end

elseif strcmp(action, 'move')
   if ud.current<0 return; end;
   pt=get(ud.ah(ud.current),'CurrentPoint');
   pt2=ud.H(:,:,ud.current)*pt(1,:)';
   pt2=pt2/pt2(3);

   set(ud.p2, 'XData', pt2(1,1), 'YData', pt2(2,1))
   set(ud.p, 'XData', pt(1,1), 'YData', pt(1,2))

elseif action(1)=='b'
   if action(2)=='1' ud.current=1;
   elseif action(2)=='2' ud.current=2;
   else return;
   end
   pt=get(ud.ah(ud.current),'CurrentPoint');

   p2=ud.H(:,:,ud.current)*pt(1,:)';
   p2=p2/p2(3);

   delete(ud.p);
   delete(ud.p2);
   axes(ud.ah(ud.current));
   ud.p=plot(pt(1,1), pt(1,2), [ud.color '+'], ...
	     'MarkerSize', 8+2*ud.size, 'LineWidth', ud.size,...
	     'EraseMode','xor');
   axes(ud.ah(3-ud.current));
   ud.p2=plot(p2(1,1), p2(2,1), [ud.color '+'], ...
	     'MarkerSize', 8+2*ud.size, 'LineWidth', ud.size,...
	     'EraseMode','xor');

elseif action(1)=='c'
   ud.color=action(2);
   %get(ud.l)
   set(ud.p2, 'Color', ud.color);
   set(ud.p, 'Color', ud.color);

elseif action(1)=='s'
   if action(2)=='+' ud.size=ud.size+1;
   elseif action(2)=='-' ud.size=max(1, ud.size-1);
   else
      ud.size = str2num(action(2));
   end
   set(ud.p, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size);
   set(ud.p2, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size);

elseif strcmp(action, 'none')
   ud.current = -1;

else
   error(['Unknown command: ' action]);
end

set(ud.h0, 'UserData',ud);


function [node,PSLG,part] = bfsgeo2(node,PSLG,seed)
%BFSGEO2 partition geometry about "seeds" via breadth-first
%search.
%   [NODE,EDGE,PART] = BFSGEO2(NODE,EDGE,SEED) returns a set
%   of 2-manifold geometry partitions by expanding about a
%   list of "seed" points. Partitions expand in a breadth-
%   first sense until a polygon bounday is encountered. SEED
%   is an S-by-2 array of XY-coordinates to expand around,
%   NODE is an N-by-2 array of polygon vertices, and EDGE is
%   an E-by-2 array of polygon edge indexing. Each row in
%   EDGE represents an edge of the polygon, such that
%   NODE(EDGE(JJ,1),:) and NODE(EDGE(JJ,2),:) are the XY-co-
%   ordinates of the endpoints of the JJ-TH edge. PART is an
%   S-by-1 cell array of geometry partitions, where each
%   PART{KK} is a list of edge indices into EDGE that define
%   the KK-TH partition.
%
%   This function may be useful when seeking to partition
%   complex, non-manifold geometry into a format that's app-
%   ropriate for the triangulation algorithms in REFINE2.
%
%   See also REFINE2, FIXGEO2, BFSTRI2

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 11/10/2017
%-----------------------------------------------------------

%---------------------------------------------- basic checks
    if ( ~isnumeric(node) || ...
         ~isnumeric(PSLG) || ...
         ~isnumeric(seed) )
        error('bfsgeo2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(node) ~= +2 || ...
        ndims(PSLG) ~= +2 || ...
        ndims(seed) ~= +2 )
        error('bfsgeo2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (size(node,2)~= +2 || ...
        size(PSLG,2)~= +2 || ...
        size(seed,2)~= +2 )
        error('bfsgeo2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nnod = size(node,1) ;
    nedg = size(PSLG,1) ;

%---------------------------------------------- basic checks
    if (min([PSLG(:)])<+1 || ...
        max([PSLG(:)])>nnod)
        error('bfsgeo2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

%------------------------------------------ assemble full CDT
   [node,PSLG,tria] = deltri2 (node,PSLG) ;

%------------------------------------------ find seeds in CDT
   [sptr,stri] = findtria(node,tria,seed) ;

    okay = sptr(:,2) ...
        >= sptr(:,1) ;
    itri = stri(sptr(okay,1));

%------------------------------------------ PART for all seed
    part = {} ;

    for ipos = +1 : size (itri,1)

    %-------------- BFS about current tria.
       [mark] = ...
            bfstri2(PSLG,tria,itri(ipos)) ;

    %-------------- match tria./poly. edges
        edge = [
            tria(mark,[1,2]) ;
            tria(mark,[2,3]) ;
            tria(mark,[3,1]) ;
               ] ;

        edge = sort(edge,+2) ;
        PSLG = sort(PSLG,+2) ;

       [same,epos] = ...
            setset2(edge(:,1:2),PSLG) ;

    %-------------- find match multiplicity
        epos = epos(epos>+0) ;
        epos = sort(epos);

        eidx = ...
            find(diff(epos)) ;

        eptr = ...
            [1;eidx+1;length(epos)+1] ;
        enum = ...
            eptr(2:end)-eptr(1:end-1) ;

    %---------- select singly-matched edges
        part{ipos} = ...
            epos(eptr(enum == 1)) ;

    end

end


%   CLASS QZSS_SS
% =========================================================================
%
% DESCRIPTION
%   container of QZSS Satellite System parameters
%
% REFERENCES
%   CRS parameters, according to each GNSS system CRS definition
%   (ICD document in brackets):
%
%   *_QZS --> GRS80    (IS-QZSS 1.8E)
%   Standard: http://qz-vision.jaxa.jp/USE/is-qzss/DOCS/IS-QZSS_18_E.pdf
%
%   Other useful links
%     - http://www.navipedia.net/index.php/QZSS_Signal_Plan
%     - Ellipsoid: http://www.unoosa.org/pdf/icg/2012/template/WGS_84.pdf
%       note that GM and OMEGAE_DOT are redefined in the standard IS-GPS200H (GPS is not using WGS_84 values)
%     - http://www.navipedia.net/index.php/Reference_Frames_in_GNSS
%     - http://gage6.upc.es/eknot/Professional_Training/PDF/Reference_Systems.pdf

%--------------------------------------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:        Andrea Gatti
%  Contributors:      Andrea Gatti, Giulio Tagliaferro ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%    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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

classdef QZSS_SS < Satellite_System
    properties (Constant, Access = 'public')
        SYS_EXT_NAME = 'QZSS';    % full name of the constellation
        SYS_NAME     = 'QZS';     % 3 characters name of the constellation, this "short name" is used as fields of the property list (struct) to identify a constellation
        SYS_C        = 'J';       % Satellite system (ss) character id

        % System frequencies as struct [MHz]
        F = struct('L1', 1575.420, ...
                   'L2', 1227.600, ...
                   'L5', 1176.450, ...
                   'LEX6', 1278.750)

        % Array of supported frequencies [MHz]
        F_VEC = struct2array(QZSS_SS.F) * 1e6;

        % Array of the corresponding wavelength - lambda => wavelengths
        L_VEC = 299792458 ./ QZSS_SS.F_VEC;

        N_SAT = 9;           % Maximum number of satellite in the constellation
        PRN = (1 : 9)';      % Satellites id numbers as defined in the constellation

        % CODE2DATA ftp://igs.org/pub/data/format/rinex303.pdf
        CODE_RIN3_ATTRIB  = {'ZXLSC F' 'XLS F', 'XIQ F', 'XLS F'}; % last letter of the observation code
        CODE_RIN3_DEFAULT_ATTRIB  = {'C' 'S' 'Q' 'S'}; % last letter of the observation code
        CODE_RIN3_2BAND  = '1256';                        % id for the freq as stored in F_VEC
        IONO_FREE_PREF  = ['12';'15';'16';'25';'26';'56']; % to be evaluated which combination is really better
    end

    properties (Constant, Access = 'private')
        % QZSS (GRS80) Ellipsoid semi-major axis [m]
        ELL_A = 6378137;
        % QZSS (GRS80) Ellipsoid flattening
        ELL_F = 1/298.257222101;
        % QZSS (GRS80) Ellipsoid Eccentricity^2
        ELL_E2 = (1 - (1 - QZSS_SS.ELL_F) ^ 2);
        % QZSS (GRS80) Ellipsoid Eccentricity
        ELL_E = sqrt(QZSS_SS.ELL_E2);
    end

    properties (Constant, Access = 'public')
        % Structure of orbital parameters (ellipsoid, GM, OMEGA_EARTH_DOT)
        ORBITAL_P = struct('GM', 3.986005e14, ...                  % Gravitational constant * (mass of Earth) [m^3/s^2]
                                    'OMEGAE_DOT', 7.2921151467e-5, ...      % Angular velocity of the Earth rotation [rad/s]
                                    'ELL',struct( ...                       % Ellipsoidal parameters QZSS (GRS80)
                                    'A', QZSS_SS.ELL_A, ...             % Ellipsoid semi-major axis [m]
                                    'F', QZSS_SS.ELL_F, ...             % Ellipsoid flattening
                                    'E', QZSS_SS.ELL_E, ...             % Eccentricity
                                    'E2', QZSS_SS.ELL_E2));             % Eccentricity^2
        ORBITAL_INC = 48;    % Orbital inclination
        ORBITAL_RADIUS  = 42164000 + 6378137; % Orbital radius
    end

    methods
        function this = QZSS_SS(offset)
            % Creator
            % SYNTAX: QZSS_SS(<offset>)
            if (nargin == 0)
                offset = 0;
            end
            this@Satellite_System(offset);
        end

        function copy = getCopy(this)
            % Get a copy of this
            copy = QZSS_SS(this.getOffset());
            copy.import(this);
        end
    end
end


function [AA,bb,p,rankA] = lusolCondense(A,b,mode,printLevel)
%
%        [AA,bb,p] = lusolCondense(A,b,mode);
% extracts from A a submatrix AA = A(p,:) of full row rank, and
% the corresponding subvector bb = b(p), where A should be sparse
% and have nonzero entries not too much bigger than 1.
%
%INPUT
% A     m x n sparse matrix
% b     m x 1 vector
%
%OPTIONAL INPUT
%    mode:     If mode=1, LUSOL operates on A itself.
%              If mode=2, LUSOL operates on A'.
%
%OUTPUT
% AA        row reduced A
% bb        row reduced B
% p         AA = A(p,:),bb = b(p)
% rankA     rank of A
%

% 27 May 2007: Michael Saunders:
%              First version of lusolCondense, intended as a
%              more efficient substitute for [Q,R,P] = qr(full(A')).
%              Michael Saunders
% 10 Oct  2019: Ronan Fleming
%              64bit linux support

if ~exist('mode','var')
    mode=1; %what is the best default.
end
if ~exist('printLevel','var')
    printLevel=0;
end

if ~issparse(A)
    A=sparse(A);
end

[m,n]   = size(A);
if isempty(b)
    b=sparse(m,1);
end

archstr = computer('arch');
archstr = lower(archstr);
switch archstr
    case {'win32', 'win64'}
        error('%s\n','lusolCondense not compatible with windows OS.')
    case {'glnxa86'}

        options = lusolSet;
        options.Pivoting  = 'TRP';
        options.FactorTol = 1.5;

        if mode==1   % Factorize A itself
            [L,U,p,q,options] = lusolFactor(A,options);
            inform   = options.Inform;
            rankA    = options.Rank;

            if rankA==m
                AA = A;
                bb = b;
                p  = (1:m)';
            else
                AA = A(p(1:rankA),:);
                bb = b(p(1:rankA));
            end

        else         % Factorize A'
            [L,U,p,q,options] = lusolFactor(A',options);
            inform  = options.Inform;
            rankA    = options.Rank;

            if rankA==m
                AA = A;
                bb = b;
                p  = (1:m)';
            else
                p  = q;
                AA = A(p(1:rankA),:);
                bb = b(p(1:rankA));
            end
        end

    case {'glnxa64','maci64'}
        if ~isempty(which('lusol_obj'))
            % generate default options
            options = lusol_obj.luset();
            % |--------+----------+----------------------------------------------------|
            % | param  |  default | description                                        |
            % |--------+----------+----------------------------------------------------|
            %
            % lusol_obj options
            % |--------+----------+----------------------------------------------------|
            % | nzinit |        0 | minimum length for storage arrays                  |
            % |--------+----------+----------------------------------------------------|
            %
            % LUSOL integer parameters
            % |--------+----------+----------------------------------------------------|
            % | maxcol |        5 | max num cols searched for piv element              |
            % | pivot  |    'TPP' | pivoting method {'TPP','TRP','TCP','TSP'}          |
            % | keepLU |        1 | keep the nonzeros, if 0, permutations are computed |
            % |--------+----------+----------------------------------------------------|
            %
            % LUSOL real parameters
            % |--------+----------+----------------------------------------------------|
            % | Ltol1  |     10.0 | max Lij allowed, default depends on pivot method   |
            % | Ltol2  |     10.0 | max Lij allowed during updates                     |
            % | small  |  eps^0.8 | absolute tolerance for treating reals as zero      |
            % | Utol1  | eps^0.67 | absolute tol for flagging small diags of U         |
            % | Utol2  | eps^0.67 | rel tol for flagging small diags of U              |
            % | Uspace |      3.0 |                                                    |
            % | dens1  |      0.3 |                                                    |
            % | dens2  |      0.5 |                                                    |
            % |--------+----------+----------------------------------------------------|

            % %modification of default options
            % options.pivot  = 'TRP';
            % options.Ltol1 = 1.5;
            % options.nzinit = 1e7;
            % %factorise
            % mylu = lusol_obj(A',options);

            if mode==1   % Factorize A itself
                %factorise
                mylu = lusol_obj(A);

                %extract results
                stats = mylu.stats();
                options.Inform = stats.inform;
                options.Nsing  = stats.nsing;
                options.Growth = stats.growth;

                %matrices
                L = mylu.L0();
                U = mylu.U();
                % row permutation
                p = mylu.p();
                % column permutation
                q = mylu.q();

                L     = L(p,p);      % New L is strictly lower triangular (and square).
                % U     = U(p,q);      % New U would be upper trapezoidal the size of S'.

                %return the rank of the matrix
                rankA=mylu.rank();

                %    lu.factorize inform codes:
                switch stats.inform
                    case 0
                        if printLevel>0
                            fprintf('%s\n','The LU factors were obtained successfully.')
                        end
                    case 1
                        if printLevel>0
                            fprintf('%s\n','U appears to be singular, as judged by lu6chk.');
                        end
                    case 3
                        fprintf('%s\n','Some index pair indc(l), indr(l) lies outside the matrix dimensions 1:m , 1:n.');
                    case    4
                        fprintf('%s\n','Some index pair indc(l), indr(l) duplicates another such pair.');
                    case    7
                        fprintf('%s\n','The arrays a, indc, indr were not large enough.');
                        fprintf('%s\n','Their length "lena" should be increase to at least');
                        fprintf('%s\n','The value "minlen" given in luparm(13).');
                    case 8
                        fprintf('%s\n','There was some other fatal error.  (Shouldn''t happen!)');
                    case 9
                        fprintf('%s\n','No diagonal pivot could be found with TSP or TDP.');
                        fprintf('%s\n','The matrix must not be sufficiently definite or quasi-definite.');
                end

                if stats.inform~=0 && stats.inform~=1
                    % solve Ax=b
                    b = ones(size(A',1),1);
                    x = mylu.solve(b);
                    % multiply Ax
                    b2 = mylu.mulA(x);
                    % check the result
                    fprintf('%s\t%g\n','Check norm(b-b2) : ', norm(b-b2))
                end

                if rankA==m
                    AA = A;
                    bb = b;
                    p  = (1:m)';
                else
                    AA = A(p(1:rankA),:);
                    bb = b(p(1:rankA));
                end

            else
                %factorise A'
                mylu = lusol_obj(A');

                %extract results
                stats = mylu.stats();
                options.Inform = stats.inform;
                options.Nsing  = stats.nsing;
                options.Growth = stats.growth;

                %matrices
                L = mylu.L0();
                U = mylu.U();
                % row permutation
                p = mylu.p();
                % column permutation
                q = mylu.q();

                L     = L(p,p);      % New L is strictly lower triangular (and square).
                % U     = U(p,q);      % New U would be upper trapezoidal the size of S'.

                %return the rank of the matrix
                rankA=mylu.rank();

                %    lu.factorize inform codes:
                switch stats.inform
                    case 0
                        if printLevel>0
                            fprintf('%s\n','The LU factors were obtained successfully.')
                        end
                    case 1
                        if printLevel>0
                            fprintf('%s\n','U appears to be singular, as judged by lu6chk.');
                        end
                    case 3
                        fprintf('%s\n','Some index pair indc(l), indr(l) lies outside the matrix dimensions 1:m , 1:n.');
                    case    4
                        fprintf('%s\n','Some index pair indc(l), indr(l) duplicates another such pair.');
                    case    7
                        fprintf('%s\n','The arrays a, indc, indr were not large enough.');
                        fprintf('%s\n','Their length "lena" should be increase to at least');
                        fprintf('%s\n','The value "minlen" given in luparm(13).');
                    case 8
                        fprintf('%s\n','There was some other fatal error.  (Shouldn''t happen!)');
                    case 9
                        fprintf('%s\n','No diagonal pivot could be found with TSP or TDP.');
                        fprintf('%s\n','The matrix must not be sufficiently definite or quasi-definite.');
                end

                if stats.inform~=0 && stats.inform~=1
                    % solve Ax=b
                    b = ones(size(A',1),1);
                    x = mylu.solve(b);
                    % multiply Ax
                    b2 = mylu.mulA(x);
                    % check the result
                    fprintf('%s\t%g\n','Check norm(b-b2) : ', norm(b-b2))
                end

                if rankA==m
                    AA = A;
                    bb = b;
                    p  = (1:m)';
                else
                    p  = q;
                    AA = A(p(1:rankA),:);
                    bb = b(p(1:rankA));
                end
            end

        else
            error('%s\n','lusolCondense.m Cannot find lusol_obj.m from lusol interface. Make sure https://github.com/nwh/lusol is intalled and added to the matlab path.')
        end
end
if printLevel>1
    fprintf('%s\n',['lusolCondense: A has dimensions ' int2str(m) ' x ' int2str(n) ' and has rank ' int2str(rankA)])
end


make_it_tight = true;
subplot = @(m,n,p) subtightplot (m, n, p, [0.01 0.05], [0.1 0.01], [0.1 0.01]);
if ~make_it_tight,  clear subplot;  end

%% Upper and Lower Subplots with Titles
income = [3.2,4.1,5.0,5.6];
outgo = [2.5,4.0,3.35,4.9];
subplot(2,1,1); plot(income)
title('Income')
subplot(2,1,2); plot(outgo)
title('Outgo')

%% Subplots in Quadrants
figure
subplot(2,2,1)
text(.5,.5,{'subplot(2,2,1)';'or subplot 221'},...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,2)
text(.5,.5,{'subplot(2,2,2)';'or subplot 222'},...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,3)
text(.5,.5,{'subplot(2,2,3)';'or subplot 223'},...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,4)
text(.5,.5,{'subplot(2,2,4)';'or subplot 224'},...
    'FontSize',14,'HorizontalAlignment','center')

%% Asymmetrical Subplots
figure
subplot(2,2,[1 3])
text(.5,.5,'subplot(2,2,[1 3])',...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,2)
text(.5,.5,'subplot(2,2,2)',...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,4)
text(.5,.5,'subplot(2,2,4)',...
    'FontSize',14,'HorizontalAlignment','center')

%%
figure
subplot(2,2,1:2)
text(.5,.5,'subplot(2,2,1:2)',...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,3)
text(.5,.5,'subplot(2,2,3)',...
    'FontSize',14,'HorizontalAlignment','center')
subplot(2,2,4)
text(.5,.5,'subplot(2,2,4)',...
    'FontSize',14,'HorizontalAlignment','center')

%% Plotting Axes Over Subplots
figure
y = zeros(4,15);
for k = 1:4
    y(k,:) = rand(1,15);
    subplot(2, 2, k)
    plot(y(k,:));
end
hax = axes('Position', [.35, .35, .3, .3]);
bar(hax,y,'EdgeColor','none')
set(hax,'XTick',[])


function img = im_crop(ori, p, out_size)
    %% crop part of the image according to the given bounding box coordinates
    %% for out-of-boundary part, just use an constance value
    sz = size(ori);
    p(1) = floor(p(1)); p(2) = floor(p(2));
    p(3) = ceil(p(3)); p(4) = ceil(p(4));
    img = ((ones(p(4), p(3), 3) * 120));

    x1 = round(p(1) - p(3) / 2);
    y1 = round(p(2) - p(4) / 2);
    x2 = x1 + p(3) - 1;
    y2 = y1 + p(4) - 1;
    crop_x1 = max(1, -x1 + 2); x1 = max(1, x1);
    crop_y1 = max(1, -y1 + 2); y1 = max(1, y1);
    crop_x2 = min(p(3), p(3) + sz(2) - x2); x2 = min(x2, sz(2));
    crop_y2 = min(p(4), p(4) + sz(1) - y2); y2 = min(y2, sz(1));
    img( crop_y1 : crop_y2, crop_x1 : crop_x2, :) = ori(y1 : y2, x1 : x2, :);

    if nargin > 2
        img = imresize(img, [out_size, out_size], 'Antialiasing', false);
    end
end


% SCRIPT TEST FOR THE 3 DOF spherical manipulator

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.

close all

%general call to inverse dynamic function
%TAU = inversedynamic(param, Q, QD, QDD, GRAV, FEXT)

%three different poses
q1 = [0, pi/2, pi/2];
q2 = [0, pi/2, 0];

%load 3dof spherical robot
%the parameters are loaded from robots/example/3dofspherical/parameters.m
spherical=load_robot('example', '3dofspherical');

spherical.graphical.draw_transparent=1;

fprintf('\nTorques at each joint given position = [0 0 0], and velocity=[0 0 0] and standard gravity acting on Z0')
%Please note that the forces and moments are specified with respect to the
%X3 Y3 Z3 reference system.
%In this mechanism, forces acting on Fx3 or Fz3 do not account for tau, nor
%moments acting on My3 or Mx3
tau = inversedynamic(spherical, q1, [0 0 0], [0 0 0], [0 0 9.81]', [0 0 0 0 0 0]')

%draw the robot at this position
drawrobot3d(spherical,q1)
disp('press any key to continue')
pause

%Now, add a force on Fy, and observe the results on tau
tau = inversedynamic(spherical, q1, [0 0 0], [0 0 0], [0 0 9.81]', [0 1 0 0 0 0]')
%draw the robot at this position
drawrobot3d(spherical,q1)
disp('press any key to continue')
pause

%torques necessary to instantaneously bring the arm to the specified state
fprintf('\nTorques at each joint given position = [0 pi/2 0], and velocity=[1 1 1] and acceleration = [1 1 1] and gravity acting on Z0')
tau = inversedynamic(spherical, q2, [1 1 1], [1 1 1], [0 0 9.81]', [0 0 0 0 0 0]')

%draw the robot
drawrobot3d(spherical,q2)
disp('press any key to continue')
pause

%In this case, all torques are caused by gravity acting on the Center Of
%Mass of each link. Note that we have changed the direction of the gravity
%vector g
fprintf('\nTorques at each joint given position = [0 0 0], and velocity=[0 0 0] and acceleration = [0 0 0] and gravity acting on Y0')
tau = inversedynamic(spherical, q1, [0 0 0], [0 0 0], [0 9.81 0]', [0 0 0 0 0 0]')

%draw the robot
drawrobot3d(spherical,q1)


function posV = xscale(h,scale,offset)
%function xscale(h,scale,offset)
%This script recenters and then scales the x-axis of the
%axis with handle h.  The offset value is in the unscaled
%(input) coordinates.
%JOD. LM: 23 Oct 03.
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

ticklabelS = get(h,'xticklabel');
tmpV = str2num(ticklabelS);
posV = (tmpV-offset)*scale;
newlabelsS = num2str(posV);
set(h,'xticklabel',newlabelsS);