function [x,rec] = fsearch(fun,x0,varargin)
% FSEARCH Finds the minimum of a combinatorial function using forward search
%
%   X = FSEARCH(FUN, X0) attempts to find a combination of elements
%   of X0 which locally minimize the function FUN using forward
%   search. FUN accepts input X and returns scalar function value F
%   evaluated at X. X0 must be a vector of candidate indexes.
%   Returned X contains indexes locally minimizing the function
%   FUN.
%
%   X = FSEARCH(FUN, X0, OPTIONS) allows use of optional
%   search parameters. See FSEARCH_OPT for details.
%
%   X = FSEARCH(FUN, X0, OPTIONS, P1, ..., Pn) P1,...,Pn are
%   additional parameters passed to the function FUN.
%
%   [X,REC] = FSEARCH(FUN, X0, ...) returns record of search as a
%   array of structs.
%
%   In forward search the elements are chosen one at the time. The
%   element that minimizes the function value when added to the set
%   is chosen at each round. In a case of tie choose randomly.
%
% Copyright (c) Aki Vehtari (2007)

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% 2007-02-23  Aki Vehtari  <Aki.Vehtari@hut.fi>
%             Use same parameter order as in fminunc etc.
%             Minimize instead of maximize
%             Total rewrite.
if nargin>2
  opt=varargin{1};
  varargin(1)=[];
end

x0=x0(:)';
nx = size(x0,2);  % number of elements
chosen = [];      % the elements chosen sofar
nchosen = 0;      % number of chosen elements
% Options
opt = fsearch_opt(opt);
if opt.nsel>nx
  opt.nsel=nx;
end

% The loop
value=Inf;
minvalue=value;
while (nchosen < opt.nsel)
  values=zeros(nx,1);
  for i1=1:nx
    values(i1) = feval(fun,union(chosen,x0(i1)),varargin{:});
    if (opt.display >= 2)
      fprintf('%d %.5g\n', x0(i1),  values(i1));
    end
  end
  % Let's get the index of the best element
  % In a case of tie, choose randomly
  value=min(values);
  mini=randpick(find(values==value));
  if (opt.stop && value>=minvalue)
    if opt.display
      fprintf('Stopping because value not decreasing\n');
    end
    break
  end
  chosen = union(chosen,x0(mini));   % Add the element into the set of chosen
  nchosen = nchosen + 1;
  % copy some information of this round to struct rec
  rec(nchosen).chosen = chosen;
  rec(nchosen).candidates = x0;
  rec(nchosen).values = values;
  x0(mini) = [];                     % remove it from the x0
  nx=nx-1;
  if value<minvalue
    minvalue=value;
    minvaluei=nchosen;
  end

  if (opt.display >= 1)
    fprintf(' Value: %.4g\n Chosen: %s\n', value, num2str(chosen));
  end

  if (minvalue<opt.stopvalue)
    if opt.display
      fprintf('Objective function value smaller than specified goal\n');
    end
    break
  end
end
x=rec(minvaluei).chosen;


function ind=findn(arr);

%FINDN   Find indices of nonzero elements.
%   I = FINDN(X) returns the indices of the vector X that are
%   non-zero. For example, I = FINDN(A>100), returns the indices
%   of A where A is greater than 100. See RELOP.
%
%   This is the same as find but works for N-D matrices using
%   ind2sub function
%
%   It does not return the vectors as the third output arguement
%   as in FIND
%
%   The returned I has the indices (in actual dimensions)
%
%   x(:,:,1)            x(:,:,2)            x(:,:,3)
%       = [ 1 2 3           =[11 12 13        =[21 22 23
%           4 5 6             14 15 16          24 25 26
%           7 8 9]            17 18 19]         27 28 29]
%
%   I=find(x==25) will return 23
%   but findn(x==25) will return 2,2,3
%
%   Also see find, ind2sub

%   Loren Shure, Mathworks Inc. improved speed on previous version of findn
%   by Suresh Joel Mar 3, 2003

in=find(arr);
sz=size(arr);
if isempty(in), ind=[]; return; end;
[out{1:ndims(arr)}] = ind2sub(sz,in);
ind = cell2mat(out);


function [f,fnorm]=setnorm(f,varargin)
%SETNORM  Set a norm of the input signal to a specified value
%   Usage:  h=setnorm(f,...);
%
%
%   `setnorm(f,...)` will set the input signal *f* to a specified norm.
%
%   `[f,fnorm]=setnorm(f,...)` does the same thing, but in addition
%   returns norm *fnorm* of a signal *f*.
%
%   The norm is specified as a string and may be one of:
%
%     '1'       Normalize the $l^1$ norm to be *1*.
%
%     'area'    Normalize the area of the signal to be *1*. This is exactly the same as `'1'`.
%
%     '2'       Normalize the $l^2$ norm to be *1*.
%
%     'energy'  Normalize the energy of the signal to be *1*. This is exactly
%               the same as `'2'`.
%
%     'inf'     Normalize the $l^{\inf}$ norm to be *1*.
%
%     'peak'    Normalize the peak value of the signal to be *1*. This is exactly
%               the same as `'inf'`.
%
%     'rms'     Normalize the Root Mean Square (RMS) norm of the
%               signal to be *1*.
%
%     's0'      Normalize the S0-norm to be *1*.
%
%     'wav'     Normalize to the $l^{\inf}$ norm to be *0.99* to avoid
%               possible clipping introduced by the quantization procedure
%               when saving as a wav file. This only works with floating
%               point data types.
%
%     'null'    Do NOT normalize, output is identical to input.
%
%
%   It is possible to specify the value to which the norm is set and the dimension:
%
%     'val',v    Value to set the norm to
%
%      'dim',d
%                Work along specified dimension. The default value of `[]`
%                means to work along the first non-singleton one.
%
%   See also: rms, s0norm

if ~isnumeric(f)
  error('%s: Input must be numerical.',upper(mfilename));
end;

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

definput.import={'setnorm'};
definput.keyvals.dim=[];
definput.keyvals.val=[];

[flags,kv]=ltfatarghelper({},definput,varargin,'setnorm');

if flags.do_null || flags.do_norm_notset || isempty(f);
  return
end;

if isa(f,'integer') && ~flags.do_wav
   error('%s: Integer data types are unsupported.',upper(mfilename));
end

if isempty(kv.val)
    kv.val=1;
end
%% ------ Computation --------------------------

[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,[],kv.dim, ...
                                                  upper(mfilename));
fnorm = zeros(W,1);

for ii=1:W

  if flags.do_1 || flags.do_area
    fnorm(ii) =  norm(f(:,ii),1);
  end;

  if flags.do_2 || flags.do_energy
    fnorm(ii) = norm(f(:,ii),2);
  end;

  if flags.do_inf || flags.do_peak
    fnorm(ii) = norm(f(:,ii),Inf);
  end;

  if flags.do_rms
    fnorm(ii) = rms(f(:,ii));
  end;

  if flags.do_s0
    fnorm(ii) = s0norm(f(:,ii));
  end;

  if flags.do_wav
    if isa(f,'float')
       fnorm(ii) = (1/0.99)*norm(f(:,ii),Inf);
    else
       error(['%s: TO DO: Normalizing integer data types not supported ',...
              'yet.'],upper(mfilename));
    end
  end;

  fnorm = 1/kv.val*fnorm;

  if fnorm(ii) > 0
     f(:,ii)=f(:,ii)/fnorm(ii);
  end
end;

f=assert_sigreshape_post(f,kv.dim,permutedsize,order);


function Q=QCoordTurn(T,x,sigmaV2,sigmaTurn2,sigmaLin2)
%%QCOORDTURN Get the discrete-time process noise covariance matrix for a
%            2D or 3D coordinated turn model with a Cartesian state. The
%            turn rate can be specified in terms of a turn rate in radians
%            per second, or in terms of a transversal acceleration.
%            Additionally, a linear acceleration can be given.
%
%INPUTS: T The time-duration of the propagation interval in seconds.
%        x The target state for 2D or 3D motion. If there is no linear
%          acceleration (acceleration along the direction of motion), then
%          x in 2D can either be x=[x;y;xdot;ydot;omega], where omega is
%          the turn rate estimate in radians per second counterclockwise
%          from the x-axis or x can be x=[x;y;xdot;ydot;at] where at is
%          the transversal acceleration, which is orthogonal to the
%          velocity and is defined such that positive values of at map to
%          positive values of omega. In 3D, the state has z and zdot
%          components after y and ydot. If there is a linear acceleration,
%          then in 2D the target state is either x=[x;y;xdot;ydot;omega;al]
%          where omega is the turn rate and al is the linear acceleration
%          or the target state is x=[x;y;xdot;ydot;at;al] if the turn is
%          expressed in terms of a transversal acceleration. Again, in 3D,
%          z and zdot components are respectively added after y and ydot. The
%          dimensionality of the state is used to determine the
%          dimensionality and whether a linear acceleration component is
%          present. The linear acceleration component changes the speed.
%          That means that it acts in the direction of the velocity vector.
%  sigmaV2 The variance driving the process noise affecting the velocity
%          components. This has units of m^2/s^4 and is assumed to be the
%          same in both the x and y dimensions.
% sigmaTurn2 If the turn is specified in terms of a turn rate in radians
%          per second, then this is the variance driving the process noise
%          of the turn rate having units of radians squared per second
%          squared. If the turn is expressed in terms of a transverse
%          acceleration, then this is the variance of the transverse
%          acceleration noise, having units of m^2/s^4.
% sigmaLin2 This parameter is only needed if a linear acceleration is
%          present. It is the variance of the linear acceleration noise
%          having units of m^2/s^4.
%
%OUTPUTS: Q The process noise covariance matrix under a direct discrete-
%           time coordinated turn model where the velocity is specified in
%           Cartesian coordinates. Note that Q is singular, due to the
%           direct-discrete modeling of the process noise.
%
%The basic 2D coordinated turn model is described in Section VA of [1]. It
%is assumed that the continuous-time turn rate model is
%omegaDot=-(1/tau)*Omega+noise, which discretizes to
%omega[k+1]=exp(-T/tau)*omega[k]+noise.
%The turn rate for an unknown process noise is taken from Equation 73 of
%the paper and is a discrete-time approximation, not an exact
%discretization. The ordering of the elements in the state has been changed
%from the paper.
%
%The velocity and position components of the process noise model are
%similar to a discrete white noise acceleration model. Thus, a starting
%point for the process noise parameter sigmaV2 can be found using the
%processNoiseSuggest function with 'PolyKalDirectDisc-ROT' as the model and
%order=1 to cover velocity changes that are not solely due to turns or
%linear accelerations. The changes in the turn rate/transverse acceleration
%that are not due to exponential decay can similarly be covered using
%process noise  starting with sigmaTurn2 being set based on
%processNoiseSuggest with 'PolyKalDirectDisc-ROT' and order=1/2. Similarly,
%a starting point for sigmaLin2 is from processNoiseSuggest with
%'PolyKalDirectDisc-ROT' and order=2.
%
%Note that the process noise added to the turn component and the linear
%acceleration term (if present) is not integrated into the velocity/
%position components. If the only changes allowed/ expected are due to
%modifications in the linear and transverse accelerations (or turn rate),
%then sigmaV2=0. However, regardless of the size of T, the uncertainty in
%the other components would not directly integrate into the position and
%velocity components over any one step. Thus, one would expect the model to
%be particularly poor when the step size is large.
%
%The corresponding transition matrix in 2D is given by the function
%FCoordTurn2D. The corresponding continuous-time functions are
%aCoordTurn2DOmega and aCoordTurn2DTrans with diffusion matrix
%DCoordTurn2D. However, note that the discrete-time functions with
%unknown noise is a direct-discrete model and not a discretization of the
%continuous-time model.
%
%REFERENCES:
%[1] X. R. Li and V. P. Jilkov, "Survey of maneuvering target tracking.
%   Part I: Dynamic models," IEEE Transactions on Aerospace and Electronic
%   Systems, vol. 39, no. 4, pp. 1333-1364, Oct. 2003.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

sigmaV=sqrt(sigmaV2);
sigmaTurn=sqrt(sigmaTurn2);

switch(length(x))
    case 5%2D, there is no linear acceleration.
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T;%Velocity x
           T;%Velocity y
           T];%Turn rate/ linear acceleration
        G(1:4)=G(1:4)*sigmaV;
        G(5)=G(5)*sigmaTurn;
        Q=G*G';
    case 6%2D, there is linear acceleration.
        sigmaLin=sqrt(sigmaLin2);
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T;%Velocity x
           T;%Velocity y
           T;%Turn rate/ linear acceleration
           T];%Transverse acceleration
        G(1:4)=G(1:4)*sigmaV;
        G(5)=G(5)*sigmaTurn;
        G(6)=G(6)*sigmaLin;
        Q=G*G';
    case 7%3D, there is no linear acceleration.
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T^2/2;%Position z
           T;%Velocity x
           T;%Velocity y
           T;%Velocity z
           T];%Turn rate/ linear acceleration
        G(1:6)=G(1:6)*sigmaV;
        G(7)=G(7)*sigmaTurn;
        Q=G*G';
    case 8%3D, there is linear acceleration.
        sigmaLin=sqrt(sigmaLin2);
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T^2/2;%Position z
           T;%Velocity x
           T;%Velocity y
           T;%Velocity z
           T;%Turn rate/ linear acceleration
           T];%Transverse acceleration
        G(1:6)=G(1:6)*sigmaV;
        G(7)=G(7)*sigmaTurn;
        G(8)=G(8)*sigmaLin;
        Q=G*G';
    otherwise
        error('The length of x is neither 5 nor 6 (for 2D) and neither 7 or 8 (for 3D).');
end
end

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


function [ x_scaled ] = ml_scale(x,a,b )
%ML_SCALE Summary of this function goes here
%   Detailed explanation goes here

x_scaled = (x-min(x))*(b-a)/(max(x)-min(x)) + a;
end


% Local Regression and Likelihood, Figure 1.1.
%
% A linear regression and plot for Spencer's mortality
% data. Unlike the book, I use locfit for this!
%
% Setting 'ev','none' means that locfit computes only
% the parametric fit. In this case, 'deg',1; i.e. linear.

load spencer;
fit = locfit(age,mortality,'deg',1,'ev','none');
figure('Name','fig1_1: locfit linear regression');
lfplot(fit);


% AssignmentToIndex Convert assignment to index.
%
%   I = AssignmentToIndex(A, D) converts an assignment, A, over variables
%   with cardinality D to an index into the .val vector for a factor.
%   If A is a matrix then the function converts each row of A to an index.
%
%   See also IndexToAssignment.m and SampleFactors.m
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function I = AssignmentToIndex(A, D)

D = D(:)'; % ensure that D is a row vector
if (any(size(A) == 1)),
    I = cumprod([1, D(1:end - 1)]) * (A(:) - 1) + 1;
else
    I = sum(bsxfun(@times, A - 1, cumprod([1, D(1:end - 1)])), 2) + 1;
end;

end


function triangle_wandzura_rule_test02 ( )

%*****************************************************************************80
%
%% TRIANGLE_WANDZURA_RULE_TEST02 tests WANDZURA_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_WANDZURA_RULE_TEST02\n' );
  fprintf ( 1, '  WANDZURA_RULE returns the points and weights\n' );
  fprintf ( 1, '  of a Wandzura rule for the triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we simply check that the weights\n' );
  fprintf ( 1, '  sum to 1.\n' );

  rule_num = wandzura_rule_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of available rules = %d\n', rule_num );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Rule    Sum of weights\n' );
  fprintf ( 1, '\n' );

  for rule = 1 : rule_num

    order_num = wandzura_order_num ( rule );

    [ xy, w ] = wandzura_rule ( rule, order_num );

    w_sum = sum ( w(1:order_num) );

    fprintf ( 1, '  %8d  %14f\n', rule, w_sum );

  end

  return
end


function [ diff_median_mask]= diff_median( i01, window_side, thres)

i01_size= size( i01);
diff_median_mat= zeros( [ i01_size]);

for j= 1+ window_side: i01_size(1)- window_side                             % rows
    for k= 1+ window_side: i01_size(2)- window_side                         % columns

        i01_temp= i01( j- window_side: j+ window_side, k- window_side:...
            k+ window_side);
        center= i01_temp( window_side+ 1, window_side+ 1);
        median_val= median( i01_temp( :));
        diff_median_mat( j, k)= abs( median_val- center);

    end
end

diff_median_mask= diff_median_mat>= ( thres* max( diff_median_mat( :)));


function [HR, HRS, HlR, HlRS, HiR, HiRS, ChiR, HshR, HshRS] = gaussian_method_v7_1_0(R, pars)
%GAUSSIAN_METHOD_V7_1_0 Estimate entropy values using the Gaussian Method.
%
% ---------
% ARGUMENTS
% ---------
% R    - response matrix
% pars - parameters structure

%   Copyright (C) 2009 Cesare Magri
%   Version 7.1.0

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

persistent previous_Nt previous_mask;

% HiR, HiRS and ChiR are not computed for the gaussian case, null values
% are returned instead:
HiR  = NaN;
HiRS = NaN;
ChiR = NaN;

% We have to recompute Nc, Nt, maxNt (in the case this routine is called by
% quadratic extrapolation)
Nt    = pars.Nt;
Nc    = size(R,1);
maxNt = size(R,2);
totNt = sum(Nt);
Ns    = pars.Ns;

logOf2times2 = 2*log(2);
logOf2piTimesExp1 = log(2*pi*exp(1));

if ~isequal(previous_Nt, Nt)
  mask = build_logical_mask(Nt, maxNt, Ns);
else
	mask = previous_mask;
end
R(:,~mask) = nan;

% H(R) and H_lin(R) -------------------------------------------------------
if pars.doHR || pars.doHlR

    % Removing mean:
    %meanRvec = mean(R(:,mask),2);
		meanRvec = nansum(nansum(R,3),2)./totNt; %JM: this is faster, some juggling is needed to account for the -1's

	  %Rvec = R(:,mask) - meanRvec(:,ones(totNt,1));
    Rvec = R(:,mask) - meanRvec*ones(1,totNt);

    covPr = (Rvec * Rvec') ./ (totNt-1);
end

% H(R)
if pars.doHR
    HR = (Nc * logOf2piTimesExp1 + log(det(covPr))) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HRbias = gaussian_bias(totNt, Nc);
        HR = HR - HRbias;
    end
else
    HR = 0;
end

% H_lin(R)
if pars.doHlR
    HlR = (Nc * logOf2piTimesExp1 + nansum(log(diag(covPr)), 1)) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HlRbias = Nc * gaussian_bias(totNt, 1);
        HlR = HlR - HlRbias;
    end
else
    HlR = 0;
end

% H(R|S) and H_lin(R|S) ---------------------------------------------------
if pars.doHRS || pars.doHlRS

    % Removing mean
    meanR = nansum(R.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
    Rzeromean = R - meanR(:, ones(maxNt,1), :);

    if Nc==1
        % HlRS is never computed for Nc==1, if we got here it's cause we
        % need to compute H(R|S):

				%covPrsMat = covPrsForSingletonNc(Rzeromean, Nt, mask, maxNt, Ns);
        covPrsMat = reshape(nansum(Rzeromean.*Rzeromean,2),1,[])./(Nt'-1); %JM:

        HRS  = (Nc * logOf2piTimesExp1 + log(covPrsMat(:))) / logOf2times2;

        HlRS = 0;

        % Bias correction:
        if pars.biasCorrNum==3
            HRSbias = gaussian_bias(Nt, 1);
            HRS = HRS - HRSbias;
        end
    else
        HRS  = zeros(Ns,1);
        HlRS = zeros(Ns,1);

        for s=1:Ns
            covPrs = Rzeromean(:, 1:Nt(s), s) * Rzeromean(:, 1:Nt(s), s).' ./ (Nt(s)-1);

            if pars.doHRS
                detCovPrs = det(covPrs);

                HRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPrs)) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HRSbias = gaussian_bias(Nt(s), Nc);
                    HRS(s) = HRS(s) - HRSbias;
                end
            end;

            if pars.doHlRS
                diagCovPrs = diag(covPrs);

                HlRS(s) = (length(diagCovPrs) * logOf2piTimesExp1 + sum(log(diagCovPrs))) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HlRSbias = Nc * gaussian_bias(Nt(s), 1);
                    HlRS(s) = HlRS(s) - HlRSbias;
                end
            end
        end
    end

    if ~pars.doHRS
        HRS = 0;
    end

    if ~pars.doHlRS
        HlRS = 0;
    end

else
    HRS  = 0;
    HlRS = 0;
end

% H_sh(R) and H_sh(R|S) ---------------------------------------------------
if pars.doHshR || pars.doHshRS

    Rsh = shuffle_R_across_cells(R, Nt);

    if pars.doHshR
        % Removing mean:
        meanRshvec = mean(Rsh(:,mask),2);
        Rshvec = Rsh(:,mask) - meanRshvec(:,ones(totNt,1));

        covPshr = (Rshvec * Rshvec') ./ (totNt-1);

        HshR = (Nc * logOf2piTimesExp1 + log(det(covPshr))) / logOf2times2;

        % Bias correction:
        if pars.biasCorrNum==3
            HshRbias = gaussian_bias(totNt, Nc);
            HshR = HshR - HshRbias;
        end
    else
        HshR = 0;
    end

    % Remark: HshRS not computed for Nc==1
    if pars.doHshRS
        % Removing mean
        meanRsh = sum(Rsh.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
        Rsh = Rsh - meanRsh(:,ones(maxNt,1),:);

        HshRS = zeros(Ns,1);
        for s=1:Ns
            covPshrs = Rsh(:, 1:Nt(s), s) * Rsh(:, 1:Nt(s), s).' ./ (Nt(s)-1);
            detCovPshrs = det(covPshrs);

            HshRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPshrs)) / logOf2times2;
        end

        % Bias correction:
        if pars.biasCorrNum==3
            HshRSbias = gaussian_bias(Nt(:), Nc);
            HshRS = HshRS - HshRSbias;
        end
    else
        HshRS = 0;
    end

else
    HshR  = 0;
    HshRS = 0;
end

previous_Nt   = Nt;
previous_mask = mask;

function covPrs = covPrsForSingletonNc(R, Nt, mask, maxNt, Ns)

% Removing first singleton dimension:
R2D = zeros(maxNt, Ns);
R2D(mask) = R(1,mask);

% Computing the variance (R.*R faster than R.^2):
covPrs = (sum(R2D.*R2D,1)) ./ (Nt.' - 1);


%  Figure 7.53      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% script for fig. 7.53
%
clf;
dp=[1 1 0];
np=[1];
nc=conv([1 1.001],[8.32 0.8]);
dc=conv([1 4.08],[1 0.0196]);
num=conv(np,nc);
den=conv(dp,dc);
dcl=[0 0 num]+den; % closed-loop denominator
t=0:.1:5;
y=step(num,dcl,t);
plot(t,y);
xlabel('Time (sec)');
ylabel('y(t)');
grid;
title('Fig.7.53: Step response for lag compensation design')


%% Physics-informed dynamic mode decompositions
%
% Computes a dynamic mode decomposition when the solution matrix is
% constrained to lie in a matrix manifold.
% The options available for the "method" so far are
%
% - "exact", "exactSVDS"
% - "orthogonal"
% - "uppertriangular, "lowertriangular"
% - "diagonal", "diagonalpinv", "diagonaltls", "symtridiagonal"
% - "circulant", "circulantTLS", "circulantunitary", "circulantsymmetric",
% "circulantskewsymmetric"
% - "BCCB", "BCCBtls", "BCCBskewsymmetric",
% "BCCBunitary", "hankel", "toeplitz"
% - "symmetric", "skewsymmetric"
%
function [A, varargout] = piDMD(X,Y,method,varargin)

[nx, nt] = size(X);

if strcmp(method,'exact') || strcmp(method,'exactSVDS')

    if nargin>3
        r = varargin{1};
    else
        r = min(nx,nt);
    end

    if strcmp(method,'exact')
        [Ux,Sx,Vx] = svd(X,0);
        Ux = Ux(:,1:r); Sx = Sx(1:r,1:r); Vx = Vx(:,1:r);
    elseif strcmp(method,'exactSVDS')
        [Ux,Sx,Vx] = svds(X,r);
    end

    Atilde = (Ux'*Y)*Vx*pinv(Sx);
    A = @(v) Ux*(Atilde*(Ux'*v));

    if nargout==2
    varargout{1} = eig(Atilde);
    elseif nargout>2
    [eVecs,eVals] = eig(Atilde);
    eVals = diag(eVals);
    eVecs = Y*Vx*pinv(Sx)*eVecs./eVals.';
    varargout{1} = eVals;
    varargout{2} = eVecs;
    end

elseif strcmp(method,'orthogonal')

    if nargin>3
        r = varargin{1};
    else
        r = min(nx,nt);
    end
    [Ux,~,~] = svd(X,0); Ux = Ux(:,1:r);
    Yproj = Ux'*Y; Xproj = Ux'*X; % Project X and Y onto principal components
    [Uyx, ~, Vyx] = svd(Yproj*Xproj',0);
    Aproj = Uyx*Vyx';
    A = @(x) Ux*(Aproj*(Ux'*x));

    if nargout==2
        eVals = eig(Aproj);
        varargout{1} = eVals;
    elseif nargout>2
        [eVecs,eVals] = eig(Aproj);
        varargout{1} = diag(eVals);
        varargout{2} = Ux*eVecs;
        if nargout > 3; varargout{3} = Aproj; end
    end

elseif strcmp(method,'uppertriangular')

    [R,Q] = rq(X); % Q*Q' = I
    Ut = triu(Y*Q');
    A = Ut/R;

elseif strcmp(method,'lowertriangular')

    A = rot90(piDMD(flipud(X),flipud(Y),'uppertriangular'),2);

% The codes allows for matrices of variable banded width. The fourth input,
% a 2xn matrix called d, specifies the upper and lower bounds of the
% indices of the non-zero elements. The first column corresponds to the width of
% the band below the diagonal and the second column is the width of the
% band above. For example, a diagonal matrix would have d = ones(nx,2) and
% a tridiagonal matrix would have d = [2 2]+zeros(nx,2). If you only specify
% d as a scalar then the algorithm converts the input to obtain a banded
% diagonal matrix of width d.
elseif startsWith(method,'diagonal')
    if nargin>3
        d = varargin{1}; % arrange d into an nx-by-2 matrix
        if numel(d) == 1
            d = d*ones(nx,2);
        elseif numel(d) == nx
             d = repmat(d,[1,2]);
        elseif any(size(d)~=[nx,2])
            error('Diagonal number is not in an allowable format.')
        end
    else
        d = ones(nx,2); % default is for a diagonal matrix
    end
    % Allocate cells to build sparse matrix
    Icell = cell(1,nx); Jcell = cell(1,nx); Rcell = cell(1,nx);
    for j = 1:nx
    l1 = max(j-(d(j,1)-1),1); l2 = min(j+(d(j,2)-1),nx);
    C = X(l1:l2,:); b = Y(j,:); % preparing to solve min||Cx-b|| along each row
    if strcmp(method,'diagonal')
            sol = b/C;
    elseif strcmp(method,'diagonalpinv')
            sol = b*pinv(C);
    elseif strcmp(method,'diagonaltls')
            sol = tls(C.',b.').';
    end
    Icell{j} = j*ones(1,1+l2-l1); Jcell{j} = l1:l2; Rcell{j} = sol;
    end
    Imat = cell2mat(Icell); Jmat = cell2mat(Jcell); Rmat = cell2mat(Rcell);
    Asparse = sparse(Imat,Jmat,Rmat,nx,nx);
    A = @(v) Asparse*v;

    if nargout==2
        eVals = eigs(Asparse,nx);
        varargout{1} = eVals;
    elseif nargout>2
        [eVecs, eVals] = eigs(Asparse,nx);
        varargout{1} = diag(eVals); varargout{2} = eVecs;
    end

elseif strcmp(method,'symmetric') || strcmp(method,'skewsymmetric')

[Ux,S,V] = svd(X,0);
C = Ux'*Y*V;
C1 = C;
if nargin>3; r = varargin{1}; else; r = rank(X); end
Ux = Ux(:,1:r);
Yf = zeros(r);
    if strcmp(method,'symmetric')
    for i = 1:r
        Yf(i,i) = real(C1(i,i))/S(i,i);
        for j = i+1:r
            Yf(i,j) = (S(i,i)*conj(C1(j,i)) + S(j,j)*C1(i,j)) / (S(i,i)^2 + S(j,j)^2);
        end
    end
    Yf = Yf + Yf' - diag(diag(real(Yf)));
    elseif strcmp(method,'skewsymmetric')
    for i = 1:r
        Yf(i,i) = 1i*imag(C1(i,i))/S(i,i);
        for j = i+1:nx
            Yf(i,j) = (-S(i,i)*conj(C1(j,i)) + S(j,j)*(C1(i,j))) / (S(i,i)^2 + S(j,j)^2);
        end
    end
    Yf = Yf - Yf' - 1i*diag(diag(imag(Yf)));
    end

    A = @(v) Ux*Yf*(Ux'*v);

    if nargout==2
        varargout{1} = eig(Yf);
    elseif nargout>2
        [eVecs,eVals] = eig(Yf);
        eVals = diag(eVals);
        eVecs = Ux*eVecs;
        varargout{1} = eVals;
        varargout{2} = eVecs;
    end

elseif strcmp(method,'toeplitz') || strcmp(method,'hankel')
   if  strcmp(method,'toeplitz'); J = eye(nx);
   elseif strcmp(method,'hankel'); J = fliplr(eye(nx)); end
    Am = fft([eye(nx) zeros(nx)].',[],1)'/sqrt(2*nx); % Define the left matrix
    B = fft([(J*X)' zeros(nt,nx)].',[],1)'/sqrt(2*nx); % Define the right matrix
    BtB = B'*B;
    AAt = ifft(fft([eye(nx) zeros(nx); zeros(nx,2*nx)]).').'; % Fast computation of A*A'
    y = diag(Am'*conj(Y)*B)'; % Construct the RHS of the linear system
    L = (AAt.*BtB.')'; % Construct the matrix for the linear system
    d = [y(1:end-1)/L(1:end-1,1:end-1) 0]; % Solve the linear system
    newA = ifft(fft(diag(d)).').'; % Convert the eigenvalues into the circulant matrix
    A = newA(1:nx,1:nx)*J; % Extract the Toeplitz matrix from the circulant matrix

elseif startsWith(method,'circulant')

 fX = fft(X); fY = fft(conj(Y));

 d = zeros(nx,1);
    if endsWith(method,'TLS') % Solve in the total least squares sense
        for j = 1:nx
        d(j) = tls(fX(j,:)',fY(j,:)');
        end
    elseif ~endsWith(method,'TLS') % Solve the other cases
    d = diag(fX*fY')./vecnorm(fX,2,2).^2;
        if endsWith(method,'unitary'); d = exp(1i*angle(d));
        elseif endsWith(method,'symmetric'); d = real(d);
        elseif endsWith(method,'skewsymmetric'); d = 1i*imag(d);
        end
    end
  eVals = d; % These are the eigenvalues
  eVecs = fft(eye(nx)); % These are the eigenvectors
 if nargin>3
    r = varargin{1}; % Rank constraint
    res = diag(abs(fX*fY'))./vecnorm(fX')'; % Identify least important eigenvalues
    [~,idx] = mink(res,nx-r); % Remove least important eigenvalues
    d(idx) = 0; eVals(idx) = []; eVecs(:,idx) = [];
 end

 if nargout>1; varargout{1} = eVals; end
 if nargout>2; varargout{2} = eVecs; end

 A = @(v) fft(d.*ifft(v)); % Reconstruct the operator in terms of FFTs

elseif strcmp(method,'BCCB') || strcmp(method,'BCCBtls') || strcmp(method,'BCCBskewsymmetric') || strcmp(method,'BCCBunitary')

    if isempty(varargin); error('Need to specify size of blocks.'); end
    s = varargin{1}; p = prod(s);
    % Equivalent to applying the block-DFT matrix F
    % defined by F = kron(dftmtx(M),dftmtx(N)) to the
    % matrix X
    aF =  @(x) reshape(     fft2(reshape(x ,[s,size(x,2)])) ,[p,size(x,2)])/sqrt(p);
    aFt = @(x) conj(aF(conj(x)));
    fX = aF(conj(X)); fY = aF(conj(Y));
    d = zeros(p,1);

    if strcmp(method,'BCCB')
    for j = 1:p; d(j) = conj(fX(j,:)*fY(j,:)')/norm(fX(j,:)').^2; end
    elseif strcmp(method,'BCCBtls')
    for j = 1:p; d(j) = tls(fX(j,:)',fY(j,:)')'; end
    elseif strcmp(method,'BCCBskewsymmetric')
    for j = 1:p; d(j) = 1i*imag(fY(j,:)/fX(j,:)); end
    elseif strcmp(method,'BCCBsymmetric')
    for j = 1:p; d(j) = real(fY(j,:)/fX(j,:)); end
    elseif strcmp(method,'BCCBunitary')
    for j = 1:p; d(j) = exp(1i*angle(fY(j,:)/fX(j,:))); end
    end

    % Returns a function handle that applies A
     if nargin>4
        r = varargin{2};
        res = diag(abs(fX*fY'))./vecnorm(fX')';
        [~,idx] = mink(res,nx-r);
        d(idx) = 0;
    end
    A = @(x) aF((conj(d).*aFt(x)));
    varargout{1} = d;
    % Eigenvalues are given by d

elseif strcmp(method,'BC') || strcmp(method,'BCtri') || strcmp(method,'BCtls')

    s = varargin{1}; p = prod(s);
        M = s(2); N = s(1);
    if isempty(s); error('Need to specify size of blocks.'); end
    % Equivalent to applying the block-DFT matrix F
    % defined by F = kron(dftmtx(M),eye(N)) to the
    % matrix X
aF  =  @(x) reshape(fft(reshape(x,[s,size(x,2)]),[],2) ,[p,size(x,2)])/sqrt(M);
aFt =  @(x) conj(aF(conj(x)));

fX = aF(X); fY = aF(Y);
    d = cell(M,1);

for j = 1:M
    ls = (j-1)*N + (1:N);
    if strcmp(method,'BC')
        d{j} = fY(ls,:)/fX(ls,:);
    elseif strcmp(method,'BCtri')
        d{j} = piDMD(fX(ls,:),fY(ls,:),'diagonal',2);
    elseif strcmp(method,'BCtls')
        d{j} = tls(fX(ls,:)',fY(ls,:)')';
    end
end

    BD = blkdiag(d{:});
    A = @(v) aFt(BD*aF(v));

elseif strcmp(method,'symtridiagonal')

    T1e = vecnorm(X,2,2).^2; % Compute the entries of the first block
    T1 = spdiags(T1e,0,nx,nx); % Form the leading block
    T2e = dot(X(2:end,:),X(1:end-1,:),2); % Compute the entries of the second block
    T2 = spdiags([T2e T2e],-1:0,nx,nx-1); % Form the second and third blocks
    T3e = [0; dot(X(3:end,:),X(1:end-2,:),2)]; % Compute the entries of the final block
    T3 = spdiags(T1e(1:end-1) + T1e(2:end),0,nx-1,nx-1) ...
         + spdiags(T3e,1,nx-1,nx-1) + spdiags(T3e,1,nx-1,nx-1)'; % Form the final block
    T = [T1 T2; T2' T3]; % Form the block tridiagonal matrix
    d = [dot(X,Y,2); dot(X(1:end-1,:),Y(2:end,:),2) + dot(X(2:end,:),Y(1:end-1,:),2)]; % Compute the RHS vector
    c = real(T)\real(d); % Take real parts then solve linear system
    % Form the solution matrix
    A = spdiags(c(1:nx),0,nx,nx) + spdiags([0;c(nx+1:end)],1,nx,nx) + spdiags([c(nx+1:end); 0],-1,nx,nx);
else
    error('The selected method doesn''t exist.');

end


function R = Ehn_GF(img)

mi = min(img(:));
ma = max(img(:));
img1 = (img-mi)/(ma-mi)*255+0;

log_img = log(img1+1);

[m,n] = size(img1);
r = floor(0.04*max(m,n));
eps = 0.1;
% ***using fast guided filter
base = 255*fastguidedfilter_md(img1/255, img1/255, r, eps^2, max(1,r/4));
% base = 255*guidedfilter(img1/255, img1/255, r, eps^2);
log_base = log(base+1);

log_detail = log_img - log_base;

target_contrast = log(4);
compressfactor = target_contrast/(max(log_base(:))-min(log_base(:)));
log_absolute_scale = (1-compressfactor)*max(log_base(:));
log_output = compressfactor*log_base + log_detail + log_absolute_scale;
R = exp(log_output);
R = min(R, 255);
end


function [rpm] = MHz2rpm(MHz)
% Convert frequency from megahertz to revolutions per minute.
% Chad A. Greene 2012
rpm = MHz*60*1e+6;


function [choices, feedbacks, simulation]=demo_QlearningSimulation()
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [choices, feedbacks] = demo_QlearningSimulation()
% Demo of Q-learning simulation. This demonstrate in particular how to use
% the toolbox to simulate data with a dynamic feedack (behaviour at times t
% can serve directly or indirectly as an input at time t+1)
%
% This is based on simple example of reinforcement learning algorithm.
% This demo first simulates 150 choices of a Q-learning agent when faced to
% a sequence binary alternatives:
% - at each trial, the agent choose one of two actions
% - the action is rewarded according to the probabilistic contingency rule
% - every 25 trials, the contingencies are reversed.
%
% /////////////////////////////////////////////////////////////////////////

f_fname = @f_Qlearning; % evolution function (Q-learning)
g_fname = @g_QLearning; % observation function (softmax mapping)

% Create the feedback rule for the simulation
% =========================================================================

% define which action should be rewarded at each trial (contingencies)
% -------------------------------------------------------------------------
% probability of a positive reward following a 'correct' action
    probRewardGood = 75/100;
% draw 25 random feedbacks
contBloc = +(rand(1,25) < probRewardGood);
% create 6 blocs with reversals
contingencies = [contBloc, 1-contBloc, ...
                 contBloc, 1-contBloc, ...
                 contBloc, 1-contBloc] ;

% create feedback structure for the simulation with VBA
% -------------------------------------------------------------------------
% feedback function. Return 1 if action follow contingencies.
h_feedback = @(yt,t,in) +(yt == contingencies(t));
% feedback structure for the VBA
fb = struct( ...
    'h_fname', h_feedback, ... % feedback function
    'indy', 1, ... % where to store simulated choice
    'indfb', 2, ... % where to store simulated feedback
    'inH', struct() ...
   );

% Simulate choices for the given feedback rule
% =========================================================================

% define parameteters of the simulated agent
% -------------------------------------------------------------------------
% learning rate
theta = VBA_sigmoid(0.65,'inverse',true); % 0.65, once sigm transformed
% inverse temperature
phi = log(2.5); % will be exp transformed
% initial state
x0 = [.5; .5];

% options for the simulation
% -------------------------------------------------------------------------
% number of trials
n_t = numel(contingencies);
% fitting binary data
options.sources.type = 1;
% Normally, the expected first observation (choice) is g(x1), ie. after
% a first iteratition x1 = f(x0). The skipf flag will prevent this evolution
% and thus set x1 = x0
options.skipf = [1 zeros(1,n_t)];

% simulate choices
% -------------------------------------------------------------------------
[y,x,x0,eta,e,u] = VBA_simulate ( ...
    n_t+1, ... number of trials
    f_fname, ... evolution function
    g_fname, ... observation function
    theta, ... evolution parameters (learning rate)
    phi, ... observation parameters,
    nan(2,n_t), ... dummy inputs
    Inf, Inf, ... deterministic evolution and observation
    options, ... options
    x0, ... initial state
    fb ... feedback rule
   );

% plot simulated choices
% -------------------------------------------------------------------------
hf = figure( ...
    'name', 'Simulated Q-learning behaviour', ...
    'color','w' ...
   );
ha = axes('parent',hf,'nextplot','add');
plot(ha,y,'kx')
plot(ha,y-e,'r')
legend(ha,{'y: agent''s choices','p(y=1|theta,phi,m): behavioural tendency'})

% Return simulated choices, feedbacks, and parameters used for the
% simulation
% =========================================================================
choices = u(1,2:end);
feedbacks = u(2,2:end);
simulation = struct( ...
    'state', x(:,1:end-1), ...
    'initial', x0, ...
    'evolution', theta, ...
    'observation', phi ...
    );


function [uu,Up,Uc] = depixellise(pix,cal)

% DEPIXELLISE  Pixellic to metric conversion
%   DEPIXELLISE(PIX,CAL) returns the metric coordinates of an image pixel
%   PIX given camera intrinsic parameters CAL = [u0 v0 au av]'
%
%   [u,Up,Uc] = DEPIXELLISE(...) gives the jacobians wrt PIX and CAL.
%
%   See also INVPINHOLE.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

u0 = cal(1);
v0 = cal(2);
au = cal(3);
av = cal(4);

u = pix(1,:);
v = pix(2,:);

uu = [(u-u0)/au
    (v-v0)/av];

if nargout > 1
    Up = [...
        [ 1/au,    0]
        [    0, 1/av]];
    Uc = [...
        [        -1/au,            0, (-u+u0)/au^2,            0]
        [            0,        -1/av,            0, (-v+v0)/av^2]];
end

return

%% jacobians test

syms u0 v0 au av u v real
pix = [u;v];
cal = [u0;v0;au;av];

[u,Up,Uc] = depixellise(pix,cal);

simplify(Up - jacobian(u,pix))
simplify(Uc - jacobian(u,cal))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [X_poly] = polyFeatures(X, p)
%POLYFEATURES Maps X (1D vector) into the p-th power
%   [X_poly] = POLYFEATURES(X, p) takes a data matrix X (size m x 1) and
%   maps each example into its polynomial features where
%   X_poly(i, :) = [X(i) X(i).^2 X(i).^3 ...  X(i).^p];
%

% You need to return the following variables correctly.
X_poly = zeros(numel(X), p);

% ====================== YOUR CODE HERE ======================
% Instructions: Given a vector X, return a matrix X_poly where the p-th
%               column of X contains the values of X to the p-th power.
%
%

% Think of some vectorized implementation
for i = 1:numel(X)
    for j = 1:p
        X_poly(i, j) = X(i)^j;
    end
end

% =========================================================================

end


% Mathematics Q1344369
% https://math.stackexchange.com/questions/1344369
% Compressive Sensing over the Complex Domain
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     14/04/2018
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

numRows = 64;
numCols = 256;

paramLambda = 4.0;

numIterations = 1000;

cSolversFun = {@(mA, vB, paramLambda, numIterations) SolveLsL1ComplexSubGrad(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealSubGrad(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexPgm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealPgm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexAdmm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexIrls(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexCd(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealCd(mA, vB, paramLambda, numIterations);};

cMethodString = {['Sub Gradient Method']; ['Sub Gradient Method - Real Domain']; ...
    ['Proximal Gradient Method']; ['Proximal Gradient Method - Real Domain']; ...
    ['ADMM Method']; ['Fixed Point Iteration (IRLS) Method']; ...
    ['Coordinate Descent (CD) Method']; ['Coordinate Descent (CD) Method - Real Domain']};

%% Generate Data

mA = randn([numRows, numCols]) + (1i * randn([numRows, numCols]));
vB = randn([numRows, 1]) + (1i * randn([numRows, 1]));

hCalcErrorNorm = @(mX, vXRef) sum((mX - vXRef) .* conj(mX - vXRef));
hCalcObjFunVal = @(mX) 0.5 * sum(((mA * mX) - vB) .* conj((mA * mX) - vB)) + paramLambda * sum(abs(mX), 1);

%% Solution by CVX

tic();
cvx_begin quiet
    variable vXCvx(numCols) complex
    minimize( (0.5 * sum_square_abs((mA * vXCvx) - vB)) + paramLambda * norm(vXCvx, 1) )
cvx_end
toc();

% disp([' ']);
% disp(['CVX Solution Summary']);
% disp(['The CVX Solver Status - ', cvx_status]);
% disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
% disp(['The Optimal Argument Is Given By - [ ', num2str(vXCvx.'), ' ]']);
% disp([' ']);

%% Method Analysis

numMethods = length(cSolversFun);

vRunTime = zeros([numMethods, 1]);
mErrorNorm = zeros([numIterations, numMethods]);
mObjFunVal = zeros([numIterations, numMethods]);

for ii = 1:numMethods
    hRunTime = tic();
    [vX, mX] = cSolversFun{ii}(mA, vB, paramLambda, numIterations);
    runTime = toc(hRunTime);

    vRunTime(ii)         = runTime;
    mErrorNorm(:, ii)    = hCalcErrorNorm(mX, vXCvx);
    mObjFunVal(:, ii)    = hCalcObjFunVal(mX);
end

%% Display Results

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot([1:numIterations], 10 * log10(mErrorNorm));
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', ['The Error Norm - $ {\left\| {x}^{k} - {x}_{CVX} \right\|}_{2}^{2} $'], ...
    'FontSize', fontSizeTitle, 'Interpreter', 'latex');
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Error Norm [dB]', ...
    'FontSize', fontSizeAxis);
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);
hLegend = ClickableLegend(cMethodString);
set(hLegend, 'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot([1:numIterations], mObjFunVal);
set(hLineSeries, 'LineWidth', lineWidthNormal);
% set(hLineSeries(5), 'LineStyle', ':', 'LineWidth', lineWidthThin);
set(get(hAxes, 'Title'), 'String', ['The Objection Function Value - $ \frac{1}{2} {\left\| A {x}^{k} - b \right\|}_{2}^{2} + \lambda {\left\| {x}^{k} \right\|}_{1} $'], ...
    'FontSize', fontSizeTitle, 'Interpreter', 'latex');
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Objective Function Value', ...
    'FontSize', fontSizeAxis);
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);
hLegend = ClickableLegend(cMethodString);
set(hLegend, 'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
hBarObj = bar(1:length(vRunTime), vRunTime);
% set(hLineSeries, 'LineWidth', lineWidthNormal);
% set(hLineSeries(2:end), 'LineStyle', ':');
set(get(hAxes, 'Title'), 'String', ['L_1 Regularized Least Squares - Methods Run Time'], ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTickLabel', cMethodString, 'XTickLabelRotation', 45);
set(get(hAxes, 'XLabel'), 'String', 'Method', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Run Time [Sec]', ...
    'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function D = dijkstra( G , S )
% --------------------------------------------------------------------
%      Mark Steyvers, Stanford University, 12/19/00
% --------------------------------------------------------------------
%
% DIJKSTRA  Find shortest paths in graphs
% 	D = dijkstra( G , S ) use the full or sparse matrix G in which
% 	an entry (i,j) represents the arc length between nodes i and j in a
%	graph. In a full matrix, the value INF represents the absence of an arc;
%	in a sparse matrix, no entry at (i,j) naturally represents no arc.
%
%	S is the one-dimensional matrix of source nodes for which the shortest
%	to ALL other nodes in the graphs will be calculated. The output matrices
%	D and P contain the shortest distances and predecessor indices respectively.
%	An infinite distance is represented by INF. The predecessor indices contain
%	the node indices of the node along the shortest path before the destination
%	is reached. These indices are useful to construct the shortest path with the
%	function pred2path (by Michael G. Kay).
%
%	This function was implemented in C++. The source code can be compiled to a
%	Matlab compatible mex file by the command "mex -O dijkstra.cpp" at the Matlab
%	prompt. In this package, we provide a compiled .dll version that is
%       compatible all Windows based machines.  If you are not working on a
%       Windows platform, delete the .dll version provided and recompile from
%       the .cpp source file.  If you do not have the Matlab compiler or a Windows
%       platform, delete the .dll version and dijkstra will then call the
%	Matlab function dijk.m (by Michael G. Kay).  Note that this Matlab
%	code is several orders of magnitude slower than the C based mex file.

N = size( G , 1 );
D = dijk( G , S , 1:N );


function V = mttkrp(X,U,n)
%MTTKRP Matricized tensor times Khatri-Rao product for sparse tensor.
%
%   V = MTTKRP(X,U,n) efficiently calculates the matrix product of the
%   n-mode matricization of X with the Khatri-Rao product of all
%   entries in U, a cell array of matrices, except the nth.  How to
%   most efficiently do this computation depends on the type of tensor
%   involved.
%
%   See also SPTENSOR, TENSOR/MTTKRP, SPTENSOR/TTV
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% In the sparse case, it is most efficient to do a series of TTV operations
% rather than forming the Khatri-Rao product.

N = ndims(X);

if (n == 1)
    R = size(U{2},2);
else
    R = size(U{1},2);
end

V = zeros(size(X,n),R);
for r = 1:R
    % Set up cell array with appropriate vectors for ttv multiplication
    Z = cell(N,1);
    for i = [1:n-1,n+1:N]
        Z{i} = U{i}(:,r);
    end
    % Perform ttv multiplication
    V(:,r) = double(ttv(X, Z, -n));
end


% distance to wall
function [data,units] = compute_dist2wall_rect(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  x = trx(fly).x_mm;
  y = trx(fly).y_mm;

  % The ordering of the arguments is to account for weird y directions of images.
  dtop = getDist(trx.landmark_params{n}.tl_x(fly),trx.landmark_params{n}.tl_y(fly), trx.landmark_params{n}.tr_x(fly),trx.landmark_params{n}.tr_y(fly),...
    x,y);
  dleft = getDist(trx.landmark_params{n}.bl_x(fly),trx.landmark_params{n}.bl_y(fly), trx.landmark_params{n}.tl_x(fly),trx.landmark_params{n}.tl_y(fly),...
    x,y);
  dright = getDist(trx.landmark_params{n}.tr_x(fly),trx.landmark_params{n}.tr_y(fly), trx.landmark_params{n}.br_x(fly),trx.landmark_params{n}.br_y(fly),...
    x,y);
  dbottom = getDist(trx.landmark_params{n}.br_x(fly),trx.landmark_params{n}.br_y(fly), trx.landmark_params{n}.bl_x(fly),trx.landmark_params{n}.bl_y(fly),...
    x,y);

  data{i} = min([dtop;dleft; dright; dbottom],[],1);

end
units = parseunits('mm');

function d = getDist(p1_x,p1_y,p2_x,p2_y,p_x,p_y)

dp1p2 = (p1_x-p2_x).^2 + (p1_y-p2_y).^2;

dotpr_x = (p1_x - p_x).*(p1_x-p2_x);
dotpr_y = (p1_y - p_y).*(p1_y-p2_y);

t = (dotpr_x+dotpr_y)/dp1p2;
proj_x = p1_x + t.*(p2_x-p1_x);
proj_y = p1_y + t.*(p2_y-p1_y);
d = sqrt(  (p_x-proj_x).^2 + (p_y-proj_y).^2);

% Find whether the mice is in the interior of the square.
side = sign((p2_x-p1_x)*(p_y-p1_y)-(p2_y-p1_y)*(p_x-p1_x));
d = side.*d;


function [x ft] = EProjSimplex_new(v, k)

%
%% Problem
%
%  min  1/2 || x - v||^2
%  s.t. x>=0, 1'x=1
%

if nargin < 2
    k = 1;
end;

ft=1;
n = length(v);

v0 = v-mean(v) + k/n;
%vmax = max(v0);
vmin = min(v0);
if vmin < 0
    f = 1;
    lambda_m = 0;
    while abs(f) > 10^-10
        v1 = v0 - lambda_m;
        posidx = v1>0;
        npos = sum(posidx);
        g = -npos;
        f = sum(v1(posidx)) - k;
        lambda_m = lambda_m - f/g;
        ft=ft+1;
        if ft > 100
            x = max(v1,0);
            break;
        end;
    end;
    x = max(v1,0);
else
    x = v0;
end;


function I = integral(f, varargin)
%INTEGRAL   Line integral of a CHEBFUN3 over a parametric curve.
%   I = INTEGRAL(F, G), returns the integral of the CHEBFUN3 object F
%   along a parametric curve defined by the Inf x 3 quasimatrix G. Columns
%   of G represent parametrization of the 3D curve.
%
%   I = INTEGRAL(F), returns the triple definite integral of the CHEBFUN3
%   object F over its domain of definition.
%
% See also CHEBFUN3/INTEGRAL2, CHEBFUN3/INTEGRAL3, CHEBFUN3/SUM,
% CHEBFUN3/SUM2, and CHEBFUN3/SUM3.

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

% Empty check:
if ( isempty(f) )
    I = [];
    return
end

if ( nargin == 1 )                       % Another way to do sum3(f)
    % Triple definite integral:
    I = sum3(f);

else
    if ( (isa(varargin{1}, 'chebfun')) && size(varargin{1}, 2) == 3 )
        % Line integral over an Inf x 3 quasimatrix.
        % Get curve:
        curve = varargin{1};
        xCurve = curve(:, 1);
        yCurve = curve(:, 2);
        zCurve = curve(:, 3);
        diffC = diff(curve);
        ds_squared = diffC(:, 1).^2 + diffC(:, 2).^2 + diffC(:, 3).^2;
        I = sum(feval(f, xCurve, yCurve, zCurve) .* sqrt(ds_squared), ...
            curve.domain);
    else
        error('CHEBFUN:CHEBFUN3:integral:badInputs',...
        'Unrecognised input arguments.');
    end
end

end


function [ n1, n2 ] = year_to_dominical_common ( y )

%*****************************************************************************80
%
%% YEAR_TO_DOMINICAL_COMMON: dominical numbers, Common calendar.
%
%  Discussion:
%
%    The Julian calendar calculations are used through the year 1582,
%    and the Gregorian thereafter.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, integer N1, N2, the dominical numbers for the year.
%    If Y is a leap year, then N1 applies before March 1, and N2 after.
%    If Y is not a leap year, then N1 applies throughout the year,
%    and N2 is returned as N1.
%
  if ( y <= 1582 )
    [ n1, n2 ] = year_to_dominical_julian ( y );
  else
    [ n1, n2 ] = year_to_dominical_gregorian ( y );
  end

  return
end


function [ind,d] = find(lookup,ref,epsilon,varargin)
% return indece and distance of all nodes within a eps neighborhood
%
% Syntax
%   % find for each quaternion in ref the closest quaternion in table
%   [ind,d] = find(table, ref)
%
%   % find for each quaternion in ref all quaternion in lookup that have
%   % angle not larger then epsilon
%   [ind,d] = find(table, rot_ref, epsilon)
%
%   % find all quaternion which have distant to fibre f less then epsilon
%   [ind,d] = find(table, f_ref, epsilon)
%
% Input
%  table   - @quaternion
%  rot_ref - reference @quaternion
%  f_ref   - reference @fibre
%  epsilon - maximum distance
%
% Output
%  ind - index of the found @quaternions
%  d   - actual distances
%

if isa(ref,'fibre')

  ind = angle(ref,lookup) < epsilon;

else

  d = dot_outer(lookup,ref);

  if nargin == 2
    [d,ind] = max(d,[],1);
  else
    ind = d > cos(epsilon/2);
  end
end


%Calling function of the visualization functions
close all
clear all
colors={'r.' 'gx' 'b+' 'ys' 'm.' 'c.' 'k.' 'r*' 'g*' 'b*' 'y*' 'm*' 'c*' 'k*' };
path(path,'..\..\FUZZCLUST')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Selecting the data set
wine=1;
iris=0;
wisc=0;

if wine
    load winedat.txt
    data=winedat(:,1:end-1);
    C=winedat(:,end);
end

if iris
    load iris
    data=iris(:,1:4);
    C=zeros(length(data),1);
    for i=1:3
        C(find(iris(:,4+i)==1))=i;
    end
end
if wisc
   %Data preprocessing
    wisc=wk1read('wisconsin.wk1');
    NI=9;
    NT=length(wisc);
    data.X=[wisc(:,11) wisc(:,2:10)];
    data.X=sortrows(data.X,1);
    [I,J]=find(data.X(:,7)~=0);
    data.X=data.X(I,:);
    [I,J]=find(data.X(:,1)==2);
    data.X(I,1)=1;
    [I,J]=find(data.X(:,1)==4);
    data.X(I,1)=2;
    C=data.X(:,1);
    data=data.X(:,2:end);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %normalization of the data
data.X=data;
data=clust_normalize(data,'range');
%fuzzy c-means clustering
param.m=2;
param.c=2;
param.val=1;
param.vis=0;
result=Kmeans(data,param);
result=validity(result,data,param);
%Assignment for classification
[d1,d2]=max(result.data.f');
Cc=[];
for i=1:param.c
    Ci=C(find(d2==i));
    dum1=hist(Ci,1:param.c);
    [dd1,dd2]=max(dum1);
    Cc(i)=dd2;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Principal Component Projection of the data and the cluster centers
param.q=2;
result = PCA(data,param,result);
%visualization
figure(1)
clf
for i=1:max(C)
    index=find(C==i);
    err=(Cc(d2(index))~=i);
    eindex=find(err);
    misclass(i)=sum(err);
    plot(result.proj.P(index,1),result.proj.P(index,2),[colors{i}])
    hold on
    plot(result.proj.P(index(eindex),1),result.proj.P(index(eindex),2),'o')
    hold on
end
    xlabel('y_1')
    ylabel('y_2')
    title('PCA projection')
%The error value of classification
perfclass=sum(misclass)/length(C)*100
%
plot(result.proj.vp(:,1),result.proj.vp(:,2),'r*');
%calculating realtion-indexes
result = samstr(data,result);
perf = [projeval(result,param) result.proj.e];
%
disp('Press any key.')
pause
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%SAMMON mapping

proj.P=result.proj.P;   %Sammon uses the output of PCA for initializing
param.alpha = 0.4;
param.max=100;

figure(2)
result = Sammon(proj,data,result,param)
%visualization
clf
for i=1:max(C)
    index=find(C==i);
    err=(Cc(d2(index))~=i);
    eindex=find(err);
    misclass(i)=sum(err);
    plot(result.proj.P(index,1),result.proj.P(index,2),[colors{i}] )
    hold on
    plot(result.proj.P(index(eindex),1),result.proj.P(index(eindex),2),'o')
    hold on
end
    xlabel('y_1')
    ylabel('y_2')
    title('Conventional Sammon mapping')
%
plot(result.proj.vp(:,1),result.proj.vp(:,2),'r*');
%calculating realtion-indexes
result = samstr(data,result);
perfs = [projeval(result,param) result.proj.e];
%
disp('Press any key.')
pause
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Modified fuzzy SAMMON mapping

proj.P=result.proj.P; %FuzSam uses the output of Sammon for initializing
param.alpha = 0.4;
param.max=100;

figure(3)
result=FuzSam(proj,result,param);

clf
for i=1:max(C)
    index=find(C==i);
    err=(Cc(d2(index))~=i);
    eindex=find(err);
    misclass(i)=sum(err);
    plot(result.proj.P(index,1),result.proj.P(index,2),[colors{i}] )
    hold on
    plot(result.proj.P(index(eindex),1),result.proj.P(index(eindex),2),'o')
    hold on
end
    xlabel('y_1')
    ylabel('y_2')
    title('Fuzzy Sammon mapping')

plot(result.proj.vp(:,1),result.proj.vp(:,2),'r*')
%calculating realtion-indexes
result = samstr(data,result);
perff = [projeval(result,param) result.proj.e];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
perf
perfs
perff


function varargout = quickshift(varargin)
% VL_QUICKSHIFT Quick shift image segmentation
%   Quick shift is a mode seeking algorithm which links each pixel to
%   its nearest neighbor which has an increase in the estimate of the
%   density. These links form a tree, where the root of the tree is
%   the pixel which correspond to the highest mode in the image.
%
%   [MAP,GAPS] = VL_QUICKSHIFT(I, KERNELSIZE, MAXDIST) computes quick shift on the
%   image I. KERNELSIZE is the bandwidth of the Parzen window estimator of
%   the density. Since searching over all pixels for the nearest
%   neighbor which increases the density would be prohibitively
%   expensive, MAXDIST controls the maximum L2 distance between neighbors
%   that should be linked. MAP and GAP represent the resulting forest
%   of trees. They are array of the same size of I.  Each element
%   (pixel) of MAP is and index to the parent elemen in the forest and
%   GAP contains the corresponding branch length. Pixels which are at
%   the root of their respective tree have MAP(x) = x and GAPS(x) =
%   inf.
%
%   [MAP,GAPS,E] = VL_QUICKSHIFT(I, KERNELSIZE, MAXDIST) also returns the estimate
%   of the density E.
%
%   [MAP,GAPS] = VL_QUICKSHIFT(I, KERNELSIZE) uses a default MAXDIST of 3 * KERNELSIZE.
%
%   Notes::
%     The distance between pixels is always measured in image
%     coordinates (not normalized), so the importance of the color
%     component should be weighted accordingly before calling this
%     function.
%
%   Options:
%
%   Verbose::
%     Toggles verbose output.
%
%   Medoid::
%     Run medoid shift instead of quick shift.
%
%   See also: VL_HELP().
[varargout{1:nargout}] = vl_quickshift(varargin{:});


function val=convertSpeedUnits(val,unitOrigNum,unitOrigDenom,unitDestNum,unitDestDenom)
%%CONVERTSPEEDUNITS Convert values of speed from one set of units to
%                   another. Speed is given as a ratio of a distance unit
%                   to a time unit. Note that 1 knot (kt)=1 nml/h. Thus, a
%                   conversion from knots to meters per second would have
%                   unitOrigNum='nml';unitOrigDenom='h';unitDestNum='m';
%                   unitDestDenom='s';
%
%INPUTS: val The matrix or vector of values that are to be converted.
% unitOrigNum,unitOrigDenom
% unitDestNum,unitDestDenom Four character strings indicating the units of
%            val and the units into which it is to be converted.
%            unitOrigNum and unitDestNum are the units of distance and can
%            take the values listed in convertTimeUnits. unitOrigDenom and
%            unitDestDenom are the length units and can take the values
%            listed in convertLengthUnits.
%
%OUTPUTS: val The values converted into the desired coordinate system.
%
%This just uses the functions convertLengthUnits and convertTimeUnits to
%get the approprimate multiplication factors to convert the length and time
%components.
%
%May 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

lengthCoeff=convertLengthUnits(1,unitOrigNum,unitDestNum);
%Inverse, because time is in the denominator.
timeCoeff=1/convertTimeUnits(1,unitOrigDenom,unitDestDenom);

val=val*lengthCoeff*timeCoeff;
end

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


filt_opt.J = 3;
filters1d = morlet_filter_bank_1d(32, filt_opt);
dual_filters1d = dual_filter_bank(filters1d);

rs = RandStream.create('mt19937ar','Seed',1234);
x = rs.randn(32,1);

wavelet_opt.oversampling = 2;
[x_phi,x_psi,meta_phi,meta_psi] = wavelet_1d(x(1:32),filters1d,wavelet_opt);
x1 = inverse_wavelet_1d(32, x_phi, x_psi, meta_phi, meta_psi, dual_filters1d);
assert(norm(x-x1)/norm(x)<0.02)

wavelet_opt.oversampling = 100;
[x_phi,x_psi,meta_phi,meta_psi] = wavelet_1d(x(1:32),filters1d,wavelet_opt);
x1 = inverse_wavelet_1d(32, x_phi, x_psi, meta_phi, meta_psi, dual_filters1d);
assert(norm(x-x1)/norm(x)<1e-14)


x=@(s,t) 3*tan(s).*cos(t);
y=@(s,t) 2*tan(s).*sin(t);
z=@(s,t) tan(s);
ezsurf(x,y,z)


function e = normcdfln(x)
% NORMCDFLN   log of normal cumulative density function.
% More accurate than log(normcdf(x)) when x is small.
% The following is a quick and dirty approximation to normcdfln:
% normcdfln(x) =approx -(log(1+exp(0.88-x))/1.5)^2

% Written by Tom Minka
% (c) Microsoft Corporation. All rights reserved.

% make e the same shape as x, and inherit any NaNs.
e = x;
t = -6.5;
i = find(x >= t);
if ~isempty(i)
  e(i) = log(normcdf(x(i)));
end
i = find(x < t);
if ~isempty(i)
  x = x(i);
  z = x.^(-2);
  if 0
    % log of asymptotic series for cdf
    % subs(x=-x,asympt(sqrt(2*Pi)*gauss_cdf(-x),x));
    c = [-1 3 -15 105 -945 10395 -135135 2027025 -34459425 654729075];
    y = 0;
    for i = length(c):-1:1
      y = z.*(y + c(i));
    end
    %y = z.*(c(1)+z.*(c(2)+z.*(c(3)+z.*(c(4)+z.*(c(5)+z.*(c(6)+z.*c(7)))))));
    y = log(1+y);
  else
    % asymptotic series for logcdf
    % subs(x=-x,asympt(log(gauss_cdf(-x)),x));
    c = [-1 5/2 -37/3 353/4 -4081/5 55205/6 -854197/7];
    y = z.*(c(1)+z.*(c(2)+z.*(c(3)+z.*(c(4)+z.*(c(5)+z.*(c(6)+z.*c(7)))))));
  end
  e(i) = y -0.5*log(2*pi) -0.5*x.^2 - log(-x);
end


%% Check rate of convergence of FEM for 3D elliptic interface problem
%     -div(A grad u)  = f,    x\in \Omega
%      where A is a piecewise constant on Omega^+ and Omega^-.
%
% Domain: Rectangular domain: [xmin,xmax] X [ymin,ymax] X [zmin,zmax]
% Mesh: Structured rectangular mesh.
% Method: Conforming Lagrange Type FEMs (Q1: Tri-Linear FEM).
%
% Last modified by Xu Zhang 08/06/2020.

%% Geometry and Boundary Conditions
clear
close all
%clc

domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1]; % Dirichelet BC

%% Finite Element Type
femtype = 'P1';
disp(['FEM Type =  Conforming ', femtype]);

%% Initial Partition
nx0 = 10;
ny0 = nx0;
nz0 = nx0;

%% Task
showErr = 0;
showMesh = 0;
computErr = 1;

%% PDE
test = 2;
switch test
    case 0
        pde = poissonNonPoly3D;
    case 1 % circular interface
        r = pi/4; bm = 1; bp = pi/(2*r^2);
        x0 = 0; y0 = 0; z0 = 0; rx = pi/4; ry = pi/4; rz = pi/4;
        pde = elli3DcircIntf(bm,bp,r,x0,y0,z0,rx,ry,rz);
    case 2 % orthotorus interface
        domain = [-1.2,1.2,-1.2,1.2,-1.2,1.2];
        bm = 1; bp = 1;
        rx = 1; ry = 0.075; rz = 3;
        pde = elli3DorthocircIntf(bm,bp,rx,ry,rz);
    case 3 % line interface
        bm = 1; bp = 100;
        rx = 1; ry = 0; rz = 1; cx = pi/10; cy = 0; cz = 0;
        cm = 1; cp = 1; a = 1;
        pde = elli3DlinIntf(bm,bp,cx,cy,cz,rx,ry,rz,a,cm,cp);
end

%% Max Iteration
maxIt = 16;
time = zeros(7,maxIt);

for i = 1:maxIt

    %% 1. Generate Mesh
    tic
    nx = nx0 + 10*(i-1); h = (domain(2) - domain(1))/nx;
    ny = ny0 + 10*(i-1);
    nz = nz0 + 10*(i-1);
    time(1,i) = nx;
    disp(' ')
    disp('*******************************************************************************')
    disp(['Partition =  ',int2str(nx),' X ',int2str(ny),' X ',int2str(nz)]);
    disp(' ')

    mesh = genMesh3D(domain, nx, ny, nz);
    disp(['number of element =  ', int2str(length(mesh.t))]);
%     mesh = genIntfMesh3D(mesh,pde.intf);
%     disp(['number of interface element =  ', int2str(-min(mesh.tLoc)),...
%         ', is ', num2str(100*-min(mesh.tLoc)/length(mesh.t)), '% of all elements']);
    if showMesh == 1
        tetramesh(mesh.t,mesh.p,ones(size(mesh.t)));
    end
    time(2,i) = toc;

    %% 2. Generate FEM DoF
    tic
    fem = genFEM3D(mesh,femtype,bc);
    disp(['number of DoF =  ', int2str(length(fem.p))]);
    time(3,i) = toc;

    %% 3. Assemble Matrix
    tic
    disp(' '); disp('Start Assembling Matrix');
    matrix = genMatEll3D(pde,mesh,fem);
    time(4,i) = toc;

    %% 3. Solve the linear system Au = f
    tic
    % L = ichol(matrix.A,struct('michol','on'));
    L = ichol(matrix.A);
    [u,flag,relres,iter,resvec] = pcg(matrix.A,matrix.f,1e-8,300,L,L');
    time(5,i) = toc;
    tu = pde.exactu(fem.p(:,1),fem.p(:,2),fem.p(:,3));
    uh = tu; uh(fem.mapper) = u;

    %% 4. Postprocess: Calculating Errors
    tic
    errND = max(abs(uh - tu)); % Error on nodes
    err.nd = errND; err.inf = 0; err.l2 = 0; err.h1 = 0;
    if computErr == 1
        disp(' ');  disp('Start computing error in Inf norm');
        errInf = getErrInf3D(uh,pde.exactu,fem,[0,0,0]);
        eNorm = 'L2'; disp(['Start computing error in ',eNorm,'  norm']);
        [errL2,errL2K] = getErr3D(uh, pde.exactu, fem, eNorm);
        eNorm = 'H1x'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1x,errH1xK] = getErr3D(uh, pde.Dxu, fem, eNorm);
        eNorm = 'H1y'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1y,errH1yK] = getErr3D(uh, pde.Dyu, fem, eNorm);
        eNorm = 'H1z'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1z,errH1zK] = getErr3D(uh, pde.Dzu, fem, eNorm);
        err.inf = max([errND,errInf]);
        err.l2 = errL2;
        err.h1 = sqrt(errH1x^2+errH1y^2+errH1z^2);
    end
    time(6,i) = toc;
    time(7,i) = 1e6*sum(time(2:6,i))/length(mesh.t);

    %% 5: Output

    disp(' ')
    disp('Errors')
    disp('Node        Inf norm    L2 norm     H1 norm')
    formatSpec = '%6.4e  %6.4e  %6.4e  %6.4e\n';
    fprintf(formatSpec, err.nd, err.inf, err.l2, err.h1)

    if i > 1
        format short
        rNd = log(err0.nd/err.nd)./log(h0/h);
        rInf = log(err0.inf/err.inf)./log(h0/h);
        rL2 = log(err0.l2/err.l2)./log(h0/h);
        rH1 = log(err0.h1/err.h1)./log(h0/h);
        disp(' ')
        disp('Convergence Rate')
        disp('Node        Inf norm    L2 norm     H1 norm')
        formatSpec = '%6.4f      %6.4f      %6.4f      %6.4f\n';
        fprintf(formatSpec, rNd, rInf, rL2, rH1)
    end
    err0 = err; h0 = h;

    disp(' '); disp('CPU Time')
    disp('   N     Mesh     FEM      Matrix   Solve    Error    Time/1M cell')
    formatSpec = '%4i  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f   %7.2f\n';
    fprintf(formatSpec, time(:,1:i))

    %% 6. Plot Solution and Error
    if showErr == 1
    end
end


function [imgThr, imgEb] = WardComputeThreshold(img, wardPercentile, wardTolerance)
%
%
%       [imgThr, imgEb] = WardComputeThreshold(img, wardPercentile, wardTolerance)
%
%       This function computes the Ward's MTB.
%
%       Input:
%           -img: an input image
%           -wardPercentile: a value for thresholding the image. This is
%           typically set to 0.5.
%           -wardTolerance: a tolerance threshold for classifying pixels
%           falling around edges.
%
%       Output:
%           -imgThr: Ward's threshold image. This image is set to 1 if the
%           the pixel value is greater or equal to the median value.
%           -imgEb: a tolerance mask of pixels around edges of imgThr.
%
%     Copyright (C) 2012  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('wardPercentile', 'var'))
    wardPercentile = 0.5;
end

if(~exist('wardTolerance', 'var'))
    wardTolerance = 4 / 256;
end

if(size(img, 3) == 1)
    grey = img;
else
    grey = (54 * img(:,:,1) + 183 * img(:,:,2) +  19 * img(:,:,3)) / 256;
end

medVal = MaxQuart(grey, wardPercentile);

imgThr = zeros(size(grey));
imgThr(grey > medVal) = 1.0;

A = medVal - wardTolerance;
B = medVal + wardTolerance;
imgEb = ones(size(grey));
imgEb((grey >= A) & (grey <= B)) = 0.0;

end


function ye = fL(t,D,nalpha,nbeta,u,h)		% Left boundary value
alpha = nalpha*pi; beta = nbeta*pi;
ye = h*cos(beta*u*t) + exp(-D*alpha*alpha*t)*cos(alpha*u*t);


%Extremely similar to the false position method. The main difference is
%that the secant method does not actually have a defined interval where
%the root lies on. It converges faster than the false position method,
%but it is not always guaranteed to converge.

%INPUTS:
%Function handle f
%x1 = a
%x2 = b
%maximum tolerated error

%OUTPUTS:
%An approximated value for the root of f.

%Written by MatteoRaso

function y = secant(f, a, b, error)
  x = [a, b];
  n = 2;
  while abs(f(x(n))) > error
    x(n + 1) = -f(x(n)) * (x(n) - x(n - 1)) / (f(x(n)) - f(x(n - 1))) + x(n);
    n = n + 1;
    disp(f(x(n)))
  endwhile
  A = ["The root is approximately ", num2str(x(n))];
  disp(A)
  y = x(n);
endfunction


function x = z1(y)
% linear normalize between 0 and 1
x = (y-min(y(:)))/(max(y(:))-min(y(:)))+eps;

% for multidimensional stuff, this normalizes each column to between 0 and
% 1 independent of other columns

% T=length(y);
% y=y';
% miny=min(y);
% x = (y-repmat(miny,T,1))./repmat(max(y)-min(y),T,1);


function [tfr,t,f] = tfrsp(x,t,N,h,trace);
%TFRSP	Spectrogram time-frequency distribution.
%	[TFR,T,F]=TFRSP(X,T,N,H,TRACE) computes the Spectrogram
%	distribution of a discrete-time signal X.
%
%	X     : signal.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	H     : analysis window, H being normalized so as to
%	        be  of unit energy.      (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency representation. When called without
%	        output arguments, TFRSP runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4);
%	 h=tftb_window(17,'Kaiser'); tfrsp(sig,1:128,64,h,1);
%
%        [tfr,t,freq]=tfrsp(sig,1:128,64,h,1); plot(fftshift(freq),fftshift(tfr(:,100)))
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin < 1),
 error('At least 1 parameter is required');
elseif (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
hlength=hlength+1-rem(hlength,2);

if (nargin == 1),
 t=1:xrow; h = tftb_window(hlength); trace=0;
elseif (nargin == 2) | (nargin == 3),
 h = tftb_window(hlength); trace=0;
elseif (nargin == 4),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol==0) | (xcol>2),
 error('X must have one or two columns');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2;
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

tfr= zeros (N,tcol) ;
if trace, disp('Spectrogram'); end;
for icol=1:tcol,
 ti= t(icol); tau=-min([round(N/2)-1,Lh,ti-1]):min([round(N/2)-1,Lh,xrow-ti]);
 indices= rem(N+tau,N)+1;
 if trace, disprog(icol,tcol,10); end;
 tfr(indices,icol)=x(ti+tau).*conj(h(Lh+1+tau))/norm(h(Lh+1+tau));
end;
tfr=abs(fft(tfr)).^2;
if trace, fprintf('\n'); end;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrsp',h);
elseif (nargout==3),
 if rem(N,2)==0,
  f=[0:N/2-1 -N/2:-1]'/N;
 else
  f=[0:(N-1)/2 -(N-1)/2:-1]'/N;
 end;
end;


function [x_lambda,rho,eta,data,X] = maxent(A,b,lambda,w,x0)
%MAXENT Maximum entropy regularization.
%
% [x_lambda,rho,eta] = maxent(A,b,lambda,w,x0)
%
% Maximum entropy regularization:
%    min { || A x - b ||^2 + lambda^2*x'*log(diag(w)*x) } ,
% where -x'*log(diag(w)*x) is the entropy of the solution x.
% If no weights w are specified, unit weights are used.
%
% If lambda is a vector, then x_lambda is a matrix such that
%    x_lambda = [x_lambda(1), x_lambda(2), ... ] .
%
% This routine uses a nonlinear conjugate gradient algorithm with "soft"
% line search and a step-length control that insures a positive solution.
% If the starting vector x0 is not specified, then the default is
%    x0 = norm(b)/norm(A,1)*ones(n,1) .

% Per Christian Hansen, IMM and Tommy Elfving, Dept. of Mathematics,
% Linkoping University, 06/10/92.

% Reference: R. Fletcher, "Practical Methods for Optimization",
% Second Edition, Wiley, Chichester, 1987.

% Set defaults.
flat = 1e-3;     % Measures a flat minimum.
flatrange = 10;  % How many iterations before a minimum is considered flat.
maxit = 150;     % Maximum number of CG iterations.
minstep = 1e-12; % Determines the accuracy of x_lambda.
sigma = 0.5;     % Threshold used in descent test.
tau0 = 1e-3;     % Initial threshold used in secant root finder.

% Initialization.
[m,n] = size(A); x_lambda = zeros(n,length(lambda)); F = zeros(maxit,1);
if (min(lambda) <= 0)
  error('Regularization parameter lambda must be positive')
end
if (nargin ==3), w  = ones(n,1); end
if (nargin < 5), x0 = ones(n,1); end

% Treat each lambda separately.
for j=1:length(lambda);

  % Prepare for nonlinear CG iteration.
  l2 = lambda(j)^2;
  x  = x0; Ax = A*x;
  g  = 2*A'*(Ax - b) + l2*(1 + log(w.*x));
  p  = -g;
  r  = Ax - b;

  % Start the nonlinear CG iteration here.
  delta_x = x; dF = 1; it = 0; phi0 = p'*g;
  while (norm(delta_x) > minstep*norm(x) & dF > flat & it < maxit & phi0 < 0)
    it = it + 1;

    % Compute some CG quantities.
    Ap = A*p; gamma = Ap'*Ap; v = A'*Ap;

    % Determine the steplength alpha by "soft" line search in which
    % the minimum of phi(alpha) = p'*g(x + alpha*p) is determined to
    % a certain "soft" tolerance.
    % First compute initial parameters for the root finder.
    alpha_left = 0; phi_left = phi0;
    if (min(p) >= 0)
      alpha_right = -phi0/(2*gamma);
      h = 1 + alpha_right*p./x;
    else
      % Step-length control to insure a positive x + alpha*p.
      I = find(p < 0);
      alpha_right = min(-x(I)./p(I));
      h = 1 + alpha_right*p./x; delta = eps;
      while (min(h) <= 0)
        alpha_right = alpha_right*(1 - delta);
        h = 1 + alpha_right*p./x;
        delta = delta*2;
      end
    end
    z = log(h);
    phi_right = phi0 + 2*alpha_right*gamma + l2*p'*z;
    alpha = alpha_right; phi = phi_right;

    if (phi_right <= 0)

      % Special treatment of the case when phi(alpha_right) = 0.
      z = log(1 + alpha*p./x);
      g_new = g + l2*z + 2*alpha*v; t = g_new'*g_new;
      beta = (t - g'*g_new)/(phi - phi0);

    else

      % The regular case: improve the steplength alpha iteratively
      % until the new step is a descent step.
      t = 1; u = 1; tau = tau0;
      while (u > -sigma*t)

        % Use the secant method to improve the root of phi(alpha) = 0
        % to within an accuracy determined by tau.
        while (abs(phi/phi0) > tau)
          alpha = (alpha_left*phi_right - alpha_right*phi_left)/...
                  (phi_right - phi_left);
          z = log(1 + alpha*p./x);
          phi = phi0 + 2*alpha*gamma + l2*p'*z;
          if (phi > 0)
            alpha_right = alpha; phi_right = phi;
          else
            alpha_left  = alpha; phi_left  = phi;
          end
        end

        % To check the descent step, compute u = p'*g_new and
        % t = norm(g_new)^2, where g_new is the gradient at x + alpha*p.
        g_new = g + l2*z + 2*alpha*v; t = g_new'*g_new;
        beta = (t - g'*g_new)/(phi - phi0);
        u = -t + beta*phi;
        tau = tau/10;

      end  % End of improvement iteration.

    end  % End of regular case.

    % Update the iteration vectors.
    g = g_new; delta_x = alpha*p;
    x = x + delta_x;
    p = -g + beta*p;
    r = r + alpha*Ap;
    phi0 = p'*g;

    % Compute some norms and check for flat minimum.
    rho(j,1) = norm(r); eta(j,1) = x'*log(w.*x);
    F(it) = rho(j,1)^2 + l2*eta(j,1);
    if (it <= flatrange)
      dF = 1;
    else
      dF = abs(F(it) - F(it-flatrange))/abs(F(it));
    end

    data(it,:) = [F(it),norm(delta_x),norm(g)];
    X(:,it) = x;

  end  % End of iteration for x_lambda(j).

  x_lambda(:,j) = x;

end


function [vMat,fMat] = spheretribydepth(depth)
% SPHERETRIBYDEPTH is a high-performance vectorized function for building
% a triangulation of a unit sphere based on recursive partitioning of each
% of Icosahedron faces into 4 triangles with vertices in the middles of
% original face
%
% Input:
%   depth: double[1,1] - depth of partitioning, use 1 for the first level of
%       Icosahedron partitioning, and greater value for a greater level
%       of partitioning
%
% Output:
%   vMat: double[nVerts,3] - (x,y,z) coordinates of triangulation
%       vertices
%   fMat: double[nFaces,3] - indices of face verties in vertMat
%
% Example:
%   [vMat, fMat] = spheretribydepth(4);
%   patch('Vertices',vMat,'Faces',fMat,'FaceColor','g','EdgeColor','k');
%
% $Author: Peter Gagarinov, PhD  <pgagarinov@gmail.com> $
% $Copyright: Peter Gagarinov, PhD,
%            Moscow State University,
%            Faculty of Computational Mathematics and Computer Science,
%            System Analysis Department 2011-2016 $
%
if ~(numel(depth)&&isnumeric(depth)&&depth>=0&&fix(depth)==depth)
    error('spheretri:wrongInput',...
        'depth is expected to be a not negative integer scalar');
end
[vMat,fMat]=icosahedron();
[vMat,fMat]=shrinkfacetri(vMat,fMat,0,depth,@normvert);
end
function x=normvert(x)
x=x./repmat(realsqrt(sum((x.*x),2)),1,3);
end