classdef matRad_SquaredUnderdosing < DoseObjectives.matRad_DoseObjective
% matRad_SquaredUnderdosing Implements a penalized squared underdosing objective
%   See matRad_DoseObjective for interface description
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2020 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'Squared Underdosing';
        parameterNames = {'d^{min}'};
        parameterTypes = {'dose'};
    end

    properties
        parameters = {60};
        penalty = 1;
    end

    methods
        function obj = matRad_SquaredUnderdosing(penalty,dMin)
            %If we have a struct in first argument
            if nargin == 1 && isstruct(penalty)
                inputStruct = penalty;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseObjectives.matRad_DoseObjective(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin == 2 && isscalar(dMin)
                    obj.parameters{1} = dMin;
                end

                if nargin >= 1 && isscalar(penalty)
                    obj.penalty = penalty;
                end
            end
        end

        %% Calculates the Objective Function value
        function fDose = computeDoseObjectiveFunction(obj,dose)
            % overdose : dose minus prefered dose
            underdose = dose - obj.parameters{1};

            % apply positive operator
            underdose(underdose>0) = 0;

            % claculate objective function
            fDose = obj.penalty/numel(dose) * (underdose'*underdose);
        end

        %% Calculates the Objective Function gradient
        function fDoseGrad   = computeDoseObjectiveGradient(obj,dose)
            % overdose : dose minus prefered dose
            underdose = dose - obj.parameters{1};

            % apply positive operator
            underdose(underdose>0) = 0;

            % calculate delta
            fDoseGrad = 2 * obj.penalty/numel(dose) * underdose;
        end
    end

end


function rs = scalogram(s, scalemin, scalemax, scalestep, mlen)

%tstoolbox/@signal/scalogram
%   Syntax:
%     * rs = scalogram(s) => scalemin=0.1
%     * rs = scalogram(s, scalemin) => scalemax=1
%     * rs = scalogram(s, scalemin, scalemax) => scalestep=0.1
%     * rs = scalogram(s, scalemin, scalemax, scalestep) => mlen=10
%     * rs = scalogram(s, scalemin, scalemax, scalestep, mlen)
%
%   Scalogram of signal s using morlet wavelet. See also: spec2.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,5);

if nargin<2
	scalemin = 0.1;
end
if nargin<3
	scalemax = 1;
end
if nargin<4
	scalestep = 0.1;
end
if nargin<6
	mlen = 10;
end

c = scalogram(s.core, scalemin, scalemax, scalestep, mlen);
rs = signal(c, s);	% special constructor calling syntax for working routines
a = achse(unit, scalemin, scalestep);
rs = setaxis(rs, 2, a);
rs = setplothint(rs, 'spectrogram');
rs = addhistory(rs, ['Calculated scalogram']);
rs = addcommandlines(rs, 's = scalogram(s', scalemin, scalemax, scalestep);


function test_bug2342

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY

% first create some data
%--------------------------------------------------------
% make 3 channels with no direct link between 1 and 2
cfg             = [];
cfg.ntrials     = 100;
cfg.triallength = 0.5;
cfg.fsample     = 200;
cfg.nsignal     = 3;
cfg.method      = 'ar';
cfg.params(:,:,1) = [ 0.8 0   0;
                      0   0.9 0.5;
                      0.4 0   0.5];
cfg.params(:,:,2) = [-0.5    0  0;
                        0 -0.8  0;
                        0    0 -0.2];
cfg.noisecov      = [0.3 0 0;
                       0 1 0;
                       0 0 0.2];

data            = ft_connectivitysimulation(cfg);
data2           = data;
data2.label     = {'signal001b';'signal002b';'signal003b'};
data            = ft_appenddata([], data, data2);

% according to Martin, padding should get rid of most the zigzags.

% freqanalysis
cfgf           = [];
cfgf.method    = 'mtmfft';
cfgf.output    = 'fourier';
cfgf.tapsmofrq = 4;
freq           = ft_freqanalysis(cfgf, data);
cfgf.padding   = 10;
freqpad        = ft_freqanalysis(cfgf, data);

% connectivityanalysis
cfg = [];
cfg.method           = 'granger';
cfg.granger.sfmethod = 'bivariate';
g    = ft_connectivityanalysis(cfg, freq);
gpad = ft_connectivityanalysis(cfg, freqpad);

%...but it doesn't

% now make it more realistic and add a bit of noise to signals 4-6 (to make
% it not perfectly collinear).
data2 = data;
for k = 1:numel(data.trial)
  data2.trial{k}(4:6,:) = data.trial{k}(4:6,:)+randn(3,100)*0.000001;
end

cfgf           = [];
cfgf.method    = 'mtmfft';
cfgf.output    = 'fourier';
cfgf.tapsmofrq = 2;
freq2           = ft_freqanalysis(cfgf, data2);
cfgf.padding   = 10;
freq2pad        = ft_freqanalysis(cfgf, data2);

% connectivityanalysis
cfg = [];
cfg.method           = 'granger';
cfg.granger.sfmethod = 'bivariate';
g2    = ft_connectivityanalysis(cfg, freq2);
g2pad = ft_connectivityanalysis(cfg, freq2pad);

% conclusion: adding a tiny bit of noise removes the zigzags.


% DEMROBOTWIRELESS2 Wireless Robot data from University of Washington, without dynamics and without back constraints.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'robotWireless';
experimentNo = 2;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('ftc');
options.back = 'mlp';
options.backOptions = mlpOptions;

latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'vector')


function x = prox_cvx(v, lambda, f, l, u)
% PROX_CVX    The proximal operator of a generic function.
%
%   prox_cvx(v,lambda,f) is the proximal operator of f with parameter lambda
%   evaluated at v. Here, f is a closure, possibly described by an anonymous
%   function. It is also possible to provide lower and upper bounds, so
%   prox_cvx(v,f,l,u) is the prox operator of f + I_[l,u].
%
%   For example,
%
%     prox_cvx(v, lambda, (@(x) norm(x,1))
%
%   is equivalent to the soft thresholding operator.
%
%   WARNING: This is a *very* inefficient way of evaluating a proximal
%   operator. This function is mainly for rapid prototyping and
%   testing custom implementations of particular proximal operators.
%   It may also be useful when suffering from extreme laziness.

    if ~exist('l', 'var') || isnan(l)
        l = -Inf;
    end

    if ~exist('u', 'var') || isnan(u)
        u = Inf;
    end

    L = 1/(2*lambda);
    [m n] = size(v);

    if min(m,n) == 1
        cvx_begin quiet
            variable x(max(m,n))
            minimize(f(x) + L*sum_square(x - v))
            subject to
                v >= l;
                v <= u;
        cvx_end
    elseif m == n && m > 1
        cvx_begin quiet
            variable x(n,n) symmetric
            minimize(f(x) + L*pow_pos(norm(x - v, 'fro'), 2))
            subject to
                x == semidefinite(n)
        cvx_end
    else
        x = nan(size(v));
    end
end


function [xnew,ynew]=splineValue(x,y)
%+++ spline for interpolation.
%+++
%+++ Aug., 2010.

minx=min(x);
maxx=max(x);
d=(maxx-minx)/128;
xnew=linspace(minx-d,maxx+d,128)';
ynew=spline(x,y,xnew);


function f1 = perform_curve_subdivision(f, nsub, options)

% perform_curve_subdivision - perform subdivision
%
%   f1 = perform_curve_subdivision(f, nsub, options);
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
if nsub<=0
    f1 = f; return;
end
if size(f,1)>size(f,2)
    f = f';
end
if size(f,1)>1
    for k=1:size(f,1)
        f1(k,:) = perform_curve_subdivision(f(k,:), nsub, options);
    end
    return;
end

h = getoptions(options, 'h', [1 4 6 4 1]);
h = 2*h/sum(h(:));

if not(isfield(options, 'bound'))
    options.bound = 'per';
end

f = perform_curve_subdivision(f, nsub-1, options);

f = f(:)';
n = length(f);
f1 = zeros(1,n*2);
f1(1:2:end) = f;
f1 = perform_convolution(f1, h, options);
f1 = f1(:)';


function [ Vehicle ] = gen_trajectory( t )

     radius = 10;
    Vehicle.position = radius* [ 5*cos(0.3*t/4); 4*sin(0.2*t/4); 2*sin(0.2*t/4+1) ];
    %Vehicle.position = radius* [ 5*cos(0.03*t); 4*sin(0.02*t); 2*sin(0.02*t+1) ];
    Vehicle.euler = [0.5*t+2; -0.3*t; 0.4*t];

    % generate rotation matrix
    for i = 1:size(Vehicle.euler, 2)
        Vehicle.orientation((i-1)*3+1:i*3, 1:3) = euler2rotation_matrix(Vehicle.euler(:, i));
    end
end


function [dS,t,f]=mtdspecgramc(data,movingwin,phi,params)
% Multi-taper derivative of the time-frequency spectrum - continuous process
%
% Usage:
%
% [dS,t,f]=mtdspecgramc(data,movingwin,phi,params)
% Input:
%   Note that all times can be in arbitrary units. But the units have to be
%   consistent. So, if E is in secs, win, t have to be in secs, and Fs has to
%   be Hz. If E is in samples, so are win and t, and Fs=1. In case of spike
%   times, the units have to be consistent with the units of data as well.
%
%       data        (in form samples x channels/trials or a single vector) -- required
%       movingwin         (in the form [window winstep] i.e length of moving
%                                                 window and step size.
%                                                 Note that units here have
%                                                 to be consistent with
%                                                 units of Fs - required
%       phi         (angle for evaluation of derivative) -- required
%                       e.g. phi=[0,pi/2] giving the time and frequency
%                       derivatives
%       params: structure with fields tapers, pad, Fs, fpass, trialave
%       -optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                    (1) A numeric vector [TW K] where TW is the
%                        time-bandwidth product and K is the number of
%                        tapers to be used (less than or equal to
%                        2TW-1).
%                    (2) A numeric vector [W T p] where W is the
%                        bandwidth, T is the duration of the data and p
%                        is an integer such that 2TW-p tapers are used. In
%                        this form there is no default i.e. to specify
%                        the bandwidth, you have to specify T and p as
%                        well. Note that the units of W and T have to be
%                        consistent: if W is in Hz, T must be in seconds
%                        and vice versa. Note that these units must also
%                        be consistent with the units of params.Fs: W can
%                        be in Hz if and only if params.Fs is in Hz.
%                        The default is to use form 1 with TW=3 and K=5
%                     Note that T has to be equal to movingwin(1).
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           trialave - (average over trials/channels when 1, don't average when 0) - optional. Default 0
% Output:
%       dS      (spectral derivative in form phi x time x frequency x channels/trials if trialave=0;
%               in form phi x time x frequency if trialave=1)
%       t       (times)
%       f       (frequencies)

if nargin < 3; error('Need data, window parameters and angle'); end;
if nargin < 4; params=[]; end;

if length(params.tapers)==3 & movingwin(1)~=params.tapers(2);
    error('Duration of data in params.tapers is inconsistent with movingwin(1), modify params.tapers(2) to proceed')
end

[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear err
data=change_row_to_column(data);
[N,C]=size(data);
Nwin=round(Fs*movingwin(1)); % number of samples in window
Nstep=round(movingwin(2)*Fs); % number of samples to step through
nfft=max(2^(nextpow2(Nwin)+pad),Nwin);
f=getfgrid(Fs,nfft,fpass); Nf=length(f);
params.tapers=dpsschk(tapers,Nwin,Fs); % check tapers
params.tapers=tapers;
winstart=1:Nstep:N-Nwin+1;
nw=length(winstart);
if trialave==0; dS=zeros(length(phi),nw,Nf,C); else dS=zeros(length(phi),nw,Nf); end;
for n=1:nw;
   indx=winstart(n):winstart(n)+Nwin-1;
   datawin=data(indx,:);
   [ds,f]=mtdspectrumc(datawin,phi,params);
   dS(:,n,:,:)=ds;
end;
dS=squeeze(dS);
sz=size(dS);
% if length(sz)==3;
%    dS=permute(dS,[2 1 3 4]);
% elseif length(phi)>1
%    dS=permute(dS,[2 1 3]);
% end;
winmid=winstart+round(Nwin/2);
t=winmid/Fs;


function out=acos(x)

precision=x(1).precision;
mpPi=mppi(precision);
out=mpPi/2+i.*log(i*x+sqrt(1-x.^2));

%%%precision=x(1).precision;
%%%out=mp(zeros(size(x)));
%%%
%%%mpPi=mppi(precision);
%%%for ii=1:numel(x)
%%% out(ii)=mpPi/2+i*log(i*x(ii)+sqrt(1-x(ii)^2));
%%%end % for ii=1:max(ex,


function [ gradientParams, SigmaInvs, CholDecomps, Sigmas ] = gradientCCRFFull( params, lambda_a, lambda_b, PrecalcBs, x, y, Precalc_yBys, PrecalcBsFlat)
%GRADIENTPRF Summary of this function goes here
%   Detailed explanation goes here

    nExamples = numel(x);

    numBetas = size(PrecalcBsFlat{1},2);
    numAlphas = numel(params) - numBetas;

    alphasInit = params(1:numAlphas);
    betasInit = params(numAlphas+1:end);
    gradientParams = zeros(size(params));

    % These might be use to calculate the LogLikelihood, don't want to
    % recompute them
    SigmaInvs = cell(nExamples, 1);
    CholDecomps = cell(nExamples, 1);
    Sigmas = cell(nExamples, 1);
    gradients = zeros(nExamples, numel(params));
    for q = 1 : nExamples

        yq = y{q};
        xq = x{q};

        PrecalcB = PrecalcBs{q};
        PrecalcB_flat = PrecalcBsFlat{q};

        [ logGradientsAlphas, logGradientsBetas, SigmaInv, CholDecomp, Sigma ] = gradientCCRF_withoutReg(alphasInit, betasInit, PrecalcB, xq, yq, Precalc_yBys(q, :), PrecalcB_flat);
        SigmaInvs{q} = SigmaInv;
        CholDecomps{q} = CholDecomp;
        Sigmas{q} = Sigma;

        gradients(q,:) = [logGradientsAlphas; logGradientsBetas];
    end
    gradientParams = sum(gradients,1)';
    regAlpha = alphasInit * lambda_a;
    regBeta = betasInit * lambda_b;
    gradientParams = gradientParams - [regAlpha; regBeta];
end


function r = plus(p,q)
% MEAS/PLUS  Implement p + q for meas.

% make a meas called r
r = meas();

% give it the right entries
r.value = p.value + q.value;
r.error = sqrt(p.error^2 + q.error^2);


function [Torr] = ftH2O2Torr(ftH2O)
% Convert pressure from feet of water column at 4 degrees to torr
% Chad Greene 2012
Torr = ftH2O*22.4198;


function ap=aperiodiccomp(apv,dpv,ashift,f0,nshift,imgi)
%	ap=aperiodiccomp(apv,dpv,ashift,f0,nshift,fftl,imgi);
%	Calculate aperiodicity index
%	Input parameters
%		apv, dpv : Upper and lower envelope
%		ashift		: shift step for aperiodicity index calculation (ms)
%		f0			: fundamental frequency (Hz)
%		nshift		: shift step for f0 information (ms)
%		fftl		: FFT size
%		imgi		: display indicator, 1: display on (default) 0: off

%   modified to add the waitbar on 08/Dec./2002
%	modified by Takahashi 10/Aug./2005
%	modified by Kawahara 10/Sept./2005

if nargin==5; imgi=1; end;
%[nn,mm]=size(nsgram);
mm=length(f0);
%%nn=fftl/2+1;
[~,m2]=size(apv);

x=(0:m2-1)'*ashift;
xi=(0:mm-1)'*nshift;
xi=min(max(x),xi);

if imgi==1; hpg=waitbar(0.1,'Interpolating periodicity information'); end;
if imgi==1; drawnow; end;
%ap=interp1q(x,(dpv-apv)',xi)';%,'*linear')';
ap = interp1(x, (dpv-apv)',xi, 'linear', 'extrap')';
if imgi==1; close(hpg); end;


function [lf] = halfspace_medium_leadfield(rd, elc, vol)

% HALFSPACE_MEDIUM_LEADFIELD calculate the halfspace medium leadfield
% on positions pnt for a dipole at position rd and conductivity cond
% The halfspace solution requires a plane dividing a conductive zone of
% conductivity cond, from a non coductive zone (cond = 0)
%
% [lf] = halfspace_medium_leadfield(rd, elc, cond)

% Copyright (C) 2011, Cristiano Micheli and Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

siz = size(rd);
if any(siz==1)
  % positions are specified as a single vector
  Ndipoles = prod(siz)/3;
  rd = rd(:)'; % ensure that it is a row vector
elseif siz(2)==3
  % positions are specified as a Nx3 matrix -> reformat to a single vector
  Ndipoles = siz(1);
  rd = rd';
  rd = rd(:)'; % ensure that it is a row vector
else
  ft_error('incorrect specification of dipole locations');
end

Nelc     = size(elc,1);
lf       = zeros(Nelc,3*Ndipoles);

for i=1:Ndipoles
  % this is the position of dipole "i"
  dip1 = rd((1:3) + 3*(i-1));

  % distances electrodes - dipole
  r1 = elc - ones(Nelc,1) * dip1;

  % Method of mirror dipoles:
  % Defines the position of mirror dipoles being symmetric to the plane
  dip2 = get_mirror_pos(dip,vol);

  % distances electrodes - mirror dipole
  r2 = elc - ones(Nelc,1) * dip2;

  % denominator
  R1 =  (4*pi*vol.cond) * (sum(r1' .^2 ) .^ 1.5)';
  % denominator, mirror term
  R2 = -(4*pi*vol.cond) * (sum(r2' .^2 ) .^ 1.5)';

  % condition of dipoles falling in the non conductive halfspace
  condition = get_dip_halfspace(dip1,vol);

  invacuum = acos(dot(ori,(P-pnt)./norm(P-pnt))) < pi/2;

  if invacuum
    ft_warning('dipole lies on the vacuum side of the plane');
    lf(:,(1:3) + 3*(i-1)) = NaN(Nelc,3);
  elseif any(R1)==0
    ft_warning('dipole coincides with one of the electrodes');
    lf(:,(1:3) + 3*(i-1)) = NaN(Nelc,3);
  else
    lf(:,(1:3) + 3*(i-1)) = (r ./ [R1 R1 R1]) + (rp ./ [R2 R2 R2]);
  end
end


% op_phaseAlignAverages_fd.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,phs]=op_phaseAlignAverages_fd(in,minppm,maxppm,Npts,avg,weighting)
%
% DESCRIPTION:
% Perform time-domain spectral registration using a limited range of
% frequencies and using only phase adjustment (no frequency adjustment).
% This is rarely used.
%
% INPUTS:
% in         = Input data structure.
% minppm     = Minimum of frequency range (ppm).
% maxppm     = Maximum of frequnecy range (ppm).
% Npts       = Number of points in time domain to use for alignment.
% avg        = Align averages to the average of the averages ('y'), or the
%              first average in the series ('n');
% weighting	 = (Optional) Apply less weight to the later points of the fid?
%
% OUTPUTS:
% out        = Output following alignment of averages.
% phs        = Vector of phases (in degrees) used for alignment.

function [out,phs]=op_phaseAlignAverages_fd(in,minppm,maxppm,Npts,avg,weighting)

if ~in.flags.addedrcvrs
    error('ERROR:  I think it only makes sense to do this after you have combined the channels using op_addrcvrs.  ABORTING!!');
end

if nargin<6
    weighting='n';
end

if in.dims.subSpecs==0
    B=1;
else
    B=in.sz(in.dims.subSpecs);
end

phs=zeros(in.sz(in.dims.averages),B);
in_avg=op_averaging(in);
fids=zeros(size(in.fids));
if weighting=='y' || weighting=='Y'
    temp=op_freqrange(in_avg,minppm,maxppm);
    wgt_func=abs(temp.fids).^2;
else
    temp=op_freqrange(in_avg,minppm,maxppm);
    wgt_func=ones(size(temp.fids));
end

for m=1:B
    if avg=='y'||avg=='Y'
        base=in_avg;
        base=op_freqrange(base,minppm,maxppm);
        base=base.fids(1:Npts,m);
        begin=1;
    else
        base=op_freqrange(in,minppm,maxppm);
        base=base.fids(1:Npts,1,m);
        begin=2;
        fids(:,1,m)=in.fids(:,1,m);
    end
    for n=begin:in.sz(in.dims.averages)
        datarange=op_freqrange(in,minppm,maxppm);
        phsdiffs=(phase(base)-phase(datarange.fids(1:Npts,n,m)))*180/pi;
        phs(n,m)=mean(phsdiffs.*wgt_func(1:Npts,m))/mean(wgt_func(1:Npts,m));
        fids(:,n,m)=addphase(in.fids(:,n,m),phs(n,m));
    end
end

%re-calculate Specs using fft
specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

%FILLING IN DATA STRUCTURE
out=in;
out.fids=fids;
out.specs=specs;

%FILLING IN THE FLAGS
out.flags=in.flags;
out.flags.writtentostruct=1;
out.flags.freqcorrected=1;

end


function Vf = fillMissingGrid(xGridV, yGridV, zGridV, Vf)
% function matrixOut = fillMissingGrid(xGridV, yGridV, zGridV, Vf)
%
% This function takes in a matrix that contains NaN values and replaces
% them with values that are linearly interpolated from the surrounding.
%
% APA, 10/03/2012

% Find NaN locations
indNaN = isnan(Vf(:,:,:,1));

% Find Not NaN locations
indNotNaN = ~isnan(Vf(:,:,:,1));

% Create x,y,z 3D grid
[xM,yM,zM] = meshgrid(xGridV, yGridV, zGridV);

% Get x,y,z of NaN points
xNaNv = xM(indNaN);
yNaNv = yM(indNaN);
zNaNv = zM(indNaN);

% Get x,y,z of Not-NaN points
xNotNaNv = xM(indNotNaN);
yNotNaNv = yM(indNotNaN);
zNotNaNv = zM(indNotNaN);

% Get values at Not-NaN points
xVf = Vf(:,:,:,1);
xVfv = xVf(indNotNaN);
yVf = Vf(:,:,:,2);
yVfv = yVf(indNotNaN);
zVf = Vf(:,:,:,3);
zVfv = zVf(indNotNaN);

Fx = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(xVfv));
Fy = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(yVfv));
Fz = TriScatteredInterp([xNotNaNv yNotNaNv zNotNaNv],double(zVfv));

xVfNaN = Fx([xNaNv yNaNv zNaNv]);
yVfNaN = Fy([xNaNv yNaNv zNaNv]);
zVfNaN = Fz([xNaNv yNaNv zNaNv]);

% xVfNaN = zeros(1,length(xNaNv));
% yVfNaN = xVfNaN;
% zVfNaN = xVfNaN;
% numVoxels = length(xNaNv);
%
% for i=1:numVoxels
%     disp([num2str(i),' / ', num2str(numVoxels)])
%     %     % Find distance of NaN from all the Not-NaN points
%     %     distV = (xNaNv(i) - xNotNaNv).^2 + (yNaNv(i) - yNotNaNv).^2 + (zNaNv(i) - zNotNaNv).^2;
%     %     % Sort by distance
%     %     [~,indV] = sort(distV);
%
%     %     indKrig = indV(1:20);
%     indKrig = [];
%     % Search in neighborhood
%     searchRadius = 0.2;
%     while sum(indKrig>0) < 5
%         indKrig = (xNaNv(i) - xNotNaNv).^2 + (yNaNv(i) - yNotNaNv).^2 + (zNaNv(i) - zNotNaNv).^2 <= searchRadius^2;
%         searchRadius = searchRadius + 0.1;
%     end
%     % Kriging
%     dmodelX = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(xVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     xVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelX);
%     dmodelY = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(yVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     yVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelY);
%     dmodelZ = dacefit([xNotNaNv(indKrig) yNotNaNv(indKrig) zNotNaNv(indKrig)], double(zVfv(indKrig)), @regpoly0, @correxp, 10, 1e-1, 20);
%     zVfKrig = predictor([xNaNv(i) yNaNv(i) zNaNv(i)], dmodelZ);
%
%     xNotNaNv = [xNotNaNv; xNaNv(i)];
%     yNotNaNv = [yNotNaNv; yNaNv(i)];
%     zNotNaNv = [zNotNaNv; zNaNv(i)];
%     xVfv = [xVfv; xVfKrig];
%     yVfv = [yVfv; yVfKrig];
%     zVfv = [zVfv; zVfKrig];
%
%     xVfNaN(i) = xVfKrig;
%     yVfNaN(i) = yVfKrig;
%     zVfNaN(i) = zVfKrig;
%
% end

xVf(indNaN) = xVfNaN;
yVf(indNaN) = yVfNaN;
zVf(indNaN) = zVfNaN;
Vf(:,:,:,1) = xVf;
Vf(:,:,:,2) = yVf;
Vf(:,:,:,3) = zVf;


function [varargout] = likGaussWarpExact(warp, hyp, y, mu, varargin)

% likGaussWarp - Warped Gaussian likelihood for regression.
% The expression for the likelihood is
%   likGaussWarp( y | t ) = likGauss( g(y) | t ) * g'(y),
% where likGauss is the Gaussian likelihood and g is the warping function.
%
% The hyperparameters are:
%
% hyp = [ theta_1
%         theta_2
%           ..
%         theta_ng
%         log(sn) ]
%
% Here, sn is the standard deviation of the underlying Gaussian and theta_i for
% i=1..ng are the ng hyperparameters of the warping function g.
%
% At the moment, likGaussWarp offers 3 different warping functions:
% id                   yields g(y) = y  =>  likGaussWarp = likGauss
% poly<m> e.g. 'poly1' yields g(y) = y  =>  likGaussWarp = likGauss
%              'poly3' yields g(y) = y + c1*sy*ay^2 + c2*sy*ay^3
%                             where sy = sign(y), ay = abs(y), cj = exp(theta_j)
% tanh<m> e.g. 'tanh0' yields g(y) = y  =>  likGaussWarp = likGauss
%              'tanh2' yields g(y) = y + a1*tanh(b1*(y+c1)) + a2*tanh(b2*(y+c2))
%                 where aj = exp(theta_j), bj = exp(theta_j+m), bj = theta_j+2*m
%
% The code is based on the exposition in the paper Warped Gaussian Processes,
% NIPS, 2003 by Edward Snelson, Carl Rasmussen and Zoubin Ghahramani.
%
% Several modes are provided, for computing likelihoods, derivatives and moments
% respectively, see likFunctions.m for the details. In general, care is taken
% to avoid numerical issues when the arguments are extreme.
%
% Copyright (c) by Hannes Nickisch, 2013-10-24.
%
% See also LIKFUNCTIONS.M.

lik = {@likGauss}; % in principle any likelihood function can be warped but only
% for homoscedastic likelihoods, in particular Gaussian has feasible integrals
if numel(warp)==0, warp = 'id'; end               % set default warping function
if ischar(warp) || isa(warp,'function_handle'); warp = {warp}; end

ng = feval(warp{:});        % number of hyperparameters for the warping function
nhyp = ['(',num2str(ng),'+',feval(lik{:}),')'];      % number of hyperparameters
if nargin<4, varargout = {nhyp}; return, end       % report number of parameters
nhyp = eval(nhyp);
if nhyp>length(hyp), error('not enough hyperparameters'), end

[gy,lgpy] = feval(warp{:},y,hyp(1:ng));                     % evaluate warping function

% [gy,ig(warp,gy,hyp(1:ng))]'
i = 0; if nargin>6, i = varargin{3}; varargin{3} = varargin{3}-ng; end
varargout = cell(nargout,1);              % allocate memory for output arguments
if i==0 || ng<i                               % only evaluate the required parts
  [varargout{:}] = feval(lik{:},hyp(ng+1:end),gy,mu,varargin{:});     % eval lik
end
if nargin<6                              % prediction mode if inf is not present
  if numel(y)==0,  y = zeros(size(mu)); end
  s2zero = 1; if nargin>4, s2 = varargin{1}; end                       % s2==0 ?
  if nargin>4&&numel(s2)>0&&norm(s2)>eps>0, s2zero = 0; end
  if s2zero                                                    % log probability
    lp = likGaussWarpExact(warp, hyp, y, mu, [], 'infLaplace'); s2 = 0*mu;
  else
    lp = likGaussWarpExact(warp, hyp, y, mu, s2, 'infEP');               % prediction
  end
  if nargout>0, varargout{1} = lp; end                       % works for any lik
  % the predictive moments are very hard to compute for lik not being likGauss
  if nargout>1
    ymu = mu;                                                % first g(y) moment
    sn2 = exp(2*hyp(ng+1));                            % Gaussian noise variance
    ys2 = s2 + sn2;                                         % second g(y) moment
%     ymuM = ig(warp,ymu,hyp(1:ng));                                    % median
%     yupp = ig(warp,ymu+2*sqrt(ys2),hyp(1:ng));       % 95% confidence interval
%     ylow = ig(warp,ymu-2*sqrt(ys2),hyp(1:ng));
%     ys2C = (yupp-ylow).^2/16;

%    N = 20; [t,w] = gauher(N); oN = ones(1,N);     % Gaussian-Hermite quadrature
%    Z = sqrt(ys2(:))*t'+ymu(:)*oN;
%    Y = ig(warp,Z,hyp(1:ng));
%    ymu = Y*w; ys2 = (Y-ymu*oN).^2*w;                % first and second y moment

    ymu = feval(warp{:},mu,hyp(1:ng),'inv');
    % muin = feval(warp{:},mu,hyp(1:ng));
    %ylow = feval(warp{:},muin-10*sqrt(ys2),hyp(1:ng),'inv');
    ylow = feval(warp{:},mu-sqrt(ys2),hyp(1:ng),'inv');
    ys2 = (ymu-ylow).^2;

    varargout{2} = reshape(ymu,size(mu));
    if nargout>2
      varargout{3} = ys2;
    end
  end

else
  inf = varargin{2};                                    % obtain input variables
  switch inf
  case {'infLaplace','infEP'}                     % they have the same structure
    if nargin<7                                             % no derivative mode
      if nargout>0, varargout{1} = varargout{1} + lgpy; end
    else                                                       % derivative mode
      if i<=ng                  % derivatives w.r.t. warping function parameters
        n = max([numel(y),numel(mu)]);
        for j=2:nargout, varargout{j} = zeros(n,1); end
        [dgy,dlgpy] = feval(warp{:},y,hyp(1:ng),i);    % warping function derivative
        out = cell(nargout+1,1);                               % allocate memory
        [out{:}] = likGaussWarpExact(warp, hyp, y, mu, varargin{1:2});    % query lik
        % works only for homoscedastic likelihoods where y and mu can be swapped
        if nargout>0, varargout{1} = dlgpy - out{2}.*dgy; end % apply chain rule
        if nargout>1, varargout{2} =       - out{3}.*dgy; end
        if nargout>2, varargout{3} =       - out{4}.*dgy; end
      end
    end

  case 'infVB'           % output does not depend on mu and following parameters
  end
end


function [ mK ] = CreateConvMtx( vK, numElements, operationMode, convShape )
%UNTITLED6 Summary of this function goes here
%   Detailed explanation goes here

OPERATION_MODE_CONVOLUTION = 1;
OPERATION_MODE_CORRELATION = 2;

CONVOLUTION_SHAPE_FULL         = 1;
CONVOLUTION_SHAPE_SAME         = 2;
CONVOLUTION_SHAPE_VALID        = 3;

switch(operationMode)
    case(OPERATION_MODE_CONVOLUTION)
        vK = vK(end:-1:1);
    case(OPERATION_MODE_CORRELATION)
        % vK = vK; %<! Default Code is correlation
end

kernelLength    = length(vK);
mK = zeros([numElements + kernelLength - 1, numElements]);

for ii = 1:numElements + kernelLength - 1
    kernelLastIdx     = min(kernelLength, kernelLength + numElements - ii);
    kernelFirstIdx    = max(kernelLastIdx - ii + 1, 1);

    kernelEffLength = kernelLastIdx - kernelFirstIdx + 1;

    colLastIdx   = min(ii, numElements);
    colFirstIdx  = colLastIdx - kernelEffLength + 1;

    mK(ii, colFirstIdx:colLastIdx)   = vK(kernelFirstIdx:kernelLastIdx);
end

switch(convShape)
    case(CONVOLUTION_SHAPE_FULL)
        % mK = mK;
    case(CONVOLUTION_SHAPE_SAME)
        rowIdxFirst = 1 + floor(kernelLength / 2);
        rowIdxLast  = rowIdxFirst + numElements - 1;
        mK = mK(rowIdxFirst:rowIdxLast, :);
    case(CONVOLUTION_SHAPE_VALID)
        mK = mK(kernelLength:end - kernelLength + 1, :);
end

end


function []=plot_reprojectVSreal_points_stereo(I1,I2,imagePoints,stereoParams,IndpairsUsed)
%% function for plot each image with reprojected points vs. real points and straight lines, and reprojection error statistics in STEP0
%
% INPUTS:
% * CBimagesInfo
% * cameraCBparameters:  a structure containing all the calibration parameters created in STEP0
%
%%

% plot each image with reprojected points vs. real points and straight lines
Nimages=length(IndpairsUsed);
reprojectedPoints1=stereoParams.CameraParameters1.ReprojectedPoints;
reprojectedPoints2=stereoParams.CameraParameters2.ReprojectedPoints;
reprojectionErrors1=stereoParams.CameraParameters1.ReprojectionErrors;
reprojectionErrors2=stereoParams.CameraParameters2.ReprojectionErrors;

if (stereoParams.CameraParameters1.ImageSize(1)>=stereoParams.CameraParameters1.ImageSize(2))
    n=1; m=8; p1=1:3; p2=4; p3=5:7; p4=8;
else
   n=2; m=6; p1=1:5; p2=6; p3=7:11; p4=12;
end

% PLOT
% define tabs
f = figure('name','Reprojected points on all images. Scroll between tabs to view the different images','units','normalized','outerposition',[.1 .1 .8 .8]);
tabgp = uitabgroup(f);

icount=0;
for iplot=IndpairsUsed'
    icount=icount+1;
    %plot
    tab(icount) = uitab(tabgp,'Title',['IM' num2str(iplot)]);

    axes('Parent',tab(icount)); % somewhere to plot

    subplot(n,m,p1)
    imshow(I1(:,:,:,iplot)); hold all;
    plot(imagePoints(:,1,icount,1), imagePoints(:,2,icount,1),'go','linewidth',1.5);
    plot(reprojectedPoints1(:,1,icount),reprojectedPoints1(:,2,icount),'r+','linewidth',1.5);
    title(['Camera left, Image ' num2str(iplot)]);
    drawnow
    legend('Detected Points','Reprojected Points');
    hold off;

    reprojectionErrorsNow1=reprojectionErrors1(:,:,icount);
    reprojectionErrorsMgnNow1=sqrt(sum(reprojectionErrorsNow1.^2,2));
    reprojectionErrorsNow1=[reprojectionErrorsNow1 reprojectionErrorsMgnNow1];

    subplot(n,m,p2)
    boxplot(reprojectionErrorsNow1,'Labels',{'X','Y','Mgn'});
    ylim([-max(abs(reprojectionErrorsNow1(:))) max(abs(reprojectionErrorsNow1(:)))]);
    title({'Reprojection error'; 'statistics [pix]'});

    subplot(n,m,p3)
    imshow(I2(:,:,:,iplot)); hold all;
    plot(imagePoints(:,1,icount,2), imagePoints(:,2,icount,2),'go','linewidth',1.5);
    plot(reprojectedPoints2(:,1,icount),reprojectedPoints2(:,2,icount),'r+','linewidth',1.5);
    title(['Camera right, Image ' num2str(iplot)]);
    drawnow
    legend('Detected Points','Reprojected Points');
    hold off;

    reprojectionErrorsNow2=reprojectionErrors2(:,:,icount);
    reprojectionErrorsMgnNow2=sqrt(sum(reprojectionErrorsNow2.^2,2));
    reprojectionErrorsNow2=[reprojectionErrorsNow2 reprojectionErrorsMgnNow2];

    subplot(n,m,p4)
    boxplot(reprojectionErrorsNow2,'Labels',{'X','Y','Mgn'});
    ylim([-max(abs(reprojectionErrorsNow2(:))) max(abs(reprojectionErrorsNow2(:)))]);
    title({'Reprojection error'; 'statistics [pix]'});

end

end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


function nwspgr_size_test ( )

%*****************************************************************************80
%
%% NWSPGR_SIZE_TEST tests NWSPGR_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NWSPGR_SIZE_TEST:\n' );
  fprintf ( 1, '  NWSPGR_SIZE returns the size of a sparse grid, based on:\n' );
  fprintf ( 1, '  one of the built-in 1D rules, or a family of 1D rules\n' );
  fprintf ( 1, '  supplied by the user.\n' );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Kronrod-Patterson, [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'kpu', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'kpu', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Kronrod-Patterson, (-oo,+oo), Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'kpn', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'kpn', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Gauss-Legendre, [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'gqu', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'gqu', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Gauss Hermite, (-oo,+oo), [0,1], Dim %d, Level %d Symmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full          %4d\n', nwspgr_size ( 'gqn', d, k, 1, 0 ) );
  fprintf ( 1, '  Compressed    %4d\n', nwspgr_size ( 'gqn', d, k, 1, 1 ) );

  d = 2;
  k = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Clenshaw Curtis, [-1,+1], [0,1], Dim %d, Level %d Unsymmetric\n', d, k );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full        %4d\n', nwspgr_size ( 'cce', d, k, 0, 0 ) );
  fprintf ( 1, '  Compressed  %4d\n', nwspgr_size ( 'cce', d, k, 0, 1 ) );
%
%  Do a table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Clenshaw Curtis Exponential (CCE) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 10
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 6
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 10
      fprintf ( 1, '  %6d', nwspgr_size ( 'cce', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Clenshaw Curtis Exponential (CCE) Uncompressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 10
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 6
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 10
      fprintf ( 1, '  %6d', nwspgr_size ( 'cce', d, k, 0, 0 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a GLL table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Gauss-Legendre Linear (GLL) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 6
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 11
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 6
      fprintf ( 1, '  %6d', nwspgr_size ( 'gqu2', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Do a GLO table.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension / Level table for Gauss-Legendre-Odd (GLO) Compressed\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dim:  ' )
  for d = 1 : 6
    fprintf ( 1, '  %6d', d );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Level:\n' );
  for k = 1 : 11
    fprintf ( 1, '  %2d:  ', k );
    for d = 1 : 6
      fprintf ( 1, '  %6d', nwspgr_size ( 'glo', d, k, 0, 1 ) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function test007 ( )

%*****************************************************************************80
%
%% TEST007 tests TET_MESH_SEARCH_NAIVE and TET_MESH_SEARCH_DELAUNAY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 August 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;
  tet_order = 4;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST007\n' );
  fprintf ( 1, '  TET_MESH_SEARCH_NAIVE uses a naive algorithm\n' );
  fprintf ( 1, '  to search a tetrahedral mesh for the tetrahedron\n' );
  fprintf ( 1, '  containing a point.\n' );
  fprintf ( 1, '  TET_MESH_SEARCH_DELAUNAY uses a faster algorithm\n' );
  fprintf ( 1, '  which is appropriate if the tet mesh is Delaunay.\n' );
%
%  Set up the example tetrahedron mesh.
%
  [ node_num, tet_num ] = tet_mesh_order4_example_size ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  This mesh has tetrahedron order %d\n', tet_order );
  fprintf ( 1, '  The number of tetrahedrons is   %d\n', tet_num );

  [ node_xyz, tet_node ] = tet_mesh_order4_example_set ( node_num, tet_num );
%
%  TET_NEIGHBOR is needed for the Delaunay search.
%
  tet_neighbor = tet_mesh_neighbor_tets ( tet_order, tet_num, tet_node );

  for test = 1 : test_num
%
%  Choose a tetrahedral index at random.
%
    [ tet1, seed ] = i4_uniform ( 1, tet_num, seed );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Point was chosen from tetrahedron    %8d\n', tet1 );
%
%  Choose a point at random from the tetrahedron.
%
    [ p, seed ] = tetrahedron_sample ( node_xyz(1:3,tet_node(1:4,tet1)), 1, seed );
%
%  Naive search.
%
    [ tet2, step_num ] = tet_mesh_search_naive ( node_num, node_xyz, ...
      tet_order, tet_num, tet_node, p );

    fprintf ( 1, '  Naive search ended in tetrahedron    %8d after %d steps\n', ...
      tet2, step_num );
%
%  Delaunay search.
%
    [ tet3, face, step_num ] = tet_mesh_search_delaunay ( node_num, ...
      node_xyz, tet_order, tet_num, tet_node, tet_neighbor, p );

    fprintf ( 1, ...
      '  Delaunay search ended in tetrahedron %8d after %d steps.\n', ...
      tet3, step_num );

  end

  return
end


function c8mat_identity_test ( )

%*****************************************************************************80
%
%% C8MAT_IDENTITY_TEST tests C8MAT_IDENTITY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2015
%
%  Author:
%
%    John Burkardt
%
  m = 4;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8MAT_IDENTITY_TEST\n' );
  fprintf ( 1, '  C8MAT_IDENTITY returns the complex identity matrix.\n' );

  a = c8mat_identity ( n );

  c8mat_print ( m, n, a, '  The identity matrix:' );

  return
end


function bounds = computeBounds(config)
% bounds = computeBounds(config)
%
% This function returns the bounds on the decision variables, to be passed
% to the trajectory optimization solver.
%
% INPUTS:
%   config.dyn = physical parameters of the model
%       .m1 = cart mass
%       .m2 = pole mass
%       .g = gravity
%       .l = pendulum length
%
% OUTPUTS:
%   bounds = struct of bounds for decision variables:
%       .initialTime.lower = scalar
%       .initialTime.upper = scalar
%       .finalTime.lower = scalar
%       .finalTime.upper = scalar
%       .initialState.lower = [ns, 1]
%       .initialState.upper = [ns, 1]
%       .finalState.lower = [ns, 1]
%       .finalState.upper = [ns, 1]
%       .state.lower = [ns, 1]
%       .state.upper = [ns, 1]
%       .control.lower = [nc, 1]
%       .control.upper = [nc, 1]
%

% Unpack physical parameters
m1 = config.dyn.m1;
% m2 = config.dyn.m2;
g = config.dyn.g;
l = config.dyn.l;

% Boundry values
bounds.initialState.lower = [0;0;0;0];
bounds.initialState.upper = bounds.initialState.lower;
bounds.finalState.lower = [config.guess.state(1:2,end);0;0];
bounds.finalState.upper = bounds.finalState.lower;

% Bounds on duration (total trajectory time)
wn = sqrt(l/g);
bounds.initialTime.lower = 0;
bounds.initialTime.upper = 0;
bounds.finalTime.lower = 0.5*(2*pi*wn);
bounds.finalTime.upper = 4*(2*pi*wn);

% Bounds on state:
bounds.state.lower = [-1.0*l; -2*pi; -10*l/wn; -10*pi/wn];
bounds.state.upper = [1.0*l; 2*pi; 10*l/wn; 10*pi/wn];

% Bounds on control:
bounds.control.lower = -5*m1*g;
bounds.control.upper = 5*m1*g;

end


function [output] = F_tanh(input_layer)
input = input_layer.a;
output = tanh(input);

end


function [mu,sig,alpha,beta] = fit_eeg_distribution(X,min_clean_fraction,max_dropout_fraction,quants,step_sizes,beta)
% Estimate the mean and standard deviation of clean EEG from contaminated data.
% [Mu,Sigma,Alpha,Beta] = fit_eeg_distribution(X,MinCleanFraction,MaxDropoutFraction,FitQuantiles,StepSizes,ShapeRange)
%
% This function estimates the mean and standard deviation of clean EEG from a sample of amplitude
% values (that have preferably been computed over short windows) that may include a large fraction
% of contaminated samples. The clean EEG is assumed to represent a generalized Gaussian component in
% a mixture with near-arbitrary artifact components. By default, at least 25% (MinCleanFraction) of
% the data must be clean EEG, and the rest can be contaminated. No more than 10%
% (MaxDropoutFraction) of the data is allowed to come from contaminations that cause lower-than-EEG
% amplitudes (e.g., sensor unplugged). There are no restrictions on artifacts causing
% larger-than-EEG amplitudes, i.e., virtually anything is handled (with the exception of a very
% unlikely type of distribution that combines with the clean EEG samples into a larger symmetric
% generalized Gaussian peak and thereby "fools" the estimator). The default parameters should be
% fine for a wide range of settings but may be adapted to accomodate special circumstances.
%
% The method works by fitting a truncated generalized Gaussian whose parameters are constrained by
% MinCleanFraction, MaxDropoutFraction, FitQuantiles, and ShapeRange. The alpha and beta parameters
% of the gen. Gaussian are also returned. The fit is performed by a grid search that always finds a
% close-to-optimal solution if the above assumptions are fulfilled.
%
% In:
%   X : vector of amplitude values of EEG, possible containing artifacts
%       (coming from single samples or windowed averages)
%
%   MinCleanFraction : Minimum fraction of values in X that needs to be clean
%                      (default: 0.25)
%
%   MaxDropoutFraction : Maximum fraction of values in X that can be subject to
%                        signal dropouts (e.g., sensor unplugged) (default: 0.1)
%
%   FitQuantiles : Quantile range [lower,upper] of the truncated generalized Gaussian distribution
%                  that shall be fit to the EEG contents (default: [0.022 0.6])
%
%   StepSizes : Step size of the grid search; the first value is the stepping of the lower bound
%               (which essentially steps over any dropout samples), and the second value
%               is the stepping over possible scales (i.e., clean-data quantiles)
%               (default: [0.01 0.01])
%
%   ShapeRange : Range that the clean EEG distribution's shape parameter beta may take (default:
%                1.7:0.15:3.5)
%
% Out:
%   Mu : estimated mean of the clean EEG distribution
%
%   Sigma : estimated standard deviation of the clean EEG distribution
%
%   Alpha : estimated scale parameter of the generalized Gaussian clean EEG distribution (optional)
%
%   Beta : estimated shape parameter of the generalized Gaussian clean EEG distribution (optional)
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2013-08-15

% Copyright (C) Christian Kothe, SCCN, 2013, christiankothe@gmail.com
%
% This program is free software; you can redistribute it and/or modify it under the terms of the GNU
% General Public License as published by the Free Software Foundation; either version 2 of the
% License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
% even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with this program; if not,
% write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

% assign defaults
if ~exist('min_clean_fraction','var') || isempty(min_clean_fraction)
    min_clean_fraction = 0.25; end
if ~exist('max_dropout_fraction','var') || isempty(max_dropout_fraction)
    max_dropout_fraction = 0.1; end
if ~exist('quants','var') || isempty(quants)
    quants = [0.022 0.6]; end
if ~exist('step_sizes','var') || isempty(step_sizes)
    step_sizes = [0.01 0.01]; end
if ~exist('beta','var') || isempty(beta)
    beta = 1.7:0.15:3.5; end

% sanity checks
if ~isvector(quants) || numel(quants) > 2
    error('Fit quantiles needs to be a 2-element vector (support for matrices deprecated).'); end
if any(quants(:)<0) || any(quants(:)>1)
    error('Unreasonable fit quantiles.'); end
if any(step_sizes<0.0001) || any(step_sizes>0.1)
    error('Unreasonable step sizes.'); end
if any(beta>=7) || any(beta<=1)
    error('Unreasonable shape range.'); end

% sort data so we can access quantiles directly
X = double(sort(X(:)));
n = length(X);

% calc z bounds for the truncated standard generalized Gaussian pdf and pdf rescaler
for b=1:length(beta)
    zbounds{b} = sign(quants-1/2).*gammaincinv(sign(quants-1/2).*(2*quants-1),1/beta(b)).^(1/beta(b)); %#ok<*AGROW>
    rescale(b) = beta(b)/(2*gamma(1/beta(b)));
end

% determine the quantile-dependent limits for the grid search
lower_min = min(quants);                    % we can generally skip the tail below the lower quantile
max_width = diff(quants);                   % maximum width is the fit interval if all data is clean
min_width = min_clean_fraction*max_width;   % minimum width of the fit interval, as fraction of data

% get matrix of shifted data ranges
X = X(bsxfun(@plus,(1:round(n*max_width))',round(n*(lower_min:step_sizes(1):lower_min+max_dropout_fraction))));
X1 = X(1,:); X = bsxfun(@minus,X,X1);

opt_val = Inf;
% for each interval width...
for m = round(n*(max_width:-step_sizes(2):min_width))
    % scale and bin the data in the intervals
    nbins = round(3*log2(1+m/2));
    H = bsxfun(@times,X(1:m,:),nbins./X(m,:));
    logq = log(histc(H,[0:nbins-1,Inf]) + 0.01);

    % for each shape value...
    for b=1:length(beta)
        bounds = zbounds{b};
        % evaluate truncated generalized Gaussian pdf at bin centers
        x = bounds(1)+(0.5:(nbins-0.5))/nbins*diff(bounds);
        p = exp(-abs(x).^beta(b))*rescale(b); p=p'/sum(p);

        % calc KL divergences
        kl = sum(bsxfun(@times,p,bsxfun(@minus,log(p),logq(1:end-1,:)))) + log(m);

        % update optimal parameters
        [min_val,idx] = min(kl);
        if min_val < opt_val
            opt_val = min_val;
            opt_beta = beta(b);
            opt_bounds = bounds;
            opt_lu = [X1(idx) X1(idx)+X(m,idx)];
        end
    end
end

% recover distribution parameters at optimum
alpha = (opt_lu(2)-opt_lu(1))/diff(opt_bounds);
mu = opt_lu(1)-opt_bounds(1)*alpha;
beta = opt_beta;

% calculate the distribution's standard deviation from alpha and beta
sig = sqrt((alpha^2)*gamma(3/beta)/gamma(1/beta));


function test_bug1082

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_prepare_vol_sens ft_prepare_headmodel ft_compute_leadfield ft_plot_topo3d ft_headmodel_openmeeg

fprintf('***  DIAGNOSTICAL INFORMATION ***\n');
fprintf('test script is running on host: %s\n', gethostname());

%%

% generate a unit sphere
[pnt, tri] = mesh_sphere(162);

% create the BEM geometries (in mm)
bnd = [];
bnd.pnt = pnt * 100;
bnd.tri = tri; % normals outwards
% bnd.tri = fliplr(tri); % normals inwards

% create a set of electrodes
sel = find(pnt(:,3)>0);
sens = [];
sens.chanpos = pnt(sel,:) * 100;
sens.elecpos = sens.chanpos;
for i=1:length(sel)
  sens.label{i} = sprintf('chan%03d', i);
end

% this is the position of the dipole
pos = [0 0 50];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% calculate volume conductors

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'singlesphere';
vol1 = ft_prepare_headmodel(tmpcfg,bnd);
[vol1, sens1] = ft_prepare_vol_sens(vol1, sens);
lf1 = ft_compute_leadfield(pos, sens1, vol1);

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'openmeeg';
vol2 = ft_prepare_headmodel(tmpcfg,bnd);
[vol2, sens2] = ft_prepare_vol_sens(vol2, sens);
lf2 = ft_compute_leadfield(pos, sens2, vol2);

tmpcfg = [];
tmpcfg.conductivity = [1];
tmpcfg.method = 'dipoli';
vol3 = ft_prepare_headmodel(tmpcfg,bnd);
[vol3, sens3] = ft_prepare_vol_sens(vol3, sens);
lf3 = ft_compute_leadfield(pos, sens3, vol3);

%%

figure;
subplot(2,2,1); ft_plot_topo3d(sens1.chanpos, lf1(:,1))
subplot(2,2,2); ft_plot_topo3d(sens1.chanpos, lf1(:,2))
subplot(2,2,3); ft_plot_topo3d(sens1.chanpos, lf1(:,3))
colorbar

figure;
subplot(2,2,1); ft_plot_topo3d(sens2.chanpos, lf2(:,1))
subplot(2,2,2); ft_plot_topo3d(sens2.chanpos, lf2(:,2))
subplot(2,2,3); ft_plot_topo3d(sens2.chanpos, lf2(:,3))
colorbar

figure;
subplot(2,2,1); ft_plot_topo3d(sens3.chanpos, lf3(:,1))
subplot(2,2,2); ft_plot_topo3d(sens3.chanpos, lf3(:,2))
subplot(2,2,3); ft_plot_topo3d(sens3.chanpos, lf3(:,3))
colorbar

% figure;
% subplot(2,2,1); ft_plot_topo3d(senst.chanpos, lft(:,1))
% subplot(2,2,2); ft_plot_topo3d(senst.chanpos, lft(:,2))
% subplot(2,2,3); ft_plot_topo3d(senst.chanpos, lft(:,3))
% colorbar


function kern = svargplvmInitDynKernel(kern, globalOpt, optionsDyn, indexInComp)

% SVARGPLVMINITDYNKERNEL
% VARGPLVM

X = optionsDyn.t;

fprintf('# Initialising kernel %s', kern.type);
if nargin > 3
    fprintf(' in place %d of the comp structure.\n', indexInComp);
else
    fprintf('\n');
end

% This is a recursive function. The recursion ending condition is for a
% kernel to not be a compound.
if isfield(kern, 'comp')
    for i=1:length(kern.comp)
        if ~isfield(kern.comp{i}, 'index') || isempty(kern.comp{i}.index)
            curInds = 1:kern.comp{i}.inputDimension;
        else
            curInds = kern.comp{i}.index;
        end
        curOptionsDyn = optionsDyn;
        curOptionsDyn.t = X(:, curInds);
        kern.comp{i} = svargplvmInitDynKernel(kern.comp{i}, globalOpt, curOptionsDyn, i);
    end
else
    % This is not a compound kernel, so we can initialise this signel
    % element.
    kernelType = kern.type;
   % if strcmp(kernelType, 'white')
   %     kern.variance = 1e-2; % Usual values: 1e-1, 1e-3
   % end

    if strcmp(kernelType, 'whitefixed')
        kern.variance = globalOpt.fixedwhiteVar;
        fprintf(1,'# fixedwhite variance: %d\n',globalOpt.fixedwhiteVar);
    end

    if strcmp(kernelType, 'bias')
        kern.variance = 0.1;
        fprintf(1,'# Bias variance: %d\n',kern.variance);
    end

    if strcmp(kernelType, 'rbfperiodic') || strcmp(kernelType, 'rbfperiodic2')
        kern.period = globalOpt.periodicPeriod;
        fprintf(1,'# periodic period %d\n',globalOpt.periodicPeriod);
         dst = dist2(optionsDyn.t, optionsDyn.t);
         lb = min(dst(:));
         ub = max(dst(:));
         inv_width = 2 / (ub+lb);
         inv_width = globalOpt.inverseWidthMult / (ub+lb); %% NEW
         kern.inverseWidth = inv_width;
         if strcmp(kernelType, 'rbfperiodic2')
             kern.factor = 2*pi/kern.period;
         end
    end

    % The following is related to the expected number of
    % zero-crossings.(larger inv.width numerator, rougher func)
    if strcmp(kernelType,'rbf') || strcmp(kernelType,'matern32') || strcmp(kernelType,'matern52')
        % NEW
         dst = dist2(optionsDyn.t, optionsDyn.t);
         lb = min(dst(:));
         ub = max(dst(:));
         %inv_width = 2 / (ub+lb);
         inv_width = globalOpt.inverseWidthMult / (ub+lb); %% NEW
        if strcmp(kernelType,'rbf')
            kern.inverseWidth = inv_width;
        elseif strcmp(kernelType, 'matern32') || strcmp(kernelType, 'matern52')
            kern.lengthScale = 1/inv_width;
        end

         %kern.comp{1}.inverseWidth = inv_width;
        %kern.inverseWidth = optionsDyn.inverseWidth./(((max(max(optionsDyn.t))-min(min(optionsDyn.t)))).^2);

        if ~globalOpt.mappingInitialisation && (exist('indexInComp') && indexInComp == 1)
            kern.variance = 1;
        end
    end
end


function residual ( )

%*****************************************************************************80
%
%% RESIDUAL assembles the residual vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2006
%
  global area
  global beta
  global difeta
  global fzero
  global indx
  global kappa
  global lambda1
  global lambda2
  global m
  global nel
  global neqn
  global nnodes
  global node
  global nq
  global nu1
  global nu2
  global nunk
  global xc
  global xl
  global xr

  resid = zeros(nunk*neqn,1 );
  xq = zeros(3);
  wq = zeros(3);
%
%  Set up quadrature information on [-1,1]
%
  [ xq, wq ] = gauss3pt;

  for it = 1 : nel

    for iq = 1 : nq

      x = xc(node(it,1)) + area(it) / 2.0 * ( xq(iq) + 1.0 );
      ar = area(it) * wq(iq);
%
%  Evaluate solutions at quadrature points.
%
      [ vh, vhx ] = eval_pt ( x, it, vcur );
      [ ch, chx ] = eval_pt ( x, it, ccur );
      [ fh, fhx ] = eval_pt ( x, it, fcur );
      [ etah, etahx ] = eval_pt ( x, it, etacur );
      [ vhold, vhxold ] = eval_pt ( x, it, vold );
      [ chold, chxold ] = eval_pt ( x, it, cold );
      [ fhold, fhxold ] = eval_pt ( x, it, fold );
      [ etahold, etahxold ] = eval_pt ( x, it, etaold );

      tauxovertau = tau_prob ( fh, fhx, ch, chx );

      for nuk = 1 : 4

        for in = 1 : nnodes

          ip = node(it,in);
          i = indx(ip);

          if ( 0 < i )

            [ bb, bx ] = quadbf ( x, it, in, xc, node );

            term_linear = 0;
            term_nonlin = 0;

            if ( nuk == 1 )
              term_time = ( vh - vhold ) / dt;
              term_nonlin = ( lambda1 * vh * etah ) / ( 1.0 + nu1 * vh );
            elseif ( nuk == 2 )
              term_time = ( ch - chold ) / dt;
              term_nonlin = -lambda1 * vh * etah / ( 1.0 + nu1 * vh );
            elseif ( nuk == 3 )
              term_time = ( fh - fhold ) / dt;
              term_nonlin = -beta * fh * ( fzero - fh ) * etah + ...
                lambda2 * ch * fh / ( 1.0 + nu2 * ch);
            elseif ( nuk == 4 )
              term_time = ( etah - etahold ) / dt;
              term_linear = difeta * bx * etah * ( etahx / etah - tauxovertau );
            end

            k = i + ( nuk - 1 ) * nunk;
            [rhs_source] = rhsfun_source ( x, time, bb, nuk );

            resid(k) = resid(k) + ( rhs_source  ...
              - ( term_time + term_nonlin ) * bb - term_linear ) * ar;

          end

        end

      end

    end

  end
%
%  Calculate norm of residual vector.
%
  resid_norm = sqrt ( resid' * resid ) / ( neqn * nunk )

  return
end


%% simple example of use of renorm_sibling_3d
clear; close all;
x = mandrill;
%% compute roto-translation scattering of an image
options.Q = 2;
Wop = wavelet_factory_3d_pyramid(options, options, options);
Sx = scat(x, Wop);

%% L1 renormalization
op = @(x)(sum(x, 3));
Sx_renorm = renorm_sibling_3d(Sx, op);

%% L1 + smoothing renormalization
op = renorm_factory_L1_smoothing(1);
[Sx_renorm, siblings] = renorm_sibling_3d(Sx, op);

%%
image_scat(Sx, 0, 0);
%%
image_scat(Sx_renorm, 0, 0);
%%
close all;
imagesc(log(image_scat_layer(Sx{3},0,1)+0.001));

%%
image_scat(Sx_renorm, 1, 1);
%%
image_scat(Sx, 1, 1);


function [Hdraw,HvarsDraw,phi_Hdraw,Adraw,h0]=sampleH_Ben(yData,Psi,A,B,phi_H,HvarsOld,priorValues,dataValues,n,h0,HH)

%yData=YData
%Psi=PsiDraw_prop
%A=Adraw
%B=Bdraw
%phi_H=phi_Hdraw'
%HvarsOld=HvarsDraw
%% Initialize
offset_c=priorValues.offset_c; %constant for log transformation

[T,M]=size(yData);
p = size(B,1)/M; %lags of the B vector that has dimensions MxM*p (no constant), each column is one regression

% obtain prior data
b0=priorValues.mean_ln_h0;  %log mean of the initial state (variance scaling parameters) as residuals from an AR(4) in the training sample
a0=eye(n)/priorValues.var_ln_h0(1,1);    %variance of the initial state for the elements of lambda

priorPhi_H=priorValues.phi_h;            %centering parameter for the inverse gamma of the variance of the innovations governing the random walk for lambda
priorD_H  =priorValues.d_h;              %scaling parameter for the inverse gamma distribution for the variance of the innovations governing the random walk for lambda

S_h = priorPhi_H*ones(n,1);
nu_h = 3*ones(n,1);
%% Prepare Data

Y_Psi=yData-Psi;  %subtract the local mean
Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*mean(yData(1:p,:)); %also generate initial conditions for the construction of the lagmatrix
X_Psi = lagmatrix(Y_Psi,1:p); %create RHS of the VAR part
X_Psi = X_Psi(p+1:end,:);     %remove the first p rows of RHS
Y_Psi=Y_Psi(p+1:end,:);       %and do so for LHS

E=Y_Psi-X_Psi*B;              %VAR residuals
Escaled=E*A';                 %transform such that VAR residuals have variance Lambda

Hvars=zeros(T,M);             %sampled states (diagonal elements of lambda)

%% Sample (States,Vars,phi)

for i=1:M
    % prepare for each i
    residsTemp=Escaled(:,i);
    yStar=log(residsTemp.^2+offset_c); %transform scaled residuals
    phi=phi_H(i); %previous draw for the variance of this particular variable
    lnSigma2=log(HvarsOld(p+1:T,i)); % note log => h = ln sigma2

    % sample offset mixture
    [yStarAdj, Ht] = bear.statesMix(yStar,lnSigma2);

   % sample h conditional on the state
    iSig_s = sparse(1:T-p,1:T-p,1./Ht); %state dependent variance
    Kh = HH/phi + iSig_s;
    h_hat = Kh\(h0(i,1)/phi*HH*ones(T-p,1) + iSig_s*(yStarAdj));
    logVarsDraw_Hi = h_hat + chol(Kh,'lower')'\randn(T-p,1);
    Hvars(:,i)=exp([zeros(1,p) logVarsDraw_Hi']');

    % sample phi
%     [phiDraw_Hi]=samplePhi(logVarsDraw_Hi,priorD_H,priorPhi_H);
%     phi_Hdraw(i)=phiDraw_Hi;

end

   % sample phi
    diff = (log(Hvars(p+1:end,:)) - [h0'; log(Hvars(p+1:end-1,:))]).^2;
    phi_Hdraw = 1./gamrnd(nu_h + size(diff,1)/2, 1./(S_h + sum(diff)'/2))';

% sample the initial value
    Kh0 = a0 + sparse(1:n,1:n,1./phi_Hdraw');
    h0_hat = Kh0\(a0*b0 + log(Hvars(p+1,:)')./phi_Hdraw');
    h0 = h0_hat + chol(Kh0,'lower')'\randn(n,1);

%% Sample A
[Adraw]=bear.sampleA_H(yData,Psi,B,Hvars,T,priorValues,dataValues);
AdrawInv=Adraw\eye(M);

%% Construct H
Hdraw=zeros(M,M,T);

for t=1:T
    Hdraw(:,:,t)=AdrawInv*diag(Hvars(t,:))*AdrawInv';
end

HvarsDraw=Hvars;
end


function [fPerMc, res2] = calc_GoodnessFit(mCatalog, fMc, fBinning)
% function [fPerMc, res2] = calc_FitComp(mCatalog, fMc, fBinning);
% -----------------------------------------------------------------------------
%
% Function to calculate  Goodness of fit percentage of a fitted to observed
% magnitude frequency distribution
%
% Incoming variables:
% mCatalog
% fMc
% fBinning
%
% Outgoing variables:
% fPerMc : Percentage of fit
% res2   : Result
%
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 23.01.03

vSel = mCatalog(:,6) > (fMc - (fBinning/2));
nNumberEvents = length(mCatalog(vSel,6));
if nNumberEvents >= 25
    [fDummy fBValue fDummy,  fDummy] =  bmemag(mCatalog(vSel,:));

    fStartMag = fMc; % Starting magnitude (hypothetical Mc)

    % log10(N)=A-B*M
    vMag = [fStartMag:fBinning:10]; % Ending magnitude must be sufficiently high
    vNumber = 10.^(log10(nNumberEvents)-fBValue*(vMag - fStartMag));
    vNumber = round(vNumber);

    % Find the last bin with an event
    nLastEventBin = min(find(vNumber == 0)) - 1;
    if isempty(nLastEventBin)
        nLastEventBin = length(vNumber);
    end
    % Determine set of all magnitude bins with number of events > 0
    ct = round((vMag(nLastEventBin)-fStartMag)*(1/fBinning) + 1);

    PM=vMag(1:ct);
    vNumber = vNumber(1:ct);
    [bval, vDummy] = hist(mCatalog(vSel,6),PM);
    b3 = fliplr(cumsum(fliplr(bval)));    % N for M >= (counted backwards)
    res2 = sum(abs(b3 - vNumber))/sum(b3)*100;
    fPerMc = 100-res2;
end


clear all;

% This script is used to test time and frequency estimation / correction of
% a simulated DroneID burst.  No equalization is done in this script.
% There is a simulated fractional time offset applied, but the script will
% fail if that time offset is anything other than 0 or 0.5.  This is
% something that will likely have to be fixed by the equalizer or by
% upsampling to find the most correct fractional starting time offset

%% Parameters
sample_rate = 15.36e6;
fft_size = get_fft_size(sample_rate);
carrier_spacing = round(sample_rate / fft_size);

% Pick a worst case Parts Per Million (PPM) to support.  20 PPM is the
% value for the HackRF SDR's oscialltor.  Then choose the worst case
% frequency that the SDR will operate at.  In this case 5.9 GHz is the
% highest frequency the radio will need to tune to for DroneID.  Use that
% to then figure out what the worst case frequency offset (in Hz) can be.
% That will be used as the search space for the Integer Frequency Offset
% (IFO) which is done later.
worst_case_ppm = 20;
worst_case_freq = 5.9e9;
max_allowed_freq_offset = worst_case_freq * worst_case_ppm / 1e6;
max_allowed_int_freq_offset = ceil(max_allowed_freq_offset / carrier_spacing);

% Pick a frequency offset to test with
frequency_offset = max_allowed_freq_offset - 3.3e3;

% Calculate the long and short cyclic prefix lengths
[long_cp_len, short_cp_len] = get_cyclic_prefix_lengths(sample_rate);

% Below are the cyclic prefix lengths for each OFDM symbol
cyclic_prefixes = [
    long_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    long_cp_len
];

% Calculate how many samples there are in one DroneID burst
total_burst_sample_count = (fft_size * length(cyclic_prefixes)) + sum(cyclic_prefixes);

% Calculate how many guard carriers there are in one OFDM symbol
[left_guards, right_guards] = get_num_guard_carriers(sample_rate);

% Number of constellation points in the underlying PSK
modulation_order = 4;

% Number of occupied OFDM carriers per symbol
num_data_carriers = 600;

%% OFDM Time Domain Creation
modulator = comm.OFDMModulator( ...
    "FFTLength", fft_size, ...
    "NumGuardBandCarriers", [left_guards; right_guards], ...
    "CyclicPrefixLength", cyclic_prefixes, ...
    "NumSymbols", length(cyclic_prefixes), ...
    "InsertDCNull", true);

% Modulate 9 random OFDM symbols worth of PSK samples
symbols = pskmod( ...
    randi([0, modulation_order - 1], num_data_carriers, length(cyclic_prefixes)), ...
    modulation_order);

% Create the OFDM time domain
modulated_samples = modulator(symbols);

%% Insert ZC Sequence at symbol 4

% Figure out where in the modulated samples that the ZC sequence should
% start (this includes the cyclic prefix)
symbol_4_offset = (fft_size * 3) + sum(cyclic_prefixes(1:3));

% Create the time domain ZC sequence for symbol 4
zc_seq = create_zc(fft_size, 4);

% Tack on the cyclic prefix to build the full symbol
full_zc_seq = [zc_seq(end-cyclic_prefixes(4)+1:end); zc_seq];

% Insert the full ZC sequence symbol in place of the modulated symbol 4
modulated_samples(symbol_4_offset:symbol_4_offset + length(full_zc_seq) - 1) = full_zc_seq;

% Convert to row vector
modulated_samples = reshape(modulated_samples, 1, []);

%% Prepend/Append Zeros

% These zeros are needed to simulate an actual received sample and the
% ambiguity of where the burst actually starts.  The number of samples
% isn't critical, but should be something > about 30 samples
num_zeros = 50;

zero_vec = zeros(1, num_zeros);
modulated_samples = [zero_vec, modulated_samples, zero_vec];

%% Add in a 0.5 sample time offset

% This simulates the clocks of the transmitter and receiver not being
% perfectly synced up.  Upsample by 4, then skip the first two samples, and
% downsample by 4
modulated_samples = resample(modulated_samples, 4, 1);
modulated_samples = [modulated_samples(2:end), 0, 0];
modulated_samples = resample(modulated_samples, 1, 4);

%% Add Noise

% This is not an exact SNR.  Just add gaussian noise to the signal to
% simulate real world reception
modulated_samples = awgn(modulated_samples, 10, 'measured');

figure(1);
plot(10 * log10(abs(modulated_samples).^2));
title('Time Domain with Noise');
%% Apply Frequency Offset

freq_offset_vector = exp(1j * 2 * pi * frequency_offset / sample_rate * [1:length(modulated_samples)]);
freq_offset_samples = modulated_samples .* freq_offset_vector;

figure(2);
subplot(3, 1, 1); plot(10 * log10(abs(fftshift(fft(modulated_samples))).^2));
title('Original Modulated Samples (FFT)')
subplot(3, 1, 2); plot(10 * log10(abs(fftshift(fft(freq_offset_vector))).^2));
title('Frequency Offset Vector (FFT)')
subplot(3, 1, 3); plot(10 * log10(abs(fftshift(fft(freq_offset_samples))).^2));
title('Frequency Offset Samples (FFT)')

%% Find the ZC sequence

% Find the start of the burst by looking at the cyclic prefixes.  Make sure
% to look PAST `num_zeros` since in the real world it will not be known
% where the burst actually starts, so the search space will be rather large
sto_estimate = est_sto(freq_offset_samples, sample_rate, num_zeros * 1.5);

% Calculate how many samples after the start of the burst that the ZC
% sequence should be located (this offset includes the ZC sequence cyclic
% prefix)
zc_seq_offset = (fft_size * 3) + long_cp_len + (short_cp_len * 3);

% Calculate where in the sample vector the ZC sequence starts
auto_corr_index = sto_estimate + zc_seq_offset;

fprintf("Sample offset error: %d\n", sto_estimate - num_zeros - 1);
%% Estimate coarse freq offset and adjust

% Use the cyclic prefix of the ZC sequence to do coarse frequency offset
% estimation (the ZC sequence is the 4th OFDM symbol).
coarse_cp_len = cyclic_prefixes(4);
cp_len_window_size = coarse_cp_len;

% The auto_corr_index points to the start of the ZC sequence, so back off
% by the length of the 4th cyclic prefix
coarse_offset_start = auto_corr_index - coarse_cp_len;

% Extract out `fft_size + coarse_cp_len` samples
window = freq_offset_samples(coarse_offset_start:coarse_offset_start + fft_size + coarse_cp_len - 1);

% Extract the first and last `coarse_cp_len` samples from the window
window_left = window(1:coarse_cp_len);
window_right = window(end - coarse_cp_len + 1:end);

% Calculate the coarse frequency offset in radians and Hz by taking the dot
% product of the two windows
cfo_est_radians = angle(sum(window_left .* conj(window_right))) / fft_size;
cfo_est_hz = cfo_est_radians * sample_rate / (2 * pi);

% Adjust for the frequency offset
cfo_est_adj_vector = exp(1j * cfo_est_radians * [1:length(freq_offset_samples)]);
freq_offset_samples = freq_offset_samples .* cfo_est_adj_vector;

%% Integer frequency offset estimation
% The ZC sequence will be used to find the integer frequency offset.  So,
% extract out just the samples for the ZC sequence (not including the
% cyclic prefix)
received_zc_sym = freq_offset_samples(auto_corr_index:auto_corr_index + fft_size - 1);

figure(3);
plot(abs(received_zc_sym));
title('ZC Sequence (Time Domain Magnitude)');

% Estimate the integer frequency offset
est_int_offset_hz = est_integer_freq_offset(received_zc_sym, sample_rate, max_allowed_int_freq_offset);

% Adjust for the integer frequency offset
ifo_est_adj_vector = exp(1j * 2 * pi * est_int_offset_hz / sample_rate * [1:length(freq_offset_samples)]);
freq_offset_samples = freq_offset_samples .* ifo_est_adj_vector;

% Print out how far off the combination of coarse and integer freq offset
% was from the true value
residual_freq_error_hz = est_int_offset_hz + cfo_est_hz + frequency_offset;
fprintf("Remaining Frequency Offset (Hz): %f\n", residual_freq_error_hz);

%% Extract OFDM Data Carriers
demod = comm.OFDMDemodulator( ...
    "FFTLength", fft_size, ...
    "NumGuardBandCarriers", [left_guards; right_guards], ...
    "CyclicPrefixLength", cyclic_prefixes, ...
    "NumSymbols", length(cyclic_prefixes), ...
    "RemoveDCCarrier", true);

% Extract out just the samples in the burst and run those through the OFDM
% demodulator
burst_samples = freq_offset_samples(sto_estimate:sto_estimate + total_burst_sample_count - 1);
demod_symbols = demod(reshape(burst_samples, [], 1));

% Plot just the data carrying OFDM symbol constellations (the 4th symbol is
% the ZC sequence)
figure(4);
plot(demod_symbols(:,[1,2,3,5,6,7,8,9]), 'o');
title('Demodulated Constellations')


function [dlnZ_dmu, dlnZ_dvs] = heavisideNoiseGradVals(noise, mu, varsigma, y)

% HEAVISIDENOISEGRADVALS Gradient wrt mu and varsigma of log-likelihood for heaviside noise model.

% IVM

D = size(mu, 2);
c = y./sqrt(varsigma);
u = zeros(size(c));
dlnZ_dmu = zeros(size(c));
for i = 1:D
  u(:, i) = c(:, i).*(mu(:, i) + noise.bias(i));
  dlnZ_dmu(:, i) = c(:, i).*ngaussian(u(:, i))...
            ./(cumGaussian(u(:, i))+ ...
               noise.eta/(1-2*noise.eta));
end
dlnZ_dvs = -.5*c.*u.*dlnZ_dmu;


function [best_overlap,best_boxes] = closest_candidates(gt_boxes, candidates)
% do a matching between gt_boxes and candidates

  num_gt_boxes = size(gt_boxes, 1);
  num_candidates = size(candidates, 1);

  iou_matrix = zeros(num_gt_boxes, num_candidates);
  for i = 1:num_gt_boxes
    iou = overlap(gt_boxes(i,:), candidates);
    iou_matrix(i,:) = iou';
  end

  best_overlap = zeros(num_gt_boxes, 1);
  best_boxes = -ones(num_gt_boxes, 4);

  [best_overlap,best_boxes] = greedy_matching(iou_matrix, gt_boxes, candidates);
end

function [best_overlap,best_boxes] = greedy_matching(iou_matrix, gt_boxes, candidates)
  [n, m] = size(iou_matrix);
  assert(n == size(gt_boxes, 1));
  assert(m == size(candidates, 1));
  if n > m
    gt_matching = greedy_matching_rowwise(iou_matrix');
    candidate_matching = (1:m)';
  else
    gt_matching = (1:n)';
    candidate_matching = greedy_matching_rowwise(iou_matrix);
  end

  best_overlap = zeros(n, 1);
  best_boxes = zeros(n, 4);
  for pair_idx = 1:numel(gt_matching)
    gt_idx = gt_matching(pair_idx);
    candidate_idx = candidate_matching(pair_idx);

    best_overlap(gt_idx) = iou_matrix(gt_idx, candidate_idx);
    best_boxes(gt_idx,:) = candidates(candidate_idx, :);
  end
end

function [matching, objective] = greedy_matching_rowwise(iou_matrix)
  assert(size(iou_matrix, 1) <= size(iou_matrix, 2));
  n = size(iou_matrix, 1);
  matching = zeros(n, 1);
  objective = 0;
  for i = 1:n
    % find max element int matrix
    [max_per_row, max_col_per_row] = max(iou_matrix, [], 2);
    [max_iou,row] = max(max_per_row);
    if max_iou == -inf
      break
    end

    objective = objective + max_iou;
    col = max_col_per_row(row);
    matching(row) = col;
    iou_matrix(row,:) = -inf;
    iou_matrix(:,col) = -inf;
  end
end


function Cbar = computeCbar(A, B, ny, nu)
    Cbar_tmp = cell(ny,nu);
    tmp = B;

    for i=1:nu
       for k=1:(i-1)
           Cbar_tmp{k,i} = zeros(size(B));
       end

       for j=i:ny
           Cbar_tmp{j,i} = tmp;
           tmp = A*tmp;
       end

       tmp = B;
    end

    Cbar = cell2mat(Cbar_tmp);
end


function rgb = gray2rgb(im,map,n)
  % GRAY2RGB
  %
  % rgb = gray2rgb(im,map,n)
  %
  % Use a colormap, map, to convert a grayscale/intensity image with n colors
  % to pseudocolor rgb image. The n colors in the original image are mapped
  % evenly to colors in the given colormap, not according to value.
  %
  % rgb = gray2rgb(im,map)
  %
  % Same as above but compute number of colors in im
  %
  % rgb = gray2rgb(im)
  %
  % Use default colormap and compute number of colors in im
  %
  % Inputs:
  %  im  grayscale image, 2d array (h,w)
  %  map  colormap
  %  n  (optional) number of colors in im, default is to compute unique colors,
  %    this is usually preferred but it's time consuming so if the value is
  %    already known then it can be given.
  % Outputs:
  %  rgb  color Red, Green, Blue image, 3d array (h,w,3)
  %
  % See also: rgb2gray, hsv2rgb, rgb2hsv, imshow
  %

  % number of unique colors
  if(~exist('n','var'))
    n = size(unique(im),1);
  end

  if(~exist('map','var'))
    if(~exist('gray2ind'))
      error( ...
        'Image processing toolbox seems not installed, try again without map')
    end
    if(isempty(get(0,'CurrentFigure')))
      map = jet(n);
    else
      map = colormap;
    end
    %map = jet(n);
    m = size(map,1);
  else
    m = size(map,1);
  end

  if(exist('gray2ind')&&n<=65536)

    in = gray2ind(im,n);

    if(strcmp(class(in),'uint8') && (n/(n/m)) > 256)
      in = uint16(in);
    end

    if( m <= 256 )
      rgb = label2rgb(idivide(in,n/m,'fix')+1,map);
    else
      rgb = ind2rgb(idivide(in,n/m,'fix'),map);
    end

    % if it's not going to change the result it's faster to convert to and
    % from uint8 and use label2rgb
    if(strcmp(class(im),'double'))
      rgb = im2double(rgb);
    elseif(strcmp(class(im),'uint16'))
      rgb = im2uint16(rgb);
    end
  else
    % slope
    m = 1/(1/4);
    % precomputation
    mim = m*im;
    nmim = -mim;
    x1 = im<1/8;
    x2 = im>=1/8 & im<3/8;
    x3 = im>=3/8 & im<5/8;
    x4 = im>=5/8 & im<7/8;
    x5 = im>=7/8;
    rgb = cat( 3, ...
      x3.*(mim - (3/8)*m)   + x4.*1 + x5.*(nmim + (9/8)*m), ...
      x2.*(mim - (1/8)*m)   + x3.*1 + x4.*(nmim + (7/8)*m), ...
      x1.*(mim + 0.5)       + x2.*1 + x3.*(nmim + (5/8)*m));
  end
end


function [childId, packetId] = calcVariantId(parentOri,childOri,p2c,varargin)
% compute variantIds and packetId from parent / child orientation pairs
%
% Syntax
%
%   variantId = calcVariantId(parentOri,childOri,p2c)
%
%   % compute packetIds
%   [variantId,packetId] = calcVariantId(parentOri,childOri,p2c,...
%     {hklParent,hklChild})
%
%   % packet determination
%   hklParent = Miller({1,1,1},{1,-1,1},{-1,1,1},{1,1,-1},p2c.CS);
%   hklChild  = Miller(1,0,1,p2c.SS);
%
%   [variantId,packetId] = calcVariantId(parentOri,childOri,p2c,...
%     {hklParent,hklChild})
%
% Input
%  parentOri - parent @orientation
%  childOri  - child @orientation
%  p2c       - parent to child mis@orientation
%  hklParent, hklChild - correspondent planes between parent and child
%
% Output
%  variantId - variant id
%  packetId  - packet id
%

% all child variants
childVariants  = variants(p2c, parentOri);

if size(childVariants,1) == 1
  childVariants = repmat(childVariants,length(childOri),1);
end

% compute distance to all possible variants
d = dot(childVariants,repmat(childOri(:),1,size(childVariants,2)));

% take the best fit
[~,childId] = max(d,[],2);

% compute packetId if required
if nargout == 2
  % Get packet definition
  tmp = getClass(varargin,'cell');
  isMiller = [];
  for ii = 1:length(tmp); isMiller(ii) = ~isempty(getClass(tmp(ii),'Miller')); end

  if sum(isMiller) == 2 % definition given
    ind = find(isMiller);
    h1 = tmp{ind(1)};
    h2 = tmp{ind(2)};
  else % definition assumed
    warning('Packet ID calculation assuming {111}_p||{110}_c');
    h1 = Miller({1,1,1},{1,-1,1},{-1,1,1},{1,1,-1},p2c.CS);
    h2 = Miller(1,0,1,p2c.SS);
  end

  omega = dot(variants(p2c,h1),h2);

  [~,packetId] = max(omega,[],1);

  packetId = packetId(childId);

end

end


function [P_O] = getColourMap(patch, bg_hist, fg_hist, n_bins, grayscale_sequence)
%% GETCOLOURMAP computes pixel-wise probabilities (PwP) given PATCH and models BG_HIST and FG_HIST
    % check whether the patch has 3 channels
    [h, w, d] = size(patch);
    % figure out which bin each pixel falls into
    bin_width = 256/n_bins;
    % convert image to d channels array
    patch_array = reshape(double(patch), w*h, d);
    % to which bin each pixel (for all d channels) belongs to
    bin_indices = floor(patch_array/bin_width) + 1;
    % Get pixel-wise posteriors (PwP)
    P_bg = getP(bg_hist, h, w, bin_indices, grayscale_sequence);
    P_fg = getP(fg_hist, h, w, bin_indices, grayscale_sequence);

    % Object-likelihood map
    P_O = P_fg ./ (P_fg + P_bg);
end


function [Y,W,A] = FDICA(X,W,A,F_i)

%First step: Initialization
% step_size=1e-5;                   %the step distance
step_size = 0.1;
% C_r=0.0002;                             % correlation
max_iteration=1000;                   % the max number of iteration

% if(F_i==33)
%     step_size = step_size/10;
% end
%%%%%%%%%%
frame_N = size(X,2);
% if(F_i==16)
%     a =1;
% end
sign_size = 0;
y_f = W*X;
norm = max(abs(y_f),[],2);
% if(norm>10)
%     norm = repmat(norm,1,size(W,2));
%     W = W./norm;
%     y_f = W*X;
% end

for n_i=1:max_iteration
%         y_fun = power(1+exp(-y_r),-1)+j*power(1+exp(-y_i),-1);
%         W_o1 = step_size*(diag(diag(y_fun*y_f'/frame_N))-(y_fun*y_f'/frame_N))*W_o{K_i}+W_o{K_i};
    y_f = W*X;
    y_r = real(y_f);
    y_i = imag(y_f);
    y_fun = tanh(y_r)+1i*tanh(y_i);
    sign = abs(det(eye(size(X,1))-(y_fun*y_f'/frame_N)));
    A(n_i,F_i-1) = sign;
    %%%%%This part trys to adjust the step_size which is unnecessary
    if(sign>100 && sign_size ==0)
        step_size = step_size/10;
        sign_size =1;
    end
    if(sign>1000 && sign_size ==1)
        step_size = step_size/10;
        sign_size =2;
    end
    if(sign>10000 && sign_size ==2)
        step_size = step_size/10;
        sign_size =3;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    W1 = step_size*(eye(size(X,1))-(y_fun*y_f'/frame_N))*W+W;
    W = W1;

    if(sign<1e-4)
        break;
    end
%     A(n_i,F_i-1) = abs(det(eye(size(X,1))-(y_fun*y_f'/frame_N)));
%     A(n_i,K_i) = abs(det(eye(size(s,2))-(y_fun*y_f'/frame_N)));
%     if(abs(A(n_i,K_i))<0.001) break; end;
end
A(end,F_i-1) = n_i;
Y = y_f;
% Y = Project_back(W,y_f);
return;


function Fitness = CalFitness(PopObj,PopCon)
% Calculate the fitness of each solution

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Kangjia Qiao

    N = size(PopObj,1);
    if nargin == 1
        CV = zeros(N,1);
    else
        CV = sum(max(0,PopCon),2);
    end

    %% Detect the dominance relation between each two solutions
    Dominate = false(N);
    for i = 1 : N-1
        for j = i+1 : N
            if CV(i) < CV(j)
                Dominate(i,j) = true;
            elseif CV(i) > CV(j)
                Dominate(j,i) = true;
            else
                k = any(PopObj(i,:)<PopObj(j,:)) - any(PopObj(i,:)>PopObj(j,:));
                if k == 1
                    Dominate(i,j) = true;
                elseif k == -1
                    Dominate(j,i) = true;
                end
            end
        end
    end

    %% Calculate S(i)
    S = sum(Dominate,2);

    %% Calculate R(i)
    R = zeros(1,N);
    for i = 1 : N
        R(i) = sum(S(Dominate(:,i)));
    end

    %% Calculate D(i)
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Distance = sort(Distance,2);
    D = 1./(Distance(:,floor(sqrt(N)))+2);

    %% Calculate the fitnesses
    Fitness = R + D';
end


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the MCWNNM algorithm for real color image denoising.
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk
%              The Hong Kong Polytechnic University
%
% Please refer to the following paper if you use this code:
%
% @article{MCWNNM,
% 	author = {Jun Xu and Lei Zhang and David Zhang and Xiangchu Feng},
% 	title = {Multi-channel Weighted Nuclear Norm Minimization for Real Color Image Denoising},
% 	journal = {ICCV},
% 	year = {2017}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------

clear;
Original_image_dir  =    'kodak_color/';
fpath = fullfile(Original_image_dir, '*.png');
im_dir  = dir(fpath);
im_num = length(im_dir);

nSig = [5 30 15];

Par.nSig      =   nSig;                                 % Variance of the noise image
Par.win =   20;                                   % Non-local patch searching window
Par.Constant         =  2 * sqrt(2);                              % Constant num for the weight vector
Par.Innerloop =   2;                                    % InnerLoop Num of between re-blockmatching
Par.ps       =   6;                            % Patch size
Par.step        =   5;
Par.Iter          =   6;                            % total iter numbers
Par.display = true;

% Par.method = 'WNNM_ADMM'
Par.method = 'MCWNNM_ADMM'
Par.maxIter = 10;

Par.model = '2';

Par.delta     =   0.1;                                  % Parameter between each iter
Par.lambda = 0.75;
Par.mu = 1.001;
Par.rho = 0.05;
% record all the results in each iteration
Par.PSNR = zeros(Par.Iter, im_num, 'single');
Par.SSIM = zeros(Par.Iter, im_num, 'single');
for i = 1:im_num
    Par.image = i;
    Par.nSig0 = nSig;
    Par.nlsp        =   70;   % Initial Non-local Patch number
    Par.I =  double( imread(fullfile(Original_image_dir, im_dir(i).name)) );
    S = regexp(im_dir(i).name, '\.', 'split');
    [h, w, ch] = size(Par.I);
    Par.nim = zeros(size(Par.I));
    for c = 1:ch
        randn('seed',0);
        Par.nim(:, :, c) = Par.I(:, :, c) + Par.nSig0(c) * randn(size(Par.I(:, :, c)));
    end
    fprintf('%s :\n',im_dir(i).name);
    PSNR =   csnr( Par.nim, Par.I, 0, 0 );
    SSIM      =  cal_ssim( Par.nim, Par.I, 0, 0 );
    fprintf('The initial value of PSNR = %2.4f, SSIM = %2.4f \n', PSNR,SSIM);
    %
    time0 = clock;
    if Par.model == '1'
        [im_out, Par] = MCWNNM_ADMM1_Denoising( Par.nim, Par.I, Par );
    elseif Par.model == '2'
        [im_out, Par] = MCWNNM_ADMM2_Denoising( Par.nim, Par.I, Par );
    else
        [im_out, Par] = MCWNNM_ADMM_Denoising( Par.nim, Par.I, Par );
    end
    fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
    im_out(im_out>255)=255;
    im_out(im_out<0)=0;
    % calculate the PSNR
    Par.PSNR(Par.Iter, Par.image)  =   csnr( im_out, Par.I, 0, 0 );
    Par.SSIM(Par.Iter, Par.image)      =  cal_ssim( im_out, Par.I, 0, 0 );
    imname = sprintf([Par.method '_nSig' num2str(nSig(1)) num2str(nSig(2)) num2str(nSig(3)) '_' Par.model '_Oite' num2str(Par.Iter) '_Iite' num2str(Par.maxIter) '_rho' num2str(Par.rho) '_mu' num2str(Par.mu) '_lambda' num2str(Par.lambda) '_' im_dir(i).name]);
    imwrite(im_out/255, imname);
    fprintf('%s : PSNR = %2.4f, SSIM = %2.4f \n',im_dir(i).name, Par.PSNR(Par.Iter, Par.image),Par.SSIM(Par.Iter, Par.image)     );
end
mPSNR=mean(Par.PSNR,2);
[~, idx] = max(mPSNR);
PSNR =Par.PSNR(idx,:);
SSIM = Par.SSIM(idx,:);
mSSIM=mean(SSIM,2);
fprintf('The best PSNR result is at %d iteration. \n',idx);
fprintf('The average PSNR = %2.4f, SSIM = %2.4f. \n', mPSNR(idx),mSSIM);
name = sprintf([Par.method '_' Par.model '_nSig' num2str(nSig(1)) num2str(nSig(2)) num2str(nSig(3)) '_' Par.model '_Oite' num2str(Par.Iter) '_Iite' num2str(Par.maxIter) '_rho' num2str(Par.rho) '_mu' num2str(Par.mu) '_lambda' num2str(Par.lambda) '.mat']);
save(name,'nSig','PSNR','SSIM','mPSNR','mSSIM');


function test_vbgppcamv(vbpca, ppca)

rand('state', 6);
randn('state', 6);

if nargin < 1
  vbpca = false;
end
if nargin < 2
  ppca = false;
end

inW = [120*rand(1,30); 80*rand(1,30)];
inX = 1:300;

M = length(inW);
N = length(inX);
D = 2;
X = zeros(D,N);
W = zeros(M,D);

% Covariance matrices
logthetaW = log(3e3);
logthetaX = log(10);

% Covariance functions
gpcovW = {@gpcovScale, @(logtheta,x1,x2) gpcov(logtheta,x1,x2,@sqdistEarth)};
logthetaW = [3; logthetaW(:)];
pseudoW = cell(D,1);%zeros([dimW,Mp,D]);
Mp = ceil(0.2*M);
for d=1:D
  permM = randperm(M);
  pseudoW{d} = inW(:,permM(1:Mp));% + randn(2,Mp);
end

gpcovX = @gpcov;

% Generate latent variables
for d=1:D
% $$$   W(:,d) = mvnrnd(zeros(1,M),CovW)';
% $$$   X(d,:) = mvnrnd(zeros(1,N),CovX);

  W(:,d) = gprnd(inW, logthetaW, gpcovW);

  lsX = 10*ceil(exp(logthetaX(end)));
  X(d,:) = filter(hamming(lsX), 1, randn(N,1));
% $$$   W(:,d) = filter(ones(lsW,1)/lsW, 1, randn(M,1));
% $$$   X(d,:) = filter(ones(lsX,1)/lsX, 1, randn(N,1));
  covfunW{d} = gpcovW;
  covfunX{d} = gpcovX;
  initthetaW{d} = logthetaW-1;
  initthetaX{d} = logthetaX-1;
end

W = W * diag( sqrt(M./rowsum(W.^2)) );
X = diag( sqrt(N./colsum(X.^2)) ) * X;

figure
plot(X');
return

% $$$ tsplot(X)
% $$$ tsplot(W')
% $$$ return

% Generate data
Y = W*X;

% Noise level
s2 = 0.1;

% Make noisy observations
Yn = Y + sqrt(s2)*randn(M,N);  % noise
Ynm = Yn; Ynm(rand(M,N)<0.5) = nan; % missing values
Ytest = Yn; Ytest(~isnan(Ynm)) = nan;

initthetaW

% Learn GP PCA
maxiter = 3e1;
Q = vbgppcamv(Ynm,D,inW,inX,covfunW,initthetaW,covfunX,initthetaX, ...
              'maxiter',maxiter, 'pseudodensityx', 0.4, 'pseudodensityw', ...
              0.4, 'loglikelihood', true, 'updatehyper', 5, 'maxsearchx', ...
              3, 'maxsearchw', 200, 'initpseudow', {pseudoW});

est_noise = 1/Q.tau
real_noise = s2

% Learn other models
if ppca
  Qppca = pca_full(Ynm,D,'maxiters',maxiter,'rotate2pca',true, ...
                   'algorithm','ppca');
  Yh_ppca = Qppca.A * Qppca.S + repmat(Qppca.Mu,1,N);
  testrmse_ppca = rmse(Ytest, Yh_ppca)
  noiselessrmse_ppca = rmse(Y, Yh_ppca)
end
if vbpca
  Qvbpca = vbpcamv(Ynm,M-1,'maxiters',maxiter);
  Yh_vbpca = Qvbpca.W * Qvbpca.X + repmat(Qvbpca.mu,1,N);
  testrmse_vbpca = rmse(Ytest, Yh_vbpca)
  noiselessrmse_vbpca = rmse(Y, Yh_vbpca)
end

Yh_gppca = Q.W * Q.X;
testrmse_gppca = rmse(Ytest, Yh_gppca)
noiselessrmse_gppca = rmse(Y, Yh_gppca)
figure
subplot(5,1,1);
plot(Y');
title('Original noiseless data')
subplot(5,1,2);
plot(Ynm');
title('Observed data')
subplot(5,1,3);
plot(Yh_gppca');
title('Reconstruction of GP VB PCA')
if vbpca
  subplot(5,1,4);
  plot(Yh_vbpca');
  title('Reconstruction of VB PCA')
end
if ppca
  subplot(5,1,5);
  plot(Yh_ppca');
  title('Reconstruction of PPCA')
end

vX = Q.varX;%diag(Q.CovX);
eX = 2*sqrt(vX);%sqrt( vX(reshape(1:(N*D), D, N)) );
vW = Q.varW;%diag(Q.CovW);
eW = 2*sqrt(vW);%sqrt( vW(reshape(1:(M*D), M, D)) );
tsgpplot(inX, Q.X', eX', 'pseudoinputs', {Q.pseudoX});

%tsgpplot(inW, Q.W, eW, 'pseudoinputs', {Q.pseudoW});
figure
for d=1:D
  subplot(D,1,d);
  mapproj('global-ellipse');
  mapplot(inW(1,:),inW(2,:),'ro');
  hold on
  mapplot(Q.pseudoW{d}(1,:),Q.pseudoW{d}(2,:),'k+');
  mapcoast
end

figure
mapproj('global-ellipse');
gpmapcolor(Q.pseudoW, Q.Wp, Q.CovWp, -180:5:180, -90:5:90, Q.logthetaW, covfunW);
% $$$ gpmapcolor({Q.pseudoW}, {Q.Wp}, {Q.CovWp}, 0:10:360, 0:10:180, {Q.logthetaW}, ...
% $$$            covfunW);

% $$$ exp(Q.logthetaW{1})
% $$$ exp(Q.logthetaW{2})
% $$$
% $$$ exp(Q.logthetaX{1})
% $$$ exp(Q.logthetaX{2})

% $$$ % DEBUG: (test the effect of CovXp)
% $$$ X = zeros(D,N);
% $$$ varX = zeros(D,N);
% $$$ for d=1:D
% $$$   [X(d,:), varX(d,:)] = gppred(Q.pseudoX{d}, Q.Xp{d}, 0, inX, covfunX{d}, ...
% $$$                                Q.logthetaX{d});
% $$$ end
% $$$ varX;
% $$$ tsgpplot(inX, X', 2*sqrt(varX'), 'pseudoinputs', {Q.pseudoX});
% $$$ exp(Q.logthetaX{1})

% $$$ figure
% $$$ pcolor(Q.CovXp{1});
% $$$ figure
% $$$ pcolor(Q.CovWp{1});

return

%inv(funcK_noiseless(D,[10;10;1;10;10]));return
[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [10;10;1;300;300;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [300;300;10;1;1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [300;300;10;5;5;10;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [1000;100;0.1;3;100;0.059;1;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [100;100;3;100;0.059;1;0.1])

%p(1:4) = [2000;30;0.1;10]
%p(3) = 0.1;
[mu,Cov] = gppred(th,tmv,ymv, @(x1,x2) funcK_noiseless(gpdist(x1,x2),p(1:(end-1))), p(end));
yh = mnorm_rnd(mu, Cov, 10);

figure
clf
plot(th, yh, 'r')
figure
clf
gpplot(th,mu,Cov);
hold on
plot(tmv,ymv,'k+')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function gpmapplot(coord, y)
x = coord';
% Use haversine distance measure
mydist = @(x1,x2) gpdist(x1,x2,@dist_coord);
D = mydist(x,x);
funcK = @(p) gpK(@() gpK_ratquad(D,p(1),p(2),p(3)), ...
                 @() gpK_noise(length(D),p(4)));
[p, loglike] = gplearn(y, funcK, [10;5;1;1e-5])
funcK_noiseless = @(D) gpK(@() gpK_ratquad(D, p(1),p(2),p(3)));
[LONI, LATI] = get_grid(40, 40);
xh = [LONI(:)';LATI(:)'];
[mu,Cov] = gppred(xh,x,y, @(x1,x2) funcK_noiseless(mydist(x1,x2)), p(4));
yh = mu;
ZI = reshape(yh, size(LONI));
plot_map
hold on
m_pcolor(LONI, LATI, ZI);

% Set nice colormap
colormap(climcolmap);
shading flat;
lim = max( -min(yh), max(yh) );
set(gca, 'clim', [-lim lim]);

return

% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function D = gpdist(x1, x2, funcDist)
% $$$ if isvector(x1)
% $$$   x1 = x1(:)';
% $$$   x2 = x2(:)';
% $$$ end
% $$$ if nargin < 3
% $$$   funcDist = @(z1,z2) abs(z1-z2);
% $$$ end
% $$$ %[X2,X1] = meshgrid(x2,x1);
% $$$ n1 = size(x1,2);
% $$$ n2 = size(x2,2);
% $$$ D = zeros(n1,n2);
% $$$ for i=1:n1
% $$$   for j=1:n2
% $$$     D(i,j) = funcDist(x1(:,i),x2(:,j));
% $$$   end
% $$$ end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d = dist_coord(coord1, coord2, varargin)
% d = dist(X1, X2)
% returns geographical distance in kilometers
% input vectors must be the same size and shape!!
% Approximates earth as a sphere.
% Distance is calculated for

if nargin > 2
  nargin
end

% Quadratic mean radius from Wikipedia
R_avg = 6372.795477598;

% Convert to radians
q = pi / 180;
lon1 = coord1(1,:) * q;
lat1 = coord1(2,:) * q;
lon2 = coord2(1,:) * q;
lat2 = coord2(2,:) * q;

% Distance calculation (haversine law)
dlat = lat2 - lat1;
dlon = lon2 - lon1;
a = sin(dlat/2).^2 + cos(lat1).*cos(lat2).*(sin(dlon/2).^2);
c = 2 * atan2(sqrt(a), sqrt(1-a));
d = R_avg * c;

return

% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [p,loglike] = gplearn(y, funcK, init_p)
% $$$ opts = optimset('GradObj', 'on');
% $$$ [p, negloglike] = fminunc(@(p) cost(y, funcK, p), init_p, opts);
% $$$ loglike = -negloglike;
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [mu,Cov] = gppred(xh, x, y, funcK, noise)
% $$$ if nargin == 1
% $$$   x = [];
% $$$   y = [];
% $$$ end
% $$$
% $$$ Kxhx = funcK(xh,x);
% $$$ invKxx = inv(funcK(x,x)+noise^2*eye(length(x)));
% $$$ Kxhxh = funcK(xh,xh);
% $$$ if isempty(Kxhx)
% $$$   Kxhx = 0;
% $$$ end
% $$$ if isempty(invKxx)
% $$$   invKxx = 0;
% $$$ end
% $$$ mu = Kxhx*invKxx*y;
% $$$ if isempty(mu)
% $$$   mu = zeros(size(xh));
% $$$ end
% $$$ Cov = Kxhxh - Kxhx*invKxx*Kxhx';

% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function gpplot(x, mu, Cov)
% $$$ % $$$ e = sqrt(diag(Cov));
% $$$ % $$$ X = [x(1:end); x(end:-1:1)];
% $$$ % $$$ Y = [mu(1:end)+e; mu(end:-1:1)-e(end:-1:1)];
% $$$ % $$$ C = [.65,.65,.65];
% $$$ % $$$ fill(X,Y,C,'EdgeColor',C);
% $$$ % $$$ hold on
% $$$ % $$$ plot(x,mu,'k');
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK(varargin)
% $$$ K = 0;
% $$$ dK = [];
% $$$ for i=1:nargin
% $$$   if nargout == 1
% $$$     Knew = varargin{i}();
% $$$   else
% $$$     [Knew,dKnew] = varargin{i}();
% $$$     n = size(dKnew,3);
% $$$     if isempty(dK)
% $$$       dK = dKnew;
% $$$     else
% $$$       dK(:,:,end+(1:n)) = dKnew;
% $$$     end
% $$$   end
% $$$   K = K + Knew;
% $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK_noise(n, p1);
% $$$ K = p1^2 * eye(n);
% $$$ if nargout >= 2
% $$$   dK = K * 2/p1;
% $$$ end
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function [K, dK] = gpK_sqexp(D, p1, p2)
% $$$ % $$$ K = p1^2*exp(-0.5*D.^2/(p2^2));
% $$$ % $$$
% $$$ % $$$ if nargout >= 2
% $$$ % $$$   dK = zeros([size(D), 2]);
% $$$ % $$$   dK(:,:,1) = K .* 2 / p1;
% $$$ % $$$   dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
% $$$ % $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK_decper(D, p1, p2, p3, p4)
% $$$ K = p1^2*exp(-0.5*(D.^2)/(p2^2)-2*sin(pi*D*p3).^2/(p4^2));
% $$$
% $$$ if nargout >= 2
% $$$   dK = zeros([size(D),4]);
% $$$   dK(:,:,1) = K .* 2 / p1;
% $$$   dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
% $$$   dK(:,:,3) = K .* (-2*sin(pi*D*p3)*2) .* cos(pi*D*p3) .* (pi*D);
% $$$   dK(:,:,4) = K .* (-2*sin(pi*D*p3).^2) * (-2)*p4^(-3);
% $$$ end
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function [K,dK] = gpK_ratquad(D, p1, p2, p3)
% $$$ % $$$ %%% p3 = p3^2;
% $$$ % $$$ f = 1 + D.^2/(2*p3^2*p2^2);
% $$$ % $$$ K = p1^2*f.^(-p3^2);
% $$$ % $$$
% $$$ % $$$ if nargout >= 2
% $$$ % $$$   dK = zeros([size(D),3]);
% $$$ % $$$   dK(:,:,1) = K .* 2 / p1;
% $$$ % $$$   dK(:,:,2) = K .* (-p3^2).*f.^(-1) .* (-2).*D.^2/(2*p3^2)*p2^(-3);
% $$$ % $$$   dK(:,:,3) = K .* (-2*p3*log(f) + (-p3^2)./f.*D.^2/(2*p2^2) * (-2) * p3^(-3));
% $$$ % $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [f, df] = cost(y,funcK,p)
% $$$ [K,dK] = funcK(p);
% $$$ [f,df] = loglikelihood(y,K,dK);
% $$$ f = -f;
% $$$ df = -df;
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [loglike, dloglike] = loglikelihood(y,K,dK)
% $$$ invK = inv(K);
% $$$ logdetK = log(det(K));
% $$$ if logdetK < -1e100;
% $$$   logdetK = -1e100;
% $$$ end
% $$$ n = length(y);
% $$$
% $$$ loglike = -0.5*y'*invK*y - 0.5*logdetK - 0.5*n*log(2*pi);
% $$$
% $$$ if nargout >= 2
% $$$   m = size(dK,3);
% $$$   dloglike = zeros(m,1);
% $$$   a = invK * y;
% $$$   W = a*a' - invK;
% $$$   for i = 1:m
% $$$     dloglike(i) = 0.5*sum(sum(W.*dK(:,:,i)));
% $$$   end
% $$$ end
% $$$
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function y = mnorm_rnd(mu, Cov, n)
% $$$ % $$$ m = max(length(mu), length(Cov));
% $$$ % $$$
% $$$ % $$$ opts.issym = true;
% $$$ % $$$ opts.isreal = true;
% $$$ % $$$ [V,D] = svd(Cov);
% $$$ % $$$ D(D<0) = 0;
% $$$ % $$$ D = sqrt(D);
% $$$ % $$$ A = V * D;
% $$$ % $$$
% $$$ % $$$ y = repmat(mu,1,n) + A*randn(m,n);