function GAconstrain
% Genetic Algorithm(real coding)
% By: Javad Ivakpour
% E-mail: javad7@gmail.com
% May 2006
% Goal: find maximum of function that introduced in fun00.m file in current
% directory and can be plotted in plot00
% this file is also include the random serach for comparision
tic
clc
figure(1)
clf
clear all
format long

%------------------------        parameters        ------------------------
% befor using this function you must specified your function in fun00.m
% file in current directory and then set the parameters
var=2;            % Number of variables (this item must be equal to the
                  %   number of variables that is used in the function in
                  %   fun00.m file)
n=100;            % Number of population

m0=30;            % Number of generations that max value remains constant
                  %   (use for termination criteria)
nmutationG=20;                  %number of mutation children(Gaussian)
nmutationR=20;                  %number of mutation children(random)
nelit=2;                        %number of elitism children
valuemin=ones(1,var)*-5*pi;     % min possible value of variables
valuemax=ones(1,var)*5*pi;      % max possible value of variables
A=[1 1;-0.5 +1;1 -1;-1 -1];     % constrains: Ax+B>0
B=[5*pi 0 5*pi +5*pi+10];
%-------------------------------------------------------------------------
[r,c]=size(A);
if c~=var
    disp('Number of columns in A must be equivalent to var')
    A
    var
    return
end
nmutation=nmutationG+nmutationR;
sigma=(valuemax-valuemin)/10;    %Parameter that related to Gaussian
                                 %   function and used in mutation step
max1=zeros(nelit,var);
parent=zeros(n,var);
cu=[valuemin(1) valuemax(1) valuemin(2) valuemax(2)];
for l=1:var
    p(:,l)=valuemin(l)+rand(n,1).*(valuemax(l)-valuemin(l));
end
initial=p;
m=m0;
maxvalue=ones(m,1)*-1e10;
maxvalue(m)=-1e5;
g=0;
meanvalue(m)=0;
%-------------   ****    termination criteria   ****-------------
while abs(maxvalue(m)-maxvalue(m-(m0-1)))>0.001*maxvalue(m) &...
        (abs(maxvalue(m))>1e-10 & abs(maxvalue(m-(m0-1)))>1e-10)...
        & m<10000 & abs(maxvalue(m)-meanvalue(m))>1e-5 | m<20
    sigma=sigma./(1.05);% reducing the sigma value
    %  ------  **** % reducing the number of mutation()random   **** ----
    g=g+1;
    if g>10 & nmutationR>0
        g=0;
        nmutationR=nmutationR-1;
        nmutation=nmutationG+nmutationR;
    end

    %-------------   ****    function evaluation   ****-------------
    for i=1:n
        y(i)=fun00(p(i,:));
        flag(i)=0;
    end
    if var==2
        figure(1)
        hold off
        plot00(cu)
        hold on
        plot3(p(:,1),p(:,2),y,'ro')
        title({'Genetic Algorithm: constrained problem'...
            ,'Performance of GA ( o : each individual)',...
            'blue o:possible solution & red o impossible solution ',...
            'black o: best solution'},'color','b')
    end
    s=sort(y);
    maxy=max(y);
    miny=min(y);
    for i=1:n
        k=length(B);
        for j=1:k
            x=p(i,:);
            if (A(j,:)*x'+B(j))<=0
                y(i)=miny;
                flag(i)=1;
            end
        end
    end
    if var==2
        for i=1:n
            if flag(i)==0
                plot3(p(i,1),p(i,2),y(i),'bo')
            end
        end
    end
    s=sort(y);
    maxvalue1(1:nelit)=s(n:-1:n-nelit+1);
    if nelit==0
        maxvalue1(1)=s(n);
        for i=1:n
            if y(i)==maxvalue1(1)
                max1(1,:)=p(i,:);
            end
        end
    end
    for k=1:nelit
        for i=1:n
            if y(i)==maxvalue1(k)
                max1(k,:)=p(i,:);
            end
        end
    end
    if var==2
        hold on
        plot3(max1(1,1),max1(1,2),maxvalue1(1),'kh')
    end
    y=y-min(y)*1.02;
    sumd=y./sum(y);
    meanvalue=y./(sum(y)/n);

    %-------------   ****   Selection: Rolette wheel   ****-------------
    for l=1:n
        sel=rand;
        sumds=0;
        j=1;
        while sumds<sel
            sumds=sumds+sumd(j);
            j=j+1;
        end
        parent(l,:)=p(j-1,:);
    end
    p=zeros(n,var);

    %-------------   ****    regeneration   ****-------------
    for l=1:var

        %-------------   ****    cross-over   ****-------------
        for j=1:ceil((n-nmutation-nelit)/2)
            t=rand*1.5-0.25;
            p(2*j-1,l)=t*parent(2*j-1,l)+(1-t)*parent(2*j,l);
            p(2*j,l)=t*parent(2*j,l)+(1-t)*parent(2*j-1,l);
        end

        %-------------   ****    elitism   ****-------------
        for k=1:nelit
            p((n-nmutation-k+1),l)=max1(k,l);
        end

        %-------------   ****    mutation   ****-------------
        for i=n-nmutation+1:n-nmutationR
            phi=1-2*rand;
            z=erfinv(phi)*(2^0.5);
            p(i,l)=z*sigma(l)+parent(i,l);

        end
        for i=n-nmutationR+1:n
            p(i,1:var)=valuemin(1:var)+rand(1,var).*(valuemax(1:var)...
                -valuemin(1:var));
        end
        for i=1:n
            for l=1:var
                if p(i,l)<valuemin(l)
                    p(i,l)=valuemin(l);
                elseif p(i,l)>valuemax(l)
                    p(i,l)=valuemax(l);
                end
            end
        end
    end
    p;
    m=m+1;
    max1;
    maxvalue(m)=maxvalue1(1);
    maxvalue00(m-m0)=maxvalue1(1);
    mean00(m-m0)=sum(s)/n;
    meanvalue(m)=mean00(m-m0);
    figure(1)
    if var~=2
        hold off
        plot(maxvalue00)
        hold on
        plot(mean00,'g')
        hold on
        title({'Performance of GA',...
            'best value GA:blue, mean value GA:green',''}...
            ,'color','b')
        xlabel('number of generations')
        ylabel('value')
    end
    pause(0.001)
end

clc
disp('     Genetic Algorithm(real coding)   ')
disp('          By: Javad Ivakpour          ')
disp('       E-mail: javad7@gmail.com       ')
disp('**************************************')
num_of_fun_evaluation=n*m
max_point_GA=max1(1,:)
maxvalue_GA=maxvalue00(m-m0)
if var==2
    figure(1)
    hold on
    plot3(max1(1,1),max1(1,2),maxvalue1,'yp')
    hold on
end
figure(2)
title('Performance of GA(best value)','color','b')
xlabel('number of generations')
ylabel('max value of best solution')
hold on
plot(maxvalue00)
hold on
toc


function [Series, Sigmas] = garchsim(parameters, model, distr, p, q, H, y, NumSamples, NumPaths)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Simulation of GARCH models
-----------------------------------------------------------------------
  USAGE:
  [Series, Sigmas] = garchsim(parameters, model, distr, p, q, H, NumSamples, NumPaths)

 INPUTS:
 parameters:    a vector of parameters (i.e. constant, ARCH, GARCH, Gamma, Delta, DoF)
 model:         'GARCH', 'GJR', 'EGARCH', 'NARCH', 'NGARCH, 'AGARCH', 'APGARCH',
                'NAGARCH'
 distr:         'GAUSSIAN', 'T', 'GED', 'CAUCHY', 'HANSEN' and 'GC'
 p:             positive scalar integer representing the order of ARCH
 q:             positive scalar integer representing the order of GARCH
 H:             a vector of time series of positive pre-sample
                conditional standard deviations.
 y:             a vector of factors for the volatility process, must be positive!
 NumSamples:    The number of samples. If is left empty 100 samples will be generated.
 NumPaths:      The number of paths. If it is left empty 1 path will be estimated.

 OUTPUTS:
 Series:       (NumSamples x NumPaths) simulated series with GARCH variances
 Sigmas:       (NumSamples x NumbPaths) vector of conditional standard deviations.

-----------------------------------------------------------------------
 Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date:     08/2011
-----------------------------------------------------------------------
%}

if nargin == 0
    error('Parameters, GARCH Model, Distribution, ARCH, GARCH, Sigmas, Number of Samples,  Number of Paths')
end

if nargin < 8 | isempty(NumPaths)
    NumPaths=1;
end

% Verifying that the vector of parameters is a column vector
[r,c]=size(parameters);
if r<c
    parameters=parameters';
end

m  =  max([p,q]);

if strcmp(distr,'GAUSSIAN')
    RandomNumbers = randn(NumSamples+m, NumPaths);
elseif strcmp(distr,'T')
    RandomNumbers = trnd(parameters(end), NumSamples+m, NumPaths);
elseif strcmp(distr,'GED')
     error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'CAUCHY')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'HANSEN')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'GC')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Logistic')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Laplace')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Rayleigh')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
end

resids=[];
h=[];
series=[];

if isscalar(y)
     factors = 0;
   else
     factors = sum(parameters(2+p+q:1+p+q+size(y,2)).*y(end,:));
end

% Set initial value to the unconditional
if isequal(strcat(model), 'GARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
elseif  isequal(strcat(model), 'GJR')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)) - sum(parameters(4:1+p+q)));
elseif isequal(strcat(model), 'EGARCH')
   h(1:m,1:NumPaths) = exp((parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)) - sum(parameters(4:1+p+q))));
elseif isequal(strcat(model), 'NARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))); % This is only an approximation
elseif isequal(strcat(model), 'NGARCH')
   h(1:m,1:NumPaths) = nthroot((parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))),parameters(end));
elseif isequal(strcat(model), 'AGARCH')
   h(1:m,1:NumPaths) = (parameters(1)-sum(parameters(2:1+p).*parameters(4:1+p+q))+factors)./(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
elseif isequal(strcat(model), 'APGARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))); % This is only an approximation
elseif isequal(strcat(model), 'NAGARCH')
    h(1:m,1:NumPaths) = (parameters(1)-sum(parameters(2:1+p).*parameters(4:1+p+q)) + factors)./(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
else error('Invalid GARCH Model');
end

if isempty(H)
    series=randn(1:m,NumPaths).*sqrt(h);
else
    series=sqrt(H(end-m+1:end))*ones(1,NumPaths).*RandomNumbers(1:m,:);
end

if strcmp(model,'GARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'* [1 ; series(t-(1:p),i).^2;  h(t-(1:q),i) ];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'GJR')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) =  parameters(1:1+2*p+q)'*[1; series(t-(1:p),i).^2; h(t-(1:q),i); (series(t-(1:p),i)<0).*(series(t-(1:p),i).^2)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'EGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = exp(parameters(1:1+2*p+q)'*[1 ; (abs(series(t-(1:p),i))./sqrt(h(t-(1:p),i))-sqrt(2/pi)); log(h(t-(1:q),i)); series(t-(1:p),i)./sqrt(h(t-(1:p),i))]);
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (abs(series(t-(1:p),i))).^parameters(2+p+q); h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = (parameters(1:1+p+q)'*[1; (series(t-(1:p),i)).^parameters(2+p+q); h(t-(1:q),i).^parameters(2+p+q)]).^(1/parameters(2+p+q));
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'AGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (series(t-(1:p),i) - parameters(2+p+q:1+2*p+q)).^2; h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'APGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = (parameters(1:1+p+q)'*[1; (abs(series(t-(1:p),i)) - parameters(2+p+q:1+2*p+q).*series(t-(1:p),i)).^parameters(2+2*p+q); h(t-(1:q),i).^parameters(2+2*p+q)]).^(1/parameters(2+2*p+q));
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NAGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (series(t-(1:p),i)./sqrt(h(t-(1:p),i)) + parameters(2+p+q:1+2*p+q)).^2; h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
else error('Invalid GARCH Model');
    end

Series=series;
Sigmas=h;

end


function determ = leslie_determinant ( b, di, da )

%*****************************************************************************80
%
%% LESLIE_DETERMINANT returns the determinant of the LESLIE matrix.
%
%  Discussion:
%
%    DETERM = a(4,4) * (
%        a(1,1) * a(2,2) * a(3,3)
%      + a(1,3) * a(2,1) * a(3,2) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real B, DI, DA, the birth rate, infant mortality rate,
%    and aged mortality rate.  These should be positive values.
%    The mortality rates must be between 0.0 and 1.0.  Reasonable
%    values might be B = 0.025, DI = 0.010, and DA = 0.100
%
%    Output, real DETERM, the determinant.
%
  determ = 9.0 * ( 1.0 - da ) / 10.0 * ...
  ( ...
      5.0 * ( 1.0 - di ) / 6.0 ...
    * 13.0 / 14.0 ...
    * 39.0 / 40.0 ...
  +   b ...
    * ( 1.0 - di ) / 6.0 ...
    * 1.0 / 14.0 ...
   );

  return
end


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

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

N = ndims(X);

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

% Compute cell array of weights to multiply into core
W = cell(N,1);
for i = [1:n-1,n+1:N]
  W{i} = (X.u{i}' * U{i});
end
Y = mttkrp(X.core,W,n);

% Find each column of answer by multiplying columns of X.u{n} with weights
V = X.u{n} * Y;


function b = r8sd_mxv ( n, ndiag, offset, a, x )

%*****************************************************************************80
%
%% R8SD_MXV multiplies a R8SD matrix times a vector.
%
%  Discussion:
%
%    The R8SD storage format is for symmetric matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0, and
%    each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%
%    Assuming there are NDIAG nonzero diagonals (ignoring subdiagonals!),
%    we then create an array B that has N rows and NDIAG columns, and simply
%    "collapse" the matrix A to the left:
%
%  Example:
%
%    The "offset" value is printed above each column.
%
%    Original matrix               New Matrix
%
%       0   1   2   3   4   5       0   1   3   5
%
%      11  12   0  14   0  16      11  12  14  16
%      21  22  23   0  25   0      22  23  25  --
%       0  32  33  34   0  36      33  34  36  --
%      41   0  43  44  45   0      44  45  --  --
%       0  52   0  54  55  56      55  56  --  --
%      61   0  63   0  65  66      66  --  --  --
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals that are stored.
%    NDIAG must be at least 1 and no more than N.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8SD matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product A * x.
%
  b(1:n) = 0.0E+00;

  for i = 1 : n
    for jdiag = 1 : ndiag
      j = i + offset(jdiag);
      if ( 1 <= j & j <= n )
        b(i) = b(i) + a(i,jdiag) * x(j);
        if ( offset(jdiag) ~= 0 )
          b(j) = b(j) + a(i,jdiag) * x(i);
        end
      end
    end
  end

  return
end


function wtime_test05 ( )

%*****************************************************************************80
%
%% TEST05 times the matrix multiplication problem problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2010
%
%  Author:
%
%    John Burkardt
%
  l_log_min = 1;
  l_log_max = 5;
  l_min = 4^l_log_min;
  l_max = 4^l_log_max;
  rep_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05\n' );
  fprintf ( 1, '  Time the matrix multiplication problem.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute C = A * B\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where\n' );
  fprintf ( 1, '    A is an L by M matrix,\n' );
  fprintf ( 1, '    B is an M by N matrix,\n' );
  fprintf ( 1, '  and so\n' );
  fprintf ( 1, '    C is an L by N matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Minimum value of L = M = N = %d\n', l_min );
  fprintf ( 1, '  Maximum value of L = M = N = %d\n', l_max );
  fprintf ( 1, '  Number of repetitions of the operation: %d\n', rep_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use triple loop for matrix multiplication.\n' );

  for rep = 1 : rep_num

    for l_log = l_log_min : l_log_max - 1

      l = 4^( l_log );
      m = l;
      n = l;

      a = rand ( l, l );
      b = rand ( l, l );

      seconds = wtime ( );

      for i = 1 : l
        for j = 1 : l
          c(i,j) = 0.0;
          for k = 1 : l
            c(i,j) = c(i,j) + a(i,k) * b(k,j);
          end
        end
      end

      delta(l_log,rep) = wtime ( ) - seconds;

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Timing results using nested loops:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );
  for l_log = l_log_min : l_log_max - 1
    l = 4^( l_log );
    fprintf ( 1, '  %8d  %12f  %12f  %12f  %12f  %12f\n', ...
      l, delta(l_log,1:rep_num) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use C = A * B matrix multiplication.\n' );

  for rep = 1 : rep_num

    for l_log = l_log_min : l_log_max - 1

      l = 4^( l_log );
      m = l;
      n = l;

      a = rand ( l, l );
      b = rand ( l, l );

      seconds = wtime ( );

      c = a * b;

      delta(l_log,rep) = wtime ( ) - seconds;

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Timing results using C = A * B:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );
  for l_log = l_log_min : l_log_max - 1
    l = 4^( l_log );
    fprintf ( 1, '  %8d  %12f  %12f  %12f  %12f  %12f\n', ...
      l, delta(l_log,1:rep_num) );
  end

  return
end


function pass = test_diff( )

tol = 2e2*chebfunpref().cheb2Prefs.chebfun2eps;

% Simple tests:
f = spherefun(@(lam,th) cos(lam).*sin(th));  % x
fx = diff(f, 1);
exact = @(lam,th) 1-(cos(lam).*sin(th)).^2;  % 1-x^2 or y^2 + z^2
pass(1) = SampleError(exact, fx) < tol;
fy = diff(f, 2);
exact = @(lam,th) -cos(lam).*sin(th).*sin(lam).*sin(th);  % -x*y
pass(2) = SampleError(exact, fy) < tol;
fz = diff(f, 3);
exact = @(lam,th) -cos(lam).*sin(th).*cos(th);  % -x*z
pass(3) = SampleError(exact, fz) < tol;

f = spherefun(@(lam,th) cos(th));  % z
fx = diff(f, 1);
exact = @(lam,th) -cos(lam).*sin(th).*cos(th);  % -x*z
pass(4) = SampleError(exact, fx) < tol;
fy = diff(f, 2);
exact = @(lam,th) -sin(lam).*sin(th).*cos(th);  % -y*z
pass(5) = SampleError(exact, fy) < tol;
fz = diff(f, 3);
exact = @(lam,th) 1-cos(th).^2;  % 1-z.^2  or x^2 + y^2
pass(6) = SampleError(exact, fz) < tol;

a = 1/sqrt(2);
f = spherefun(@(lam,th) sin(lam-a).*sin(th));  % Shifted y
fx = diff(f, 1);
exact = @(lam,th) 0.25*(2*cos(lam).*cos(2*th).*sin(lam-a) - sin(2*lam-a) ...
    - 3*sin(a));
pass(7) = SampleError(exact, fx) < tol;
fy = diff(f, 2);
exact = @(lam,th) 0.25*(2*sin(lam).*cos(2*th).*sin(lam-a) + cos(2*lam-a) ...
    + 3*cos(a));
pass(8) = SampleError(exact, fy) < tol;
fz = diff(f, 3);
exact = @(lam,th) -0.5*sin(lam-a).*sin(2*th);
pass(9) = SampleError(exact, fz) < tol;

% Gaussian
lam0 = pi/sqrt(2);
th0 = (sqrt(5) - 2)/2;
sig2 = 1;
r2 = @(lam,th) 2*(1 - (sin(th)*sin(th0)).*cos(lam-lam0)-cos(th)*cos(th0));
f = @(lam,th) exp(-sig2 * r2(lam, th));
fx = diff(spherefun(f), 1);
exact = @(lam,th) -0.5*sig2*exp(-sig2*r2(lam,th)).*(2*cos(lam)...
    .*cos(th0).*sin(2*th)+(cos(2*lam-lam0)-3*cos(lam0)-...
    2*cos(lam).*cos(lam-lam0).*cos(2*th))*sin(th0));
pass(10) = SampleError(exact, fx) < tol;

end

function sample_error = SampleError(h, g)
m = 6;
n = m;
[x, y] = getPoints(m, n);
[L2, T2] = meshgrid(x, y);
F = h(L2, T2);
approx = fevalm(g, x, y);
sample_error = norm(F(:) - approx(:), inf);
end

function [x, y] = getPoints(m, n)

x = trigpts(2*n,[-pi pi]);
y = linspace(0,pi,m).';

end


function [projectedData, mapping] = ml_projection(X,options)
%ML_PROJECTION Does a proection of the data.
%
%   input -----------------------------------------------------------------
%
%       o X : (N x D), a data set with N samples each being of dimension D.
%
%       o options : structure containing the options for the projection
%                   method in question.
%
%               options.method_name  = 'PCA' : this options should be given
%               options.nbDimensions =  3    : the number of dimensions to
%                                             keep when doing the
%                                             projection.
%
%   output ----------------------------------------------------------------
%
%      o projectedData : data after the projection, keeping only the number
%      of dimensions given in the options
%      o mapping : a structure containing the main parameter, results of
%      the algorithm (eigen values ...)
%
%
%

if ~isfield(options,'method_name')
    error('field of method_name of structure options was not defined!');
end

if ~isfield(options,'nbDimensions')
    warning('options.nbDimensions not specified!, setting default value, options.nbDimensions=2');
    options.nbDimensions = 2;
end

if ~isfield(options,'neighbors') && (strcmp(options.method_name,'Isomap') || strcmp(options.method_name,'LLE') || strcmp(options.method_name,'Laplacian'))
    warning('options.neighbors not specified!, setting default value, options.neighbors=7');
    options.neighbors = 7;
end

nbDimensions        = options.nbDimensions;

switch options.method_name

    case {'PCA', 'MDS'}
        [projectedData, mapping] = compute_mapping(X, options.method_name, nbDimensions);

    case 'KPCA'

        [kernel,kpar]                = sanitise_kernel_input(options);
        norm_K = true;

        if isfield(options,'norm_K')
            if (options.norm_K == false)
            display('Kernel (Gram) matrix will NOT be normalized!');
            norm_K = false;
            end
        end

        if length(kpar) == 1
            [projectedData, mapping] = compute_mapping(X, 'KPCA',nbDimensions,norm_K,kernel,kpar(1));
        else
            [projectedData, mapping] = compute_mapping(X, 'KPCA',nbDimensions,norm_K,kernel,kpar(1),kpar(2));
        end

    case {'Isomap'}
	neighbors = options.neighbors;
    [projectedData, mapping] = compute_mapping(X, options.method_name, nbDimensions, neighbors);

    case {'Laplacian'}
	neighbors = options.neighbors;
    sigma = options.sigma;
    [projectedData, mapping] = compute_mapping(X, options.method_name, nbDimensions, neighbors, sigma);

    otherwise
        error(['no such method [' options.method_name '] handled!']);

end

end


function normA = norm(A, n)
%NORM   Norm of a CHEBMATRIX object.
%   NORM(A) computes the Frobenius norm of the CHEBMATRIX object A, defined as
%   the sum of the squares of the 2-norms of each of the blocks.
%
%   NORM(A, 2) or NORM(A, 'fro') is the same as above.
%
%   NORM(A, INF) return the infinity norm of the CHEBMATRIX A, defined as the
%   maximum infinity norm of each of the blocks.
%
%   NORM(A, -INF) returns the negative infinity norm of the CHEBMATRIX A,
%   defined as the minimum of the negative infinity norm of each of the blocks.
%
% See also CHEBMATRIX, CHEBFUN/NORM.

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

% [TODO]: Add support for norms of operators.

% Empty CHEBMATRIX has norm 0.
if ( isempty(A) )
    normA = 0;
    return
end

if ( nargin == 1 )
    n = 'fro'; 	% Frobenius norm is the default.
end

% The norm of a CHEBMATRIX with inf x inf block(s) that are neither
% CHEBFUN2 nor CHEBFUN3 is not supported:
s = cellfun(@(b) min(size(b)), A.blocks);
t = cellfun(@(b) isa(b, 'chebfun2') || isa(b, 'chebfun3'), A.blocks);
if ( ~all(isfinite(s(:))) )
    if ( ~any(t) )
        error('CHEBFUN:CHEBMATRIX:norm:notSupported', ...
            'Norm of a chebmatrix with operator block(s) is not supported.')
    end
end

% Deal with different cases.
switch n

    case {'fro', 2}
        normA = cellfun(@(v) norm(v, 2), A.blocks);
        normA = sqrt(sum(normA(:).^2));

    case {inf, 'inf'}
        normA = cellfun(@(v) norm(v, inf), A.blocks);
        normA = max(normA(:));

    case {-inf, '-inf'}
        normA = cellfun(@(v) norm(v, -inf), A.blocks);
        normA = min(normA(:));

    otherwise
        if ( ~ischar(n) )
            n = num2str(n);
        end
        error('CHEBFUN:CHEBMATRIX:norm:unknown', ...
            'unsupported norm type ''%s''', n)

end

end


function [x, E, s, lambdamax] = tvdip(y, lambda, display, stoptol, maxiter)
% Performs discrete total variation denoising (TVD) using a primal-dual
% interior-point solver. It minimizes the following discrete functional:
%
%  E=(1/2)||y-x||_2^2+lambda*||Dx||_1,
%
% over the variable x, given the input signal y, according to each
% value of the regularization parameter lambda > 0. D is the first
% difference matrix. Uses hot-restarts from each value of lambda to speed
% up convergence for subsequent values: best use of this feature is made by
% ensuring that the chosen lambda values are close to each other.
%
% Usage:
% [x, E, s] = tvdip(y, lambda, display, stoptol, maxiter)
%
% Input arguments:
% - y          Original signal to denoise, size N x 1.
% - lambda     A vector of positive regularization parameters, size L x 1.
%              TVD will be applied to each value in the vector.
% - display    (Optional) Set to 0 to turn off progress display, 1 to turn
%              on. If not specifed, defaults to progress display on.
% - stoptol    (Optional) Precision as determined by duality gap tolerance,
%              if not specified, defaults to 1e-3.
% - maxiter    (Optional) Maximum interior-point iterations, if not
%              specified defaults to 60.
%
% Output arguments:
% - x          Denoised output signal for each value of lambda, size N x L.
% - E          Objective functional at minimum for each lambda, size L x 1.
% - s          Optimization result, 1 = solved, 0 = maximum iterations
%              exceeded before reaching duality gap tolerance, size L x 1.
% - lambdamax  Maximum value of lambda for the given y. If
%              lambda >= lambdamax, the output is the trivial constant
%              solution x = mean(y).
%
% (c) Max Little, 2010. Based around code originally written by
% S.J. Kim, K. Koh, S. Boyd and D. Gorinevsky. If you use this code for
% your research, please cite:
% M.A. Little, Nick S. Jones (2010)
% "Sparse Bayesian Step-Filtering for High-Throughput Analysis of Molecular
% Machine Dynamics", in 2010 IEEE International Conference on Acoustics,
% Speech and Signal Processing, 2010, ICASSP 2010 Proceedings.
%
% This code is released under the terms of GNU General Public License as
% published by the Free Software Foundation; version 2 or later.

error(nargchk(2,5,nargin));
if (nargin < 3)
    display = 1;
end
if (nargin < 4)
    stoptol = 1e-3;
end
if (nargin < 5)
    maxiter = 60;
end

y = y(:);

% Search tuning parameters
ALPHA     = 0.01;   % Backtracking linesearch parameter (0,0.5]
BETA      = 0.5;    % Backtracking linesearch parameter (0,1)
MAXLSITER = 20;     % Max iterations of backtracking linesearch
MU        = 2;      % t update

N = length(y);    % Length of input signal y
M = N-1;          % Size of Dx

% Construct sparse operator matrices
I1 = speye(M,M);
O1 = spalloc(M,1,M);
D = [I1 O1]-[O1 I1];

DDT = D*D';
Dy  = D*y;

% Find max value of lambda
lambdamax = max(abs(DDT\Dy));

if (display)
    fprintf('lambda_max=%5.2e\n', lambdamax);
end

L = length(lambda);
x = zeros(N, L);
s = zeros(L, 1);
E = zeros(L, 1);

% Optimization variables set up once at the start
z    = zeros(M,1);   % Dual variable
mu1  = ones(M,1);    % Dual of dual variable
mu2  = ones(M,1);    % Dual of dual variable

% Work through each value of lambda, with hot-restart on optimization
% variables
for l = 1:L

    t    =  1e-10;
    step =  Inf;
    f1   =  z-lambda(l);
    f2   = -z-lambda(l);

    % Main optimization loop
    s(l) = 1;

    if (display)
        fprintf('Solving for lambda=%5.2e, lambda/lambda_max=%5.2e\nIter# Primal    Dual      Gap\n', ...
            lambda(l), lambda(l)/lambdamax);
    end
    for iters = 0:maxiter

        DTz  = (z'*D)';
        DDTz = D*DTz;
        w    = Dy-(mu1-mu2);

        % Calculate objectives and primal-dual gap
        pobj1 = 0.5*w'*(DDT\w)+lambda(l)*sum(mu1+mu2);
        pobj2 = 0.5*DTz'*DTz+lambda(l)*sum(abs(Dy-DDTz));
        pobj = min(pobj1,pobj2);
        dobj = -0.5*DTz'*DTz+Dy'*z;
        gap  = pobj - dobj;

        if (display)
            fprintf('%5d %7.2e %7.2e %7.2e\n', iters, pobj, dobj, gap);
        end

        % Test duality gap stopping criterion
        if (gap <= stoptol)
            s(l) = 1;
            break;
        end

        if (step >= 0.2)
            t = max(2*M*MU/gap, 1.2*t);
        end

        % Do Newton step
        rz      =  DDTz - w;
        S       =  DDT-sparse(1:M,1:M,mu1./f1+mu2./f2);
        r       = -DDTz + Dy + (1/t)./f1 - (1/t)./f2;
        dz      =  S\r;
        dmu1    = -(mu1+((1/t)+dz.*mu1)./f1);
        dmu2    = -(mu2+((1/t)-dz.*mu2)./f2);

        resDual = rz;
        resCent = [-mu1.*f1-1/t; -mu2.*f2-1/t];
        residual= [resDual; resCent];

        % Perform backtracking linesearch
        negIdx1 = (dmu1 < 0);
        negIdx2 = (dmu2 < 0);
        step = 1;
        if (any(negIdx1))
            step = min( step, 0.99*min(-mu1(negIdx1)./dmu1(negIdx1)) );
        end
        if (any(negIdx2))
            step = min( step, 0.99*min(-mu2(negIdx2)./dmu2(negIdx2)) );
        end

        for liter = 1:MAXLSITER
            newz    =  z  + step*dz;
            newmu1  =  mu1 + step*dmu1;
            newmu2  =  mu2 + step*dmu2;
            newf1   =  newz - lambda(l);
            newf2   = -newz - lambda(l);

            % Update residuals
            newResDual  = DDT*newz - Dy + newmu1 - newmu2;
            newResCent  = [-newmu1.*newf1-1/t; -newmu2.*newf2-1/t];
            newResidual = [newResDual; newResCent];

            if ((max(max(newf1),max(newf2)) < 0) && ...
                    (norm(newResidual) <= (1-ALPHA*step)*norm(residual)))
                break;
            end
            step = BETA*step;
        end

        % Update primal and dual optimization parameters
        z = newz;
        mu1 = newmu1;
        mu2 = newmu2;
        f1 = newf1;
        f2 = newf2;
    end

    x(:,l) = y-D'*z;
    E(l) = 0.5*sum((y-x(:,l)).^2)+lambda(l)*sum(abs(D*x(:,l)));

    % We may have a close solution that does not satisfy the duality gap
    if (iters >= maxiter)
        s(l) = 0;
    end

    if (display)
        if (s(l))
            fprintf('Solved to precision of duality gap %5.2e\n', gap);
        else
            fprintf('Max iterations exceeded - solution may be inaccurate\n');
        end
    end

end


function	X = make_nn_diag(N,K)
% Make N x N matrix with K-principal dimension
% X = make_nn_diag(N,K)
Adiag = 0.2;

Z = rand(N,K);
Z = repmultiply( Z, 1./sqrt(sum(Z.^2,1)) );
X = Adiag * eye(N) + Z * Z';

return
%%%--- END ---%%%

% Make N x N matrix whose n-th diag elements have correlation with neighbor
% X(n,n+k) = 1, k=-K:K

X  = speye(N,N);

%ix = randperm(N);
%ix = ix(1:K);

Nd = max(fix(N/K),1);
ix = 1:Nd:N;

for n = ix
	%a = exp(-(n/NN).^2);
%	X1= spdiags(a*ones(N,1),n,N,N);
	X1= spdiags(rand(N,1),n,N,N);
%	X = X + X1 ;
	X = X + X1 + X1';
end

X = X ./ repmat( 1./sqrt(sum(X.^2,2)) , [1, N]);


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% Inverse DFT of X(k)=[6, -1-j,0,-1+j], k=0,1,2,3

clear;
Xk=[6, -1-j,0,-1+j];
N=length(Xk);
for n=0:N-1
for k=0:N-1
xn(k+1)=Xk(k+1)*exp(j*2*pi*n*k/N);
end
x(n+1)=sum(xn);
end
x=(1/N)*x


classdef Quad < handle
    %QUAD Summary of this class goes here
    %   Detailed explanation goes here

    properties
    V00,V01,V10,V11;
    end

    methods
        function obj =Quad(V00,V01,V10,V11)
            obj.V00 = V00;
            obj.V01 = V01;
            obj.V10 = V10;
            obj.V11 = V11;
        end
        function a = isPointIn(obj,pt)
           in1 = isPointInTriangular(pt,obj.V00,obj.V01,obj.V11);
           in2 = isPointInTriangular(pt,obj.V00,obj.V10,obj.V11);
           if in1==1 || in2==1
               a = 1;
           else
               a = 0;
           end
        end

        function a = isPointsIn(obj,pts)
            in1 = isPointsInTriangular(pts(:,1),pts(:,2),obj.V00,obj.V01,obj.V11);
            in2 = isPointsInTriangular(pts(:,1),pts(:,2),obj.V00,obj.V10,obj.V11);

            a = double(in1|in2);

        end

        function [coefficients,bool] = getBilinearCoordinates(obj,pt)

            a_x = obj.V00.x - obj.V01.x - obj.V10.x + obj.V11.x;
            b_x = -obj.V00.x + obj.V01.x;
            c_x = -obj.V00.x + obj.V10.x;
            d_x = obj.V00.x - pt.x;

            a_y = obj.V00.y - obj.V01.y - obj.V10.y + obj.V11.y;
            b_y = -obj.V00.y + obj.V01.y;
            c_y = -obj.V00.y + obj.V10.y;
            d_y = obj.V00.y - pt.y;

            bigA = -a_y*b_x + b_y*a_x;
            bigB = -a_y*d_x - c_y*b_x + d_y*a_x +b_y*c_x;
            bigC = -c_y*d_x + d_y*c_x;

            tmp1 = -1;
            tmp2 = -1;
            tmp3 = -1;
            tmp4 = -1;

            if bigB*bigB - 4*bigA*bigC >= 0.0
                if abs(bigA) >= 0.000001
                    tmp1 = ( -bigB + sqrt(bigB*bigB - 4*bigA*bigC) ) / ( 2*bigA );
                    tmp2 = ( -bigB - sqrt(bigB*bigB - 4*bigA*bigC) ) / ( 2*bigA );
                else
                    tmp1 = -bigC/bigB;
                end

                if ( tmp1 >= -0.999999 && tmp1 <= 1.000001)

                    tmp3 = -(b_y*tmp1 + d_y) / (a_y*tmp1 + c_y);
                    tmp4 = -(b_x*tmp1 + d_x) / (a_x*tmp1 + c_x);
                    if tmp3 >= -0.999999 && tmp3 <= 1.000001
                        k1 = tmp1;
                        k2 = tmp3;
                    else if tmp4 >= -0.999999 && tmp4 <= 1.000001
                            k1 = tmp1;
                            k2 = tmp4;
                        end
                    end
                end
                if ( tmp2 >= -0.999999 && tmp2 <= 1.000001)

                    if tmp3 >= -0.999999 && tmp3 <= 1.000001
                        k1 = tmp2;
                        k2 = tmp3;
                    else if tmp4 >= -0.999999 && tmp4 <= 1.000001
                            k1 = tmp2;
                            k2 = tmp4;
                        end
                    end
                end
            end

            if k1>=-0.999999 && k1<=1.000001 && k2>=-0.999999 && k2<=1.000001

                coe1 = (1.0-k1)*(1.0-k2);
                coe2 = k1*(1.0-k2);
                coe3 = (1.0-k1)*k2;
                coe4 = k1*k2;

                coefficients(1) = coe1;
                coefficients(2) = coe2;
                coefficients(3) = coe3;
                coefficients(4) = coe4;

                bool = 1;
            else
                bool = 0;
            end

        end

        function minx = getMinX(obj)
            minx = min(obj.V00.x,obj.V01.x);
            minx = min(minx, obj.V10.x);
            minx = min(minx,obj.V11.x);
        end
        function maxx = getMaxX(obj)
            maxx = max(obj.V00.x,obj.V01.x);
            maxx = max(maxx,obj.V10.x);
            maxx = max(maxx,obj.V11.x);
        end
        function miny = getMinY(obj)
            miny = min(obj.V00.y,obj.V01.y);
            miny = min(miny,obj.V10.y);
            miny = min(miny,obj.V11.y);
        end
        function maxy = getMaxY(obj)
           maxy = max(obj.V00.y,obj.V01.y);
           maxy = max(maxy,obj.V10.y);
           maxy = max(maxy,obj.V11.y);
        end

    end
end

function a = isPointInTriangular(pt,V0,V1,V2)
    lambda1 = ((V1.y-V2.y)*(pt.x-V2.x) + (V2.x-V1.x)*(pt.y-V2.y)) / ((V1.y-V2.y)*(V0.x-V2.x) + (V2.x-V1.x)*(V0.y-V2.y));
	lambda2 = ((V2.y-V0.y)*(pt.x-V2.x) + (V0.x-V2.x)*(pt.y-V2.y)) / ((V2.y-V0.y)*(V1.x-V2.x) + (V0.x-V2.x)*(V1.y-V2.y));
    lambda3 = 1-lambda1-lambda2;
    if lambda1 >= 0.0 && lambda1 <= 1.0 && lambda2 >= 0.0 && lambda2 <= 1.0 && lambda3 >= 0.0 && lambda3 <= 1.0
        a = 1; %true
    else
        a = 0; %false
    end
end

function a = isPointsInTriangular(ptx,pty,V0,V1,V2)
    y12 = V1.y - V2.y;
    x21 = V2.x - V1.x;
    x02 = V0.x - V2.x;
    x21 = V2.x - V1.x;
    y02 = V0.y - V2.y;
    y20 = V2.y - V0.y;
    x12 = V1.x - V2.x;
    s1 = y12*x02 + x21*y02;
    s2 = y20*x12 + x02*y12;

    lambda1 = (y12.*(ptx-V2.x) + x21.*(pty-V2.y)) ./ s1;
    lambda2 = (y20.*(ptx-V2.x) + x02.*(pty-V2.y)) ./ s2;
    lambda3 = 1-lambda1-lambda2;

    condition1 = lambda1>=0.0;
    condition2 = lambda1<=1.0;
    condition3 = lambda2>=0.0;
    condition4 = lambda2<=1.0;
    condition5 = lambda3>=0.0;
    condition6 = lambda3<=1.0;

    a = condition1 & condition2 & condition3 & condition4 & condition5 & condition6;

end


%ROTX Rotation about X axis
%
% R = ROTX(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA
% radians about the x-axis.
%
% R = ROTX(THETA, 'deg') as above but THETA is in degrees.
%
% See also ROTY, ROTZ, ANGVEC2R, ROT2.

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

function R = rotx(t, deg)

    if nargin > 1 && strcmp(deg, 'deg')
        t = t *pi/180;
    end

    ct = cos(t);
    st = sin(t);
    R = [
        1   0    0
        0   ct  -st
        0   st   ct
        ];


function pass = test_coeffs
% TAD, 10 Jan 2014

%% Building blocks
dom = [ - 2  - 0.5 1 2];
I = operatorBlock.eye(dom);
D = operatorBlock.diff(dom);
Z = operatorBlock.zeros(dom);
x = chebfun('x', dom);
u = sin(x.^2);
U = operatorBlock.mult(u);

%% Coefficient realizations
% Quasimatrix [ 1 ]
Ic = toCoeff(I);
err(1) = norm(Ic{1} - x.^0);
% Quasimatrix [ 1 0 ]
Dc = toCoeff(D);
err(2) = norm(Dc{1} - x.^0);
err(3) = norm(Dc{2} - 0);
% Quasimatrix [ x.^2 ]
Uc = toCoeff(U);
err(4) = norm(Uc{1} - u);

%%
A = D*(I+U*D);
Ac = toCoeff(A);
err(5) = norm(Ac{1} - u);
err(6) = norm(Ac{2} - (diff(u)+1));
err(7) = norm(Ac{3} - 0);

pass = err < 1e-15;

end


function I_d = Akashi2016(I)
%Akashi2016 I_d = Akashi2016(I)
%  The core mechanism is herein implemented fully in accordance with their
%  paper. I have not implemented the repeated score trials. It is pending.
%  This is because this method is somewhat slow.
%
%  Convergence criterion is based on Yamamoto and Nakazawa (exp(-15)), you
%  can change it:
%    edit Akashi2016.m
%  Search 'eps' and change it.
%
%  Also, because of random initialization, I believe averaging the
%  estimated diffuse component over many applications of the method will
%  greatly improve the quality.
%
%  See also SIHR.

assert(isa(I, 'float'), 'SIHR:I:notTypeSingleNorDouble', ...
    'Input I is not type single nor double.')
assert(min(I(:)) >= 0 && max(I(:)) <= 1, 'SIHR:I:notWithinRange', ...
    'Input I is not within [0, 1] range.')
[n_row, n_col, n_ch] = size(I);
assert(n_row > 1 && n_col > 1, 'SIHR:I:singletonDimension', ...
    'Input I has a singleton dimension.')
assert(n_ch == 3, 'SIHR:I:notRGB', ...
    'Input I is not a RGB image.')

I = 255 * I;
M = 3; % = number of color channels
N = n_row * n_col; % = number of pixels
R = 7; % = number of color clusters - 1
I = reshape(I, [N, M])';
i_s = ones(3, 1, class(I)) / sqrt(3);
H = 254 * rand(R, N, class(I)) + 1;
W_d = 254 * rand(3, R-1, class(I)) + 1;
W_d = my_normc(W_d); % W_d ./ vecnorm(W_d, 2, 1); % normalize(W_d,1,'norm');
W = [i_s, W_d];
A = ones(M, class(I));
lambda = 3;
eps = exp(-15);
F_t_1 = Inf;
iter = uint16(0);
max_iter = uint16(10e3); % change for later convergence (takes way longer)
% tic
while true
    W_bar = my_normc(W);
    H = H .* ((W_bar') * I) ./ ...
        ((W_bar') * W_bar * H + lambda);
    H_d = H(2:end, :);
    Vl = max(0, I-i_s*H(1, :));
    % W_d = W(:,2:end); % not sure
    W_d_bar = W_bar(:, 2:end);
    W_d = W_d_bar .* (Vl * (H_d') + W_d_bar .* (A * W_d_bar * H_d * (H_d'))) ./ ...
        (W_d_bar * H_d * (H_d') + W_d_bar .* (A * Vl * (H_d')));
    W = [i_s, W_d];
    F_t = 0.5 * norm((I - W * H), 'fro') + lambda * sum(H(:));
    if abs(F_t-F_t_1) < eps * abs(F_t) || iter >= max_iter
        break
    end
    F_t_1 = F_t;
    iter = iter + 1;
end
% toc
W_d = W(:, 2:end);
% h_s = H(1, :);
H_d = H(2:end, :);
% I_s = i_s * h_s;
I_d = W_d * H_d;
% I_s = reshape(full(I_s)', [n_row, n_col, n_ch]) / 255;
I_d = reshape(full(I_d)', [n_row, n_col, n_ch]) / 255;

% figure(1), imshow(I_s)
% figure(2), imshow(I_d)

end


%  computing the Householder transformation  w = H*v  with
%            H = I - 2*u*u'
%
%  syntax  >> w = HouseholderTransform(u,v)


%computes the spectral flatness from the magnitude spectrum
%> called by ::ComputeFeature
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data (unused)
%>
%> @retval vtf spectral flatness
% ======================================================================
function [vtf] = FeatureSpectralFlatness (X, f_s)

    % replace product sum with log/exp
    XLog = log(X + 1e-20);
    vtf = exp(mean(XLog, 1)) ./ (mean(X, 1));

    % avoid NaN for silence frames
    vtf (min(X, [], 1) == 0) = 0;

end


function [Xi W] = SigmaPoints(xm, P, kappa)
%
%
n  = numel(xm);
Xi = zeros(n, 2*n+1);              % sigma points = col of Xi
W  = zeros(n, 1);

Xi(:, 1) = xm;
W(1)     = kappa / (n + kappa);

U = chol((n+kappa)*P);             % U'*U = (n+kappa)*P

for k=1:n
  Xi(:, k+1) = xm + U(k, :)';      % row of U
  W(k+1)     = 1 / (2*(n+kappa));
end

for k=1:n
  Xi(:, n+k+1) = xm - U(k, :)';
  W(n+k+1)      = 1 / (2*(n+kappa));
end


function res = isCounterClockwise(p1, p2, p3, varargin)
%ISCOUNTERCLOCKWISE Compute relative orientation of 3 points
%
%   CCW = isCounterClockwise(P1, P2, P3);
%   Computes the orientation of the 3 points. The returns is:
%   +1 if the path P1->P2->P3 turns Counter-Clockwise (i.e., the point P3
%       is located "on the left" of the line P1-P2)
%   -1 if the path turns Clockwise (i.e., the point P3 lies "on the right"
%       of the line P1-P2)
%   0  if the point P3 is located on the line segment [P1 P2].
%
%   This function can be used in more complicated algorithms: detection of
%   line segment intersections, convex hulls, point in triangle...
%
%   CCW = isCounterClockwise(P1, P2, P3, EPS);
%   Specifies the threshold used for detecting colinearity of the 3 points.
%   Default value is 1e-12 (absolute).
%
%   Example
%   isCounterClockwise([0 0], [10 0], [10 10])
%   ans =
%       1
%   isCounterClockwise([0 0], [0 10], [10 10])
%   ans =
%       -1
%   isCounterClockwise([0 0], [10 0], [5 0])
%   ans =
%       0
%
%   See also
%   points2d, isPointOnLine, isPointInTriangle
%
%   References
%   Algorithm adapated from Sedgewick's book.
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-04-09
% Copyright 2011 INRA - Cepia Software Platform.

%   HISTORY
%   2011-05-16 change variable names, add support for point arrays

% get threshold value
eps = 1e-12;
if ~isempty(varargin)
    eps = varargin{1};
end

% ensure all data have same size
np = max([size(p1, 1) size(p2, 1) size(p3,1)]);
if np > 1
    if size(p1,1) == 1
        p1 = repmat(p1, np, 1);
    end
    if size(p2,1) == 1
        p2 = repmat(p2, np, 1);
    end
    if size(p3,1) == 1
        p3 = repmat(p3, np, 1);
    end
end

% init with 0
res = zeros(np, 1);

% extract vector coordinates
x0  = p1(:, 1);
y0  = p1(:, 2);
dx1 = p2(:, 1) - x0;
dy1 = p2(:, 2) - y0;
dx2 = p3(:, 1) - x0;
dy2 = p3(:, 2) - y0;

% check non colinear cases
res(dx1 .* dy2 > dy1 .* dx2) =  1;
res(dx1 .* dy2 < dy1 .* dx2) = -1;

% case of colinear points
ind = abs(dx1 .* dy2 - dy1 .* dx2) < eps;
res(ind( (dx1(ind) .* dx2(ind) < 0) | (dy1(ind) .* dy2(ind) < 0) )) = -1;
res(ind(  hypot(dx1(ind), dy1(ind)) <  hypot(dx2(ind), dy2(ind)) )) =  1;


function J = convBox( I, r, s, nomex )
% Extremely fast 2D image convolution with a box filter.
%
% Convolves an image by a F=ones(2*r+1,2*r+1)/(2*r+1)^2 filter. The
% convolution can be performed in constant time per-pixel, independent of
% the radius r. In fact the implementation is nearly optimal, with the
% convolution taking only slightly more time than creating a copy of the
% input array. Boundary effects are handled as if the image were padded
% symmetrically prior to performing the convolution. An optional integer
% downsampling parameter "s" can be specified, in which case the output is
% downsampled by s (the implementation is efficient with downsampling
% occurring simultaneously with smoothing, saving additional time).
%
% The output is exactly equivalent to the following Matlab operations:
%  f = ones(1,2*r+1); f=f/sum(f);
%  J = padarray(I,[r r],'symmetric','both');
%  J = convn(convn(J,f,'valid'),f','valid');
%  if(s>1), t=floor(s/2)+1; J=J(t:s:end-s+t,t:s:end-s+t,:); end
% The computation, however, is an order of magnitude faster than the above.
%
% When used as a smoothing filter, the standard deviation (sigma) of a box
% filter with radius r can be computed using [sigma=sqrt(r*(r+1)/3)]. For
% the first few values of r this translates to: r=1: sigma=sqrt(2/3), r=2:
% sigma=sqrt(2), r=3: sigma=2. Given sigma, the equivalent value of r can
% be computed via [r=sqrt(12*sigma*sigma+1)/2-.5].
%
% The related function convTri performs convolution with a triangle filter,
% which has nicer properties if used for smoothing, but is slightly slower.
%
% This code requires SSE2 to compile and run (most modern Intel and AMD
% processors support SSE2). Please see: http://en.wikipedia.org/wiki/SSE2.
%
% USAGE
%  J = convBox( I, r, [s], [nomex] )
%
% INPUTS
%  I      - [hxwxk] input k channel single image
%  r      - integer filter radius
%  s      - [1] integer downsampling amount after convolving
%  nomex  - [0] if true perform computation in matlab (for testing/timing)
%
% OUTPUTS
%  J      - [hxwxk] smoothed image
%
% EXAMPLE
%  I = single(imResample(imread('cameraman.tif'),[480 640]))/255;
%  r = 5; s = 2; % set parameters as desired
%  tic, J1=convBox(I,r,s); toc % mex version (fast)
%  tic, J2=convBox(I,r,s,1); toc % matlab version (slow)
%  figure(1); im(J1); figure(2); im(abs(J2-J1));
%
% See also conv2, convTri
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.02
% Copyright 2014 Piotr Dollar & Ron Appel.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

assert( r>=0 );
if( nargin<3 ), s=1; end
if( nargin<4 ), nomex=0; end
if( isempty(I) || (r==0 && s==1) ), J = I; return; end
m=min(size(I,1),size(I,2)); if( m<4 || 2*r+1>=m ), nomex=1; end

if( nomex==0 )
  if( r==1 && s<=2 )
    J = convConst('convTri1',I,1,s);
  else
    J = convConst('convBox',I,r,s);
  end
else
  f = ones(1,2*r+1); f=f/sum(f);
  J = padarray(I,[r r],'symmetric','both');
  J = convn(convn(J,f,'valid'),f','valid');
  if(s>1), t=floor(s/2)+1; J=J(t:s:end-s+t,t:s:end-s+t,:); end
end


% Mathematics Q2410741
% https://math.stackexchange.com/questions/2410741
% Matrix differentiation on ∥A−B∘X∥2F+λ∥X∥2F
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     30/08/2017
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

DIFF_MODE_FORWARD   = 1;
DIFF_MODE_BACKWARD  = 2;
DIFF_MODE_CENTRAL   = 3;

%% Simulation Parameters

numRows = 2;

paramLambda = 0.1 * rand([1, 1]);

difMode = DIFF_MODE_FORWARD;
epsVal  = 1e-6;

numSamples      = 1000;
numIterations   = 2500;
stepSize        = 0.001;

%% Generate Data

vX = rand([numRows, 1]);

mA = randn([numRows, numRows]);
mB = randn([numRows, numRows]);

hObjFun = @(vX) (norm(mA - (mB .* (vX * vX.')), 'fro') .^ 2) + (paramLambda * (norm(vX * vX.', 'fro') ^ 2));
% hObjFun = @(vX) sum(sum((mA - mB .* (vX * vX.')) .^ 2)) + (paramLambda * sum(sum((vX * vX.') .^ 2)));

%% Numerical Solution

vGNumerical = CalcFunGrad(vX, hObjFun, difMode, epsVal);

%% Analytic Solution

vG = zeros([numRows, 1]);

for kk = 1:numRows
    for ii = 1:numRows
        for jj = 1:numRows
            if(ii == kk)
                vG(kk) = vG(kk) + (-2 * mB(ii, jj) * vX(jj) * (mA(ii, jj) - mB(ii, jj) * vX(ii) * vX(jj)));
                vG(kk) = vG(kk) + (2 * paramLambda * vX(jj) * (vX(ii) * vX(jj)));
            end
            if(jj == kk)
                vG(kk) = vG(kk) + (-2 * mB(ii, jj) * vX(ii) * (mA(ii, jj) - mB(ii, jj) * vX(ii) * vX(jj)));
                vG(kk) = vG(kk) + (2 * paramLambda * vX(ii) * (vX(ii) * vX(jj)));
            end
        end
    end
end

%% Analysis

disp(['Maximum Error Between Numercial Differntiation to Analytic - ', num2str(max(abs(vG - vGNumerical)))]);

%% Minimization

mZ      = zeros([numSamples, numSamples]);
vXGrid  = linspace(-5, 5, numSamples);

for jj = 1:numSamples
    for ii = 1:numSamples
        mZ(ii, jj) = hObjFun([vXGrid(jj); vXGrid(ii)]);
    end
end

[minVal, argMin] = min(mZ(:));
[minValRowIdx, minValColIdx] = ind2sub([numSamples, numSamples], argMin);

% Numercial Optimization

mX          = zeros([numRows, numIterations]);
vObjFunVal  = zeros([numIterations, 1]);

% Initialization
vX              = rand([numRows, 1]);
vX              = [-2; 4];

mX(:, 1)        = vX;
vObjFunVal(1)   = hObjFun(vX);

for ii = 2:numIterations
    vG              = CalcFunGrad(vX, hObjFun, difMode, epsVal);
    vX              = vX - (stepSize * vG);
    mX(:, ii)       = vX;
    vObjFunVal(ii)  = hObjFun(vX);
end

figureIdx = figureIdx + 1;

hFigure         = figure('Position', [100, 100, 720, 960]);
hAxes           = subplot(3, 1, 1:2);
set(hAxes, 'NextPlot', 'add');
hImageObject    = image('XData', vXGrid, 'YData', vXGrid, 'CData', mZ, 'CDataMapping', 'scaled');
[mCountourLines, hContouObj]  = contour(vXGrid, vXGrid, mZ);
set(hContouObj, 'LineColor', 'white');
hLineSeries(1)  = line(vXGrid(minValColIdx), vXGrid(minValRowIdx));
set(hLineSeries(1), 'LineStyle', 'none', 'Marker', 'diamond', 'MarkerSize', markerSizeLarge, 'MarkerEdgeColor', 'r', 'MarkerFaceColor', 'r');
hLineSeries(2)  = line(mX(2, :), mX(1, :));
set(hLineSeries(2), 'LineStyle', 'none', 'Marker', 'o', 'MarkerSize', 5, 'MarkerEdgeColor', 'm', 'MarkerFaceColor', 'w');
set(hAxes, 'DataAspectRatio', [1, 1, 1]);
set(get(hAxes, 'Title'), 'String', {['Objective Function Value']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['x_2']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['x_1']}, ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend([hLineSeries(1), hLineSeries(2)], {['Optimal Argument'], ['Gradient Descent Iterations']});

hAxes = subplot(3, 1, 3);
hLineSeries = plot(1:numIterations, vObjFunVal);
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', {['Objective Function Value']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['Iteration Number']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['Value']}, ...
    'FontSize', fontSizeAxis);

saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);

%% Restore Defaults

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


function [ babel ] = babel( Phi )
%BABEL computes the Babel function of a dictionary
% Columns of Phi must be unit norm
% The function has been defined by Joel A Tropp

% Let us compute the Gram matrix
G = Phi' * Phi;
% Let us take element wise absolute
absG = abs(G);
% Let us sort each row in descending order
GS = sort(absG, 2,'descend');
rowSums = cumsum(GS(:, 2:end), 2);
babel = max(rowSums);


function value = year_is_leap_gregorian ( y )

%*****************************************************************************80
%
%% YEAR_IS_LEAP_GREGORIAN returns TRUE if the Gregorian year was a leap year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, logical VALUE, TRUE if the year was a leap year,
%    FALSE otherwise.
%
  if ( y == 0 )
    value = 0;
    return
  end
%
%  BC years have to have 1 added to them to make a proper leap year evaluation.
%
  y2 = y_common_to_astronomical ( y );

  if ( mod ( y2, 400 ) == 0 )
    value = 1;
  elseif ( mod ( y2, 100 ) == 0 )
    value = 0;
  elseif ( mod ( y2, 4 ) == 0 )
    value = 1;
  else
    value = 0;
  end

  return
end


function plot_Bell_Contraction()

offset = 0.125;
tP = 0.25;
total_time = 3*tP + 2*offset;
dt = tP/125;

%Coefficients for Polynomial Phase-Interpolation
a = 2.739726027397260;  % y1(t) = at^2
b = 2.739726027397260;  % y3(t) = -b(t-1)^2+1
c = -2.029426686960933; % y2(t) = ct^3 + dt^2 + gt + h
d = 3.044140030441400;
g = -0.015220700152207;
h = 0.000253678335870;

t1 = 0.05*tP;
t2 = 0.95*tP;
tt1 = 0:dt:tP;
for i=1:length(tt1)
    t = tt1(i);
    if (t <t1)
        r1(i) = a*(t/tP)^2;                             %a*pow((t/tP),2);
    elseif ( (t >=t1 ) && ( t<t2) )
        r1(i) = c*(t/tP)^3 + d*(t/tP)^2 + g*(t/tP) + h; %c*pow((t/tP),3) + d*pow((t/tP),2) + g*(t/tP) + h;
    elseif (t>=t2)
        r1(i) = -b*( (t/tP) - 1.0 )^2 + 1;                  %-b*pow( ((t/tP) - 1),2) + 1;
    end
end

tt2 = tP:dt:tP+2*offset;
tt4 = (2*tP+2*offset):dt:(3*tP+2*offset);
r2 = ones(1,length(tt2));
r4 = zeros(1,length(tt4));
%%RHS Expansion Phase
tt3 = (tP+2*offset):dt:(2*tP+2*offset);
r3 = r1 - 0.5;
r3 = -r3 + 0.5;

%plot(tt1,r1,'r.','MarkerSize',6); hold on;
%plot(tt2,r2,'r.','MarkerSize',6); hold on;
%plot(tt3,r3,'r.','MarkerSize',6); hold on;
%plot(tt4,r4,'r.','MarkerSize',6); hold on;

%%%%%%%%%%%%%%%%%%%%%%%%%
%
% LHS
%
%%%%%%%%%%%%%%%%%%%%%%%%%

tprev = offset;
t1 = 0.05*tP+offset;
t2 = 0.95*tP+offset;
frac= 0.9;
ttt1 = offset:dt:(tP+offset);
for i=1:length(ttt1)
    t = ttt1(i);
    if (t <t1)
        Lgeo(i) =  a*( (t-tprev) /tP)^2;                             %a*pow((t/tP),2);
    elseif ( (t >=t1 ) && ( t<t2) )
        Lgeo(i) = c*( (t-tprev)/tP)^3 + d*( (t-tprev)/tP)^2 + g*( (t-tprev)/tP) + h; %c*pow((t/tP),3) + d*pow((t/tP),2) + g*(t/tP) + h;
    elseif (t>=t2)
        Lgeo(i) = -b*( ((t-tprev)/tP) - 1.0 )^2 + 1;                  %-b*pow( ((t/tP) - 1),2) + 1;
    end
end

tt0 = 0:dt:offset;
L0 = zeros(1,length(tt0));

%Contraction
L1 = frac*Lgeo;

%Phase Two -> Expansion
ttt2 = (tP+offset):dt:(2*tP+offset);
L2 = Lgeo - 0.5;
L2 = -1.2*L2 + 0.4 - (1-frac);

%Phase Three -> Contraction to equilibrium
ttt3 = (2*tP+offset):dt:(3*tP+offset);
L3 = Lgeo - 1.0;
L3 = (0.3)*L3;

YAXISy = -0.75:dt:1.6;
YAXISx = zeros(1,length(YAXISy));

XAXISx = -0.25:dt:1.5;
XAXISy = zeros(1,length(XAXISx));

plot(tt1,r1,'r.','MarkerSize',8); hold on;
plot(tt0,L0,'.','MarkerSize',8); hold on;

plot(XAXISx,XAXISy,'k-','LineWidth',5);
plot(YAXISx,YAXISy,'k-','LineWidth',5);

plot(tt1,r1,'r.','MarkerSize',8); hold on;
plot(tt0,L0,'.','MarkerSize',8); hold on;

plot(tt2,r2,'r.','MarkerSize',8); hold on;
plot(tt3,r3,'r.','MarkerSize',8); hold on;
plot(tt4,r4,'r.','MarkerSize',8); hold on;

plot(ttt1,L1,'.','MarkerSize',8); hold on;
plot(ttt2,L2,'.','MarkerSize',8); hold on;
plot(ttt3,L3,'.','MarkerSize',8); hold on;
axis([-offset 3*tP+3*offset -0.75 1.5]);

vL1y = -0.75:0.25:0;
vL1x = offset*ones(1,length(vL1y));

vL2y = -0.75:0.25:0.9;
vL2x = (tP+offset)*ones(1,length(vL2y));

vL3y = -0.75:0.25:-0.21;
vL3x = (2*tP+offset)*ones(1,length(vL3y));

vL4y = -0.75:0.25:0.0;
vL4x = (3*tP+offset)*ones(1,length(vL4y));

vL5y = -0.75:0.25:0.0;
vL5x = (3*tP+2*offset)*ones(1,length(vL4y));

vR1y = 1:0.2:1.6;
vR1x = tP*ones(1,length(vR1y));

vR2y = 1:0.2:1.6;
vR2x = (tP+2*offset)*ones(1,length(vR2y));

vR3y = 0:0.2:1.6;
vR3x = (2*tP+2*offset)*ones(1,length(vR3y));

plot(vR1x,vR1y,'k--','LineWidth',1); hold on;
plot(vR2x,vR2y,'k--','LineWidth',1); hold on;
plot(vR3x,vR3y,'k--','LineWidth',1); hold on;

plot(vL1x,vL1y,'k--','LineWidth',1); hold on;
plot(vL2x,vL2y,'k--','LineWidth',1); hold on;
plot(vL3x,vL3y,'k--','LineWidth',1); hold on;
plot(vL4x,vL4y,'k--','LineWidth',1); hold on;
plot(vL5x,vL5y,'k--','LineWidth',1); hold on;
axis([-2*offset 3*tP+4*offset -1.0 1.75]);

legend('RIGHT','LEFT');


function [E] = gdf(V1, V2)
%
% GDF   Ground distance between two vectors
%    [E] = GDF(F1, F2) is the ground distance between two feature vectors.
%
%    Example:
%    -------
%        v1 = [100, 40, 22];
%        v2 = [50, 100, 80];
%        ...
%        [e] = gdf(v1, v2);
%        ...
%
%    This file and its content belong to Ulas Yilmaz.
%    You are welcome to use it for non-commercial purposes, such as
%    student projects, research and personal interest. However,
%    you are not allowed to use it for commercial purposes, without
%    an explicit written and signed license agreement with Ulas Yilmaz.
%    Berlin University of Technology, Germany 2006.
%    http://www.cv.tu-berlin.de/~ulas/RaRF
%

E = norm(V1 - V2, 2);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  In this script, we perform phase transition analysis
%  of Orthogonal matching pursuit.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
rng('default');
% Create the directory for storing results
[status_code,message,message_id] = mkdir('bin');
target_file_path = 'bin/mmv_phase_transition_noiseless_s_2.mat';
N = 256;
S = 2;
pta = spx.pursuit.PhaseTransitionAnalysis(N);
% pta.NumTrials = 100;
dict_model = @(M, N) spx.dict.simple.gaussian_dict(M, N);
data_model = @(N, K) spx.data.synthetic.SparseSignalGenerator(N, K, S).gaussian;
recovery_solver = @(Phi, K, y) spx.pursuit.joint.CoSaMP(Phi, K).solve(y).Z;
pta.run(dict_model, data_model, recovery_solver);
pta.save_results(target_file_path);


% theta = choose_orientations2(x,y,phi,wtheta,wphi)
%
% we will set the orientation to theta_t = phi_t + s_t * pi
% we want to choose s_t to minimize
% \sum_t cost(s_t|s_{t-1})
% cost(s_t|s_{t-1}) = [wtheta_t*d(\theta_t,\theta_{t-1}) +
%                      wphi(||v_t||^2)*d(\theta_t,angle(v_t))]
%
% we will find the most likely states s_t using the recursion
% cost_t(s_t) = min_{s_{t-1}} { cost_{t-1}(s_{t-1}) + cost(s_t|s_{t-1})
%
% Inputs:
% x: N x 1 vector where x(t) is the x-coordinate of the center of the fly
% at time t
% y: N x 1 vector where y(t) is the y-coordinate of the center of the fly
% at time t
% phi: N x 1 vector where phi(t) is the orientation of the fly at time t
% wtheta: N x 1 vector where wtheta(t) is the weight of the change in
% orientation term at time t
% wphi: N x 1 vector where wphi(t) is the weight of the velocity
% direction term at time t
%
function theta = choose_orientations2(x,y,theta,weight_theta,weight_phi)

inputsz = size(x);
x = x(:);
y = y(:);
theta = theta(:);
weight_phi = weight_phi(:);
weight_theta = weight_theta(:);

% number of frames
N = length(x);

% allocate space for storing the optimal path
stateprev = zeros(N-1,2);

% allocate space for computing costs
tmpcost = zeros(2,1);
costprevnew = zeros(2,1);

% initialize first frame
costprev = zeros(2,1);

% compute velocity
vx = [0;diff(x)];
vy = [0;diff(y)];

% compute angle of velocity
velocityangle = atan2(vy,vx);

% compute iteratively
for t = 2:N,

  % compute for both possible states
  for scurr = 1:2,

    % try both previous states
    thetacurr = theta(t) + (scurr-1)*pi;

    for sprev = 1:2,

      thetaprev = theta(t-1) + (sprev-1)*pi;
      costcurr = weight_theta(t)*angledist(thetaprev,thetacurr) + ...
        weight_phi(t)*angledist(thetacurr,velocityangle(t));
      tmpcost(sprev) = costprev(sprev) + costcurr;

    end

    % choose the minimum
    sprev = argmin(tmpcost);

    % set pointer for path
    stateprev(t-1,scurr) = sprev;

    % set cost
    costprevnew(scurr) = tmpcost(sprev);

  end

  % copy over
  costprev(:) = costprevnew(:);

end

% choose the best last state
scurr = argmin(costprev);

if scurr == 2,
  theta(end) = modrange(theta(end)+pi,-pi,pi);
end

% choose the best states
for t = N-1:-1:1,
  scurr = stateprev(t,scurr);
  if scurr == 2,
    theta(t) = modrange(theta(t)+pi,-pi,pi);
  end
end

theta = reshape(theta,inputsz);


% WAVELET_FACTORY_3D Build roto-translation wavelet operators
%
% Usage
%   [Wop, filters, filters_rot] = WAVELET_FACTORY_3D(size_in, filt_opt, filt_rot_opt, scat_opt)
%
% Input
%    size_in (2x1 int): the size of the image to be transformed
%    filt_opt (struct): the filter options, same as for MORLET_FILTER_BANK_2D
%    filt_rot_opt (struct): the filter options for the
%		filters along angular parameter, same as for MORLET_FILTER_BANK_1D
%	 scat_opt (struct): the scattering and wavelet options, same as
%		WAVELET_LAYER_3D
%
% Output
%    Wop: A cell array of wavelet transforms needed for the scattering trans-
%       form.
%    filters: A cell array of the filters used in defining the wavelets.
%
% Description
%   This function builds the wavelet operators used to compute the
%   roto-translation scattering. The first operator is a 2d wavelet
%   transform obtained with WAVELET_LAYER_2D. The second operator and third
%   operators are roto-translation wavelet transform obtained with
%   WAVELET_LAYER_3D.
%
% See also
%   SCAT, WAVELET_2D, WAVELET_LAYER_2D, WAVELET_3D, WAVELET_3D_LAYER

function [ Wop, filters, filters_rot ] = ...
		wavelet_factory_3d(size_in, filt_opt, filt_rot_opt, scat_opt)

    % initialize the non-provided options with empty structure
    if (nargin < 4)
        scat_opt = struct();
    end
    if (nargin < 3)
        filt_rot_opt = struct();
    end
    if (nargin < 2)
        filt_opt = struct();
    end

	% filters along spatial variable
	filters = morlet_filter_bank_2d(size_in, filt_opt);

	% filters along angular variable
	sz = filters.meta.L * 2; % L orientations between 0 and pi
	% periodic convolutions along angles
	filt_rot_opt.boundary = 'per';
	filt_rot_opt.filter_format = 'fourier_multires';
	filt_rot_opt.J = 3;
	filt_rot_opt.P = 0;
	filters_rot = morlet_filter_bank_1d(sz, filt_rot_opt);

	% number of layer
	scat_opt = fill_struct(scat_opt, 'M', 2);
    wav_opt = rmfield(scat_opt, 'M');

	% first wavelet transform is an usual wavelet transform
	Wop{1} = @(x)(wavelet_layer_2d(x, filters, wav_opt));

	% other wavelet transform are roto-translation wavelet
	for m = 2:scat_opt.M+1
		Wop{m} = @(x)(wavelet_layer_3d(x, filters, filters_rot, wav_opt));
	end
end


function OUT = Date2Num(DATA)
% =======================================================================
% Substitute cell dates 1989Q4 with numeric dates 1989.75. Only for
% quarterly data.
% =======================================================================
% OUT = Date2Num(DATA)
% -----------------------------------------------------------------------
% INPUTS
%	- DATA: a (T x 1) vector with dates in cell format
% -----------------------------------------------------------------------
% OUTPUT
%	- OUT:  a (T x 1) vector with dates in numeric format
% =========================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogiocesabianchi@gmail.com
%-------------------------------------------------------------------------

OUT = nan(size(DATA,1),size(DATA,2));
for i=1:size(DATA,1)
    for j=1:size(DATA,2)
        aux = DATA{i,j};
        year = str2double(aux(1:4));
        quarter = str2double(aux(6));
        OUT(i,j) = year + quarter/4 -0.25;
    end
end


function [x,obj,err,iter] = fusedl1(A,b,lambda,opts)

% Solve the fused Lasso (Fused L1) minimization problem by ADMM
%
% min_x ||x||_1 + lambda*\sum_{i=2}^p |x_i-x_{i-1}|,
%   s.t. Ax=b
%
% ---------------------------------------------
% Input:
%       A       -    d*n matrix
%       b       -    d*1 vector
%       lambda  -    >=0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       x       -    n*1 vector
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 20/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n] = size(A);
x = zeros(n,1);

z = x;
Y1 = zeros(d,1);
Y2 = x;

Atb = A'*b;
I = eye(n);
invAtAI = (A'*A+I)\I;

% parameters for "flsa" (from SLEP package)
tol2 = 1e-10;      % the duality gap for termination
max_step = 50;     % the maximal number of iterations
x0 = zeros(n-1,1); % the starting point

iter = 0;
for iter = 1 : max_iter
    xk = x;
    zk = z;
    % update x.
    % flsa solves min_x 1/2||x-v||_2^2+lambda1*||x||_1+lambda2*\sum_{i=2}^p |x_i-x_{i-1}|
    x = flsa(z-Y2/mu,x0,1/mu,lambda/mu,n,max_step,tol2,1,6);
    % update z
    z = invAtAI*(-A'*Y1/mu+Atb+Y2/mu+x);
    dY1 = A*z-b;
    dY2 = x-z;
    chgx = max(abs(xk-x));
    chgz = max(abs(zk-z));
    chg = max([chgx chgz max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = comp_fusedl1(x,1,lambda);
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = comp_fusedl1(x,1,lambda);
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);

function f = comp_fusedl1(x,lambda1,lambda2)
% compute f = lambda1*||x||_1 + lambda2*\sum_{i=2}^p |x_i-x_{i-1}|.
% x - p*1 vector
f = 0;
p = length(x);
for i = 2 : p
   f = f+abs(x(i)-x(i-1));
end
f = lambda1*norm(x,1)+lambda2*f;


function [idp,IDP_p,IDP_p0] = p2idp(p,p0)

% P2IDP Point to inverse-depth point conversion.
%   P2IDP(P,P0) is the inverse depth point anchored at P0 corresponding to
%   the Euclidean point P.
%
%   An inverse-depth point (idp) is a 6-vector:
%
%           idp = [x0 y0 z0 el az rho]',
%
%   where:
%       x0, z0, y0: anchor: the 3D point P0 where where distance is referred to.
%       el, az: azimuth and elevation of the ray through P that starts at P0.
%       rho: inverse of the distance from point P to P0.
%
%   [idp,IDP_p,IDP_p0] returns the Jacobians wrt P and P0.
%
%   See also IDP2P.

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

if nargout == 1
    m   = p-p0;
    rho = 1/sqrt(dot(m,m));
    py  = vec2py(m);

    idp = [p0;py;rho];
else

    m           = p-p0;
    M_p         = 1;
    M_p0        = -1;
    [mn2,MN2_m] = dotJ(m,m);
    rho         = 1/mn2;
    RHO_mn2     = -1/mn2^2;
    [py,PY_m]   = vec2py(m);

    idp = [p0;py;rho];

    RHO_m = RHO_mn2*MN2_m;

    IDP_p = [zeros(3);PY_m*M_p;RHO_m*M_p];
    IDP_p0 = [eye(3);PY_m*M_p0;RHO_m*M_p0];

end

return

%% Jac

syms x y z x0 y0 z0 real
p = [x;y;z];
p0 = [x0;y0;z0];

idp = p2idp(p,p0);

simplify(IDP_p  - jacobian(idp,p))
simplify(IDP_p0 - jacobian(idp,p0))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [plane, inlier_count, inliers] = ransac_depth_plane(x, depth, alpha,...
    iter, p)
%RANSAC_DEPTH_PLANE  Fit a 3D depth plane to a set of input points using RANSAC.
%The distance of a point from the plane is defined as the vertical offset.
%
%   INPUTS:
%
%   -|x|: |P|-by-2 matrix with image-plane coordinates of input points.
%
%   -|depth|: |P|-by-1 vector with depth values for input points.
%
%   -|alpha|: positive constant regulating the inlier threshold used in RANSAC.
%
%   -|iter|: maximum number of RANSAC iterations.
%
%   -|p|: target probability for having obtained a pure inlier set, used to cut
%   down on number of RANSAC iterations.
%
%   OUTPUTS:
%
%   -|plane|: 3-by-1 vector holding the parameters of the fitted plane, so that
%   for each point [x, y, d] it holds d = [x, y, 1] * plane.
%
%   -|inlier_count|: cardinality of the final inlier set.
%
%   -|inliers|: |inlier_count|-by-1 vector with indices of the members of the
%   final inlier set.

% Determine inlier threshold |theta| based on the depth values. Goal: invariance
% to range of depth values.
theta = alpha * median(depth);

% Number of plane parameters.
N = 3;

% Total number of points.
P = length(depth);

% Represent pixel coordinates homogeneously.
x_h = [x, ones(P, 1)];

% Initialize variables to save the best fit across iterations.
inliers = [];
inlier_count = 0;
inlier_ratio = 0;

% Main RANSAC loop.
for i = 1:iter
    colinear = true;
    while colinear
        % Select a sample at random.
        permut = randperm(P);
        sample = permut(1:N);
        x_h_sample = x_h(sample, :);

        % Proceed to plane fitting only if the points of the sample are not
        % colinear, i.e. square matrix with homogeneous coordinates of
        % points in the sample is of full rank.
        if rank(x_h_sample) == N
            colinear = false;
        end
    end

    % Fit a plane to the current sample.
    plane_tmp = fit_depth_plane(x_h_sample, depth(sample));

    % Identify inliers for current fit.

    % Vertical distance.
    inliers_tmp = abs(x_h * plane_tmp - depth) <= theta;
    % Orthogonal distance.
    % plane_tmp_normal = [plane_tmp(1:N-1); 1];
    % inliers_tmp = abs([points(known_pixels_finite, :), depth_known_finite] *...
    %     plane_tmp_normal - plane_tmp(N)) / norm(plane_tmp_normal) <= theta;
    inlier_tmp_count = nnz(inliers_tmp);

    % Update best fit.
    if inlier_tmp_count > inlier_count
        inlier_count = inlier_tmp_count;
        inlier_ratio = inlier_count / P;
        inliers = find(inliers_tmp);
    end

    % An adaptive version of RANSAC is implemented. If a pure inlier sample
    % has been already examined with probability greater that |p|, avoid
    % performing more iterations.
    p_bound = 1 - (1 - inlier_ratio ^ N) ^ i;
    if p <= p_bound
        break;
    end
end

% Perform a final least squares fit to compute the best depth plane with respect
% to all points in the maximum inlier set that has been found with RANSAC.
plane = fit_depth_plane(x_h(inliers, :), depth(inliers));

end


% Local Regression and Likelihood, Figure 6.2.
% Author: Catherine Loader
%
% Local smooth of CO2 dataset, main trend.

load co2;
fit = locfit(year+month/12,co2,'alpha',0.5,'deg',1);
figure('Name','fig6_2: CO2 Dataset: Local smoothing, main trend' );
lfplot(fit);
xlabel('Date');
ylabel('CO2')


function y = atan(x)
%ATAN         Implements  atan(x)  for intervals
%
%   y = atan(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,atan(full(sx)),m,n);
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if x.complex
    y = log( 2./(1-j*x) - 1) / (2*j);
    if rndold
      setround(rndold)
    end
    return
  end

  y = x;

  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );

  Y = atan_pos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ( xinf<0 );
  len1 = sum(IndexInfNeg);
  IndexSupPos = ( xsup>0 );

  Y = atan_pos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : end );

  setround(rndold)


function [M]=gen_MassMatrix(q,Pcii,Icii,mcii)
%% This function is used to calculate the mass matrix of the KUKA iiwa 7 R 800 manipulator

% Arreguments:
%--------------------
% q: is 1x7 vector, joint nagles vector of the manipulator
% Pcii: is 3X7 matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii: is (3x3x7) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii: is (1x7) vector, each element of which specifies a mass of one of
% the links

% Return value:
%--------------------
% M: 7x7 matrix, the mass matrix of the KUKA iiwa 7 R 800.

% Copyright: Mohammad SAFEEA

T=kukaGetKenimaticModelAccelerated(q);
[M]=GetInertiaMatrix5(T,Pcii,Icii,mcii);


function combo_test11 ( )

%*****************************************************************************80
%
%% COMBO_TEST11 tests KNAPSACK_REORDER and KNAPSACK_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  mass_limit = 26.0;
  p = [ 24.0, 13.0, 23.0, 15.0, 16.0 ]';
  w = [ 12.0,  7.0, 11.0,  8.0,  9.0 ]';

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST11\n' );
  fprintf ( 1, '  KNAPSACK_REORDER reorders the knapsack data.\n' );
  fprintf ( 1, '  KNAPSACK_01 solves the 0/1 knapsack problem.\n' );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Profit, Mass, "Profit Density"\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %7.3f  %7.3f  %7.3f\n', i, p(i), w(i), p(i) / w(i) );
  end

  [ p, w ] = knapsack_reorder ( n, p, w );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  After reordering by Profit Density:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Profit, Mass, "Profit Density"\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %7.3f  %7.3f  %7.3f\n', i, p(i), w(i), p(i) / w(i) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Total mass restriction is %f\n', mass_limit );

  [ x, mass, profit ] = knapsack_01 ( n, mass_limit, p, w );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Density, Choice, Profit, Mass\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '%6d  %7.3f  %7.3f  %7.3f  %7.3f\n', ...
      i, p(i) / w(i), x(i),  x(i) * p(i), x(i) * w(i) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Total:                  %7.3f  %7.3f\n', profit, mass );

  return
end


function jac = p36_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P36_JAC evaluates the jacobian for problem p36.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  jac(1,1) =  0.0;
  jac(1,2) =  1.0;

  jac(2,1) = 1.0 - 3.0 * y(1).^2;
  jac(2,2) = 0.0;

  return
end


function [xcon,ycon] = consolidator13(x,y,aggregation_mode,tol,tolerance_style)
% consolidator: consolidate "replicates" in x, also aggregate corresponding y
% usage: [xcon,ycon] = consolidator13(x,y,aggregation_mode,tol,tolerance_style)
%
% arguments: (input)
%  x - rectangular array of data to be consolidated. If multiple (p)
%      columns, then each row of x is interpreted as a single point in a
%      p-dimensional space. (x may be character, in which case xcon will
%      be returned as a character array.)
%
%      x CANNOT be complex. If you do have complex data, split it into
%      real and imaginary components as columns of an array.
%
%      If x and y are both ROW vctors, they will be transposed and
%      treated as single columns.
%
%  y - outputs to be aggregated. If y is not supplied (is left empty)
%      then consolidator is similar to unique(x,'rows'), but with a
%      tolerance on which points are distinct. (y may be complex)
%
%      y MUST have the same number of rows as x.
%
%  aggregation_mode - (OPTIONAL) character flag, denotes the method
%      to be used in the aggregation of elements of y. If y was
%      left empty, then aggregation_mode is ignored. Contractions
%      are allowed and capitalization is ignored.
%
%      DEFAULT: 'mean'
%
%      'mean'  --> compute the average y for each consolidated x
%      'sum'   --> compute the sum of y for each consolidated x
%      'min'   --> compute the minimum y for each consolidated x
%      'max'   --> compute the maximum y for each consolidated x
%      'std'   --> compute the standard deviation of y for each x
%      'var'   --> compute the variance of y for each x
%      'prod'  --> compute the product of y for each consolidated x
%      'geom'  --> compute the geometric mean at each consolidated x
%      'harm'  --> compute the harmonic mean at each consolidated x
%      'median' --> compute the median at each consolidated x
%      'count' --> compute the # of consolidated elements 7at each x
%
%      Note: 'count' only returns 1 column, regardless
%      of the size of y. All other operations operate on
%      each column of y independently.
%
%  tol - (OPTIONAL) tolerance to identify replicate elements of x. If
%      x has multiple columns, then the same tolerance is applied to
%      all columns of x.
%
%      DEFAULT (if tolerance_style is 'absolute'): 0
%      DEFAULT (if tolerance_style is 'relative'): 1.e-12
%
%      Note: Tolerances are a complicated thing, especially in
%      higher dimensions and when trying to do it effficiently for
%      high numbers of data points. A complete interpoint distance
%      matrix would be the proper way to do t, but for many thousands
%      of points it would use too much memory. One also has the problem
%      of transitivity. If A is within tol of B, and B is within tol
%      of C, then is A near enough to C? If not, then how is the
%      consolidation to be done? So please view these tolerances
%      with some tolerance of your own.
%
%      Note: relative tolerances near 1 are not meaningful.
%
% tolerance_style - (OPTIONAL) character flag, denotes whether
%      the tolerance criterion is absolute over all columns or
%      a relative tolerance within a single column of x.
%
%      Note: DO NOT use a relative tolerance of 0, nor greater
%      than 1. Even a relative tolerance near 1 is not meaningful.
%
%      DEFAULT: 'absolute'
%
%      'relative' --> Tol is a relative tolerance between the
%         minimum and maximum of each column of x.
%
%      'absolute' --> look for differences of size tol in any
%         column of x.
%
%
% arguments: (output)
%  xcon - consolidated x. Replicates wthin the tolerance are removed.
%      if no y was specified, then consolidation is still done on x.
%
%  ycon - aggregated value as specified by the aggregation_mode.
%
%
% Example usages:
%
% Group means:
%  x = round(rand(1000,1)*5);
%  y = x+randn(size(x));
%  [xg,yg] = consolidator(x,y,'mean');
%  [xg,yg]
%  ans =
%         0    0.1668
%    1.0000    0.9678
%    2.0000    2.0829
%    3.0000    2.9688
%    4.0000    4.0491
%    5.0000    4.8852
%
% Group counts on x
%  x = round(randn(100000,1));
%  [xg,c] = consolidator(x,[],'count');
%  [xg,c]
%  ans =
%         -4          26
%         -3         633
%         -2        5926
%         -1       24391
%          0       38306
%          1       24156
%          2        5982
%          3         559
%          4          21
% Unique(x,'rows'), but with a tolerance
%  x = rand(100,2);
%  xc = consolidator(x,[],[],.05);
%  size(xc)
%  ans =
%      62     2
%
% See also: unique
%
% Author: John D'Errico
% e-mail address: woodchips@rochester.rr.com
% Release: 3
% Release date: 5/2/06

% is it a character array?
if ischar(x)
  charflag = 1;
  x=double(x);
else
  charflag = 0;
end

% check for/supply defaults
if (nargin<5) | isempty(tolerance_style)
  tolerance_style = 'absolute';
else
  tolerance_style = lower(tolerance_style);
end

if (nargin<4) | isempty(tol)
  switch tolerance_style
    case 'absolute'
      tol = 0;
    case 'relative'
      tol = 1.e-12;
    otherwise
      error 'tolerance_style must be either ''absolute'' or ''relative'''
  end
end
if (tol<0) & strcmp(tolerance_style,'absolute')
  error 'Absolute tolerance must be non-negative.'
elseif (tol<=0) & strcmp(tolerance_style,'relative')
  error 'Absolute tolerance must be non-negative.'
end
tol = tol*(1+10*eps);

if (nargin<3) | isempty(aggregation_mode)
  aggregation_mode = 'mean';
else
  aggregation_mode = lower(aggregation_mode);
end
valid_agmode = {'mean' 'sum' 'min' 'max' 'std' ...
  'var' 'prod' 'geom' 'harm' 'count' 'median'};
k = strmatch(aggregation_mode,valid_agmode);
if isempty(k)
  error 'Invalid aggregation_mode'
else
  aggregation_mode = valid_agmode{k};
end

% was y supplied, or empty?
[n,p] = size(x);
if (nargin<2) | isempty(y)
  y = zeros(n,0);
end
% check for mismatch between x and y
[junk,q] = size(y);
if n~=junk
  error 'y must have the same number of rows as x.'
end

% are both x and y row vectors?
if (n == 1)
  x=x';
  n = length(x);
  p = 1;

  if ~isempty(y)
    y=y';
  else
    y=zeros(n,0);
  end

  q = size(y,2);
end

if isempty(y)
  aggregation_mode = 'count';
end

% consolidate elements of x.
% first shift, scale, and then round up.
switch tolerance_style
  case 'absolute'
    if tol>0
      xhat = x - repmat(min(x,[],1),n,1)+tol*eps;
      xhat = ceil(xhat/tol);
    else
      xhat = x;
    end
  case 'relative'
    xhat = x - repmat(min(x,[],1),n,1)+tol*eps;
    xhat = ceil(xhat*diag(1 ./ (tol*max(xhat,[],1))));
end
[xhat,tags] = sortrows(xhat);
x=x(tags,:);
y=y(tags,:);

% count the replicates
iu = [1;any(diff(xhat),2)];
eb = cumsum(iu);
% count is the vector of counts for the consolidated
% x values
% Consolidator code was:  count=accumarray(eb,1).';
count = full(sparse(1,eb,1,1,eb(end)));

% ec is the expanded counts, i.e., counts for the
% unconsolidated x
ec = count(eb);

% special case for aggregation_mode of 'count',
% but we still need to aggregate (using the mean) on x
if strcmp(aggregation_mode,'count')
  ycon = count.';
  q = 0;
else
  ycon = zeros(length(count),q);
end

% loop over the different replicate counts, aggregate x and y
ucount = unique(count);
xcon = repmat(NaN,[length(count),p]);
for k=ucount
  if k==1
    xcon(count==1,:) = x(ec==1,:);
  else
    v=permute(x(ec==k,:),[3 2 1]);
    v=reshape(v,[p,k,prod(size(v))/p/k]);
    v=permute(v,[2 1 3]);
    xcon(count==k,:)=reshape(mean(v),p,prod(size(v))/p/size(v,1)).';
  end

  if q>0
    % aggregate y as specified
    if k==1
      switch aggregation_mode
        case {'std' 'var'}
          ycon(count==1,:) = 0;
        otherwise
          ycon(count==1,:) = y(ec==1,:);
      end
    else
      v=permute(y(ec==k,:),[3 2 1]);
      v=reshape(v,[q,k,prod(size(v))/q/k]);
      v=permute(v,[2 1 3]);

      nv = prod(size(v));
      n1 = size(v,1);

      switch aggregation_mode
        case 'mean'
          ycon(count==k,:)=reshape(mean(v),q,nv/n1/q).';
        case 'sum'
          ycon(count==k,:)=reshape(sum(v),q,nv/n1/q).';
        case 'min'
          ycon(count==k,:)=reshape(min(v),q,nv/n1/q).';
        case 'max'
          ycon(count==k,:)=reshape(max(v),q,nv/n1/q).';
        case 'std'
          ycon(count==k,:)=reshape(std(v),q,nv/n1/q).';
        case 'var'
          ycon(count==k,:)=reshape(var(v),q,nv/n1/q).';
        case 'median'
          ycon(count==k,:)=reshape(median(v),q,nv/n1/q).';
        case 'prod'
          ycon(count==k,:)=reshape(prod(v),q,nv/n1/q).';
        case 'harm'
          ycon(count==k,:)=reshape(1./mean(1./v),q,nv/n1/q).';
        case 'geom'
          ycon(count==k,:)=reshape(exp(mean(log(v))),q,nv/n1/q).';
%       case 'count'
%         % we already did count above
      end
    end
  end
end

% was it originally a character array?
if charflag
  xcon=char(xcon);
end


function Sest = cosamp_cgls(Phi,u,K,tol,maxiterations, err_tol)

% Cosamp algorithm
%   Input
%       K : sparsity of Sest
%       Phi : measurement matrix
%       u: measured vector
%       tol : tolerance for approximation between successive solutions.
%   Output
%       Sest: Solution found by the algorithm
%
% Algorithm as described in "CoSaMP: Iterative signal recovery from
% incomplete and inaccurate samples" by Deanna Needell and Joel Tropp.
%

% This implementation was written by David Mary,
% but modified 20110707 by Bob L. Sturm to make it much clearer,
% and corrected multiple times again and again.
% To begin with, see: http://media.aau.dk/null_space_pursuits/2011/07/ ...
% algorithm-power-hour-compressive-sampling-matching-pursuit-cosamp.html
%
% This script/program is released under the Commons Creative Licence
% with Attribution Non-commercial Share Alike (by-nc-sa)
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% Short Disclaimer: this script is for educational purpose only.
% Longer Disclaimer see  http://igorcarron.googlepages.com/disclaimer

%%Code modified by Praneeth 14March2017 to include case when there is no
%%signal to estimate;replaced pinv() by A\b syntax for speed.
% Initialization
Sest = zeros(size(Phi,2),1);
v = u;
t = 1;
numericalprecision = 1e-12;
T = [];
while ((t <= maxiterations) && (norm(v)/norm(u) > tol) ...
        && (norm(Phi'*v) > err_tol))

  y = abs(Phi'*v);
  [vals,z] = sort(y,'descend');
  Omega = find(y >= vals(2*K) & y > numericalprecision);

  T = union(Omega,T);
  x = v(T);
  %b = pinv(Phi(:,T))*u;
%   b = Phi(:,T) \ u;     %% edit PKN -- 3/3/17
  b = cgls(Phi(:, T), u, 0, err_tol, 3, 0, x);
  [vals,z] = sort(abs(b),'descend');
  Kgoodindices = (abs(b) >= vals(K) & abs(b) > numericalprecision);
  T = T(Kgoodindices);
  Sest = zeros(size(Phi,2),1);
  b = b(Kgoodindices);
  Sest(T) = b;
  v = u - Phi(:,T)*b;
  t = t+1;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% s10.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function s10(x)
% Shekel10 function
function f = s10(x)
a = [4, 1, 8, 6, 3, 2, 5, 8, 6, 7;
     4, 1, 8, 6, 7, 9, 5, 1, 2, 3.6;
     4, 1, 8, 6, 3, 2, 3, 8, 6, 7;
     4, 1, 8, 6, 7, 9, 3, 1, 2, 3.6];
c = [ 0.1, 0.2, 0.2, 0.4, 0.4, 0.6, 0.3, 0.7, 0.5, 0.5];
if size(x,1) == 1
 x = x';
end
for i=1:10
 b = (x - a(:,i)).^2;
 d(i) = sum(b);
end
f = -sum((c+d).^(-1));


function [PC, powers] = powercentersPD(T, E, wts)
% function [PC, powers] = powercentersPD(T, E, wts)
%
% T: triangulation
% E: set of points
% wts: weights of points in E
%
% The output array PC contains the power centers of the triangles in T.
% That is, row i of PC contains the point that is equidistant from each
% vertex of the triangle specified by row i of T, with respect to the power
% of each vertex. The output array powers contains the power of each power
% center.

[m, n] = size(T);
PC = zeros(m,n-1);
powers = zeros(m,1);

for i=1:m
    tr = E(T(i,:),:);
    wt = wts(T(i,:));
    p = tr(1,:);
    Rp = repmat(p, n-1, 1);
    Pts = tr(2:n,:);
    Ac = 2*(Pts - Rp);

    Rw1 = repmat(wt(1), n-1, 1);
    Wts = wt(2:n);
    Sp1 = repmat(sum(p.^2), n-1, 1);
    SPts = sum(Pts.^2, 2);
    Bc = Rw1 - Wts - Sp1 + SPts;

    pc = Ac \ Bc;

    PC(i,:) = pc;
    powers(i,1) = norm(pc - p')^2 - wt(1);
end


function [x_nml_wave,UB] = normalize_UB( H_wave,y_wave,x,N,l,u)
%UNTITLED9 Summary of this function goes here
%   Detailed explanation goes here
x_cplx = x(1:N)+j*x(N+1:2*N);
x_temp = x_cplx./abs(x_cplx);
for ii = 1:N
    delta = u(ii)-l(ii);
    if delta <=pi
        x_nml(ii,1) = x_temp(ii);
    else
        if ((angle(x_temp(ii))<=u(ii))&&(angle(x_temp(ii))>=l(ii)))
            x_nml(ii,1) = x_temp(ii);
        else
            d_l = abs(x_cplx(ii)-exp(j*l(ii)));
            d_u = abs(x_cplx(ii)-exp(j*u(ii)));
           if d_l <= d_u
                x_nml(ii,1) = exp(j*l(ii));
           else
                x_nml(ii,1) = exp(j*u(ii));
           end
        end
    end
end
x_nml_wave = [real(x_nml);imag(x_nml)];
UB = objval_func(x_nml_wave,H_wave,y_wave);
end

github-repos/MATLAB/yuanhao-cui-Must-Reading-on-ISAC/Must-Reading-on-ISAC-34cd6615c52ebca121428a979e756c608b195040/Codes/Fan2018TSP/Codes for DFRC Waveform Design/Constant Modulus/normalize_UB.m


function truncated_normal_test22 ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_PDF_TEST tests TRUNCATED_NORMAL_B_PDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_B_PDF_TEST\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_B_PDF evaluates the Upper Truncated Normal PDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean = mu\n' );
  fprintf ( 1, '    standard deviation = sigma\n' );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval (-oo,b]\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                                  Stored         Computed\n' );
  fprintf ( 1, '       X        Mu         S         B             PDF             PDF\n' );
  fprintf ( 1, '\n');

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, b, x, pdf1 ] ...
      = truncated_normal_b_pdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    pdf2 = truncated_normal_b_pdf ( x, mu, sigma, b );

    fprintf( 1, '  %8.1f  %8.1f  %8.1f  %8.1f  %14g  %14g\n', x, mu, sigma, b, pdf1, pdf2 );

  end

  return
end


function [z varargout] = intersectm(varargin)

% INTERSECTM Find set intersection of several vectors
%
%   Z = INTERSECTM(A, B, C...) returns the values common to A, B, C... which can be numeric or character
%                              vectors or cell arrays of strings. Z is a vector sorted in ascending order.
%
%   Z = INTERSECTM(A, B, C...,'rows') when A, B, C... are matrices with the same number of columns returns
%                                     the rows common to the sets supplied.
%
%   [Z, IA, IB, IC...] = INTERSECTM(...) also returns column index vectors IA, IB, IC... such that Z = A(IA),
%                                        Z = B(IB), Z = C(IC)... (or Z = A(IA,:), Z = B(IB,:), Z = C(IC,:)...).
%
%
% EXAMPLES:
%   - Retrieve Z the set intersection of the elements in "a", "b" and "c" and the indexes IA, IB and IC
%
%       a =  1:5   ;
%       b = (3:9).';
%       c = -1:4   ;
%
%       [z, ia, ib, ic] = intersectm(a, b, c)
%       z =
%           3     4
%       ia =
%           3
%           4
%       ib =
%           1
%           2
%       ic =
%           5
%           6
%
% NOTE: IA, IB and IC have the same size as their corresponding inputs A, B and C while Z
%       has the size of A.
%
%   - Retrieve Z the set intersection of the rows "a", "b" and "c" and the indexes IA, IB and IC
%
%       a = [ 1:5 ;11:15];
%       b =  11:15;
%       c = [16:20;11:15];
%
%       [z, ia, ib, ic] = intersectm(a, b, c, 'rows')
%       z =
%           11    12    13    14    15
%       ia =
%           2
%       ib =
%           1
%       ic =
%           2
%
% Additional features:
% - FEX set functions page
%
% See also: INTERSECT, ISMEMBERM, SETDIFFM, SETXORM, UNIONM

% Author: Oleg Komarov (oleg.komarov@hotmail.it)
% Tested on R14SP3 (7.1) and on R2009b
% 23 jul 2010 - Created
% 30 jul 2010 - Added link to fex

% # inputs
error(nargchk(2,nargin,nargin))

% Check for 'rows' flag
IDXflag = strcmp('rows',varargin);
if any(IDXflag)
    % Extract flag
    flag     = varargin{ IDXflag};
    varargin = varargin(~IDXflag);
    % Check that all inputs have the same number of columns
    if logical(diff(cellfun('size',varargin,2)))
        error('intersectm:sameNumCols','A, B, C... must have the same number of columns.')
    end
else flag = [];
    % Check that all inputs are vectors
    if any(cellfun('prodofsize',varargin) ~= cellfun('length',varargin))
        error('intersectm:allVectors','A, B, C... must be all vectors or ''rows'' must be specified.')
    end
end

% Check same class
if ~cellfun('isclass',varargin(2:end), class(varargin{1}))
    error('intersectm:sameClass','A, B, C... should belong to the same class.')
end

% # of sets to intersect
numSets = numel(varargin);
if numSets == 1
    error('intersectm:noSet','Insufficient inputs: must supply at least two sets.')
end

% # outputs
error(nargoutchk(0,numSets+1,nargout))

% Find intersection set with ismemberm
if isempty(flag)
    z = varargin{1}(all(ismemberm(varargin{:}     ),2)  );
else
    z = varargin{1}(all(ismemberm(varargin{:},flag),2),:);
end

% Retrieve column index vectors
if nargout > 1
    varargout = cellfun(@(x) reshape(find(ismember(x,z,flag)),[],1),varargin(1:nargout-1),'un',0);
end
end