function [trans,rot,scale,skew] = affineDecompose(A)
%
% [trans,rot,scale,skew] = affineDecompose(A)
%
% Decomposes an affine (orthogonal linear) transformation matrix into its
% four componets: translations, rotations scales and skews.
%
% (See affineBuild for more info.)
%
% HISTORY:
%   2004.03.12 RFD (bob@white.stanford.edu) shamelessly copied the core
%   algorithm from spm99's spm_imatrix.m.
%   2005.07.08 ras (sayres at stanford edu) imported into mrVista 2.0

% Translations and zooms
%-----------------------------------------------------------------------
R         = A(1:3,1:3);
C         = chol(R'*R);
if(size(A,2)>3)
    trans = A(1:3,4)';
else
    trans = [0 0 0];
end
scale = diag(C)';
if det(R)<0, scale(1)=-scale(1);end % Fix for -ve determinants

% Shears
%-----------------------------------------------------------------------
C         = diag(diag(C))\C;
skew      = C([4 7 8]);
R0        = affineBuild([0 0 0], [0 0 0], scale, skew);
R0        = R0(1:3,1:3);
R1        = R/R0;

% This just leaves rotations in matrix R1
%-----------------------------------------------------------------------
%[          c5*c6,           c5*s6, s5]
%[-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
%[-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]

rot(2) = asin(rang(R1(1,3)));
if (abs(rot(2))-pi/2).^2 < 1e-9,
	rot(1) = 0;
	rot(3) = atan2(-rang(R1(2,1)), rang(-R1(3,1)/R1(1,3)));
else,
	c    = cos(rot(2));
	rot(1) = atan2(rang(R1(2,3)/c), rang(R1(3,3)/c));
	rot(3) = atan2(rang(R1(1,2)/c), rang(R1(1,1)/c));
end;

return;

% There may be slight rounding errors making b>1 or b<-1.
function a = rang(b)
a = min(max(b, -1), 1);
return;


function m = similarity(sp1, sp2, opts)
% Returns the similarity score of two superpixels using various methods
% As seen with Matlab profiler, this function is responsible for majority
% of calculation for the whole algorithm.

d = zeros(1, length(opts.features));
a = 1;

for fn = 1:length(opts.features)

    % Intersection distance
    %d(fn) = 1 - sum(min([sp1.hist{fn}, sp2.hist{fn}], [], 2));
    % Hellinger's distance (Hellinger's kernel)
    %d(fn) = sqrt(sum((sqrt(sp1.hist{fn}) - sqrt(sp2.hist{fn})).^2));
    % l^1 distance (Intersection kernel)
    %d(fn) = sum(abs(sp1.hist{fn} - sp2.hist{fn}));

    if opts.features(fn) == 6 % size
        d(fn) = sp1.size + sp2.size;
        a = min(sp1.size, sp2.size); % for removing very small superpixels early
    else
        d(fn) = 0.5*sum(((sp1.hist{fn} - sp2.hist{fn}).^2)./(eps + sp1.hist{fn} + sp2.hist{fn})); % \chi^2 distance (\chi^2 kernel)
    end

end

d = 1 - d; % Low distance = high similarity. This is the monotone decreasing mapping we use
m = sum(d.*opts.feature_weights)/sum(opts.feature_weights); % The similarity value. If weights are ones, then this is just mean(d)

% Force small superpixels to merge first
if a < 0.002
    m = 10 + m; % adding constant will preserve the original ordering among overloaded pairs
end


%% Tests the solvers on a simple unconstrained quadratic function

%{
    Solve:

    minimize_x  c'x + x'Dx/2

    as an example of using TFOCS without the "SCD" interface

%}

randn( 'state', sum('quadratic test') );
N       = 100;
c       = randn(N,1);
D       = randn(N,N);
D       = D * D' + .5*eye(N);
x_star  = - D \ c;          % the true answer
x0      = zeros(N,1);

% Here's what you could do...
% f       = @(x) c'*x + x'*D*x/2;
% grad_f  = @(x) c + D*x;
% smoothF = @(x) wrapper_objective( f, grad_f, x );

% Here's a simpler way:
smoothF = smooth_quad(D,c);

x_error = @(x) norm(x-x_star,Inf);

opts = [];
opts.errFcn     = { @(f,x) x_error(x)};
opts.restart = 100;

[ x, out, optsOut ] = tfocs( smoothF, [], [],x0, opts );
% Check that we are within allowable bounds
if out.err(end) < 1e-5
    disp('Everything is working');
else
    error('Failed the test');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function C = partitions(M,K)
%PARTITIONS Find all partitions of a set.
% C = PARTITIONS(N), for scalar N, returns a cell array, wherein each cell
% has a various number of other cells representing the partitions of the
% set of numbers {1,2,3,...N}.  The length of C is the Bell number for N.
% C = PARTITIONS(N), for vector N, returns the partitions of the vector
% elements treated as members of a set.
% C = PARTITIONS(N), for cell N, returns the partitions of the cell
% elements treated as members of a set.
% PARTITIONS(N,K) returns only the partitions of which have K members.
%
% K must be less than or equal to N for scalar N, or length(N).
%
%
% Examples:
%
%     C = partitions(4); % Find the partitions of set {1 2 3 4}
%     home  % Makes for nice display for small enough N.  Else use clc.
%     partdisp(C) % File accompanying PARTITIONS
%
%     C = partitions({'peanut','butter','yummy'});
%     home
%     partdisp(C)
%
%     C = partitions([5 7 8 50],3); % partitions of set {5 7 8 50}
%     home
%     partdisp(C,3)
%
%     C = partitions(['a','b','c','d']); % for longer chars, use {}, not []
%     home
%     partdisp(C)
%
% Class support for inputs N,K:
%      float: double, single, char (N only)
%
% See also, nchoosek, perms, npermutek (On the FEX)
%
% Author: Matt Fig,
% Contact: popkenai@yahoo.com
% Date: 5/17/2009

Kflg = false;  % No K passed in.
Cflg = false;  % A set not passed in.

if length(M)>1
    N = length(M); % User passed {'here','there','everywhere'} for example.
    Cflg = true;
else
    if iscell(M)  % User passed {2} for example.
        error('Set arguments must have more than one element.')
    end

    N = M;
end

if nargin>1
    Kflg = true;  % Used in while loop below, K passed in.
    S = stirling(N,K);  % The number of partitions, Stirling number.
    C = cell(S,min(1,K)); % Main Cell.
    cnt = 1;  % Start the while loop counter.
else
    K = 0;  % Since user doesn't want only certain partitions.
    S = ceil(sum((1:2*N).^N./cumprod(1:2*N))/exp(1)); % Bell number.
    C = cell(S,1); % Main Cell.

    if Cflg
        C{1} = {M};
    else
        C{1} = {1:N}; % First one is easy.
    end

    cnt = 2; % Start the while loop counter.
end

if K~=ceil(K) || numel(K)~=1
    error('Second argument must be an integer of length 1.  See help.')
end

if N<0 || K>N || K<0
   error('Arguments must be greater than zero, and K cannot exceed N.')
end

if N==0
    C = {{}}; % Easy case.
    return
end

if K==1
    if Cflg
        C{1} = {M};  % Easy case.
    else
        C{1} = {1:N};  % Easy case.
    end
    return
end

if Cflg
    NV = M;
else
    NV = 1:M; % Vector: base partition, RGF indexes into this guy.
end

NU = 1; % Number of unique indices in current partition.
stp1 = N; % Controls assigning of indices.
RGF = ones(1,N); % Holds the indexes.
BLD = cell(1,N); % Smaller cell array will be used in creating larger.

while cnt<=S
    idx1 = N; % Index into RGF.
    stp2 = RGF(idx1); % Works with stp1.

    while stp1(stp2)==1
        RGF(idx1) = 1;  % Assign value to RGF.
        idx1 = idx1 - 1; % Need to increment idx1 for translation below.
        stp2 = RGF(idx1); % And set this guy for stp1 assign below.
    end

    NU = NU + idx1 - N;  % Get provisional number of unique vals.
    stp1(1) = stp1(1) + N - idx1;

    if stp2==NU % Increment the number of unique elements.
        NU = NU +1;
        stp1(NU) = 0;
    end

    RGF(idx1) = stp2 + 1;  % Increment this position.
    stp1(stp2) = stp1(stp2) - 1;  % Translate indices of these two.
    stp1(stp2+1) = stp1(stp2+1) + 1; % Re-assign stopper.

    if NU==(~Kflg * NU + Kflg * K)
    % We could use:  C{cnt} = accumarray(RGF',NV',[],@(x) {x});   (SLOW!!)
    % or the next lines to C{cnt} = TMP;
        TMP = BLD(1:NU); % Build subcell of correct size.
        TMP{1} = NV(RGF==1); % These first two are always here.... no loop.
        TMP{2} = NV(RGF==2);

        for ii = 3:NU % Build the rest of cell array, if any.
            TMP{ii} = NV(RGF==ii);
        end

        C{cnt} = TMP;  % Assign cell at jj.
        cnt = cnt + 1;
    end
end

function S = stirling(N,K)
% Calculate the Stirling number of the second kind. Subfunc to partitions.

for ii = K:-1:0
    S(ii+1) = (-1)^ii * prod(1:K) / (prod(1:(K-ii)) *...
              (prod(1:ii)))*(K - ii)^N;  %#ok
end

S = 1/prod(1:K) * sum(S);


function [ alpha ] = LiMapS(s, D, DINV, gamma, maxIter)
%
% [ alpha ] = LIMAPS(s, D, DINV, gamma)
%
%   Returns the sparsest vector alpha which satisfies underdetermined
%   system of equations  D*alpha=s, using  Lipschitzian Mapping for Sparsity
%   Recovery algorithm.
%   The dictionary matrix  D should have more columns than rows. The
%   number of rows of dictionary D  must be equal to the length of the
%   column observation vector(or matrix) s.
%
%     The observations signal(s) s and the dictionary D necessarily, must
%   be provided by the user. The other parameters have defult values, or
%   may be provided by the user.
%
%   This algorithm work fine with matrix too, returning the sparsest matrix
%   alpha that satisfy the underdetermined system of equations D*alpha=s.
%   Then the dictionary D is of size [n x m], s must be of size [ s x T ] and
%   as consiquence the coefficients matirx alpha is of size [ m x T]
%
%   s:
%       The observation vector (matrix)
%
%   D:
%       The dictionary
%
%   DINV (optional):
%       The pseudo-inverse of the dictionary matrix D
%
%   gamma (optional):
%       The increase factor (default 1.01)
%
%   maxIter (optional):
%       Maximum number of iterations (default 1000)
%
% Authors: Alessandro Adamo and Giuliano Grossi
% Version: 1.0
% Last modified: 1 Aug. 2011.
%
% WEB PAGE:
% ------------------
%    http://dalab.dsi.unimi.it/limaps
%
% HISTORY:
%--------------
% Version 1.0: 1 Aug. 2011
%        first official version.
%

if(nargin<2||nargin>5)
    error('Wrong parameters number');
end
if(nargin==2)
    DINV=pinv(D);
    gamma=1.01;
    maxIter=1000;
end
if(nargin==3)
    gamma=1.01;
    maxIter=1000;
end
if(nargin==4)
    maxIter=1000;
end

%% --  INITIALIZATION  --
alpha = DINV*s;

lambda = 1/max(abs(alpha(:)));
epsilon=1e-5;                    % stopping criteria
alpha_min=1e-2;

%% --  CORE  --
for i=1:maxIter

    alphaold=alpha;
    % apply sparsity constraction mapping: increase sparsity
    beta = alpha.*(1-exp(-lambda.*abs(alpha)));

    beta(abs(beta)<alpha_min)=0;

    % apply the orthogonal projection
    alpha = beta-DINV*(D*beta-s);

    % update the lambda coefficient
    lambda = lambda*gamma;

    % check the stopping criteria
    if (norm(alpha(:)-alphaold(:))<epsilon)
        break;
    end

    if (mod(i,100))
        if(sum(abs(1-exp(-lambda*abs(alpha)))>1/lambda)>size(D,1))
           break;
        end
    end

end

%% -- REFINEMENT --

% threshold the coefficients
alpha(abs(alpha)<alpha_min) = 0;

% final refinements of the solution
for i=1:size(alpha,2);
    alpha_ind = alpha(:,i)~=0;
    D1 = D(:,alpha_ind);
    alpha(alpha_ind,i) = alpha(alpha_ind,i) - pinv(D1)*(D1*alpha(alpha_ind,i)-s(:,i));
end

end


function [S, mu, sigma2] = standardize(M, mu, sigma2)
% function S = standardize(M, mu, sigma2)
% Make each column of M be zero mean, std 1.
% Thus each row is scaled separately.
%
% If mu, sigma2 are omitted, they are computed from M

M = double(M);
if nargin < 2
  mu = mean(M,2);
  sigma2 = std(M,0,2);
  sigma2 = sigma2 + eps*(sigma2==0);
end

[nrows ncols] = size(M);
S = M - repmat(mu(:), [1 ncols]);
S = S ./ repmat(sigma2, [1 ncols]);


function F = spm_Fcdf(x,v,w)
% Cumulative Distribution Function (CDF) of F (Fisher-Snedecor) distribution
% FORMAT F = spm_Fcdf(x,df)
% FORMAT F = spm_Fcdf(x,v,w)
%
% x  - F-variate   (F has range [0,Inf) )
% df - Degrees of freedom, concatenated along last dimension
%      Eg. Scalar (or column vector) v & w. Then df=[v,w];
% v  - Shape parameter 1 /   numerator degrees of freedom (v>0)
% w  - Shape parameter 2 / denominator degrees of freedom (w>0)
% F  - CDF of F-distribution with [v,w] degrees of freedom at points x
%__________________________________________________________________________
%
% spm_Fcdf implements the Cumulative Distribution Function of the F-distribution.
%
% Definition:
%--------------------------------------------------------------------------
% The CDF F(x) of the F distribution with degrees of freedom v & w,
% defined for positive integer degrees of freedom v & w, is the
% probability that a realisation of an F random variable X has value
% less than x F(x)=Pr{X<x} for X~F(v,w). The F-distribution is defined
% for v>0 & w>0, and for x in [0,Inf) (See Evans et al., Ch16).
%
% Variate relationships: (Evans et al., Ch16 & 37)
%--------------------------------------------------------------------------
% The square of a Student's t variate with w degrees of freedom is
% distributed as an F-distribution with [1,w] degrees of freedom.
%
% For X an F-variate with v,w degrees of freedom, w/(w+v*X^2) has
% distributed related to a Beta random variable with shape parameters
% w/2 & v/2.
%
% Algorithm:
%--------------------------------------------------------------------------
% Using the relationship with the Beta distribution: The CDF of the
% F-distribution with v,w degrees of freedom is related to the
% incomplete beta function by:
%       Pr(X<x) = 1 - betainc(w/(w+v*x^2),w/2,v/2)
% See Abramowitz & Stegun, 26.6.2; Press et al., Sec6.4 for
% definitions of the incomplete beta function. The relationship is
% easily verified by substituting for w/(w+v*x^2) in the integral of the
% incomplete beta function.
%
% MATLAB's implementation of the incomplete beta function is used.
%
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%
%__________________________________________________________________________
% Copyright (C) 1992-2013 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_Fcdf.m 5602 2013-08-12 13:35:52Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

%-Unpack degrees of freedom v & w from single df parameter (v)
if nargin<3
    vs = size(v);
    if prod(vs)==2
        %-DF is a 2-vector
        w = v(2); v = v(1);
    elseif vs(end)==2
        %-DF has last dimension 2 - unpack v & w
        nv = prod(vs);
        w  = reshape(v(nv/2+1:nv),vs(1:end-1));
        v  = reshape(v(1:nv/2)   ,vs(1:end-1));
    else
        error('Can''t unpack both df components from single argument')
    end
end

%-Check argument sizes
ad = [ndims(x);ndims(v);ndims(w)];
rd = max(ad);
as = [[size(x),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))];...
      [size(w),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size'), end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
F = zeros(rs);

%-Only defined for strictly positive v & w. Return NaN if undefined.
md = ( ones(size(x))  &  v>0  &  w>0 );
if any(~md(:))
    F(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Non-zero where defined and x>0
Q  = find( md  &  x>0 );
if isempty(Q), return, end
if xa(1), Qx=Q; else Qx=1; end
if xa(2), Qv=Q; else Qv=1; end
if xa(3), Qw=Q; else Qw=1; end

%-Compute
F(Q) = 1 - betainc(w(Qw)./(w(Qw) + v(Qv).*x(Qx)),w(Qw)/2,v(Qv)/2);


clear all; close all; clc

t=0:0.01:100;
x0=[5 5 5];

[t,Y]=ode45('lrz_rhs',t,x0);

x=Y(:,1); y=Y(:,2); z=Y(:,3);
plot3(x,y,z,'Linewidth',[2]), grid on

figure(2)
plot(t,x,t,y,t,z,'Linewidth',[2])

%%
figure(2)
plot(t,x,'Linewidth',[2])

%%
H=[x(1:5000).'
   x(2:5001).'
   x(3:5002).'
   x(4:5003).'
   x(5:5004).'
   x(6:5005).'
   x(7:5006).'
   x(8:5007).'];

[u,s,v]=svd(H,'econ');
figure(3), plot(diag(s)/(sum(diag(s))),'ro','Linewidth',[3])

%%
figure(4), plot(u(:,1:5),'Linewidth',[2])
figure(5), plot3(v(:,1),v(:,2),v(:,3),'Linewidth',[2])

figure(6), plot(v(:,4),'Linewidth',[2])


% SHIFT_TEST_1D.M
% Plot how step responses vary at level 1 to 4 with 16 shifted steps
% for DT CWT and DWT.
%
% Nick Kingsbury, Cambridge University, May 2002.

% Generate 16 step functions as columns of matrix X.
n = 120;
x = cumsum([zeros(n,16);diag(ones(16,1));zeros(n,16)]) - 0.5;

% Specify DT CWT filters here.
biort = 'near_sym_b';
qshift = 'qshift_b';

% Forward DT CWT on each column of x.
[Yl,Yh] = dtwavexfm(x,4,biort,qshift);

% Inverse DT CWT, one subband at a time, using gain_mask to select subbands.
z1 = dtwaveifm(Yl*0,Yh,biort,qshift,[1 0 0 0]);
z01 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 1 0 0]);
z001 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 0 1 0]);
z0001 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 0 0 1]);
z0000 = dtwaveifm(Yl,Yh,biort,qshift,[0 0 0 0]);

% Check for perfect reconstruction: abs(error) < 1e-12.
z = z1 + z01 + z001 + z0001 + z0000;
DTCWT_error = max(abs(z(:)-x(:)))

% Plot 16 input steps, x, and 16 step responses at each level, z1 to z0000.
setfig(1)
set(gcf,'DefaultTextFontSize',12,'Color',[1 1 1]);
set(gcf,'numbertitle','off','name',['Step responses at 16 adjacent shifts']);
subplot('position',[0.15 0.1 0.3 0.8]);
sc = 1.2; % Scale offsets so sets of curves do not overlap.
on = ones(size(x,2)-1,1)/5;
offset = sc*cumsum([0;on;1;on;1;on;1;on;1;on;1;on]/4).'; % Offsets for each curve.
zdtcwt = [x+0.5 z1 z01 z001 z0001 z0000-0.5] - ones(size(x,1),1)*offset;
tt = -40:40;  % Limits of plot horizontally.
plot(tt,zdtcwt(n+8+tt,:),'-b');
axis off
text(0,-7*sc,'(a) Dual Tree CWT','horiz','c')
xpos = -42; % Position of text labels.
text(xpos,0*sc,'Input','horiz','r','vert','m');
text(xpos,-0.9*sc,'Wavelets','horiz','r','vert','m');
text(xpos,-1.4*sc,'Level 1','horiz','r','vert','m');
text(xpos,-2.4*sc,'Level 2','horiz','r','vert','m');
text(xpos,-3.4*sc,'Level 3','horiz','r','vert','m');
text(xpos,-4.4*sc,'Level 4','horiz','r','vert','m');
text(xpos,-5.5*sc,'Scaling fn','horiz','r','vert','m');
text(xpos,-6*sc,'Level 4','horiz','r','vert','m');
drawnow

% Now do DWT for comparison.

% Specify DWT filters here.
biort = 'antonini';

% Forward DWT on each column of x.
[Yl,Yh] = wavexfm(x,4,biort);

% Inverse DWT, one subband at a time, using gain_mask to select subbands.
z1 = waveifm(Yl*0,Yh,biort,[1 0 0 0]);
z01 = waveifm(Yl*0,Yh,biort,[0 1 0 0]);
z001 = waveifm(Yl*0,Yh,biort,[0 0 1 0]);
z0001 = waveifm(Yl*0,Yh,biort,[0 0 0 1]);
z0000 = waveifm(Yl,Yh,biort,[0 0 0 0]);

% Check for perfect reconstruction: abs(error) < 1e-12.
z = z1 + z01 + z001 + z0001 + z0000;
DWT_error = max(abs(z(:)-x(:)))

subplot('position',[0.47 0.1 0.3 0.8]);
on = ones(size(x,2)-1,1)/5;
offset = sc*cumsum([0;on;1;on;1;on;1;on;1;on;1;on]/4).';
zdwt = [x+0.5 z1 z01 z001 z0001 z0000-0.5] - ones(size(x,1),1)*offset;
tt = -40:40;
plot(tt,zdwt(n+8+tt,:),'-r');
axis off
% xlabel('(b) Real DWT')
text(0,-7*sc,'(b) Real DWT','horiz','c')

return


function h = p13_h ( n, x )

%*****************************************************************************80
%
%% P13_H evaluates the Hessian for problem 13.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  s1 = sum ( cos ( x(1:n) ) );

  for j = 1 : n
    h(j,j) = sin ( x(j) );
  end

  s2 = 0.0;

  for j = 1 : n

    th = cos ( x(j) );
    t = ( n + j ) - h(j,j) - s1 - j * th;
    s2 = s2 + t;
    for k = 1 : j - 1
      h(j,k) = 2.0 * ( sin ( x(k) ) * ( ( n + j + k ) * h(j,j) - th ) - ...
        h(j,j) * cos ( x(k) ) );
    end

    h(j,j) =  ( j * ( j + 2 ) + n ) * h(j,j) * h(j,j) + th * ...
      ( th -  ( 2 * j + 2 ) * h(j,j) ) + t * ( j * th + h(j,j) );

  end

  for j = 1 : n
    h(j,j) = 2.0 * ( h(j,j) + cos ( x(j) ) * s2 );
  end

  for i = 1 : n
    h(i,i+1:n) = h(i+1:n,i);
  end

  return
end


function Y = fft2_mid0(X)
% fft2_mid0 -- 2d fft with argument [-pi,pi] (instead of [0,2pi])
% Usage:
%   Y = fft_mid0(X)
% Inputs:
%   X	Array(n)
% Outputs:
%   Y   Array(n)
% Description:
%  Performs 1d fft with grid (-n/2):(n/2-1) on both time
%  and frequency side.
%
% y(k) = sum_{-n/2 <= t1,t2 < n/2} exp(-i 2pi/n (k1 t1 + k2 t2)) x(t1,t2),
%                                         (-n/2) <= k1,k2 < n/2
%

Y = fftshift(fft2(fftshift(X)));


function [lat, lon, alt] = ecf_to_geodetic(x, y, z)
%ECF_TO_GEODETIC Convert ECF coordinates to geodetic
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////
%
% Convert ECF (Earth Centered Fixed) coordinates to geodetic latitude,
% longitude, and altitude.
%
% USAGE:
%   pos_lla = ecf_to_geodetic(pos_ecf)
%   [lat, lon, alt] = geodetic_to_ecf(pos_ecf_x, pos_ecf_y, pos_ecf_z)
%
% INPUTS:
%   pos_ecf - required : ecf x, y, z coordinates                      [m, m, m]
%
% OUTPUTS:
%   pos_lla - required : geodetic latitude, longitude, and altitude   [deg, deg, m]
%
% NOTES:
%   Zhu, J. Conversion of Earth-centered, Earth-fixed coordinates to
%   geodetic coordinates. IEEE Transactions on Aerospace and Electronic
%   Systems, 30, 3 (July 1994), 957-962.
%
% VERSION:
%   1.0
%     - Sean Hatch 20070911
%     - initial version
%   1.1
%     - Wade Schwartzkopf 20130708
%     - vectorized and componentwise data handling
%
% TODO:

% define constants
e2 = 6.6943799901377997e-3;  % eccentricity squared of Earth (WGS 84 value)
a = 6378137.0;               % semimajor radius of the Earth (WGS 84 value)

% calculate derived constants
e4 = e2 .* e2;
ome2 = 1.0 - e2;
a2 = a .* a;
b = a .* sqrt(ome2);         % semiminor radius of the Earth
b2 = b .* b;
e_b2 = (a2 - b2) ./ b2;

% Handle different forms in input arguments
if nargin==3 % Componentwise inputs, separate arguments for X,Y,Z
    % Nothing to do.  Processing uses this form.
elseif size(x,1)==3 % Array of 3-element vectors
    z = x(3,:);
    y = x(2,:);
    x = x(1,:);
elseif numel(x)==3 % Horizontal 3-element vector
    z = x(3);
    y = x(2);
    x = x(1);
else
    error('WGS_84_NORM:INVALID_INPUTS', 'Invalid inputs.');
end

% calculate intermediates
z2 = z .* z;
r2 = (x .* x) + (y .* y);
r = sqrt(r2);

% Check for invalid solution
valid = ((a .* r) .* (a .* r) + (b .* z) .* (b .* z) > (a2 - b2) .* (a2 - b2));
lon = nan(size(x)); % Default values for invalid solutions
lat = nan(size(x));
alt = nan(size(x));

% calculate longitude
lon(valid) = atan2(y(valid), x(valid))*180/pi; % atan2d not available until MATLAB 2012b

% calculate intermediates
F = 54.0 .* b2 .* z2;
G = r2 + ome2 .* z2 - e2 .* (a2 - b2);
c = e4 .* F .* r2 ./ (G .* G .* G);
s = (1.0 + c + sqrt(c .* c + 2 .* c)) .^ (1 ./ 3);
templ = s + 1.0 ./ s + 1.0;
P = F ./ (3.0 .* templ .* templ .* G .* G);
Q = sqrt(1.0 + 2.0 .* e4 .* P);
r0 = -P .* e2 .* r ./ (1.0 + Q) + ...
    sqrt(abs(0.5 .* a2 .* (1.0 + 1.0 ./ Q) - ...
    P .* ome2 .* z2 ./ (Q .* (1.0 + Q)) - 0.5 .* P .* r2));
temp2 = r - e2 .* r0;
U = sqrt(temp2 .* temp2 + z2);
V = sqrt(temp2 .* temp2 + ome2 .* z2);
z0 = b2 .* z ./ (a .* V);

% calculate latitude
lat(valid) = atan2(z(valid) + e_b2 .* z0(valid), r(valid))*180/pi; % atan2d not available until MATLAB 2012b

% calculate altitude
alt(valid) = U(valid) .* (1.0 - b2 ./ (a .* V(valid)));

if nargout < 2 % Matrix/vector form, rather than componentwise form, was requested
    lat = [lat; lon; alt];
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


% [mu,S] = weighted_mean_cov(x,w)
% x is n x d, w is n x 1
function [mu,S] = weighted_mean_cov(x,w)

%[n,d] = size(x);

z = sum(w);
mu = sum(bsxfun(@times,x,w(:)),1)/z;

diffs = bsxfun(@minus,x,mu);
diffs = bsxfun(@times,diffs,sqrt(w));
S = (diffs'*diffs)/z;


function [ o, x, w ] = epn_lag_01_1 ( n )

%*****************************************************************************80
%
%% EPN_LAG_01_1 implements a precision 1 rule for region EPN_LAG.
%
%  Discussion:
%
%    The rule has order O = 1.
%
%    The rule has precision P = 1.
%
%    EPN is the N-dimensional positive space [0,+oo)^N with exponential
%    or Laguerre weight function:
%
%      w(x(1:n)) = exp ( - sum ( x(1:n) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = 1;

  expon = 0;
  value1 = ep1_lag_monomial_integral ( expon );
  volume = value1 ^ n;

  expon = 1;
  value2 = ep1_lag_monomial_integral ( expon );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  1 point.
%
  k = k + 1;
  x(1:n,k) = value2 / value1;
  w(k) = volume;

  return
end


%% Machine Learning  Online Class === Exercise 7:K-Means Clustering and Principle Component Analysis
close all;clc

%% Optional (ungraded) Exercise: PCA for Visualization--1
A = double(imread('lena.png'));
A = A / 255;
[m,n,l] = size(A);
X = reshape(A,m*n, l);
K = 16;
max_iters = 10;
init_centroids = kMeansInitCentroids(X, K);
[centroids, idx] = runkMeans(X, init_centroids, max_iters);

palette = hsv(K);
colors = palette(idx, :);

figure(1);
scatter3(X(:, 1), X(:, 2), X(:,3), 10, colors);
title('Pixel dataset plotted in 3D. Color shows centroid memberships');
hold off;

%% Optional (ungraded) Exercise: PCA for Visualization--2
[X_norm, mu, sigma] = featureNormalize(X);

[U, S] = pca(X_norm);
Z = projectData(X_norm, U, 2);

figure(2);
plotDataPoints(Z,idx, K);
title('Pixel dataset plotted in 2D, using PCA for dimensionality reduction');


%% computing the bounds from the mean and standard deviations from dynare
prob=0.9;
Params={
	'alp'  ,  'beta'     ,    0.356000, 0.02
	'bet'  ,  'beta'     ,    0.993000, 0.002
	'gam'  ,  'normal'   ,    0.008500, 0.003
	'mst'  ,  'normal'   ,    1.000200, 0.007
	'rho'  ,  'beta'     ,    0.129000, 0.223
	'psi'  ,  'beta'     ,    0.650000, 0.05
	'del'  ,  'beta'     ,    0.010000, 0.005
	'sig_a',  'inv_gamma',    0.035449, inf
	'sig_m',  'inv_gamma',    0.008862, inf
	};
npars=size(Params,1);
bounds=nan(npars,2);
for ii=1:npars
    bounds(ii,:)=distributions.find_bounds(Params{ii,2},Params{ii,3},Params{ii,4},prob);
end
disp(bounds)
%% recover means and standard deviations
MOMS=nan(npars,2);
for ii=1:npars
    [a,b,moments,fval]=distributions.(Params{ii,2})(bounds(ii,1),bounds(ii,2),prob);
    MOMS(ii,1)=moments.mean;
    MOMS(ii,2)=moments.sd;
end
%% display and compare
disp('================ Means and Standard Deviations ================ ')
disp(num2cell(MOMS))
disp('================ Discrepancies ================')
disp(abs(cell2mat(Params(:,3:end))-MOMS))


function zi = pwl_interp_2d ( nxd, nyd, xd, yd, zd, ni, xi, yi )

%*****************************************************************************80
%
%% PWL_INTERP_2D: piecewise linear interpolant to data defined on a 2D grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NXD, NYD, the number of X and Y data values.
%
%    Input, real XD(NXD), YD(NYD), the sorted X and Y data.
%
%    Input, real ZD(NXD,NYD), the Z data.
%
%    Input, integer NI, the number of interpolation points.
%
%    Input, real XI(NI), YI(NI), the coordinates of the
%    interpolation points.
%
%    Output, real ZI(NI), the value of the interpolant.
%
  zi = zeros ( ni, 1 );

  for k = 1 : ni

    i = r8vec_bracket5 ( nxd, xd, xi(k) );
    if ( i == -1 )
      zi(k) = r8_huge ( );
      continue
    end

    j = r8vec_bracket5 ( nyd, yd, yi(k) );
    if ( j == -1 )
      zi(k) = r8_huge ( );
      continue
    end

    if ( yi(k) < yd(j+1) ...
      + ( yd(j) - yd(j+1) ) * ( xi(i) - xd(i) ) / ( xd(i+1) - xd(i) ) )

      dxa = xd(i+1) - xd(i);
      dya = yd(j)   - yd(j);

      dxb = xd(i)   - xd(i);
      dyb = yd(j+1) - yd(j);

      dxi = xi(k)   - xd(i);
      dyi = yi(k)   - yd(j);

      det = dxa * dyb - dya * dxb;

      alpha = ( dxi * dyb - dyi * dxb ) / det;
      beta =  ( dxa * dyi - dya * dxi ) / det;
      gamma = 1.0 - alpha - beta;

      zi(k) = alpha * zd(i+1,j) + beta * zd(i,j+1) + gamma * zd(i,j);

    else

      dxa = xd(i)   - xd(i+1);
      dya = yd(j+1) - yd(j+1);

      dxb = xd(i+1) - xd(i+1);
      dyb = yd(j)   - yd(j+1);

      dxi = xi(k)   - xd(i+1);
      dyi = yi(k)   - yd(j+1);

      det = dxa * dyb - dya * dxb;

      alpha = ( dxi * dyb - dyi * dxb ) / det;
      beta =  ( dxa * dyi - dya * dxi ) / det;
      gamma = 1.0 - alpha - beta;

      zi(k) = alpha * zd(i,j+1) + beta * zd(i+1,j) + gamma * zd(i+1,j+1);

    end

  end

  return
end


% MAP_INTERPOLATE - Plots interpolated values over a map.
%
% Plots a smooth coloring over a map based on interpolation using values at
% some specific locations. It is possible to make several interpolated plots
% if the set of locations is identical.
%
% MAP_INTERPOLATE(C, Y, LONH, LATH, ...)
%
% where
%
% C    : 2 x N matrix of longitudes and latitudes
% Y    : N x K matrix of values
% LONH : vector defining the longitudinal grid points
% LATH : vector defining the latitudinal grid points
%
% Optional parameters:
%
% 'method'   : 'nn' is nearest neighbour interpolation
%              'rbf' is linear radial basis functions interpolation (default)
% 'layout'   : vector of the number of rows and columns of the subplot
%              layout (default is square layout)
% 'distfunc' : distance function to be used (default: 'haversine')

% Last modified 2010-06-11
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function hax = map_interpolate(C, Y, lonh, lath, varargin)

options = struct( ...
    'method',   'rbf', ...
    'layout',   [],   ...
    'distfunc', 'haversine');

% Check arguments
[options, errmsg] = argparse(options, varargin{:});
error(errmsg)

% Form a regular grid
[LONH,LATH] = meshgrid(lonh,lath);

% Evaluate distances for interpolation
Ch = [LONH(:),LATH(:)]';
D = dist_haversine(C, C);
Dh = dist_haversine(Ch, C);

% Settings for multiple plots
K = size(Y,2);
if K > 1 && ~isempty(options.layout)
  M = options.layout(1);
  N = options.layout(2);
else
  M = ceil(sqrt(K));
  N = ceil(K/M);
end
hax = zeros(K,1);

% Plot maps
for k=1:K

  hax(k) = subplot(M,N,k);

  % Interpolate
  yh = interpolate(D, Y(:,k), Dh, 'method', options.method);
  yh = reshape(yh, size(LONH));

  % Plot
  map_pcolor(LONH, LATH, yh);

end

if nargout < 1
  clear hax;
end


% Studies the recovery probability of rank aware thresholding
% with sparsity level and number of channels in joint recovery.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 32;
% Number of signals
Ss = [1, 2, 4, 8, 16, 32];
% Sparsity levels
Ks = 2:30;
num_trials = 200;
num_ks = length(Ks);
num_ss = length(Ss);
success_with_k = zeros(num_ss, num_ks);

snr_threshold = 100;

for ns=1:num_ss
    % Current sparsity level
    S = Ss(ns);
    for nk=1:num_ks
        K = Ks(nk);
        num_successes = 0;
        for nt=1:num_trials
            % Sensing matrix
            Phi = spx.dict.simple.gaussian_dict(M, N);
            % Sparse signal generator
            gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
            % Gaussian distributed non-zero samples
            X = gen.gaussian;
            % Measurement vectors
            Y = Phi * X;
            % Create the solver for rank aware thresholding
            solver = spx.pursuit.joint.RankAwareThresholding(Phi, K);
            result = solver.solve(Y);
            % Solution vectors
            X_Rec = result.Z;
            % Comparison
            cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
            % Reconstruction SNR
            snr = cs.cum_signal_to_noise_ratio;
            success = snr > snr_threshold;
            num_successes = num_successes + success;
            fprintf('S: %d, K=%d, trial=%d, residual: %e, SNR: %f dB\n'...
                , S, K, nt, cs.cum_difference_norm, snr);
        end
        success_with_k(ns, nk) = num_successes / num_trials;
    end
end

save ('bin/figure_1_ra_thresholding_success_with_K_S.mat');


clear all; close all;
I=imread('cameraman.tif');
I=im2double(I);
LEN=21;
THETA=11;
PSF=fspecial('motion', LEN, THETA);
J=imfilter(I, PSF, 'conv', 'circular');
noise_mean=0;
noise_var=0.0001;
K=imnoise(J, 'gaussian', noise_mean, noise_var);
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(K);
NSR1=0;
L1=deconvwnr(K, PSF, NSR1);
NSR2=noise_var/var(I(:));
L2=deconvwnr(K, PSF, NSR2);
figure;
subplot(121);  imshow(L1);
subplot(122);  imshow(L2);


function circle_segment_test13 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST13 demonstrates GAUSS for quadrature rules.
%
%  Discussion:
%
%    Some recursion coefficients ALPHA and BETA are listed in Kautsky
%    and Elhay.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference
%
%    Jaroslav Kautsky, Sylvan Elhay,
%    Calculation of the Weights of Interpolatory Quadratures,
%    Numerische Mathematik,
%    Volume 40, Number 3, October 1982, pages 407-422.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST13\n' );
  fprintf ( 1, '  GAUSS computes the points and weights for a\n' );
  fprintf ( 1, '  Gauss quadrature rule, given the ALPHA and BETA\n' );
  fprintf ( 1, '  recursion coefficients.\n' );
%
%  Legendre rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 0.0;
    if ( i == 1 )
      beta(i) = 2.0;
    else
      beta(i) = 1.0 / ( 4.0 - 1.0 / ( i - 1.0 )^2 );
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEGENDRE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end
%
%  Hermite rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 0.0;
    if ( i == 1 )
      beta(i) = sqrt ( pi );
    else
      beta(i) = ( i - 1 ) / 2;
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  HERMITE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end
%
%  Laguerre rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 2.0 * i - 1.0;
    if ( i == 1 )
      beta(i) = 1;
    else
      beta(i) = ( i - 1 )^2;
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LAGUERRE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end

  return
end


function chu_test ( )

%*****************************************************************************80
%
%% CHU_TEST tests R4_CHU and R8_CHU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHU_TEST:\n' );
  fprintf ( 1, '  Test HYPERGEOMETRIC_U_VALUES, R4_CHU, R8_CHU.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             A               B               X     CHU(A,B,X)\n' );
  fprintf ( 1, '                                                R4_CHU(A,B,X)' );
  fprintf ( 1, '         Diff\n' );
  fprintf ( 1, '                                                R8_CHU(A,B,X)' );
  fprintf ( 1, '         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, x, fx1 ] = hypergeometric_u_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_chu ( single ( a ), single ( b ), single ( x ) );
    fx3 = r8_chu ( a, b, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.4f  %14.4g  %14.6g\n', a, b, x, fx1 );
    fprintf ( 1, '                                                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                                                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


% StackExchange Mathematics Q3537129
% https://math.stackexchange.com/questions/3537129
% Projection of z onto the Affine Half Space {x∣Ax=b,x≥0}.
% References:
%   1.
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     27/12/2020
%   *   First release.

%% General Parameters

subStreamNumberDefault = 2088;0; %<! Set to 0 for Random

run('InitScript.m');

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

generateFigures = ON;

%% Parameters

numRows         = 10;
numCols         = 20;

% Solvers parameters
numIterations   = 500;
stopThr         = 1e-6;

%% Load / Generate Data

mA = randn(numRows, numCols);
vB = randn(numRows, 1);
vZ = randn(numCols, 1);

mAA     = mA * mA.';
mInvAA  = pinv(mAA);

hObjFun = @(vX) 0.5 * sum((vX - vZ) .^ 2);

hP1 = @(vX) vX - (mA.' * mInvAA * ((mA * vX) - vB)); %<! Projection onto the 1st constraint set
hP2 = @(vX) max(vX, 0); %<! Projection onto the 2nd constraint set

hP = @(vX) OrthogonalProjectionOntoConvexSets({hP1; hP2}, vX, numIterations, stopThr);

solverIdx       = 0;
cMethodString   = {};

%% Solution by CVX

solverString = 'CVX';

% cvx_solver('SDPT3'); %<! Default, Keep numRows low
% cvx_solver('SeDuMi');
% cvx_solver('Mosek'); %<! Can handle numRows > 500, Very Good!
% cvx_solver('Gurobi');

hRunTime = tic();

cvx_begin('quiet')
% cvx_begin()
    % cvx_precision('best');
    variable vX(numCols, 1);
    minimize( 0.5 * sum_square(vX - vZ) );
    subject to
        mA * vX == vB;
        vX >= 0;
cvx_end

runTime = toc(hRunTime);

% vX = mX(:);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The ', solverString, ' Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

sCvxSol.vXCvx     = vX;
sCvxSol.cvxOptVal = hObjFun(vX);

%% Solution by Dykstra's Projection Algorithm

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Method'];

hRunTime = tic();

vX = hP(vZ);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Restore Defaults

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


function res = ssc_W(Y,x,ta,sc,type,opC,rl)
% PURPOSE: Temporal disaggregation using the dynamic Chow-Lin method
%          proposed by Santos Silva-Cardoso (2001).
%          Without pretesting for intercept
% ------------------------------------------------------------
% SYNTAX: res = ssc_W(Y,x,ta,sc,type,opC,rl);
% ------------------------------------------------------------
% OUTPUT: res: a structure
%           res.meth    ='Santos Silva-Cardoso';
%           res.ta      = type of disaggregation
%           res.type    = method of estimation
%           res.opC     = option related to intercept
%           res.N       = nobs. of low frequency data
%           res.n       = nobs. of high-frequency data
%           res.pred    = number of extrapolations
%           res.sc      = frequency conversion between low and high freq.
%           res.p       = number of regressors (including intercept)
%           res.Y       = low frequency data
%           res.x       = high frequency indicators
%           res.y       = high frequency estimate
%           res.y_dt    = high frequency estimate: standard deviation
%           res.y_lo    = high frequency estimate: sd - sigma
%           res.y_up    = high frequency estimate: sd + sigma
%           res.u       = high frequency residuals
%           res.U       = low frequency residuals
%           res.gamma    = estimated model parameters (including y(0))
%           res.gamma_sd = estimated model parameters: standard deviation
%           res.gamma_t  = estimated model parameters: t ratios
%           res.phi      = dynamic parameter phi
%           res.beta     = estimated model parameters (excluding y(0))
%           res.beta_sd = estimated model parameters: standard deviation
%           res.beta_t  = estimated model parameters: t ratios
%           res.phi     = innovational parameter
%           res.aic     = Information criterion: AIC
%           res.bic     = Information criterion: BIC
%           res.val     = Objective function used by the estimation method
%           res.wls     = Weighted least squares as a function of phi
%           res.loglik  = Log likelihood as a function of phi
%           res.r       = grid of innovational parameters used by the estimation method
%           res.et       = elapsed time
% ------------------------------------------------------------
% INPUT: Y: Nx1 ---> vector of low frequency data
%        x: nxp ---> matrix of high frequency indicators (without intercept)
%        ta: type of disaggregation
%            ta=1 ---> sum (flow)
%            ta=2 ---> average (index)
%            ta=3 ---> last element (stock) ---> interpolation
%            ta=4 ---> first element (stock) ---> interpolation
%        sc: number of high frequency data points for each low frequency data points
%            sc= 4 ---> annual to quarterly
%            sc=12 ---> annual to monthly
%            sc= 3 ---> quarterly to monthly
%        type: estimation method:
%            type=0 ---> weighted least squares
%            type=1 ---> maximum likelihood
%        opC: 1x1 option related to intercept
%            opc = 0 : no intercept in hf model
%            opc = 1 : intercept in hf model
%        rl: innovational parameter
%            rl = []: 0x0 ---> rl=[0.05 0.99], 100 points grid
%            rl: 1x1 ---> fixed value of phi parameter
%            rl: 1x2 ---> [r_min r_max] search is performed
%                on this range, using a 200 points grid
% ------------------------------------------------------------
% LIBRARY: aggreg, criterion_SSC
% ------------------------------------------------------------
% SEE ALSO: chowlin, litterman, fernandez, td_plot, td_print
% ------------------------------------------------------------
% REFERENCE: Santos, J.M.C. and Cardoso, F.(2001) "The Chow-Lin method
% using dynamic models",Economic Modelling, vol. 18, p. 269-280.
% Di Fonzo, T. (2002) "Temporal disaggregation of economic time series:
% towards a dynamic extension", Dipartimento di Scienze Statistiche,
% Universita di Padova, Working Paper n. 2002-17.

% written by:
%  Enrique M. Quilis
%  Macroeconomic Research Department
%  Ministry of Economy and Competitiveness
%  <enrique.quilis@mineco.es>

% Version 2.1 [April 2012]

t0=clock;

% ------------------------------------------------------------
% Size of the problem

[N,M] = size(Y);    % Size of low-frequency input
[n,p] = size(x);    % Size of p high-frequency inputs (without intercept)

% ------------------------------------------------------------
% Initial checks

if ((opC ~= 0) & (opC ~= 1))
      error ('*** c has an invalid value ***');
end

% ------------------------------------------------------------
% Preparing the X matrix: including an intercept if opC==1

if (opC == 1)
   e=ones(n,1);
   x=[e x];       % Expanding the regressor matrix
   p=p+1;         % Number of p high-frequency inputs (plus intercept)
end

% ------------------------------------------------------------
% Generating the aggregation matrix

C = aggreg(ta,N,sc);

% -----------------------------------------------------------
% Expanding the aggregation matrix to perform
% extrapolation if needed.

if (n > sc * N)
   pred = n - sc*N;           % Number of required extrapolations
   C = [C zeros(N,pred)];
else
   pred = 0;
end

% -----------------------------------------------------------
% Temporal aggregation of the indicators

X = C*x;

% -----------------------------------------------------------
% Optimization of objective funcion (Lik. or WLS)

nrl = length(rl);

switch nrl
    case 0 %Default
        rl = [0.05 0.99];
        rex = criterion_SSC(Y,x,ta,type,opC,rl,X,C,N,n,p);
        phi = rex.phi;
        r   = rex.r;
    case 1 %Fixed innovational parameter
        phi = rl;
        type = 4;
    case 2
        rex = criterion_SSC(Y,x,ta,type,opC,rl,X,C,N,n,p);
        phi = rex.phi;
        r   = rex.r;
    otherwise
        error('*** Grid search on phi is improperly defined. Check rl ***');
end

% -----------------------------------------------------------
% Final estimation with optimal phi
% -----------------------------------------------------------

% Auxiliary matrix useful to simplify computations

I=eye(n); w=I;
LL = diag(-ones(n-1,1),-1);

% -----------------------------------------------------------
% Final estimation with optimal phi

% -----------------------------------------------------------
% Generation of difference matrix D_phi

D_phi = I + phi*LL;
D_phi(1,1)=sqrt(1-phi^2);
iD_phi = inv(D_phi);

% -----------------------------------------------------------
% Truncation remainder: q parameter

q=zeros(n,1);
q(1)=phi;

% -----------------------------------------------------------
% Expanded set of regressors: high and low frequency

z = [x q];
z_phi = iD_phi * z;
Z_phi = C * z_phi;

% -----------------------------------------------------------
% GLS estimator of gamma

w = inv(D_phi' * D_phi);
W = C * w * C';
iW = inv(W);

gamma = (Z_phi' * iW * Z_phi) \ (Z_phi' * iW * Y); % gamma GLS

U = Y - Z_phi * gamma;           % Low frequency residuals
scp = U' * iW * U;               % Weighted least squares
sigma_a = scp/(N-p-1);           % sigma_a estimator (p+1 due to lagged endogenous)
L = w * C' * iW;                 % Filtering matrix
u = L * U;                       % High frequency residuals

% -----------------------------------------------------------
% Temporally disaggregated time series

y = z_phi * gamma + u;

% -----------------------------------------------------------
% Information criteria
% p is expanded to include lagged endogenous

aic=log(sigma_a)+2*(p+1)/N;
bic=log(sigma_a)+log(N)*(p+1)/N;

% -----------------------------------------------------------
% VCV matrix of high frequency estimates

sigma_gamma = sigma_a * inv(Z_phi' * iW * Z_phi);

VCV_y = sigma_a * (eye(n)-L*C) * w + (z_phi-L*Z_phi) * sigma_gamma * (z_phi-L*Z_phi)';

d_y=sqrt((diag(VCV_y)));   % Std. dev. of high frequency estimates
y_li=y-d_y;                % Lower lim. of high frequency estimates
y_ls=y+d_y;                % Upper lim. of high frequency estimates

% -----------------------------------------------------------
% -----------------------------------------------------------
% Loading the structure

res.meth='Santos Silva-Cardoso';

% -----------------------------------------------------------
% Basic parameters

res.ta        = ta;
res.type      = type;
res.N         = N;
res.n         = n;
res.pred      = pred;
res.sc        = sc;
res.p         = p;
res.opC       = opC;
res.rl        = rl;

% -----------------------------------------------------------
% Series

res.Y         = Y;
res.x         = x;
res.y         = y;
res.y_dt      = d_y;
res.y_lo      = y_li;
res.y_up      = y_ls;

% -----------------------------------------------------------
% Residuals

res.u         = u;
res.U         = U;

% -----------------------------------------------------------
% Parameters

res.gamma     = gamma;
res.gamma_sd  = sqrt(diag(sigma_gamma));
res.gamma_t   = gamma./sqrt(diag(sigma_gamma));
res.phi       = phi;

res.beta      = gamma(1:end-1);
res.beta_sd   = res.gamma_sd(1:end-1);
res.beta_t    = res.gamma_t(1:end-1);
res.sigma_a   = sigma_a;

% -----------------------------------------------------------
% Information criteria

res.aic       = aic;
res.bic       = bic;

% -----------------------------------------------------------
% Objective function

if (nrl == 1)
   res.val      = [];
   res.wls      = [];
   res.loglik   = [];
   res.r        = [];
else
   res.val       = rex.val;
   res.wls  	 = rex.wls;
   res.loglik    = rex.loglik;
   res.r         = r;
end

% -----------------------------------------------------------
% Elapsed time

res.et  = etime(clock,t0);


function [llh] = tapas_mdlinear_hier_llh(data, theta, ptheta)
%% Likelihood of the nodes of a linear multivariate model with diagonal
% precisions on each parameter.
%
% It is the likelihood of
%
% p(y|x, beta, pi) = N(y - x*b, 1/pi)
%
% Note that this is a multivariate regression problem, so beta is a matrix
% in which each column correspond to one parameter of the model.
%

%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

y = data.y;
u = data.u;

x = data.u.x;
theta = theta.y;

[np, nc] = size(y);

llh = zeros(np, nc);

ln2pi = log(2 * pi);

for j = 1:nc
    % Mean
    beta = theta{j}.mu;
    % Precision
    pe = theta{j}.pe;
    % Regressors

    lpe = log(pe);

    % number of subjects x number of parameters
    py = u.x * beta;
    [ins, inp] = size(py);
    % TODO this is a problem because it is not consistent
    ty = cell2mat(y(:, j));

    % Residuals
    r = (reshape(ty, inp, ins)' - py);

    % Prediction error
    e = sum(bsxfun(@times, pe, r .* r), 2);

    % This is a vector. For each subject needs to include the precision.
    llh(:, j) = sum(-0.5 * ln2pi +  0.5 * lpe) - 0.5 * e;
end

end


function latLonEnd=directRhumbProbGen(latLonStart,azimuth,dist,height,useHeightApprox,a,f,numSteps4Circ)
%%DIRECTRHUMBLINEPROBGEN Given a starting location near the surface of the
%                    reference ellipsoid as well as a constant heading a
%                    distance to travel, determine the location of a vessel
%                    traveling at the constant heading after traveling the
%                    distance. A constant heading trajectory is a rhumb
%                    line (loxodrome) and is usually not the shortest path
%                    between two points. This function is general in that
%                    it will also solve the direct rhumb problem at a fixed
%                    height above the reference ellipsoid, if desired.
%
%INPUTS: latLonStart A 2X1 vector of the starting ellipsoidal latitude
%                    (North) and longitude (East) in radians.
%            azimuth The constant heading that is to be traveled in
%                    radians East of North.
%               dist The distance that is to be traveled on a constant
%                    heading course.
%             height The heights above the reference ellipsoid at which the
%                    trajectory should be determined. If this parameter
%                    is omitted, then the default value of 0 is used.
%    useHeightApprox If true, and the height is not zero, then a distance
%                    scaling height approximation is used. An equatorial
%                    trajectory traveling a distance dist at a height h
%                    above the reference ellipsoid will only travel
%                    a/(a+h)*dist over the surface. If useHeightApprox is
%                    true, then this scaling applied to any trajectry
%                    (including non-equatorial). If useHeightApprox is
%                    false, a slow iterative optimization used. The default
%                    value if omitted or an empty matrix is passed is
%                    false. This parameter is ignored if height=0.
%                  a The semi-major axis of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84SemiMajorAxis is used.
%                  f The flattening factor of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84Flattening is used.
%      numSteps4Circ If height!=0, and useHeightApprox=false, then an
%                    algorithm propagating a state in ECEF coordinates
%                    around the curved Earth is used. This parameter
%                    determines the number of steps that would be needed
%                    for a target that circumnavigates the globe around the
%                    equator. The default value if this parameter is not
%                    provided is 2000. A value of 6000 appears to be about
%                    the best number for overall precision. Reducing the
%                    number of steps will speed up the function. This
%                    parameter is not used if height=0.
%
%OUTPUTS: latLonEnd The ellipsoidal latitude and longitude that one will be
%                    after starting at latLonStart and traveling a distance
%                    of dist at a constant heading on azimuth.
%
%For zero-altitude, the directRhumbProb function is called. The solution
%for a non-zero height is based on [1]. The function is significantly
%faster if a zero hight is used or if useHeightApprox=true, in which case
%the approximation described in [2] is used.
%
%The algorithm is generally the opposite of indirectRhumbProblem, except
%when a starting or ending point is at a geographic pole, as discussed in
%the comments to the indirectRhumbProblem function.
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%[2] D. F. Crouse, "Worldwide Ground Target State Propagation," in
%    Proceedings of the 23rd International Conference on Information
%    Fusion, 6-9 Jul. 2020.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<8||isempty(numSteps4Circ))
%This is the number of steps that would be taken to travel a distance of
%2*pi*a (circumnavigate the globe the long way). This is a design parameter
%that should be large enough to ensure that the results are accurate.
%However, if it is too large, finite precision errors will accumulate.
    numSteps4Circ=2000;
end

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

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

if(nargin<5||isempty(useHeightApprox))
    useHeightApprox=false;
end

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

if(useHeightApprox)
    %Scale the distance based on the height and then solve the rumb problem
    %with the scaled distance.
    dist=a/(a+height)*dist;
    height=0;
end

if(height==0)
    latLonEnd=directRhumbProblem(latLonStart,azimuth,dist,0,a,f);
    return
else
    %If the height is not zero, then use the algorithm for propagating a
    %level, non-maneuvering dynamic model along a rhumb line forward in
    %time.

    %This is the number of Runge-Kutta steps on a curved Earth that will be
    %performed.
    numSteps=ceil(numSteps4Circ*(dist/(2*pi*a)));
    stepSize=1/numSteps;

    %We have to calculate the initial location and heading in CARTESIAN
    %ECEF coordinates.
    xyzInit=ellips2Cart([latLonStart;height],a,f);

    %The initial heading in the LOCAL East-North-Up tangent-plane
    %coordinate system for level flight.
    uh=[sin(azimuth);cos(azimuth);0];

    %The speed of the target is such that it travels a distance of dist in
    %1 unit of time.
    speed=dist;

    %The initial target state with the position in the GLOBAL ECEF
    %coordinate system and the velocity in a LOCAL flat-Earth coordinate
    %system.
    xInit=[xyzInit;speed*uh];

    aDyn=@(x,t)aPoly(x,t,3);%A constant velocity model.

    %When traveling along a rhumb-line, the local coordinate system is
    %known at all times: it is the local ENU coordinate system. The local
    %basis vectors are deterministially known at all places on the Earth.
    uDet=@(x,t)getENUAxes(Cart2Ellipse(x(1:3),[],a,f));
    xList=RungeKCurvedAtTimes(xInit,[],[0;1],aDyn,uDet,stepSize,4);
    plhEnd=Cart2Ellipse(xList(1:3,end),[],a,f);
    latLonEnd=plhEnd(1:2);%The Cartesian location at the end of the
                          %trajectory.
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 [W, stat] = gsp_learn_graph_log_degrees(Z, a, b, params)
%GSP_LEARN_GRAPH_LOG_DEGREES Learn graph from pairwise distances using negative log prior on nodes degrees
%   Usage:  [W, stat] = gsp_learn_graph_log_degrees(Z, a, b)
%           [W, stat] = gsp_learn_graph_log_degrees(Z, a, b, params)
%
%   Inputs:
%         Z         : Matrix with (squared) pairwise distances of nodes
%         a         : Log prior constant  (bigger a -> bigger weights in W)
%         b         : ||W||_F^2 prior constant  (bigger b -> more dense W)
%         params    : Optional parameters
%
%   Outputs:
%         W         : Weighted adjacency matrix
%         stat      : Optional output statistics (adds small overhead)
%
%
%   'W = gsp_learn_graph_log_degrees(Z, a, b, params)' computes a weighted
%   adjacency matrix $W$ from squared pairwise distances in $Z$, using the
%   smoothness assumption that $\text{trace}(X^TLX)$ is small, where $X$ is
%   the data (columns) changing smoothly from node to node on the graph and
%   $L = D-W$ is the combinatorial graph Laplacian. See the paper of the
%   references for the theory behind the algorithm.
%
%   Alternatively, Z can contain other types of distances and use the
%   smoothness assumption that
%
%   .. sum(sum(W .* Z))
%
%   .. math:: \sum_i\sum_j W_{ij}Z_{ij}
%
%   is small.
%
%   The minimization problem solved is
%
%   .. minimize_W sum(sum(W .* Z)) - a * sum(log(sum(W))) + b * ||W||_F^2/2 + c * ||W-W_0||_F^2/2
%
%   .. math:: \min_W \sum_i\sum_j W_{ij}Z_{ij} -\alpha\sum_i\log(\sum_jW_{ij}) + \frac{\beta}{2} \|W\|_F^2 + c/2 ||W - W0||_F^2
%
%   subject to $W$ being a valid weighted adjacency matrix (non-negative,
%   symmetric, with zero diagonal).
%
%   The algorithm used is forward-backward-forward (FBF) based primal dual
%   optimization (see references).
%
%   Example:::
%
%         G = gsp_sensor(256);
%         f1 = @(x,y) sin((2-x-y).^2);
%         f2 = @(x,y) cos((x+y).^2);
%         f3 = @(x,y) (x-.5).^2 + (y-.5).^3 + x - y;
%         f4 = @(x,y) sin(3*((x-.5).^2+(y-.5).^2));
%         X = [f1(G.coords(:,1), G.coords(:,2)), f2(G.coords(:,1), G.coords(:,2)), f3(G.coords(:,1), G.coords(:,2)), f4(G.coords(:,1), G.coords(:,2))];
%         figure; subplot(2,2,1); gsp_plot_signal(G, X(:,1)); title('1st smooth signal');
%         subplot(2,2,2); gsp_plot_signal(G, X(:,2)); title('2nd smooth signal');
%         subplot(2,2,3); gsp_plot_signal(G, X(:,3)); title('3rd smooth signal');
%         subplot(2,2,4); gsp_plot_signal(G, X(:,4)); title('4th smooth signal');
%         Z = gsp_distanz(X').^2;
%         % we can multiply the pairwise distances with a number to control sparsity
%         [W] = gsp_learn_graph_log_degrees(Z*25, 1, 1);
%         % clean up zeros
%         W(W<1e-5) = 0;
%         G2 = gsp_update_weights(G, W);
%         figure; gsp_plot_graph(G2); title('Graph with edges learned from above 4 signals');
%
%
%   Additional parameters
%   ---------------------
%
%   * *params.W_init*   : Initialization point. default: zeros(size(Z))
%   * *verbosity*       : Default = 1. Above 1 adds a small overhead
%   * *maxit*           : Maximum number of iterations. Default: 1000
%   * *tol*             : Tolerance for stopping criterion. Defaul: 1e-5
%   * *step_size*       : Step size from the interval (0,1). Default: 0.5
%   * *max_w*           : Maximum weight allowed for each edge (or inf)
%   * *w_0*             : Vector for adding prior c/2*||w - w_0||^2
%   * *c*               : multiplier for prior c/2*||w - w_0||^2 if w_0 given
%   * *fix_zeros*       : Fix a set of edges to zero (true/false)
%   * *edge_mask*       : Mask indicating the non zero edges if "fix_zeros"
%
%   If fix_zeros is set, an edge_mask is needed. Only the edges
%   corresponding to the non-zero values in edge_mask will be learnt. This
%   has two applications: (1) for large scale applications it is cheaper to
%   learn a subset of edges. (2) for some applications we don't want some
%   connections to be allowed, for example for locality on images.
%
%   The cost of each iteration is linear to the number of edges to be
%   learned, or the square of the number of nodes (numel(Z)) if fix_zeros
%   is not set.
%
%   The function is using the UNLocBoX functions sum_squareform and
%   squareform_sp.
%   The stopping criterion is whether both relative primal and dual
%   distance between two iterations are below a given tolerance.
%
%   To set the step size use the following rule of thumb: Set it so that
%   relative change of primal and dual converge with similar rates (use
%   verbosity > 1).
%
%   See also: gsp_learn_graph_l2_degrees gsp_distanz gsp_update_weights
%       squareform_sp sum_squareform gsp_compute_graph_learning_theta
%
%   References: kalofolias2016learn komodakis2015playing kalofolias2017large
%

% Author: Vassilis Kalofolias
% Testing: gsp_test_learn_graph
% Date: June 2015

%% Default parameters
if nargin < 4
    params = struct;
end

if not(isfield(params, 'verbosity')),   params.verbosity = 1;   end
if not(isfield(params, 'maxit')),       params.maxit = 1000;      end
if not(isfield(params, 'tol')),         params.tol = 1e-5;      end
if not(isfield(params, 'step_size')),   params.step_size = .5;      end     % from (0, 1)
if not(isfield(params, 'fix_zeros')),   params.fix_zeros = false;      end
if not(isfield(params, 'max_w')),       params.max_w = inf;         end

%% Fix parameter size and initialize
if isvector(Z)
    z = Z;  % lazy copying of matlab doesn't allocate new memory for z
else
    z = squareform_sp(Z);
end
% clear Z   % for large scale computation

z = z(:);
l = length(z);                      % number of edges
% n(n-1)/2 = l => n = (1 + sqrt(1+8*l))/ 2
n = round((1 + sqrt(1+8*l))/ 2);    % number of nodes

if isfield(params, 'w_0')
    if not(isfield(params, 'c'))
        error('When params.w_0 is specified, params.c should also be specified');
    else
        c = params.c;
    end
    if isvector(params.w_0)
        w_0 = params.w_0;
    else
        w_0 = squareform_sp(params.w_0);
    end
    w_0 = w_0(:);
else
    w_0 = 0;
end

% if sparsity pattern is fixed we optimize with respect to a smaller number
% of variables, all included in w
if params.fix_zeros
    if not(isvector(params.edge_mask))
        params.edge_mask = squareform_sp(params.edge_mask);
    end
    % use only the non-zero elements to optimize
    ind = find(params.edge_mask(:));
    z = full(z(ind));
    if not(isscalar(w_0))
        w_0 = full(w_0(ind));
    end
else
    z = full(z);
    w_0 = full(w_0);
end

w = zeros(size(z));

%% Needed operators
% S*w = sum(W)
if params.fix_zeros
    [S, St] = sum_squareform(n, params.edge_mask(:));
else
    [S, St] = sum_squareform(n);
end

% S: edges -> nodes
K_op = @(w) S*w;

% S': nodes -> edges
Kt_op = @(z) St*z;

if params.fix_zeros
    norm_K = normest(S);
    % approximation:
    % sqrt(2*(n-1)) * sqrt(nnz(params.edge_mask) / (n*(n+1)/2)) /sqrt(2)
else
    % the next is an upper bound if params.fix_zeros
    norm_K = sqrt(2*(n-1));
end

%% TODO: Rescaling??
% we want    h.beta == norm_K   (see definition of mu)
% we can multiply all terms by s = norm_K/2*b so new h.beta==2*b*s==norm_K

%% Learn the graph
% min_{W>=0}     tr(X'*L*X) - gc * sum(log(sum(W))) + gp * norm(W-W0,'fro')^2, where L = diag(sum(W))-W
% min_W       I{W>=0} + W(:)'*Dx(:)  - gc * sum(log(sum(W))) + gp * norm(W-W0,'fro')^2
% min_W                f(W)          +       g(L_op(W))      +   h(W)

% put proximal of trace plus positivity together
f.eval = @(w) 2*w'*z;    % half should be counted
%f.eval = @(W) 0;
f.prox = @(w, c) min(params.max_w, max(0, w - 2*c*z));  % all change the same

param_prox_log.verbose = params.verbosity - 3;
g.eval = @(z) -a * sum(log(z));
g.prox = @(z, c) prox_sum_log(z, c*a, param_prox_log);
% proximal of conjugate of g: z-c*g.prox(z/c, 1/c)
g_star_prox = @(z, c) z - c*a * prox_sum_log(z/(c*a), 1/(c*a), param_prox_log);

if w_0 == 0
    % "if" not needed, for c = 0 both are the same but gains speed
    h.eval = @(w) b * norm(w)^2;
    h.grad = @(w) 2 * b * w;
    h.beta = 2 * b;
else
    h.eval = @(w) b * norm(w)^2 + c * norm(w - w_0,'fro')^2;
    h.grad = @(w) 2 * ((b+c) * w - c * w_0);
    h.beta = 2 * (b+c);
end

%% My custom FBF based primal dual (see [1] = [Komodakis, Pesquet])
% parameters mu, epsilon for convergence (see [1])
mu = h.beta + norm_K;     %TODO: is it squared or not??
epsilon = lin_map(0.0, [0, 1/(1+mu)], [0,1]);   % in (0, 1/(1+mu) )

% INITIALIZATION
% primal variable ALREADY INITIALIZED
%w = params.w_init;
% dual variable
v_n = K_op(w);
if nargout > 1 || params.verbosity > 1
    stat.f_eval = nan(params.maxit, 1);
    stat.g_eval = nan(params.maxit, 1);
    stat.h_eval = nan(params.maxit, 1);
    stat.fgh_eval = nan(params.maxit, 1);
    stat.pos_violation = nan(params.maxit, 1);
end
if params.verbosity > 1
    fprintf('Relative change of primal, dual variables, and objective fun\n');
end

tic
gn = lin_map(params.step_size, [epsilon, (1-epsilon)/mu], [0,1]);              % in [epsilon, (1-epsilon)/mu]
for i = 1:params.maxit
    Y_n = w - gn * (h.grad(w) + Kt_op(v_n));
    y_n = v_n + gn * (K_op(w));
    P_n = f.prox(Y_n, gn);
    p_n = g_star_prox(y_n, gn); % = y_n - gn*g_prox(y_n/gn, 1/gn)
    Q_n = P_n - gn * (h.grad(P_n) + Kt_op(p_n));
    q_n = p_n + gn * (K_op(P_n));

    if nargout > 1 || params.verbosity > 2
        stat.f_eval(i) = f.eval(w);
        stat.g_eval(i) = g.eval(K_op(w));
        stat.h_eval(i) = h.eval(w);
        stat.fgh_eval(i) = stat.f_eval(i) + stat.g_eval(i) + stat.h_eval(i);
        stat.pos_violation(i) = -sum(min(0,w));
    end
    rel_norm_primal = norm(- Y_n + Q_n, 'fro')/norm(w, 'fro');
    rel_norm_dual = norm(- y_n + q_n)/norm(v_n);

    if params.verbosity > 3
        fprintf('iter %4d: %6.4e   %6.4e   %6.3e', i, rel_norm_primal, rel_norm_dual, stat.fgh_eval(i));
    elseif params.verbosity > 2
        fprintf('iter %4d: %6.4e   %6.4e   %6.3e\n', i, rel_norm_primal, rel_norm_dual, stat.fgh_eval(i));
    elseif params.verbosity > 1
        fprintf('iter %4d: %6.4e   %6.4e\n', i, rel_norm_primal, rel_norm_dual);
    end

    w = w - Y_n + Q_n;
    v_n = v_n - y_n + q_n;

    if rel_norm_primal < params.tol && rel_norm_dual < params.tol
        break
    end
end
stat.time = toc;
if params.verbosity > 0
    fprintf('# iters: %4d. Rel primal: %6.4e Rel dual: %6.4e  OBJ %6.3e\n', i, rel_norm_primal, rel_norm_dual, f.eval(w) + g.eval(K_op(w)) + h.eval(w));
    fprintf('Time needed is %f seconds\n', stat.time);
end

% use the following for testing:
% g.L = K_op;
% g.Lt = Kt_op;
% g.norm_L = norm_K;
% [w, info] = fbf_primal_dual(w, f, g, h, params);
% %[w, info] = fb_based_primal_dual(w, f, g, h, params);

%%

if params.verbosity > 3
    figure; plot(real([stat.f_eval, stat.g_eval, stat.h_eval])); hold all; plot(real(stat.fgh_eval), '.'); legend('f', 'g', 'h', 'f+g+h');
    figure; plot(stat.pos_violation); title('sum of negative (invalid) values per iteration')
    figure; semilogy(max(0,-diff(real(stat.fgh_eval'))),'b.-'); hold on; semilogy(max(0,diff(real(stat.fgh_eval'))),'ro-'); title('|f(i)-f(i-1)|'); legend('going down','going up');
end

if params.fix_zeros
    w = sparse(ind, ones(size(ind)), w, l, 1);
end

if isvector(Z)
    W = w;
else
    W = squareform_sp(w);
end


% A test script for the Half-vectorization package

% Random matrix
A=ceil(20*rand(5,7))

% Numerotation of diagonals
disp('Numerotation of diagonals');
B=zeros(size(A));
[I J]=itril(size(A),Inf);
B(itril(size(A),Inf))=J-I;
disp(B);

% Get the lower part, shift down by 2
disp('Get the lower part, shift down by 2...');

% Using Matlab
T = tril(ones(size(A)),-2);
HalfV = A(T==1);
disp('Using Matlab TRIL');
disp(HalfV);

% Using c
I = itril(size(A),-2);
HalfV = A(I);
disp('Using indices returned by itril');
disp(HalfV);

% Using Elimination Matrix
HaftV = EliminationM(size(A),-2)*A(:);
disp('Using elimination matrix');
disp(HalfV);

% Replace the diagonal
disp('Replace the diagonal');
Idiag = idiag(size(A));
A(Idiag) = -A(Idiag);
disp(A);

% TOEPLITZ
c = ceil(20*rand(5,1))
r = ceil(20*rand(7,1))
T = mytoeplitz(c, r)


clear all, close all, clc
load ../../CH01_SVD/DATA/allFaces.mat
mustache = double(rgb2gray(imread('mustache.jpg'))/255);

%% Build Training and Test sets
Training = zeros(size(faces,1),30*20);
Test = zeros(size(faces,1),20*20);
for k=1:20
    baseind = 0;
    if(k>1) baseind = sum(nfaces(1:k-1));
    end
%     inds = baseind + randperm(nfaces(k));
    inds = baseind + (1:nfaces(k));
    Training(:,(k-1)*30+1:k*30) = faces(:,inds(1:30));
    Test(:,(k-1)*20+1:k*20) = faces(:,inds(31:50));
end

%% Oclude test image (image 6 of test set for person 7)
% 126 = 6 + 20*(7-1);
% b1 - clean test image
b1 = Test(:,126);
% b2 - mustache disguise
b2 = Test(:,126).*reshape(mustache,n*m,1);
randvec = randperm(n*m);
first40 = randvec(1:floor(.3*length(randvec)));
vals40 = uint8(255*rand(size(first40)));
b3 = b1;
b3(first40) = vals40;
% imagesc(reshape(b3,n,m)), colormap gray
b4 = b1 + 50*randn(size(b1));
imagesc(reshape(b4,n,m)), colormap gray

B = [b1 b2 b3 b4];
for k=1:4
    axes('position',[0  0  1  1])
    imagesc(reshape(B(:,k),n,m)), colormap gray
    axis off
    set(gcf,'Position',[100 100 168*2 192*2])
    set(gcf,'PaperPositionMode','auto')
%     print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b',num2str(k)]);
end
%% DOWNSAMPLE ALL IMAGES
M = size(Training,2);

TrainingSmall = zeros(120,M);
for k=1:M
    temp = reshape(Training(:,k),n,m);
%     tempSmall = temp(16:16:end,16:16:end);
    tempSmall = imresize(temp,[12 10],'lanczos3');
    TrainingSmall(:,k) = reshape(tempSmall,120,1);
end

Bsmall = zeros(120,4);
for k=1:4
    temp = reshape(B(:,k),n,m);
%     tempSmall = temp(16:16:end,16:16:end);
    tempSmall = imresize(temp,[12 10],'lanczos3');
    Bsmall(:,k) = reshape(tempSmall,120,1);
end

% RENORMALIZE COLUMNS OF TRAINING SMALL

for k=1:M
    scalingFactor(k) = norm(TrainingSmall(:,k));
    TrainingSmall(:,k) = TrainingSmall(:,k)/scalingFactor(k);
end

for k=1:4
    axes('position',[0  0  1  1])
    imagesc(reshape(Bsmall(:,k),12,10)), colormap gray
    axis off
    set(gcf,'Position',[100 100 168 192])
    set(gcf,'PaperPositionMode','auto')
%     print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_bsmall',num2str(k)]);
end

%% L1 SEARCH, TESTCLEAN
b1s = Bsmall(:,1);
eps = .01;
cvx_begin;
    variable a1(M);                 % alph is slope of fit to be optimized
    minimize( norm(a1,1) );
    subject to
        norm(TrainingSmall*a1 - b1s,2) < eps;
cvx_end;

figure
plot(a1,'k')
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRCa1');

figure
axes('position',[0  0  1  1])
imagesc(reshape(Training*(a1./scalingFactor'),n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b1L']);

imagesc(reshape(b1-(Training*(a1./scalingFactor')),n,m)), colormap gray
caxis([0 255])
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b1S']);

figure
binnedError = zeros(20,1);
for k=1:20
    L = (k-1)*30+1:k*30;
    binnedError(k) = norm(b1-(Training(:,L)*(a1(L)./scalingFactor(L)')))/norm(b1)
end
bar(binnedError,'FaceColor',[.7 .7 .7])
hold on
bar(7,binnedError(7),'FaceColor',[.7 0 0])
box on
xlim([0 21])
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRChistogram_b1');

%% L1 SEARCH, MUSTACHE
b2s = Bsmall(:,2);
eps = 500;
cvx_begin;
    variable a2(M);                 % alph is slope of fit to be optimized
    minimize( norm(a2,1) );
    subject to
        norm(TrainingSmall*a2 - b2s,2) < eps;
cvx_end;

figure
plot(a2,'k')
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRCa2');

figure
axes('position',[0  0  1  1])
imagesc(reshape(Training*(a2./scalingFactor'),n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b2L']);

imagesc(reshape(b2-(Training*(a2./scalingFactor')),n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b2S']);

binnedError = zeros(20,1);
for k=1:20
    L = (k-1)*30+1:k*30;
    binnedError(k) = norm(b2-(Training(:,L)*(a2(L)./scalingFactor(L)')))/norm(b2)
end
bar(binnedError,'FaceColor',[.7 .7 .7])
hold on
bar(7,binnedError(7),'FaceColor',[.7 0 0])
box on
xlim([0 21])
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRChistogram_b2');

%% L1 SEARCH, OCCLUSION
b3s = Bsmall(:,3);
eps = 1000;
cvx_begin;
    variable a3(M);                 % alph is slope of fit to be optimized
    minimize( norm(a3,1) );
    subject to
        norm(TrainingSmall*a3 - b3s,2) < eps;
cvx_end;

figure
plot(a3,'k')
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRCa3');

figure
axes('position',[0  0  1  1])
imagesc(reshape(Training*(a3./scalingFactor'),n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b3L']);

imagesc(reshape(b3-(Training*(a3./scalingFactor')),n,m)), colormap gray
caxis([0 255])
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b3S']);

binnedError = zeros(20,1);
for k=1:20
    L = (k-1)*30+1:k*30;
    binnedError(k) = norm(b3-(Training(:,L)*(a3(L)./scalingFactor(L)')))/norm(b3)
end
bar(binnedError,'FaceColor',[.7 .7 .7])
hold on
bar(7,binnedError(7),'FaceColor',[.7 0 0])
box on
xlim([0 21])
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRChistogram_b3');

%% L1 SEARCH, NOISE
b4s = Bsmall(:,4);
eps = 10;
cvx_begin;
    variable a4(M);                 % alph is slope of fit to be optimized
    minimize( norm(a4,1) );
    subject to
        norm(TrainingSmall*a4 - b4s,2) < eps;
cvx_end;

figure
plot(a4,'k')
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRCa4');

figure
axes('position',[0  0  1  1])
imagesc(reshape(Training*(a4./scalingFactor'),n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b4L']);

imagesc(reshape(b4-(Training*(a4./scalingFactor')),n,m)), colormap gray
caxis([0 255])
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b4S']);

binnedError = zeros(20,1);
for k=1:20
    L = (k-1)*30+1:k*30;
    binnedError(k) = norm(b4-(Training(:,L)*(a4(L)./scalingFactor(L)')))/norm(b4)
end
bar(binnedError,'FaceColor',[.7 .7 .7])
hold on
bar(7,binnedError(7),'FaceColor',[.7 0 0])
box on
xlim([0 21])
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRChistogram_b4');

%% LEAST SQUARES IS NO GOOD

a4L2 = pinv(Training)*b4;
figure
plot(a4L2,'k')
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRCa4L2');

figure
axes('position',[0  0  1  1])
imagesc(reshape(Training*a4L2,n,m)), colormap gray
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b4L_L2']);

imagesc(reshape(b4-(Training*a4),n,m)), colormap gray
% caxis([0 255])
axis off
set(gcf,'Position',[100 100 168*2 192*2])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_b4S_L2']);

binnedError = zeros(20,1);
for k=1:20
    L = (k-1)*30+1:k*30;
    binnedError(k) = norm(b4-(Training(:,L)*(a4L2(L))))/norm(b4)
end
bar(binnedError,'FaceColor',[.7 .7 .7])
xlim([0 21])
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/f_chCS_ex06_SRChistogram_b4L2');

%%  PLOT LIBRARY AND TEST SAMPLE
figure
axes('position',[0  0  1  1])
imagesc(TrainingSmall), colormap gray
axis off
set(gcf,'Position',[100 100 1200 240])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_LIBRARYSMALL']);

imagesc((b1s)), colormap gray, axis off
caxis([0 255])
set(gcf,'Position',[100 100 50 900])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_TESTIMAGE']);

axes('position',[0  0  1  1])

imagesc((450-a1')), colormap gray, axis off
caxis([0 550])
set(gcf,'Position',[1 1 1440 50])
set(gcf,'PaperPositionMode','auto')
% print('-dpng','-r0','-loose', ['../figures/f_chCS_EX06SRC_TESTSPARSE']);


function M1 = image_resize(M,p1,q1,r1)

% image_resize - resize an image using bicubic interpolation
%
%   M1 = image_resize(M,nx,ny,nz);
% or
%   M1 = image_resize(M,newsize);
%
%   Works for 2D, 2D 2 or 3 channels, 3D images.
%
%   Copyright (c) 2004 Gabriel Peyre

if nargin==2
    % size specified as an array
    q1 = p1(2);
    if length(p1)>2
        r1 = p1(3);
    else
        r1 = size(M,3);
    end
    p1 = p1(1);
end

if nargin<4
    r1 = size(M,3);
end

if ndims(M)>=3
    % RVB image
    M1 = zeros(p1,q1, size(M,3), size(M,4));
    for i=1:size(M,3)
        for j=1:size(M,4)
            M1(:,:,i,j) = image_resize(M(:,:,i,j), p1, q1);
        end
    end
    return;
% elseif ndims(M)==3
%     p = size(M,1);
%     q = size(M,2);
%     r = size(M,3);
%     [Y,X,Z] = meshgrid( (0:q-1)/(q-1), (0:p-1)/(p-1), (0:r-1)/(r-1)  );
%     [YI,XI,ZI] = meshgrid( (0:q1-1)/(q1-1), (0:p1-1)/(p1-1), (0:r1-1)/(r1-1) );
%     M1 = interp3( Y,X,Z, M, YI,XI,ZI ,'cubic');
%     return;
end

p = size(M,1);
q = size(M,2);
[Y,X] = meshgrid( (0:q-1)/(q-1), (0:p-1)/(p-1) );
[YI,XI] = meshgrid( (0:q1-1)/(q1-1), (0:p1-1)/(p1-1) );
M1 = interp2( Y,X, M, YI,XI ,'cubic');


function f = niederreiter_mccurley4 ( x )

%*****************************************************************************80
%
%% NIEDERREITER_MCCURLEY4 evaluates the Niederreiter-McCurley function 4.
%
%  Discussion:
%
%    The spatial dimension is taken as 4.
%
%    The maximum is sought within the unit hypercube.
%
%    The reported maximum value occurs at approximately
%
%      X = [ 1, 1, 1, 1 ]
%
%    with
%
%      F(X) = 9.196986029286059
%
%    Note that FMINCON is designed to minimize functions, so we must negate
%    the output value here in order to satisfy FMINCON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, real X(N), the argument of the objection function.
%
%    Output, real F, the value of the objective function.
%
  f = - 100 * prod ( x ) * exp ( - x(4) ) ./ ( 1 + x(1) * x(2) * x(3) ).^2;

  return
end


close all;
clearvars;
clc;
rng default;

% Ambient space dimension
M = 10;
% Number of subspaces
K = 4;
% common dimension for each subspace
D = 5;

% Construct bases for random subspaces
bases = spx.data.synthetic.subspaces.random_subspaces(M, K, D);
cluster_sizes = [10 4 4 8];
data_points = spx.data.synthetic.subspaces.uniform_points_on_subspaces(bases, cluster_sizes);


function [out] = GLCM_Features4(glcmin,pairs)
%
%
% This is an update of GLCM_Features2 (vectorized) without ismember()
%
% GLCM_Features2 helps to calculate the features from the different GLCMs
% that are input to the function. The GLCMs are stored in a i x j x n
% matrix, where n is the number of GLCMs calculated usually due to the
% different orientation and displacements used in the algorithm. Usually
% the values i and j are equal to 'NumLevels' parameter of the GLCM
% computing function graycomatrix(). Note that matlab quantization values
% belong to the set {1,..., NumLevels} and not from {0,...,(NumLevels-1)}
% as provided in some references
% http://www.mathworks.com/access/helpdesk/help/toolbox/images/graycomatrix
% .html
%
% This vectorized version of GLCM_FEatures1.m reduces the 19 'for' loops
% used in the earlier code to 5 'for' loops
% http://blogs.mathworks.com/loren/2006/07/12/what-are-you-really-measuring
% /
% Using tic toc and cputime as in above discussion
%
% Although there is a function graycoprops() in Matlab Image Processing
% Toolbox that computes four parameters Contrast, Correlation, Energy,
% and Homogeneity. The paper by Haralick suggests a few more parameters
% that are also computed here. The code is not fully vectorized and hence
% is not an efficient implementation but it is easy to add new features
% based on the GLCM using this code. Takes care of 3 dimensional glcms
% (multiple glcms in a single 3D array)
%
% If you find that the values obtained are different from what you expect
% or if you think there is a different formula that needs to be used
% from the ones used in this code please let me know.
% A few questions which I have are listed in the link
% http://www.mathworks.com/matlabcentral/newsreader/view_thread/239608
%
%
%
% Features computed
% Autocorrelation: [2]                      (out.autoc)
% Contrast: matlab/[1,2]                    (out.contr)
% Correlation: matlab                       (out.corrm)
% Correlation: [1,2]                        (out.corrp)
% Cluster Prominence: [2]                   (out.cprom)
% Cluster Shade: [2]                        (out.cshad)
% Dissimilarity: [2]                        (out.dissi)
% Energy: matlab / [1,2]                    (out.energ)
% Entropy: [2]                              (out.entro)
% Homogeneity: matlab                       (out.homom)
% Homogeneity: [2]                          (out.homop)
% Maximum probability: [2]                  (out.maxpr)
% Sum of squares: Variance [1]              (out.sosvh)
% Sum average [1]                           (out.savgh)
% Sum variance [1]                          (out.svarh)
% Sum entropy [1]                           (out.senth)
% Difference variance [1]                   (out.dvarh)
% Difference entropy [1]                    (out.denth)
% Information measure of correlation1 [1]   (out.inf1h)
% Informaiton measure of correlation2 [1]   (out.inf2h)
% Inverse difference (INV) is homom [3]     (out.homom)
% Inverse difference normalized (INN) [3]   (out.indnc)
% Inverse difference moment normalized [3]  (out.idmnc)
%
% The maximal correlation coefficient was not calculated due to
% computational instability
% http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
%
% Formulae from MATLAB site (some look different from
% the paper by Haralick but are equivalent and give same results)
% Example formulae:
% Contrast = sum_i(sum_j(  (i-j)^2 * p(i,j) ) ) (same in matlab/paper)
% Correlation = sum_i( sum_j( (i - u_i)(j - u_j)p(i,j)/(s_i.s_j) ) ) (m)
% Correlation = sum_i( sum_j( ((ij)p(i,j) - u_x.u_y) / (s_x.s_y) ) ) (p[2])
% Energy = sum_i( sum_j( p(i,j)^2 ) )           (same in matlab/paper)
% Homogeneity = sum_i( sum_j( p(i,j) / (1 + |i-j|) ) ) (as in matlab)
% Homogeneity = sum_i( sum_j( p(i,j) / (1 + (i-j)^2) ) ) (as in paper)
%
% Where:
% u_i = u_x = sum_i( sum_j( i.p(i,j) ) ) (in paper [2])
% u_j = u_y = sum_i( sum_j( j.p(i,j) ) ) (in paper [2])
% s_i = s_x = sum_i( sum_j( (i - u_x)^2.p(i,j) ) ) (in paper [2])
% s_j = s_y = sum_i( sum_j( (j - u_y)^2.p(i,j) ) ) (in paper [2])
%
%
% Normalize the glcm:
% Compute the sum of all the values in each glcm in the array and divide
% each element by it sum
%
% Haralick uses 'Symmetric' = true in computing the glcm
% There is no Symmetric flag in the Matlab version I use hence
% I add the diagonally opposite pairs to obtain the Haralick glcm
% Here it is assumed that the diagonally opposite orientations are paired
% one after the other in the matrix
% If the above assumption is true with respect to the input glcm then
% setting the flag 'pairs' to 1 will compute the final glcms that would result
% by setting 'Symmetric' to true. If your glcm is computed using the
% Matlab version with 'Symmetric' flag you can set the flag 'pairs' to 0
%
% References:
% 1. R. M. Haralick, K. Shanmugam, and I. Dinstein, Textural Features of
% Image Classification, IEEE Transactions on Systems, Man and Cybernetics,
% vol. SMC-3, no. 6, Nov. 1973
% 2. L. Soh and C. Tsatsoulis, Texture Analysis of SAR Sea Ice Imagery
% Using Gray Level Co-Occurrence Matrices, IEEE Transactions on Geoscience
% and Remote Sensing, vol. 37, no. 2, March 1999.
% 3. D A. Clausi, An analysis of co-occurrence texture statistics as a
% function of grey level quantization, Can. J. Remote Sensing, vol. 28, no.
% 1, pp. 45-62, 2002
% 4. http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
%
%
% Example:
%
% Usage is similar to graycoprops() but needs extra parameter 'pairs' apart
% from the GLCM as input
% I = imread('circuit.tif');
% GLCM2 = graycomatrix(I,'Offset',[2 0;0 2]);
% stats = GLCM_features4(GLCM2,0)
% The output is a structure containing all the parameters for the different
% GLCMs
%
% [Avinash Uppuluri: avinash_uv@yahoo.com: Last modified: 04/05/2010]

% If 'pairs' not entered: set pairs to 0
if ((nargin > 2) || (nargin == 0))
   error('Too many or too few input arguments. Enter GLCM and pairs.');
elseif ( (nargin == 2) )
    if ((size(glcmin,1) <= 1) || (size(glcmin,2) <= 1))
       error('The GLCM should be a 2-D or 3-D matrix.');
    elseif ( size(glcmin,1) ~= size(glcmin,2) )
        error('Each GLCM should be square with NumLevels rows and NumLevels cols');
    end
elseif (nargin == 1) % only GLCM is entered
    pairs = 0; % default is numbers and input 1 for percentage
    if ((size(glcmin,1) <= 1) || (size(glcmin,2) <= 1))
       error('The GLCM should be a 2-D or 3-D matrix.');
    elseif ( size(glcmin,1) ~= size(glcmin,2) )
       error('Each GLCM should be square with NumLevels rows and NumLevels cols');
    end
end

format long e
if (pairs == 1)
    newn = 1;
    for nglcm = 1:2:size(glcmin,3)
        glcm(:,:,newn)  = glcmin(:,:,nglcm) + glcmin(:,:,nglcm+1);
        newn = newn + 1;
    end
elseif (pairs == 0)
    glcm = glcmin;
end

size_glcm_1 = size(glcm,1);
size_glcm_2 = size(glcm,2);
size_glcm_3 = size(glcm,3);

% checked
out.autoc = zeros(1,size_glcm_3); % Autocorrelation: [2]
out.contr = zeros(1,size_glcm_3); % Contrast: matlab/[1,2]
out.corrm = zeros(1,size_glcm_3); % Correlation: matlab
out.corrp = zeros(1,size_glcm_3); % Correlation: [1,2]
out.cprom = zeros(1,size_glcm_3); % Cluster Prominence: [2]
out.cshad = zeros(1,size_glcm_3); % Cluster Shade: [2]
out.dissi = zeros(1,size_glcm_3); % Dissimilarity: [2]
out.energ = zeros(1,size_glcm_3); % Energy: matlab / [1,2]
out.entro = zeros(1,size_glcm_3); % Entropy: [2]
out.homom = zeros(1,size_glcm_3); % Homogeneity: matlab
out.homop = zeros(1,size_glcm_3); % Homogeneity: [2]
out.maxpr = zeros(1,size_glcm_3); % Maximum probability: [2]

out.sosvh = zeros(1,size_glcm_3); % Sum of sqaures: Variance [1]
out.savgh = zeros(1,size_glcm_3); % Sum average [1]
out.svarh = zeros(1,size_glcm_3); % Sum variance [1]
out.senth = zeros(1,size_glcm_3); % Sum entropy [1]
out.dvarh = zeros(1,size_glcm_3); % Difference variance [4]
%out.dvarh2 = zeros(1,size_glcm_3); % Difference variance [1]
out.denth = zeros(1,size_glcm_3); % Difference entropy [1]
out.inf1h = zeros(1,size_glcm_3); % Information measure of correlation1 [1]
out.inf2h = zeros(1,size_glcm_3); % Informaiton measure of correlation2 [1]
%out.mxcch = zeros(1,size_glcm_3);% maximal correlation coefficient [1]
%out.invdc = zeros(1,size_glcm_3);% Inverse difference (INV) is homom [3]
out.indnc = zeros(1,size_glcm_3); % Inverse difference normalized (INN) [3]
out.idmnc = zeros(1,size_glcm_3); % Inverse difference moment normalized [3]

glcm_sum  = zeros(size_glcm_3,1);
glcm_mean = zeros(size_glcm_3,1);
glcm_var  = zeros(size_glcm_3,1);

% http://www.fp.ucalgary.ca/mhallbey/glcm_mean.htm confuses the range of
% i and j used in calculating the means and standard deviations.
% As of now I am not sure if the range of i and j should be [1:Ng] or
% [0:Ng-1]. I am working on obtaining the values of mean and std that get
% the values of correlation that are provided by matlab.
u_x = zeros(size_glcm_3,1);
u_y = zeros(size_glcm_3,1);
s_x = zeros(size_glcm_3,1);
s_y = zeros(size_glcm_3,1);

% checked p_x p_y p_xplusy p_xminusy
p_x = zeros(size_glcm_1,size_glcm_3); % Ng x #glcms[1]
p_y = zeros(size_glcm_2,size_glcm_3); % Ng x #glcms[1]
p_xplusy = zeros((size_glcm_1*2 - 1),size_glcm_3); %[1]
p_xminusy = zeros((size_glcm_1),size_glcm_3); %[1]
% checked hxy hxy1 hxy2 hx hy
hxy  = zeros(size_glcm_3,1);
hxy1 = zeros(size_glcm_3,1);
hx   = zeros(size_glcm_3,1);
hy   = zeros(size_glcm_3,1);
hxy2 = zeros(size_glcm_3,1);

corm = zeros(size_glcm_3,1);
corp = zeros(size_glcm_3,1);

for k = 1:size_glcm_3

    glcm_sum(k) = sum(sum(glcm(:,:,k)));
    glcm(:,:,k) = glcm(:,:,k)./glcm_sum(k); % Normalize each glcm
    glcm_mean(k) = mean2(glcm(:,:,k)); % compute mean after norm
    glcm_var(k)  = (std2(glcm(:,:,k)))^2;

    for i = 1:size_glcm_1

        for j = 1:size_glcm_2
            p_x(i,k) = p_x(i,k) + glcm(i,j,k);
            p_y(i,k) = p_y(i,k) + glcm(j,i,k); % taking i for j and j for i
            %if (ismember((i + j),[2:2*size_glcm_1]))
                p_xplusy((i+j)-1,k) = p_xplusy((i+j)-1,k) + glcm(i,j,k);
            %end
            %if (ismember(abs(i-j),[0:(size_glcm_1-1)]))
                p_xminusy((abs(i-j))+1,k) = p_xminusy((abs(i-j))+1,k) +...
                    glcm(i,j,k);
            %end
        end
    end

end

% marginal probabilities are now available [1]
% p_xminusy has +1 in index for matlab (no 0 index)
% computing sum average, sum variance and sum entropy:

%Q    = zeros(size(glcm));

i_matrix  = repmat([1:size_glcm_1]',1,size_glcm_2);
j_matrix  = repmat([1:size_glcm_2],size_glcm_1,1);
% i_index = [ 1 1 1 1 1 .... 2 2 2 2 2 ... ]
i_index   = j_matrix(:);
% j_index = [ 1 2 3 4 5 .... 1 2 3 4 5 ... ]
j_index   = i_matrix(:);
xplusy_index = [1:(2*(size_glcm_1)-1)]';
xminusy_index = [0:(size_glcm_1-1)]';
mul_contr = abs(i_matrix - j_matrix).^2;
mul_dissi = abs(i_matrix - j_matrix);
%div_homop = ( 1 + mul_contr); % used from the above two formulae
%div_homom = ( 1 + mul_dissi);

for k = 1:size_glcm_3 % number glcms

    out.contr(k) = sum(sum(mul_contr.*glcm(:,:,k)));
    out.dissi(k) = sum(sum(mul_dissi.*glcm(:,:,k)));
    out.energ(k) = sum(sum(glcm(:,:,k).^2));
    out.entro(k) = - sum(sum((glcm(:,:,k).*log(glcm(:,:,k) + eps))));
    out.homom(k) = sum(sum((glcm(:,:,k)./( 1 + mul_dissi))));
    out.homop(k) = sum(sum((glcm(:,:,k)./( 1 + mul_contr))));
    % [1] explains sum of squares variance with a mean value;
    % the exact definition for mean has not been provided in
    % the reference: I use the mean of the entire normalized glcm
    out.sosvh(k) = sum(sum(glcm(:,:,k).*((i_matrix - glcm_mean(k)).^2)));
    out.indnc(k) = sum(sum(glcm(:,:,k)./( 1 + (mul_dissi./size_glcm_1) )));
    out.idmnc(k) = sum(sum(glcm(:,:,k)./( 1 + (mul_contr./(size_glcm_1^2)))));
    out.maxpr(k) = max(max(glcm(:,:,k)));

    u_x(k)       = sum(sum(i_matrix.*glcm(:,:,k)));
    u_y(k)       = sum(sum(j_matrix.*glcm(:,:,k)));
    % using http://www.fp.ucalgary.ca/mhallbey/glcm_variance.htm for s_x
    % s_y : This solves the difference in value of correlation and might be
    % the right value of standard deviations required
    % According to this website there is a typo in [2] which provides
    % values of variance instead of the standard deviation hence a square
    % root is required as done below:
    s_x(k)  = (sum(sum( ((i_matrix - u_x(k)).^2).*glcm(:,:,k) )))^0.5;
    s_y(k)  = (sum(sum( ((j_matrix - u_y(k)).^2).*glcm(:,:,k) )))^0.5;

   corp(k) = sum(sum((i_matrix.*j_matrix.*glcm(:,:,k))));
   corm(k) = sum(sum(((i_matrix - u_x(k)).*(j_matrix - u_y(k)).*glcm(:,:,k))));

   out.autoc(k) = corp(k);
   out.corrp(k) = (corp(k) - u_x(k)*u_y(k))/(s_x(k)*s_y(k));
   out.corrm(k) = corm(k) / (s_x(k)*s_y(k));

   out.cprom(k) = sum(sum(((i_matrix + j_matrix - u_x(k) - u_y(k)).^4).*...
                glcm(:,:,k)));
   out.cshad(k) = sum(sum(((i_matrix + j_matrix - u_x(k) - u_y(k)).^3).*...
                glcm(:,:,k)));

   out.savgh(k) = sum((xplusy_index + 1).*p_xplusy(:,k));
   % the summation for savgh is for i from 2 to 2*Ng hence (i+1)
   out.senth(k) =  - sum(p_xplusy(:,k).*...
            log(p_xplusy(:,k) + eps));

    % compute sum variance with the help of sum entropy
    out.svarh(k) = sum((((xplusy_index + 1) - out.senth(k)).^2).*...
        p_xplusy(:,k));
        % the summation for savgh is for i from 2 to 2*Ng hence (i+1)

    % compute difference variance, difference entropy,
    % out.dvarh2(k) = var(p_xminusy(:,k));
    % but using the formula in
    % http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
    % we have for dvarh
    out.denth(k) = - sum((p_xminusy(:,k)).*...
        log(p_xminusy(:,k) + eps));
    out.dvarh(k) = sum((xminusy_index.^2).*p_xminusy(:,k));

    % compute information measure of correlation(1,2) [1]
    hxy(k) = out.entro(k);
    glcmk  = glcm(:,:,k)';
    glcmkv = glcmk(:);

    hxy1(k) =  - sum(glcmkv.*log(p_x(i_index,k).*p_y(j_index,k) + eps));
    hxy2(k) =  - sum(p_x(i_index,k).*p_y(j_index,k).*...
        log(p_x(i_index,k).*p_y(j_index,k) + eps));
     hx(k) = - sum(p_x(:,k).*log(p_x(:,k) + eps));
     hy(k) = - sum(p_y(:,k).*log(p_y(:,k) + eps));

    out.inf1h(k) = ( hxy(k) - hxy1(k) ) / ( max([hx(k),hy(k)]) );
    out.inf2h(k) = ( 1 - exp( -2*( hxy2(k) - hxy(k) ) ) )^0.5;

    %     eig_Q(k,:)   = eig(Q(:,:,k));
    %     sort_eig(k,:)= sort(eig_Q(k,:),'descend');
    %     out.mxcch(k) = sort_eig(k,2)^0.5;
    % The maximal correlation coefficient was not calculated due to
    % computational instability
    % http://murphylab.web.cmu.edu/publications/boland/boland_node26.html

end

%       GLCM Features (Soh, 1999; Haralick, 1973; Clausi 2002)
%           f1. Uniformity / Energy / Angular Second Moment (done)
%           f2. Entropy (done)
%           f3. Dissimilarity (done)
%           f4. Contrast / Inertia (done)
%           f5. Inverse difference
%           f6. correlation
%           f7. Homogeneity / Inverse difference moment
%           f8. Autocorrelation
%           f9. Cluster Shade
%          f10. Cluster Prominence
%          f11. Maximum probability
%          f12. Sum of Squares
%          f13. Sum Average
%          f14. Sum Variance
%          f15. Sum Entropy
%          f16. Difference variance
%          f17. Difference entropy
%          f18. Information measures of correlation (1)
%          f19. Information measures of correlation (2)
%          f20. Maximal correlation coefficient
%          f21. Inverse difference normalized (INN)
%          f22. Inverse difference moment normalized (IDN)


clear all, close all, clc
A = imread('../../CH01_SVD/DATA/dog.jpg');
B = rgb2gray(A);

%% Wavelet decomposition (2 level)
n = 2; w = 'db1'; [C,S] = wavedec2(B,n,w);

% LEVEL 1
A1 = appcoef2(C,S,w,1); % Approximation
[H1 V1 D1] = detcoef2('a',C,S,k); % Details
A1 = wcodemat(A1,128);
H1 = wcodemat(H1,128);
V1 = wcodemat(V1,128);
D1 = wcodemat(D1,128);

% LEVEL 2
A2 = appcoef2(C,S,w,1); % Approximation
[H2 V2 D2] = detcoef2('a',C,S,k); % Details
A2 = wcodemat(A2,128);
H2 = wcodemat(H2,128);
V2 = wcodemat(V2,128);
D2 = wcodemat(D2,128);

dec2 = [A2 H2; V2 D2];
dec1 = [imresize(dec2,size(H1)) H1 ; V1 D1];
image(dec1);
colormap gray
axis off, axis tight
set(gcf,'Position',[100 100 600 800])