% LOWPASSFILTER - Constructs a low-pass butterworth filter.
%
% usage: f = lowpassfilter(sze, cutoff, n)
%
% where: sze    is a two element vector specifying the size of filter
%               to construct [rows cols].
%        cutoff is the cutoff frequency of the filter 0 - 0.5
%        n      is the order of the filter, the higher n is the sharper
%               the transition is. (n must be an integer >= 1).
%               Note that n is doubled so that it is always an even integer.
%
%                      1
%      f =    --------------------
%                              2n
%              1.0 + (w/cutoff)
%
% The frequency origin of the returned filter is at the corners.
%
% See also: HIGHPASSFILTER, HIGHBOOSTFILTER, BANDPASSFILTER
%

% Copyright (c) 1999 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% October 1999
% August  2005 - Fixed up frequency ranges for odd and even sized filters
%                (previous code was a bit approximate)

function f = lowpassfilter(sze, cutoff, n)

    if cutoff < 0 | cutoff > 0.5
	error('cutoff frequency must be between 0 and 0.5');
    end

    if rem(n,1) ~= 0 | n < 1
	error('n must be an integer >= 1');
    end

    if length(sze) == 1
	rows = sze; cols = sze;
    else
	rows = sze(1); cols = sze(2);
    end

    % Set up X and Y matrices with ranges normalised to +/- 0.5
    % The following code adjusts things appropriately for odd and even values
    % of rows and columns.
    if mod(cols,2)
	xrange = [-(cols-1)/2:(cols-1)/2]/(cols-1);
    else
	xrange = [-cols/2:(cols/2-1)]/cols;
    end

    if mod(rows,2)
	yrange = [-(rows-1)/2:(rows-1)/2]/(rows-1);
    else
	yrange = [-rows/2:(rows/2-1)]/rows;
    end

    [x,y] = meshgrid(xrange, yrange);
    radius = sqrt(x.^2 + y.^2);        % A matrix with every pixel = radius relative to centre.
    f = ifftshift( 1.0 ./ (1.0 + (radius ./ cutoff).^(2*n)) );   % The filter


function term  = sdlfmjMeanCompute(sdlfmKern, t , option)

% SDLFMJMEANCOMPUTE Jolt mean for the switching dynamical LFM model.
% Computes the terms $r_d$ and $q_d$ that appear in the mean function
% associated with the switching dynamical LFM model. If the mean function
% is mu(t), then
%
%     mu(t) = r_d(t)y_d(t_0) + q_d(t)\dot{y}_d(t_0),
%
% where $y_d(t_0)$ is the initial condition associated to the position and
% $\dot{y}_d(t_0)$ is the initial condition associated to the velocity.
%
% FORMAT
% DESC
% ARG sdlfmKern : switching dynamical LFM kernel structure with the
% parameters.
% ARG t : input times for which the mean is to be computed.
% RETURN term : the value of $r_d$.
%
% FORMAT
% DESC
% Computes the terms that appear in the mean function associated with the
% switching dynamical LFM model.
% ARG sdlfmKern : switching dynamical LFM kernel structure with the
% parameters.
% ARG t : input times for which the mean is to be computed.
% ARG option : indicates which term of the mean should be computed. Option
% 'Pos' computes the term $r_d$ and option 'Vel' computes $q_d$ that
% accompanies the initial condition of the velocity.
% RETURN term : the value of $r_d$ or $q_d$ depending of option.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 3
    option = 'Pos';
end

alpha = sdlfmKern.damper/(2*sdlfmKern.mass);
omega = sqrt(sdlfmKern.spring/sdlfmKern.mass-alpha^2);
freq = omega*t;

switch option
    case 'Pos'
        term = (alpha^2/omega + omega)*exp(-alpha*t).*...
            ((omega^2 - alpha^2)*sin(freq) + 2*alpha*omega*cos(freq));
    case 'Vel'
        term = exp(-alpha*t).*((3*alpha*omega - alpha^3/omega)*sin(freq)...
            +(3*alpha^2- omega^2)*cos(freq));
    otherwise
     error('No recognized option')
end


function [W] = amuse(X)
% BSS using eigenvalue value decomposition
% Program written by A. Cichocki and R. Szupiluk
%
% X [m x N] matrix of observed (measured) signals,
% W separating matrix,
% y estimated separated sources
% p time delay used in computation of covariance matrices
% optimal time-delay default p= 1
%
% First stage: Standard prewhitening

[m,N]=size(X);
if nargin==1,
 n=m; %
 end;

Rxx=(X*X')/N;

[Ux,Dx,Vx]=svd(Rxx);
 Dx=diag(Dx);
% n=xxx;
 if n<m, % under assumption of additive white noise and
        %when the number of sources are known or can a priori estimated
  Dx=Dx-real((mean(Dx(n+1:m))));
  Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
%
else    % under assumption of no additive noise and when the
        % number of sources is unknown
   n=max(find(Dx>1e-199)); %Detection the number of sources
   Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
end;
%
% else    %assumes no noise
%    Q=inv(sqrtm(Rxx));
% end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Second stage: Fast separation using sorting EVD
% notation the same as used in the Chapter 4
Xb=Q*X;
p=1;
% paramter p can take here value different than 1
% for example -1 or 2.
N=max(size(Xb));
Xb=Xb-kron(mean(Xb')',ones(1,N));

Rxbxbp=(Xb(:,1:N-1)*Xb(:,2:N)')/(N-1);
Rxbxbp= Rxbxbp+Rxbxbp';
[Vxb Dxb]=eig(Rxbxbp);
[D1 perm]=sort(diag(Dxb));
D1=flipud(D1);
Vxb=Vxb(:,flipud(perm));
W = Vxb'*Q;
%y = Vxb' * x1;


function [U,c] = MgnCalibration(X)
% performs magnetometer calibration from a set of data
% using Merayo technique with a non iterative algoritm
% J.Merayo et al. "Scalar calibration of vector magnemoters"
% Meas. Sci. Technol. 11 (2000) 120-132.
%
%   X      : a Nx3 (or 3xN) data matrix
%              each row (columns) contains x, y, z measurements
%              N must be such that the data set describes
%              as completely as possible the 3D space
%              In any case N > 10
%
%    The calibration tries to find the best 3D ellipsoid that fits the data set
%    and returns the parameters of this ellipsoid
%
%    U     :  shape ellipsoid parameter, (3x3) upper triangular matrix
%    c      : ellipsoid center, (3x1) vector
%
%    Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
%    with v a rough triaxes magnetometer  measurement
%
%    calibrated measurement w = U*(v-c)
%
%   author : Alain Barraud, Suzanne Lesecq 2008
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,m] = size(X);
if m>3&&N==3,X = X';N = m;m = 3;end;%check that X is not transposed
if N<=10,U = [];c = [];return;end;%not enough data no calibration !!
% write  the ellipsoid equation as D*p=0
% the best parameter is the solution of min||D*p|| with ||p||=1;
% form D matrix from X measurements
x = X(:,1); y = X(:,2); z = X(:,3);
D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
D=triu(qr(D));%avoids to compute the svd of a large matrix
[U,S,V] = svd(D);%because usually N may be very large
p = V(:,end);if p(1)<0,p =-p;end;
% the following matrix A(p) must be positive definite
% The optimization done by svd does not include such a constraint
% With "good" data the constraint is allways satisfied
% With too poor data A may fail to be positive definite
% In this case the calibration fails
%
A = [p(1) p(4)/2 p(5)/2;
       p(4)/2 p(2) p(6)/2;
       p(5)/2 p(6)/2 p(3)];
[U,ok] = fchol(m,A);
if ~ok,U = [];c = [];return;end%calibration fails too poor data!!
b = [p(7);p(8);p(9)];
v = Utsolve(U,b/2,m);
d = p(10);
s = 1/sqrt(v*v'-d);
c =-Usolve(U,v,m)';%ellipsoid center
U = s*U;%shape ellipsoid parameter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,ok] = fchol(n,A)
% performs Cholesky factoristation
A(1,1:n) = A(1,1:n)/sqrt(A(1,1));
A(2:n,1) = 0;
for j=2:n
  A(j,j:n) = A(j,j:n) - A(1:j-1,j)'*A(1:j-1,j:n);
  if A(j,j)<=0,ok=0;break;end%A is not positive definite
  A(j,j:n) = A(j,j:n)/sqrt(A(j,j));
  A(j+1:n,j) = 0;
end
ok=1;
function x=Utsolve(U,b,n)
% solves U'*x=b
x(1) = b(1)/U(1,1);
for k=2:n
    x(k) = (b(k)-x(1:k-1)*U(1:k-1,k))/U(k,k);
end
function x=Usolve(U,b,n)
% solves U*x=b
x(n) = b(n)/U(n,n);
for k=n-1:-1:1
    x(k) = (b(k)-U(k,k+1:n)*x(k+1:n)')/U(k,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function[f,name]=tidefreq(str)
%TIDEFREQ  Frequencies of the eight major tidal components.
%
%   [F,NAME]=TIDEFREQ returns the frequencies of the eight major tidal
%   components together with a string matrix containing their names.  In
%   order of increasing frequency, these are
%
%        Mf O1 P1 K1 N2 M2 S2 K2.
%
%   The units are given in *radians* per hour. See Gill (1982) page 335.
%
%   F=TIDEFREQ(STR) returns the frequency for the component named STR,
%   if STR is a string.  F=TIDEFREQ(N) where N is a number also works.
%
%   Usage:   [f,name]=tidefreq;
%            f=tidefreq(str);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2005--2015 J.M. Lilly --- type 'help jlab_license' for details

name=('MFO1P1K1N2M2S2K2');

%Gill only gives first two decimal points
%I got the more exact numbers from
%http://oceanworld.tamu.edu/resources/ocng_textbook/chapter17/chapter17_04.htm

p(1)=327.85;
p(2)=25.8194;
p(3)=24.0659;
p(4)=23.9344;
p(5)=12.6584;
p(6)=12.4206;
p(7)=12.0000;
p(8)=11.9673;

f=1./p;
if nargin==1
  if ~ischar(str)
    f=f(str);
  else
    str=upper(str);
    ii=strfind(name,str);
    f=f((ii-1)/2+1);
  end
  clear name
else
  name=reshape(name,2,8)';
end

f=f*2*pi;


function [pTS,fmin]=grTravSale(C)
% Function [pTS,fmin]=grTravSale(C) solve the nonsymmetrical
% traveling salesman problem.
% Input parameter:
%   C(n,n) - matrix of distances between cities,
%     maybe, nonsymmetrical;
%     n - number of cities.
% Output parameters:
%   pTS(n) - the order of cities;
%   fmin - length of way.
% Uses the reduction to integer LP-problem:
% Look: Miller C.E., Tucker A. W., Zemlin R. A.
% Integer Programming Formulation of Traveling Salesman Problems.
% J.ACM, 1960, Vol.7, p. 326-329.
% Needed other products: MIQP.M.
% This software may be free downloaded from site:
% http://control.ee.ethz.ch/~hybrid/miqp/
% Author: Sergiy Iglin
% e-mail: siglin@yandex.ru
% personal page: http://iglin.exponenta.ru

% ============= Input data validation ==================
if nargin<1,
  error('There are no input data!')
end
if ~isnumeric(C),
  error('The array C must be numeric!')
end
if ~isreal(C),
  error('The array C must be real!')
end
s=size(C); % size of array C
if length(s)~=2,
  error('The array C must be 2D!')
end
if s(1)~=s(2),
  error('Matrix C must be square!')
end
if s(1)<3,
  error('Must be not less than 3 cities!')
end

% ============ Size of problem ====================
n=s(1); % number of vertexes
m=n*(n-1); % number of arrows

% ============ Parameters of integer LP problem ========
Aeq=[]; % for the matrix of the boundary equations
for k1=1:n,
  z1=zeros(n);
  z1(k1,:)=1;
  z2=[z1;eye(n)];
  Aeq=[Aeq z2([1:2*n-1],setdiff([1:n],k1))];
end
Aeq=[Aeq zeros(2*n-1,n-1)];
A=[]; % for the matrix of the boundary inequations
for k1=2:n,
  z1=[];
  for k2=1:n,
    z2=eye(n)*(n-1)*(k2==k1);
    z1=[z1 z2(setdiff([2:n],k1),setdiff([1:n],k2))];
  end
  z2=-eye(n);
  z2(:,k1)=z2(:,k1)+1;
  A=[A;[z1 z2(setdiff([2:n],k1),2:n)]];
end
beq=ones(2*n-1,1); % the right parts of the boundary equations
b=ones((n-1)*(n-2),1)*(n-2); % the right parts of the boundary inequations
C1=C'+diag(ones(1,n)*NaN);
C2=C1(:);
c=[C2(~isnan(C2));zeros(n-1,1)]; % the factors for objective function
vlb=[zeros(m,1);-inf*ones(n-1,1)]; % the lower bounds
vub=[ones(m,1);inf*ones(n-1,1)]; % the upper bounds
H=zeros(n^2-1); % Hessian

% ============= We solve the MIQP problem ==========
[xmin,fmin]=MIQP(H,c,A,b,Aeq,beq,[1:m],vlb,vub);

% ============= We return the results ==============
eik=round(xmin(1:m)); % the arrows of the way
e1=[zeros(1,n-1);reshape(eik,n,n-1)];
e2=[e1(:);0]; % we add zero to a diagonal
e3=(reshape(e2,n,n))'; % the matrix of the way
pTS=[1 find(e3(1,:))]; % we build the way
while pTS(end)>1, % we add the city to the way
  pTS=[pTS find(e3(pTS(end),:))];
end
return


% Test how accurately we can orthogonalize ill-conditioned bases of tangent
% vectors. We test three metrics:
%  1) Is Q actually orthonormal (the figure displays log(|Q'Q|); since Q'Q
%     should be identity, we hope to see 0 on diagonal and -inf everywhere
%     else.)
%  2) Is A = QR? This is in the title of the plots.
%  3) How much time does it take to compute?
%
% Nicolas Boumal, Oct. 5, 2017

clear all;
clc;

M = spherefactory(100);

% M = stiefelcomplexfactory(200, 56);
% M = productmanifold(struct('S', spherefactory(5), 'R', rotationsfactory(3, 10)));

x = M.rand();

% Create a poorly conditioned basis
A = cell(M.dim()-5, 1);
A{1} = M.randvec(x);
for k = 2 : numel(A)
    A{k} = M.lincomb(x, 1, A{k-1}, 1e-6, M.randvec(x));
end

t1 = tic();
[Q1, R1] = orthogonalize(M, x, A);
t1 = toc(t1);

t2 = tic();
[Q2, R2] = orthogonalizetwice(M, x, A);
t2 = toc(t2);

t3 = tic();
[Q3, R3] = orthogonalize_legacy(M, x, A);
t3 = toc(t3);

% To check if Q is really orthonormal
G1 = grammatrix(M, x, Q1);
G2 = grammatrix(M, x, Q2);
G3 = grammatrix(M, x, Q3);

% To check of QR = A
distsq = 0;
for k = 1 : numel(A)
    distsq = distsq + M.norm(x, M.lincomb(x, 1, lincomb(M, x, Q1(1:k), R1(1:k, k)), -1, A{k}))^2;
end
dist1 = sqrt(distsq);

distsq = 0;
for k = 1 : numel(A)
    distsq = distsq + M.norm(x, M.lincomb(x, 1, lincomb(M, x, Q2(1:k), R2(1:k, k)), -1, A{k}))^2;
end
dist2 = sqrt(distsq);

distsq = 0;
for k = 1 : numel(A)
    distsq = distsq + M.norm(x, M.lincomb(x, 1, lincomb(M, x, Q3(1:k), R3(1:k, k)), -1, A{k}))^2;
end
dist3 = sqrt(distsq);

subplot(1, 3, 1);
imagesc(log10(abs(G1))); axis equal, axis tight; colorbar; set(gca, 'CLim', [-20, 0]);
title(sprintf('MGS: CPU time: %g\n||A - QR|| = %g', t1, dist1));
subplot(1, 3, 2);
imagesc(log10(abs(G2))); axis equal, axis tight; colorbar; set(gca, 'CLim', [-20, 0]);
title(sprintf('MGS-twice: CPU time: %g\n||A - QR|| = %g', t2, dist2));
subplot(1, 3, 3);
imagesc(log10(abs(G3))); axis equal, axis tight; colorbar; set(gca, 'CLim', [-20, 0]);
title(sprintf('Legacy: CPU time: %g\n||A - QR|| = %g', t3, dist3));


function box_display_test06 ( )

%*****************************************************************************80
%
%% BOX_DISPLAY_TEST06 compares several methods that reach degree 4 in X, 8 in Y.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BOX_DISPLAY_TEST06:\n' );
  fprintf ( 1, '  Plot index sets for various anisotropic methods\n' );
  fprintf ( 1, '  that reach degree 4 in X, 8 in Y.\n' );
  fprintf ( 1, '\n' );

  m = 10;
  n = 10;
  title_string = '';
%
%  Total degree <= 8.
%
  box_display ( m, n, @(x,y)x+y<=-1, @(x,y)2*x+y<=8, title_string );
  filename = 'degree48_total.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created "%s".\n', filename );
%
%  Maximum degree <= 8.
%
  box_display ( m, n, @(x,y)x+y<=-1, @(x,y)max(2*x,y)<=8, title_string );
  filename = 'degree48_max.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created "%s".\n', filename );
%
%  Hyperbolic cross.
%
  box_display ( m, n, @(x,y)x+y<=-1, @(x,y)(2*x+1)*(y+1)<=9, title_string );
  filename = 'degree48_hyper.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created "%s".\n', filename );
%
%  Clenshaw-Curtis sparse grid.
%
% box_display ( m, n, @(x,y)x+y<=-1, @cc_aniso, title_string );
% filename = 'degree48_cc.png';
% print ( '-dpng', filename );
% fprintf ( 1, '  Created "%s".\n', filename );
%
%  Smolyak: Log2(2*x) + Log2(y) <= Log2(L)
%
  box_display ( m, n, @(x,y)x+y<=-1, @(x,y)log2(max(2*x,1))+log2(max(y,1))<=log2(8.1), title_string );
  filename = 'degree48_smolyak.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created "%s".\n', filename );

  return
end


function b = isPointOnLine3d(point, line, varargin)
%ISPOINTONLINE3D Test if a 3D point belongs to a 3D line.
%
%   B = isPointOnLine3d(POINT, LINE)
%   with POINT being [xp yp zp], and LINE being [x0 y0 z0 dx dy dz].
%   Returns 1 if point lies on the line, 0 otherwise.
%
%   If POINT is an N-by-3 array of points, B is a N-by-1 array of booleans.
%
%   If LINE is a N-by-6 array of lines, B is a N-by-1 array of booleans.
%
%   B = isPointOnLine3d(POINT, LINE, TOL)
%   Specifies the tolerance used for testing location on 3D line.
%
%   See also
%   lines3d, distancePointLine3d, linePosition3d, isPointOnLine
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

% extract computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% size of inputs
np = size(point,1);
nl = size(line, 1);

if np == 1 || nl == 1 || np == nl
    % test if lines are colinear, using norm of the cross product
    b = bsxfun(@rdivide, vectorNorm3d( ...
        crossProduct3d(bsxfun(@minus, line(:,1:3), point), line(:,4:6))), ...
        vectorNorm3d(line(:,4:6))) < tol;
else
    % same test, but after reshaping arrays to manage difference of
    % dimensionality
    point = reshape(point, [np 1 3]);
    line = reshape(line, [1 nl 6]);
    b = bsxfun(@rdivide, vectorNorm3d( ...
        cross(bsxfun(@minus, line(:,:,1:3), point), line(ones(1,np),:,4:6), 3)), ...
        vectorNorm3d(line(:,:,4:6))) < tol;
end


function geometry_test011 ( )

%*****************************************************************************80
%
%% TEST011 tests CIRCLE_DIA2IMP_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST011\n' );
  fprintf ( 1, '  CIRCLE_DIA2IMP_2D converts a diameter to an\n' );
  fprintf ( 1, '  implicit circle in 2D.\n' );

  theta = 2.0;

  p1(1,1) = 2.0 + 5.0 * cos ( theta );
  p1(2,1) = 3.0 + 5.0 * sin ( theta );

  p2(1,1) = 2.0 - 5.0 * cos ( theta );
  p2(2,1) = 3.0 - 5.0 * sin ( theta );

  r8vec_print ( dim_num, p1, '  P1:' )
  r8vec_print ( dim_num, p2, '  P2:' )

  [ r, center ] = circle_dia2imp_2d ( p1, p2 );

  circle_imp_print_2d ( r, center, '  The implicit circle:' );

  return
end


function [cc] = chaincode(b,unwrap)
%   Freeman Chain Code
%
%   Description: Give Freeman chain code 8-connected representation of a
%                boundary
%   Author.....: Alessandro Mannini <mannini@esod.it>
%   Date.......: 2010, september
%
%   usage:
%   --------------------------------------------------------
%   [cc] = chaincode(b,u)
%
%   INPUT:
%   --------------------------------------------------------
%   b       - boundary as np-by-2 array;
%             np is the number of pixels and each element is a pair (y,x) of
%             pixel coordinates
%   unwrap  - (optional, default=false) unwrap code;
%             if enable phase inversions are eliminated
%
%
%   OUTPUT:
%   --------------------------------------------------------
%   cc is structure with the following fields:
%
%   cc.code     - 8-connected Freeman chain code as 1-by-np array (or
%                 1-by-(np-1) if the boundary isn't close)
%   cc.x0,cc.y0 - respectively the abscissa and ordinate of start point
%   cc.ucode    - unwrapped 8-connected Freeman chain code (if required)
%

%
%
%   used direction-to-code convention is:       3  2  1
%                                                \ | /
%                                             4 -- P -- 0
%                                                / | \
%                                               5  6  7
%
%   and in terms of deltax,deltay if next pixel compared to the current:
%   --------------------------
%   | deltax | deltay | code |
%   |------------------------|
%   |    0   |   +1   |   2  |
%   |    0   |   -1   |   6  |
%   |   -1   |   +1   |   3  |
%   |   -1   |   -1   |   5  |
%   |   +1   |   +1   |   1  |
%   |   +1   |   -1   |   7  |
%   |   -1   |    0   |   4  |
%   |   +1   |    0   |   0  |
%   --------------------------
%

% check input arguments
if nargin>2
    error('Too many arguments');
elseif nargin==0
    error('Too few arguments');
elseif nargin==1
    unwrap=false;
end
% compute dx,dy  by a circular shift on coords arrays by 1 element
sb=circshift(b,[-1 0]);
delta=sb-b;
% check if boundary is close, if not cut last element
if abs(delta(end,1))>1 || abs(delta(end,2))>1
    delta=delta(1:(end-1),:);
end
% check if boundary is 8-connected
n8c=find(abs(delta(:,1))>1 | abs(delta(:,2))>1);
if size(n8c,1)>0
    s='';
    for i=1:size(n8c,1)
        s=[s sprintf(' idx -> %d \n',n8c(i))];
    end
    error('Curve isn''t 8-connected in elements: \n%s',s);
end

% convert dy,dx pairs to scalar indexes thinking to them (+1) as base-3 numbers
% according to: idx=3*(dy+1)+(dx+1)=3dy+dx+4 (adding 1 to have idx starting
% from 1)
% Then use a mapping array cm
%   --------------------------------------
%   | deltax | deltay | code | (base-3)+1 |
%   |-------------------------------------|
%   |    0   |   +1   |   2  |      8     |
%   |    0   |   -1   |   6  |      2     |
%   |   -1   |   +1   |   3  |      7     |
%   |   -1   |   -1   |   5  |      1     |
%   |   +1   |   +1   |   1  |      9     |
%   |   +1   |   -1   |   7  |      3     |
%   |   -1   |    0   |   4  |      4     |
%   |   +1   |    0   |   0  |      6     |
%   ---------------------------------------

idx=3*delta(:,1)+delta(:,2)+5;
cm([1 2 3 4 6 7 8 9])=[5 6 7 4 0 3 2 1];

% finally the chain code array and the starting point
cc.x0=b(1,2);
cc.y0=b(1,1);
cc.code=(cm(idx))';

% If unwrapping is required, use the following algorithm
%
% if a(k), k=1..n is the original code and u(k) the unwrapped:
%
% - u(1)=a(1)
% - u(k)=g(k),
%        g(k) in Z | (g(k)-a(k)) mod 8=0 and |g(k)-u(k-1)| is minimized
%
if (unwrap)
    a=cc.code;
    u(1)=a(1);
    la=size(a,1);
    for i=2:la
        n=round((u(i-1)-a(i))/8);
        u(i)=a(i)+n*8;
    end
    cc.ucode=u';
end

end


% this script represents the evolution of the covariance of an OU process in terms of the dispersion ellipsoid
% see A. Meucci (2009)
% "Review of Statistical Arbitrage, Cointegration, and Multivariate Ornstein-Uhlenbeck"
% available at ssrn.com

% Code by A. Meucci, April 2009
% Most recent version available at www.symmys.com > Teaching > MATLAB

clear; clc; close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% input parameters of multivariate OU process
K=1;
J=1;

x0=rand(K+2*J,1);

Mu=rand(K+2*J,1);

A=rand(K+2*J,K+2*J)-.5;
ls=rand(K,1)-.5;
gs=rand(J,1)-.5;
os=rand(J,1)-.5;

S=rand(K+2*J,K+2*J)-.5;

ts=.01*[0:10:100];
NumSimul=10000;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% process inputs
Gamma=diag(ls);
for j=1:J
    G=[gs(j)  os(j)
           -os(j) gs(j)];
    Gamma=blkdiag(Gamma,G);
end
Theta=A*Gamma*inv(A);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% one-step exact simulation
Sigma=S*S';
X_0=repmat(x0',NumSimul,1);
[X_t1, MuHat_t1, SigmaHat_t1]=OUstep(X_0,ts(end),Mu,Theta,Sigma);

% multi-step simulation: exact and Euler approximation
X_t=repmat(x0',NumSimul,1);
X_tE=X_t;
for s=1:length(ts)
    Dt=ts(1);
    if s>1
        Dt=ts(s)-ts(s-1);
    end
    [X_t,MuHat_t,SigmaHat_t]=OUstep(X_t,Dt,Mu,Theta,Sigma);
    %[X_tE,MuHat_tE,SigmaHat_tE]=OUstepEuler(X_tE,Dt,Mu,Theta,Sigma);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plots
Pick=[K+2*J-1 K+2*J];

% horizon simulations
hold on
h5=plot(X_t1(:,Pick(1)),X_t1(:,Pick(2)),'.');
set(h5,'color','r','markersize',4)

% horizon location
hold on
h4=plot(MuHat_t1(Pick(1)),MuHat_t1(Pick(2)),'.');
set(h4,'color','k','markersize',5)

% horizon dispersion ellipsoid
hold on
h3=TwoDimEllipsoid(MuHat_t1(Pick),SigmaHat_t1(Pick,Pick),2,0,0);
set(h3,'color','k','linewidth',2);

% starting point
hold on
h2=plot(x0(Pick(1)),x0(Pick(2)),'.');
set(h2,'color','b','markersize',5)

% starting generating dispersion ellipsoid
hold on
h1=TwoDimEllipsoid(x0(Pick),Sigma(Pick,Pick),2,0,0);
set(h1,'color','b','linewidth',2);

legend([h1 h3],'generator','horizon');


function fx = p15_fun ( option, nvar, x )

%*****************************************************************************80
%
%% P15_FUN evaluates the function for problem 15.
%
%  Title:
%
%    The Trigger Circuit.
%
%  Description:
%
%    The current flow of a trigger circuit with an operational amplifier
%    is modeled.  The variables are voltages, with X(6) the output
%    voltage and X(7) the input voltage.
%
%    The function has the form
%
%      F(X) = A * X + PHI ( X )
%
%    where A is a 6 by 7 matrix, and PHI is a nonlinear term, that is,
%
%      F(I) = SUM ( 1 <= J <= 7 ) A(I,J) * X(J) + PHI ( X )
%
%  Options:
%
%    Melhem lists the following limit points in X(7):
%
%    ( 0.04936  0.54735  0.04944  0.04944  0.12920  1.16602  0.60185 )
%    ( 0.23577  0.66296  0.23759  0.23760  0.62083  9.60913  0.32286 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 20008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rami Melhem, Werner Rheinboldt,
%    A Comparison of Methods for Determining Turning Points of Nonlinear Equations,
%    Computing,
%    Volume 29, Number 3, September 1982, pages 201-226.
%
%    Gerd Poenisch, Hubert Schwetlick,
%    Computing Turning Points of Curves Implicitly Defined by Nonlinear
%    Equations Depending on a Parameter,
%    Computing,
%    Volume 26, Number 2, June 1981, pages 107-121.
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the function.
%
%    Output, real FX(NVAR-1), the value of the function at X.
%

%
%  Get the linear coefficients.
%
  array = p15_gx ( );
%
%  Compute the linear portion of the function.
%
  fx(1:nvar-1) = 0.0;

  for i = 1 : nvar - 1
    for j = 1 : nvar
      fx(i) = fx(i) + array(i,j) * x(j);
    end
  end
%
%  Add the nonlinear terms.
%
  fx(2) = fx(2) + 5.6D-08 * ( exp ( 25.0 * x(2) ) - 1.0 );
  fx(5) = fx(5) + 5.6D-08 * ( exp ( 25.0 * x(5) ) - 1.0 );
  fx(6) = fx(6) - 7.65 * atan ( 1962.0 * ( x(3) - x(1) ) ) / 0.201;

  return
end


function out=prox_l1_squared(x,alpha)
%PROX_L1_SQUARED computes the proximal operator of the function alpha*(norm(x(:),1)^2)
%
%  Usage:
%  out = PROX_L1_SQUARED(x,alpha)
%  ===========================================
%  INPUT:
%  x - point to be projected (vector/matrix)
%  alpha - positive scalar
%  ===========================================
%  Output:
%  out - proximal operator at x

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

if (nargin < 2)
    error ('usage: prox_l1_squared(x,alpha)') ;
end

if (alpha < 0)
    error('usage: prox_l1_squared(x,alpha) - alpha should be positive')
end

%setting eps to defalut value : 1e-10
eps = 1e-10 ;

if (norm(x) < eps)
    out = x;
    return ;
end

%defining f on mu - to be used by the bisetion
f=@(mu)   sum(sum( max(sqrt(alpha) *abs(x)/sqrt(mu) - 2*alpha,0))) - 1 ;

mu_min = 0 ;
mu_max = 1 ;
while(f(mu_max)> 0)
    mu_max = mu_max * 2 ;
end

final_mu = bisection(f,mu_min,mu_max,eps) ;
lam = max(sqrt(alpha) * abs(x)/sqrt(final_mu) - 2 *alpha,0) ;

out = (lam .* x) ./ (lam + 2 * alpha) ;

end


% MAIN - chain integrator
%
% Problem statement:
%
% Find the minimum-snap trajectory that moves a system between two boundary
% points. Note that snap is the 4th derivative of position. Since the
% dynamics are in first-order form, we need to include position, velocity,
% acceleration, jerk in our state vector. We then set the control to be the
% snap of the trajectory.
%

clc; clear;
addpath ../../..

%%%% Boundary-value problem:

t0 = 0;        %initial time
x0 = [1;0];    %initial position
dx0 = [0;0];   %initial velocity
ddx0 = [0;0];  %initial acceleration
dddx0 = [0;0]; %initial jerk (derivative of acceleration)
z0 = [x0;dx0;ddx0;dddx0];   %Full initial state

tF = 1;        %final time
xF = [0;1];    %final position
dxF = [0;0];   %final velocity
ddxF = [0;0];  %final acceleration
dddxF = [0;0]; %final jerk (derivative of acceleration)
zF = [xF;dxF;ddxF;dddxF];  %full final state

%%%% Construct bounds struct, given problem specifications

problem.bounds.initialTime.low = t0;
problem.bounds.initialTime.upp = t0;
problem.bounds.finalTime.low = tF;
problem.bounds.finalTime.upp = tF;

problem.bounds.initialState.low = z0;
problem.bounds.initialState.upp = z0;
problem.bounds.finalState.low = zF;
problem.bounds.finalState.upp = zF;

%%%% Construct a simple initial guess (linear between boundary)
problem.guess.time = [t0, tF];
problem.guess.state = [z0, zF];
problem.guess.control = [zeros(size(x0)), zeros(size(xF))];

%%%% Define dynamics and objective functions:

% Enforce the chain integrator dynamics:
problem.func.dynamics = @(t,z,u)(  dynamics(z,u)  );

% Minimize the integral of the snap-squared along the trajectory.
% Sum along each dimension of the state space.
problem.func.pathObj = @(t,z,u)( sum(u.^2,1) );

%%%% Select the method of choice:

% problem.options.method = 'trapezoid';
% problem.options.method = 'hermiteSimpson';
problem.options.method = 'chebyshev';
% problem.options.method = 'rungeKutta';
% problem.options.method = 'gpops';    % requires license for GPOPS-II

%%%% Solve!
soln = optimTraj(problem);

%%%% Unpack the solution

tGrid = soln.grid.time;
xGrid = soln.grid.state(1:2, :);
dxGrid = soln.grid.state(3:4, :);
ddxGrid = soln.grid.state(5:6, :);
dddxGrid = soln.grid.state(7:8, :);
ddddxGrid = soln.grid.control;

t = linspace(tGrid(1), tGrid(end), 100);
z = soln.interp.state(t);
x = z(1:2,:);
dx = z(3:4,:);
ddx = z(5:6,:);
dddx = z(7:8,:);
ddddx = soln.interp.control(t);

%%%% Plot the trajectory against time
figure(1); clf;

subplot(5,1,1); hold on;
plot(t,x)
plot(tGrid,xGrid,'ko','MarkerSize',8,'LineWidth',2);

subplot(5,1,2); hold on;
plot(t,dx)
plot(tGrid,dxGrid,'ko','MarkerSize',8,'LineWidth',2);

subplot(5,1,3); hold on;
plot(t,ddx)
plot(tGrid,ddxGrid,'ko','MarkerSize',8,'LineWidth',2);

subplot(5,1,4); hold on;
plot(t,dddx)
plot(tGrid,dddxGrid,'ko','MarkerSize',8,'LineWidth',2);

subplot(5,1,5); hold on;
plot(t,ddddx)
plot(tGrid,ddddxGrid,'ko','MarkerSize',8,'LineWidth',2);


% Example of use for function 'power_flow_solver'.
% Written by Dr. Yoash Levron, October 2012.
%
% All documentation may be obtained by typing:
% 'HELP power_flow_solver' at the matlab command prompt.
%
% This script defines a simple power network
% and uses the 'power_flow_solver' function to
% compute the power flow in it.
%
% Associated file: 'power_flow_solver.m'

%%%%%%% Define the network  %%%%%%%
% The network composes three buses.
% bus 1 - (slack bus) a voltage source, E=1.0
% bus 2 - a generator and a load.
% bus 3 - a load.
%
% Define the line impedances:
z12 = 0.05 + j*0.1;  % impedance (inductive) connecting bus 1 to 2.
z13 = 0.02 + j*0.05; % impedance (inductive) connecting bus 1 to 3.
z23 = j*0.05; % impedance (inductive) connecting bus 2 to 3.
z11 = -j*100;  % shunt impedance (capacitive) at bus 1
z22 = inf;  % no shunt impedance at bus 2
z33 = -j*40;  % shunt impedance (capacitive) at bus 3

% A matrix of the network impedances
Zmat = [z11 z12 z13;
        z12 z22 z23;
        z13 z23 z33];

% compute the admittance matrix (admittamce = 1/impedance):
Ymat = 1./Zmat;

% Define voltage at bus 1
E1_phase = 0; % degree (reference phase angle).
E1_mag = 1.0; % voltage magnitude
E1 = E1_mag*exp(j*E1_phase*(pi/180));

% Define generators and loads.
% Pg&Qg - generators.   Pl&Ql - loads.
Pg1=0;  Qg1=0; Pl1=0; Ql1=0; % no power source on the slack bus.
Pg2=2;  Qg2=0;  Pl2=1; Ql2=1;  % bus 2. generator and load.
Pg3=0;  Qg3=0;  Pl3=3; Ql3=0.5; % bus 3. a load.

% sum generators and load to form united power sources.
% (generators taken positive, loads taken negative).
P1 = Pg1 - Pl1;
P2 = Pg2 - Pl2;
P3 = Pg3 - Pl3;
Q1 = Qg1 - Ql1;
Q2 = Qg2 - Ql2;
Q3 = Qg3 - Ql3;

% Define power vectors:
Pbus = [P1 P2 P3];
Qbus = [Q1 Q2 Q3];

% run function to solve the power flow
[Ebus, Ibus, Imat, iter] = ...
    power_flow_solver(Ymat, Pbus, Qbus, E1);

% display results
clc;
disp('---  SUMMARY OF RESULTS  ---');
disp('number of iterations until convergence:');
disp(iter);
disp('voltage magnitudes');
disp(abs(Ebus.'));
disp('voltage phase angles (degree)');
disp(phase(Ebus.')*180/pi);
disp('current supplied by each source (amplitude)');
disp(abs(Ibus.'));
disp('Active power supplied by each source');
disp(real(Ebus.*conj(Ibus)));
disp('Reactive power supplied by each source');
disp(imag(Ebus.*conj(Ibus)));
disp('current in bus 1 --> bus 2');
disp(Imat(1,2));
disp('current in bus 1 --> bus 3');
disp(Imat(1,3));
disp('current in bus 2 --> bus 3');
disp(Imat(2,3));
disp('shunt currents in each bus (magnitude)');
disp(abs(diag(Imat).'));


clear all; close all; clc

n=100; L=4;
x=linspace(0,L,n);
f=(x.^2).';  % parabola with 100 data points

M=21;  % polynomial degree
for j=1:M
    phi(:,j)=(x.').^(j-1);  % build matrix A
end

trials=[2 10 100];
for j=1:3
    for jj=1:trials(j)
       f=(x.^2+0.2*randn(1,n)).';
       a1=pinv(phi)*f; f1=phi*a1; E1(jj)=norm(f-f1)/norm(f);
       a2=phi\f; f2=phi*a2; E2(jj)=norm(f-f2)/norm(f);
       [a3,stats]=lasso(phi,f,'Lambda',0.1); f3=phi*a3; E3(jj)=norm(f-f3)/norm(f);
       A1(:,jj)=a1; A2(:,jj)=a2; A3(:,jj)=a3;
    end
    A1m=mean(A1.'); A2m=mean(A2.'); A3m=mean(A3.');
    Err=[E1; E2; E3];

    subplot(3,3,j), bar(A1m), axis([0 21 -1 1.2])
    subplot(3,3,3+j), bar(A2m), axis([0 21 -1 1.2])
    subplot(3,3,6+j), bar(A3m), axis([0 21 -1 1.2])
end

%subplot(3,3,j), bar(A1m,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis([0 21 -1 1.2])
%subplot(3,3,3+j), bar(A2m,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis([0 21 -1 1.2])
%subplot(3,3,6+j), bar(A3m,'FaceColor',[.6 .6 .6],'EdgeColor','k'), axis([0 21 -1 1.2])

%%
figure(2)

Atot=[A1m; A2m; A3m];  % average loadings of three methods
Atot2=(Atot>0.2).*Atot; % threshold
Atot3=[Atot; Atot2];    % combine both thresholded and not

figure(3), bar3(Atot.'), axis([0 4 0 20 0 1]), view(-70,38), set(gca,'Xtick',[],'Fontsize',[18])
figure(4), bar3(Atot2.'), axis([0 4 0 20 0 1]), view(-70,38), set(gca,'Xtick',[],'Fontsize',[18])

n=200; L=8;
x=linspace(0,L,n);
x1=x(1:100);   % train (interpolation)
x2=x(101:200); % test (extrapolation)

ftrain=(x1.^2).';  % interpolated parabola x=[0,4]
ftest=(x2.^2).';   % extrapolated parbola x=[4,5]

for j=1:M
    phi_i(:,j)=(x1.').^(j-1); % interpolation key
    phi_e(:,j)=(x2.').^(j-1); % extrapolation key
end

for jj=1:6 % compute inter/extra-polation scores
    ani=Atot3(jj,:).';
    fnai=phi_i*ani;
    Eni(jj)=norm(ftrain-fnai)/norm(ftrain);
    fnae=phi_e*ani;
    Ene(jj)=norm(ftest-fnae)/norm(ftest);
end

figure(5)
subplot(3,2,1), bar(Eni)
subplot(3,2,2), bar(Ene)

subplot(3,2,3), bar(Eni), axis([0 7 0 0.01])
subplot(3,2,4), bar(Ene), axis([0 7 0 0.1])

figure(3)
legend('','','','Location','NorthEast')
legend boxoff

%% Dendrograms


function t = dec2twos(x, nbits)

% DEC2TWOS Convert decimal integer to binary string two's complement.
%
% Usage: T = DEC2TWOS(X, NBITS)
%
% Converts the signed decimal integer given by X (either a scalar, vector,
% or matrix) to the two's complement representation as a string. If X is a
% vector or matrix, T(i, :) is the representation for X(i) (the shape of X
% is not preserved).
%
% Example:
%     >> dec2twos([23 3 -23 -3])
%
%     ans =
%
%     010111
%     000011
%     101001
%     111101
%
% Inputs:
%   -X: decimal integers to convert to two's complement.
%   -NBITS: number of bits in the representation (optional, default is the
%   fewest number of bits necessary).
%
% Outputs:
%   -T: two's complement representation of X as a string.
%
% See also: TWOS2DEC, DEC2FIX, FIX2DEC, DEC2BIN, DEC2HEX, DEC2BASE.

error(nargchk(1, 2, nargin));
x = x(:);
maxx = max(abs(x));
nbits_min = nextpow2(maxx + (any(x == maxx))) + 1;

% Default number of bits.
if nargin == 1 || isempty(nbits)
    nbits = nbits_min;
elseif nbits < nbits_min
    warning('dec2twos:nbitsTooSmall', ['Minimum number of bits to ' ...
        'represent maximum input x is %i, which is greater than ' ...
        'input nbits = %i. Setting nbits = %i.'], ...
        nbits_min, nbits, nbits_min)
    nbits = nbits_min;
end

t = repmat('0', numel(x), nbits); % Initialize output:  Case for x = 0
if any(x > 0)
    t(x > 0, :) = dec2bin(x(x > 0), nbits);           % Case for x > 0
end
if any(x < 0)
    t(x < 0, :) = dec2bin(2^nbits + x(x < 0), nbits); % Case for x < 0
end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

% x * all_theta' : 5000 * 10，每一行的每一个列值代表图像是1,2...10的可能性
% use max(),M = max(A,[],dim) 沿着维度 dim 返回最大元素。例如，如果 A 为矩阵，则 max(A,[],2) 是包含每一行的最大值的列向量。
% probaitilty代表每一行最大的值，p代表索引
[probability, p] = max(sigmoid(X * all_theta'), [], 2);

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

end


function [J,J_od,J_id,J_bl] = jdegree(CIJ)
%JDEGREE    Joint degree distribution
%
%   [J,J_od,J_id,J_bl] = jdegree(CIJ);
%
%   This function returns a matrix in which the value of each element (u,v)
%   corresponds to the number of nodes that have u outgoing connections
%   and v incoming connections.
%
%   Input:      CIJ,    directed (weighted/binary) connection matrix
%
%   Outputs:    J,      joint degree distribution matrix (shifted by one)
%               J_od,   number of vertices with od>id.
%               J_id,   number of vertices with id>od.
%               J_bl,   number of vertices with id=od.
%
%   Note: Weights are discarded.
%
%
%   Olaf Sporns, Indiana University, 2002/2006/2008

% ensure CIJ is binary...
CIJ = double(CIJ~=0);

N = size(CIJ,1);

id = sum(CIJ,1);    % indegree = column sum of CIJ
od = sum(CIJ,2)';   % outdegree = row sum of CIJ

% Create the joint degree distribution matrix
% Note:  the matrix is shifted by one, to accomodate zero id and od in the first row/column.
% Upper triangular part of the matrix has vertices with an excess of
%    outgoing edges (od>id)
% Lower triangular part of the matrix has vertices with an excess of
%    outgoing edges (id>od)
% Main diagonal has units with id=od

szJ = max(max(id,od))+1;
J = zeros(szJ);

for i=1:N
   J(id(i)+1,od(i)+1) = J(id(i)+1,od(i)+1) + 1;
end;

J_od = sum(sum(triu(J,1)));
J_id = sum(sum(tril(J,-1)));
J_bl = sum(diag(J));


function zList=genAllNestedZParenth(n,algorithm)
%%GENALLNESTEDZPARENTH0 Generate all nested sets of n parenthesis pairs,
%           recording only the indices of the left parantheses.
%
%INPUTS: n The number of nested paranthesis pairs; n>=0. Passing zero will
%          result in an empty matrix being returned.
% algorithm An optional parameter specifying the algorithm that should be
%          used to generate the parenthesis pairs. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Use
%            the algorithm of Problem 2 of Section 7.2.1.6 of [1].
%          1 Use algorithm N of Section 7.2.1.6 of [1].
%
%OUTPUTS: zList The nXnumPairs list of indices where the left parenthesis
%               goes.
%
%There is a total of CatalanNumber(n) possible arrangements of nested
%parentheses.
%
%EXAMPLE:
%To recreate the values in Table I of Section 7.2.1.6 of [1],
%one can run
% zList=genAllNestedZParenth(4)
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(algorithm))
    algorithm=0;
end

if(n==0)
    zList=[];
    return;
end

switch(algorithm)
    case 0
        zList=genAllNestedZParenth0(n);
    case 1
        zList=genAllNestedZParenth1(n);
    otherwise
        error('Unknown algorithm specified.')
end

end

function zList=genAllNestedZParenth0(n)
%%GENALLNESTEDZPARENTH0 Generate all nested sets of n parenthesis pairs,
%           recording only the indices of the left parantheses using the
%           algorithm in Problem 2 of Section 7.2.1.6 of [1].
%
%REFERENCES
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(n==1)
    zList=1;
    return;
end

numBinTrees=CatalanNumber(n);
%Allocate space.
zList=zeros(n,numBinTrees);

%Step T1 Initialize
z=2*(0:n).'-1;

for curTree=1:numBinTrees
    %Step T2
    zList(:,curTree)=z(2:(n+1));

    %Step T3
    if(z(n-1+1)<z(n+1)-1)
        z(n+1)=z(n+1)-1;
        continue;
    end

    %Step T4, find j.
    j=n-1;
    z(n+1)=2*n-1;
    while(z(j-1+1)==z(j+1)-1)
        z(j+1)=2*j-1;
        j=j-1;
    end

    if(j==1)
        break;
    end
    z(j+1)=z(j+1)-1;
end

end

function zList=genAllNestedZParenth1(n)
%%GENALLNESTEDZPARENTH0 Generate all nested sets of n parenthesis pairs,
%           recording only the indices of the left parantheses using
%           Algorithm N of Section 7.2.1.6 of [1].
%
%REFERENCES
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

numBinTrees=CatalanNumber(n);
%Allocate space.
zList=zeros(n,numBinTrees);

%Step N1
z=2*(1:n).'-1;
g=2*(1:n).'-2;

for curTree=1:numBinTrees
    %Step N2
    zList(:,curTree)=z;
    j=n;

    %Step N3, Find j.
    while(z(j)==g(j))
        g(j)=bitxor(g(j),1);
        j=j-1;
    end

    %Step N4
    if(mod(g(j)-z(j),2)==0)
        z(j)=z(j)+2;
        continue;
    end

    %Step N5
    t=z(j)-2;
    if(t<0)
        return
    end

    if(t<=z(j-1))
        t=t+2*(t<z(j-1))+1;
    end
    z(j)=t;
end

end

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


function P=ar1spectrum(ar1,period)
% AR1 power spectrum...
%
% power=ar1spectrum(ar1,period)
%
%
% (c) Aslak Grinsted 2002-2004
%

% -------------------------------------------------------------------------
%   Copyright (C) 2002-2004, Aslak Grinsted
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made.  This
%   routine is provided as is without any express or implied warranties
%   whatsoever.

freq=1./period;
P=(1-ar1.^2)./(abs(1-ar1.*exp(-2*pi*i*freq))).^2; %http://www.madsci.org/posts/archives/may97/864012045.Eg.r.html
%fixed typo in numerical recipes


function y = t_lpdf(x,v,mu,sigma)
% T_LPDF  Log probability density function (lpdf) for Student's T distribution
%
%   Y = T_LPDF(X,V,MU,SIGMA) returns the lpdf of Student's T distribution with
%   degrees of freedom, V, mean, MU and standard deviation, SIGMA, at the
%   values in X.
%
%   The size of Y is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.
%
%   Default values for MU and SIGMA are 0 and 1 respectively.

% Copyright (c) 1998-2004 Aki Vehtari

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

if nargin < 4,
  sigma = 1;
end

if nargin < 3;
  mu = 0;
end

if nargin < 2,
  error('Requires at least two input arguments.');
end

term = gammaln((v + 1) / 2) - gammaln(v/2) -log(v.*pi)/2;
y = term - log(sigma) + log(1 + ((((x-mu)./sigma).^2)./v)) .* (-(v+1)/2);


function dist = pntdist(p1, p2)

% PNTDIST returns the euclidian distance between two points
%
%  [dist] = pntdist(pnt1, pnt2)
%
% where pnt1 and pnt2 must be Npnt x 3
% or either one can be Npnt x 1

% Copyright (C) 2002, Robert Oostenveld
%
% $Log: pntdist.m,v $
% Revision 1.2  2003/03/04 21:46:19  roberto
% added CVS log entry and synchronized all copyright labels
%

if size(p1,1)==1
  p1 = repmat(p1, size(p2,1), 1);
elseif size(p2,1)==1
  p2 = repmat(p2, size(p1,1), 1);
end

dist = sqrt(sum((p1-p2).^2, 2));


function [L,S] = low_rank_matrix_completion(params)
% Given partial observation of a low rank matrix, attempts to complete it.
%
% function [L,S] = low_rank_matrix_completion(A)
%
% This example demonstrates how to use the geometry factory for the
% embedded submanifold of fixed-rank matrices, fixedrankembeddedfactory.
% This geometry is described in the paper
% "Low-rank matrix completion by Riemannian optimization"
% Bart Vandereycken - SIAM Journal on Optimization, 2013.
%
% This can be a starting point for many optimization problems of the form:
%
% minimize f(X) such that rank(X) = k, size(X) = [m, n].
%
% Note that the code is long because it showcases quite a few features of
% Manopt: most of the code is optional.
%
% Input:  None. This example file generate random data.
%
% Output: None.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 15, 2014
% Contributors:
%
% Change log:
%
global P;
global PA;
global problem;

k = 1; % rank
A = params.A;
P = params.Omega;
[m,n] = size(A);
%P = sparse(randi([0 1],m,n));

% Random data generation. First, choose the size of the problem.
% We will complete a matrix of size mxn of rank k:
%m = 200;
%n = 500;
%k = 10;
% Generate a random mxn matrix A of rank k
%L = randn(m, k);
%R = randn(n, k);
%A = L*R';
% Generate a random mask for observed entries: P(i, j) = 1 if the entry
% (i, j) of A is observed, and 0 otherwise.
%fraction = 4 * k*(m+n-k)/(m*n);
%P = sparse(rand(m, n) <= fraction);
% Hence, we know the nonzero entries in PA:
PA = P.*A;

% Pick the manifold of matrices of size mxn of fixed rank k.
problem.M = fixedrankembeddedfactory(m, n, k);

% Define the problem cost function. The input X is a structure with
% fields U, S, V representing a rank k matrix as U*S*V'.
% f(X) = 1/2 * || P.*(X-A) ||^2
problem.cost = @cost;

% Define the Euclidean gradient of the cost function, that is, the
% gradient of f(X) seen as a standard function of X.
% nabla f(X) = P.*(X-A)
problem.egrad = @egrad;

% This is optional, but it's nice if you have it.
% Define the Euclidean Hessian of the cost at X, along H, where H is
% represented as a tangent vector: a structure with fields Up, Vp, M.
% This is the directional derivative of nabla f(X) at X along Xdot:
% nabla^2 f(X)[Xdot] = P.*Xdot
problem.ehess = @euclidean_hessian;

% Check consistency of the gradient and the Hessian. Useful if you
% adapt this example for a new cost function and you would like to make
% sure there is no mistake.
% warning('off', 'manopt:fixedrankembeddedfactory:exp');
% checkgradient(problem); pause;
% checkhessian(problem); pause;

% Compute an initial guess. Points on the manifold are represented as
% structures with three fields: U, S and V. U and V need to be
% orthonormal, S needs to be diagonal.
[U, S, V] = svds(PA, k);
X0.U = U;
X0.S = S;
X0.V = V;

% Minimize the cost function using Riemannian trust-regions, starting
% from the initial guess X0.
X = trustregions(problem, X0);

% The reconstructed matrix is X, represented as a structure with fields
% U, S and V.
L = X.U*X.S*X.V'; % low-rank
S = A - L; % sparse

fprintf('||X-A||_F = %g\n', norm(L - A, 'fro'));

% Alternatively, we could decide to use a solver such as
% steepestdescent or conjugategradient. These solvers need to solve a
% line-search problem at each iteration. Standard line searches in
% Manopt have generic purpose systems to do this. But for the problem
% at hand, it so happens that we can rather accurately guess how far
% the line-search should look, and it would be a waste to not use that.
% Look up the paper referenced above for the mathematical explanation
% of the code below.
%
% To tell Manopt about this special information, we specify the
% linesearch hint function in the problem structure. Notice that this
% is not the same thing as specifying a linesearch function in the
% options structure.
%
% Both the SD and the CG solvers will detect that we
% specify the hint function below, and they will use an appropriate
% linesearch algorithm by default, as a result. Typically, they will
% try the step t*H first, then if it does not satisfy an Armijo
% criterion, they will decrease t geometrically until satisfaction or
% failure.
%
% Just like the cost, egrad and ehess functions, the linesearch
% function could use a store structure if you like. The present code
% does not use the store structure, which means quite a bit of the
% computations are made redundantly, and as a result a better method
% could appear slower. See the Manopt tutorial about caching when you
% are ready to switch from a proof-of-concept code to an efficient
% code.
%
% The inputs are X (a point on the manifold) and H, a tangent vector at
% X that is assumed to be a descent direction. That is, there exists a
% positive t such that f(Retraction_X(tH)) < f(X). The function below
% is supposed to output a "t" that it is a good "guess" at such a t.
% problem.linesearch = @linesearch_helper;

% Notice that for this solver, the Hessian is not needed.
% [Xcg, xcost, info, options] = conjugategradient(problem, X0);
%
% fprintf('Take a look at the options that CG used:\n');
% disp(options);
% fprintf('And see how many trials were made at each line search call:\n');
% info_ls = [info.linesearch];
% disp([info_ls.costevals]);
%
% fprintf('Try it again without the linesearch helper.\n');

% Remove the linesearch helper from the problem structure.
% problem = rmfield(problem, 'linesearch');
%
% [Xcg, xcost, info, options] = conjugategradient(problem, X0);
%
% fprintf('Take a look at the options that CG used:\n');
% disp(options);
% fprintf('And see how many trials were made at each line search call:\n');
% info_ls = [info.linesearch];
% disp([info_ls.costevals]);

% If the problem has a small enough dimension, we may (for analysis
% purposes) compute the spectrum of the Hessian at a point X. This may
% help in studying the conditioning of a problem. If you don't provide
% the Hessian, Manopt will approximate the Hessian with finite
% differences of the gradient and try to estimate its "spectrum" (it's
% not a proper linear operator). This can give some intuition, but
% should not be relied upon.
% if problem.M.dim() < 100
%   fprintf('Computing the spectrum of the Hessian...');
%   s = hessianspectrum(problem, X);
%   hist(s);
% end

end


classdef constrained_quadratic
% This file defines constrained quadratic problem class
%
% Inputs:
%           Q       a positive definite matrix of size nxn
%           p       a column vector of size n
%           A       a matrix of size nxm
%           b       a column vector of size m
%           G       a matrix of size nxp
%           h       a column vector of size p
%
% Output:
%           Problem     problem instance.
%
%
% The problem of interest is defined as
%
%           min f(x) = 1/2 * x^T * Q * x - p^T * x.
%           s.t.
%           A * x = b, G * x <= h
%
% "w" is the model parameter of size d vector.
%
%
% This file is part of GDLibrary.
%
% Created by H.Kasai on Oct. 22, 2020
% Modified by H.Kasai on Dec. 13, 2022

    properties
        d;
        n;
        name;
        dim;
        samples;
        Q;
        p;
        A;
        b;
        G;
        h;

        prox_flag;

    end

    methods
        function obj = constrained_quadratic(Q, p, A, b, G, h)
            obj.d = length(p);
            obj.n = obj.d;

            obj.name = 'constrained_quadratic';
            obj.dim = obj.d;
            obj.samples = obj.n;
            obj.Q = Q;
            obj.p = p;
            obj.A = A;
            obj.b = b;
            obj.G = G;
            obj.h = h;

            obj.prox_flag = true;
        end

        function v = prox(obj, w, t)
            % project y onto the feasible set Ax <= b, i.e. nearest feasible point x to y
            % minimize 0.5||x-y||^2
            % s.t.     Ax <= b

            % equivalent to
            % min J   = 0.5 x'x - x'y + 0.5 y'y
            % st. Ax <= b

            %n = length(y);
            H = eye(obj.d);
            w0 = w;
            qp_options = optimset('Display','off');
            v = quadprog(H,-w,obj.G,obj.h,[],[],[],[],w0, qp_options);
        end

        function f = cost(obj, x)

            f = 1/2 * x' * obj.Q * x - obj.p' * x;
        end

        function g = grad(obj, x)

            g = obj.Q * x - obj.p;

        end

        function g = full_grad(obj, x)

            g = obj.grad(x);

        end

        function h = hess(obj, x)

            h = obj.Q;

        end

        function h = full_hess(obj, x)

            h = obj.hess(x);

        end
    end

end


function [res,R,p,sigma,Ms] = TransformK(p,rho,sigma,Ms)
%TRANSFORMK   Transformation res+R+sum(ps) = rho+sum(p)
%
%   [res,R,ps] = TransformK(p,rho)
%
%Input vector p may be single or double precision.
%
%Implements Algorithm 6.2 from
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation II:
%    Sign, K-fold Faithful and Rounding to Nearest, Siam J. Sci. Comput.,
%    31(2):1269-1302, 2008.
%Requires (4m+3)n flops for m executions of repeat-until loop if
%  sigma,Ms not given, otherwise (4m+1)n flops.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
%

  if nargin==4                          % sigma and Ms already known
    kPhi = 0;
    [tau1,tau2,p,sigma,Ms] = Transform(p,rho,kPhi,sigma,Ms);
  else                                  % standard call
    [tau1,tau2,p,sigma,Ms] = Transform(p,rho);
  end
  res = tau1 + ( tau2 + sum(p) );
  R = tau2 - ( res - tau1 );


function [ iou ] = IOU( xi, X )
%IOU Intersection over union similarity measure for two bounding boxes.
%   Each sample must be composed of x = [ULx ULy BRx BRy]

    % Check area on xi
    xi_height = (xi(4) - xi(2) + 1);
    xi_width = (xi(3) - xi(1) + 1);
    xi_area = xi_height * xi_width;

    nSamples = size(X,1);
    iou = zeros(nSamples, 1);
    for j = 1:nSamples

        xj = X(j,:);

        % Check area on xj
        xj_height = (xj(4) - xj(2) + 1);
        xj_width = (xj(3) - xj(1) + 1);
        xj_area = xj_height * xj_width;

        % Check intersection
        count_intersect = rectint([xi(2), xi(1), xi_height, xi_width], [xj(2), xj(1), xj_height, xj_width]);

        % Calculate overlap score (intersection over union)
        iou(j) = count_intersect / (xi_area + xj_area - count_intersect);
    end

end


function test_int_2d_test02 ( )

%*****************************************************************************80
%
%% TEST_INT_2D_TEST02 applies a product of composite midpoint rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 July 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_INT_2D_TEST02\n' );
  fprintf ( 1, '  Apply a product of composite midpoint rules.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Repeatedly multiply the number of points by 16.\n' );

  problem_num = p00_problem_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1,'   Problem      Points      Approx          Error\n' );

  for problem = 1 : problem_num

    fprintf ( 1, '\n' );

    nx = 1;
    ny = 1;

    for i = 1 : 12

      n = nx * ny;

      x = zeros ( 2, n );

      [ a, b ] = p00_lim ( problem );

      k = 0;

      for ix = 1 : nx

        xval = ( ( 2 * nx - 2 * ix + 1 ) * a(1)   ...
               + (          2 * ix - 1 ) * b(1) ) ...
               / ( 2 * nx              );

        for iy = 1 : ny

          yval = ( ( 2 * ny - 2 * iy + 1 ) * a(2)   ...
                 + (          2 * iy - 1 ) * b(2) ) ...
                 / ( 2 * ny              );

          k = k + 1;
          x(1,k) = xval;
          x(2,k) = yval;

        end

      end

      volume = prod ( b(1:2) - a(1:2) );

      fx = p00_fun ( problem, n, x );

      quad = volume * sum ( fx(1:n) ) / n;

      exact = p00_exact ( problem );

      error = abs ( quad - exact );

      fprintf ( 1, '  %8d  %10d  %14e  %14e\n', ...
        problem, n, quad, error );

      nx = nx * 2;
      ny = ny * 2;

    end

    fprintf ( 1, '  %8d       Exact  %14e\n', ...
      problem, exact );

  end

  return
end


function fx = exp_cordic ( x, n )

%*****************************************************************************80
%
%% EXP_CORDIC evaluates the exponential function using the CORDIC method.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frederick Ruckdeschel,
%    BASIC Scientific Subroutines,
%    Volume II,
%    McGraw-Hill, 1980,
%    ISBN: 0-07-054202-3,
%    LC: QA76.95.R82.
%
%    Pitts Jarvis,
%    Implementing CORDIC Algorithms,
%    Dr Dobb's Journal,
%    October 1990.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Input, integer N, the number of steps to take.
%
%    Output, real FX, the exponential of X.
%
%  Local Parameters:
%
%    Local, real A(1:25) = exp ( (1/2)^(1:25) );
%
  a_length = 25;

  a = [ ...
    1.648721270700128, ...
    1.284025416687742, ...
    1.133148453066826, ...
    1.064494458917859, ...
    1.031743407499103, ...
    1.015747708586686, ...
    1.007843097206488, ...
    1.003913889338348, ...
    1.001955033591003, ...
    1.000977039492417, ...
    1.000488400478694, ...
    1.000244170429748, ...
    1.000122077763384, ...
    1.000061037018933, ...
    1.000030518043791, ...
    1.0000152589054785, ...
    1.0000076294236351, ...
    1.0000038147045416, ...
    1.0000019073504518, ...
    1.0000009536747712, ...
    1.0000004768372719, ...
    1.0000002384186075, ...
    1.0000001192092967, ...
    1.0000000596046466, ...
    1.0000000298023228 ];
  e = 2.718281828459045;

  x_int = floor ( x );
%
%  Determine the weights.
%
  poweroftwo = 0.5;
  z = x - x_int;

  for i = 1 : n
    w(i) = 0.0;
    if ( poweroftwo < z )
      w(i) = 1.0;
      z = z - poweroftwo;
    end
    poweroftwo = poweroftwo / 2.0;
  end
%
%  Calculate products.
%
  fx = 1.0;

  for i = 1 : n

    if ( i <= a_length )
      ai = a(i);
    else
      ai = 1.0 + ( ai - 1.0 ) / 2.0;
    end

    if ( 0.0 < w(i) )
      fx = fx * ai;
    end

  end
%
%  Perform residual multiplication.
%
  fx = fx             ...
    * ( 1.0 + z       ...
    * ( 1.0 + z / 2.0 ...
    * ( 1.0 + z / 3.0 ...
    * ( 1.0 + z / 4.0 ))));
%
%  Account for factor EXP(X_INT).
%
  if ( x_int < 0 )

    for i = 1 : -x_int
      fx = fx / e;
    end

  else

    for i = 1 : x_int
      fx = fx * e;
    end

  end

  return
end


function newpt=rotatevec3d(pt,v1,u1,p0)
%
% newpt=rotatevec3d(pt,v1,u1,p0)
%
% rotate 3D points from one Cartesian coordinate system to another
%
% author: Qianqian Fang, <fangq at nmr.mgh.harvard.edu>
%
% input:
%   pt: 3D points defined in a standard Cartesian system where a unitary
%       z-vector is (0,0,1), 3 columns for x, y and z
%   v1: the unitary z-vector for the target coordinate system
%   u1: the unitary z-vector for the source coordinate system, if ignored,
%       u1=(0,0,1)
%   p0: offset of the new coordinate system, if ignored, p0=(0,0,0)
%
% output:
%   newpt: the transformed 3D points
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<2)
    error('you must give at least pt and v1');
end
if(nargin==2)
    u1=[0,0,1];
end
if(nargin<=3)
    p0=[0,0,0];
end

u1=u1/norm(u1);
v1=v1/norm(v1);

[R,s]=rotmat2vec(u1,v1);
newpt=(R*pt'*s)';

if(nargin>3)
  p0=p0(:)';
  newpt=newpt+repmat(p0,size(newpt,1),1);
end


function [out1, out2] = gpS00(X, P);
%%function [out1, out2] = gpS00(X, {input, target, test});

% gpS00: Gaussian process regression with "squared negative exponential"
% covariance function and independent Gaussian noise model. Two modes are
% possible: training and prediction: if no test data are given, the function
% returns minus the log likelihood and its partial derivatives with respect to
% the hyperparameters; this mode is used to fit the hyperparameters. If test
% data are given, then (marginal) Gaussian predictions are computed, whose mean
% and (noise free) variance are returned.
%
% usage: [fX dfX] = gpS00(X, input, target)
%    or: [mu S2]  = gpS00(X, input, target, test)
%
% where:
%
%   X      is a (column) vector (of size D+2) of hyperparameters
%   input  is a n by D matrix of training inputs
%   target is a (column) vector (of size n) of targets
%   test   is a nn by D matrix of test inputs
%   fX     is the returned value of minus log likelihood
%   dfX    is a (column) vector (of size D+2) of partial derivatives
%            of minus the log likelihood wrt each of the hyperparameters
%   mu     is a (column) vector (of size nn) of prediced means
%   S2     is a (column) vector (of size nn) of predicted variances
%
% where D is the dimension of the input. The form of the covariance function is
%
% C(x^p,x^q) = v^2 * exp[-(x^p - x^q)'*inv(P)*(x^p - x^q)/2]
%            + u^2 * delta_{p,q}
%
% where the first term is the squared negative exponential and the second term
% with the kronecker delta is the noise contribution. The P matrix is diagonal
% with "Automatic Relevance Determination" (ARD) or "input length scale"
% parameters w_1^2,...,w_D^2; The hyperparameter v is the "signal std dev" and
% u is the "noise std dev". All hyperparameters are collected in the vector X
% as follows:
%
% X = [ log(w_1)
%       log(w_2)
%        .
%       log(w_D)
%       log(v)
%       log(u) ]
%
% Note: the reason why the log of the parameters are used in X is that this
% often leads to a better conditioned (and unconstrained) optimization problem
% than using the raw hyperparameters themselves.
%
% This function can conveniently be used with the "minimize" function to train
% a Gaussian process:
%
% [X, fX, i] = minimize(X, 'gpS00', length, input, target)
%
% See also: minimize, hybrid
%
% (C) Copyright 1999 - 2003, Carl Edward Rasmussen (2003-08-08).

input=P{1};
target=P{2};
if length(P) == 3
	test=P{3};
	testmode=1;
else
	testmode=0;
end

[n, D] = size(input);         % number of examples and dimension of input space
input = input ./ repmat(exp(X(1:D))',n,1);

% first, we write out the covariance matrix Q

warning off;

Q = zeros(n,n);
for d = 1:D
  Q = Q + (repmat(input(:,d),1,n)-repmat(input(:,d)',n,1)).^2;
end
Q = exp(2*X(D+1))*exp(-0.5*Q);

if ~testmode   % if no test cases, we compute the negative log likelihood ...

  W = (Q+exp(2*X(D+2))*eye(n))\eye(n);               % W is inv (Q plus noise term)
  invQt = W*target;                               % don't compute determinant..
  [C,flag]=chol(Q+exp(2*X(D+2))*eye(n));
  logdetQ = 2*sum(log(diag(C)));        % ..directly
  out1 = 0.5*logdetQ + 0.5*target'*invQt + 0.5*n*log(2*pi);

  % ... and its partial derivatives

  out2 = zeros(D+2,1);                  % set the size of the derivative vector
  W = W-invQt*invQt';
  Q = W.*Q;
  for d = 1:D
    out2(d) = ...
            sum(sum(Q.*(repmat(input(:,d),1,n)-repmat(input(:,d)',n,1)).^2))/2;
  end
  out2(D+1) = sum(sum(Q));
  out2(D+2) = trace(W)*exp(2*X(D+2));

else                    % ... otherwise compute (marginal) test predictions ...

  [nn, D] = size(test);     % number of test cases and dimension of input space
  test = test ./ repmat(exp(X(1:D))',nn,1);

  a = zeros(n, nn);    % compute the covariance between training and test cases
  for d = 1:D
    a = a + (repmat(input(:,d),1,nn)-repmat(test(:,d)',n,1)).^2;
  end
  a = exp(2*X(D+1))*exp(-0.5*a);

  % ... write out the desired terms

  if nargout == 1
    out1 = a'*((Q+exp(2*X(D+2))*eye(n))\target);              % predicted means
  else
    invQ = inv(Q+exp(2*X(D+2))*eye(n));
    out1 = a'*(invQ*target);                                  % predicted means
    out2 = exp(2*X(D+1)) - sum(a.*(invQ*a),1)'; % predicted noise-free variance
  end

end


clear all; close all; clc

n=100; L=4;
x=linspace(0,L,n);
f=(x.^2).';  % parabola with 100 data points

M=20;  % polynomical degree
for j=1:M
  phi(:,j)=(x.').^(j-1);  % build matrix A
end

for j=1:4
    fn=(x.^2+0.1*randn(1,n)).';
    an=pinv(phi)*fn; fna=phi*an;  % least-square fit
    En=norm(f-fna)/norm(f);
    subplot(4,2,4+j),bar(an,'FaceColor',[.6 .6 .6],'EdgeColor','k')
end

subplot(2,1,1), plot(x,f,'k'), hold on

%% different regressions
subplot(2,1,1)
lambda=0.1; phi2=phi(:,2:end);
for jj=1:100
  f=(x.^2+0.2*randn(1,n)).';
  a1=pinv(phi)*f; f1=phi*a1; E1(jj)=norm(f-f1)/norm(f);
  a2=phi\f; f2=phi*a2; E2(jj)=norm(f-f2)/norm(f);
  [a3,stats]=lasso(phi,f,'Lambda',lambda); f3=phi*a3; E3(jj)=norm(f-f3)/norm(f);
  [a4,stats]=lasso(phi,f,'Lambda',lambda,'Alpha',0.8); f4=phi*a4; E4(jj)=norm(f-f4)/norm(f);
a5=robustfit(phi2,f);f5=phi*a5;E5(jj)=norm(f-f5)/norm(f);
a6=ridge(f,phi2,0.5,0);f6=phi*a6;E6(jj)=norm(f-f6)/norm(f);

  A1(:,jj)=a1;A2(:,jj)=a2;A3(:,jj)=a3;A4(:,jj)=a4;A5(:,jj)=a5;A6(:,jj)=a6;
  plot(x,f), hold on
end
Err=[E1; E2; E3; E4; E5; E6];
Err2=[E1; E2; E3; E4; E5];

figure(2)
subplot(3,2,1), boxplot(A1.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})
subplot(3,2,2), boxplot(A2.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})
subplot(3,2,3), boxplot(A3.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})
subplot(3,2,4), boxplot(A4.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})
subplot(3,2,5), boxplot(A5.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})
subplot(3,2,6), boxplot(A6.'), set(gca,'Xtick',1:20,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20'})

figure(3)
subplot(3,3,1), boxplot(Err.'), axis([0 7 0.02 0.07])

%%
clear phi
M=10;
En=zeros(100,M);
for jj=1:M
  for j=1:jj
    phi(:,j)=(x.').^(j-1);
  end
  f=(x.^2).';
  for j=1:100
    fn=(x.^2+0.1*randn(1,n)).';
    an=pinv(phi)*fn; fna=phi*an;
    En(j,jj)=norm(f-fna)/norm(f);
  end
end

figure(3)
subplot(3,3,2)
boxplot(En)

subplot(3,3,3)
boxplot(En), axis([0.5 10.5 0 0.008])


function tetrahedron_unit_monomial_test ( degree_max )

%*****************************************************************************80
%
%% TETRAHEDRON_UNIT_MONOMIAL_TEST tests TETRAHEDRON_UNIT_MONOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DEGREE_MAX, the maximum total degree of the
%    monomials to check.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_UNIT_MONOMIAL_TEST\n' );
  fprintf ( 1, '  For the unit tetrahedron,\n' );
  fprintf ( 1, '  TETRAHEDRON_UNIT_MONOMIAL returns the exact value of the\n' );
  fprintf ( 1, '  integral of X^ALPHA Y^BETA Z^GAMMA\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Volume = %f\n', tetrahedron_unit_volume ( ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     ALPHA      BETA     GAMMA      INTEGRAL\n' );
  fprintf ( 1, '\n' );

  for alpha = 0 : degree_max
    expon(1) = alpha;
    for beta = 0 : degree_max - alpha
      expon(2) = beta;
      for gamma = 0 : degree_max - alpha - beta
        expon(3) = gamma;
        value = tetrahedron_unit_monomial ( expon );
        fprintf ( 1, '  %8d  %8d  %8d  %14.6e\n', expon(1:3), value );
      end
    end
  end

  return
end


function C = ProdKL(A,B,K,L)
%PRODKL       Matrix product in approximately K-fold precision stored in L results
%
%   C = ProdKL(A,B,K,L)
%
% Input A or B or both may be cell arrays, 1 <= L <= K.
% Output C is cell array iff L>1, default is L=1.
% Simple application of SumKL. All input must be real.
% Relative error of the result is of order  eps^L + eps^K cond(product) , see
%   S.M. Rump: Inversion of extremely ill-conditioned matrices in floating-point,
%      Japan J. Indust. Appl. Math. (JJIAM), 26:249-277, 2009.
%
%Reference implementation! Slow due to interpretation!
%

% written  02/17/08     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, default L=1
%

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

  if nargin==3
    L = 1;
  end

  if iscell(A)              % input A cell array
    n = size(A{1},1);
    lenA = length(A);
    AA = zeros(n,lenA*n);
    for i=1:lenA
      AA(:,(i-1)*n+1:i*n) = A{i};
    end
    if iscell(B)            % both A and B cell arrays
      lenB = length(B);
      m = lenA*n;
      AAA = zeros(n,lenB*m);
      BBB = zeros(lenB*m,n);
      for i=1:lenB
        AAA(:,(i-1)*m+1:i*m) = AA;
        BBB((i-1)*m+1:i*m,:) = repmat(B{i},lenA,1);
      end
      A = AAA;
      B = BBB;
    else
      A = AA;
      B = repmat(B,lenA,1);
    end
  else
    n = size(A,1);
  end
  if iscell(B)              % input B cell array
    lenB = length(B);
    A = repmat(A,1,lenB);
    BB = zeros(lenB*n,n);
    for i=1:lenB
      BB((i-1)*n+1:i*n,:) = B{i};
    end
    B = BB;
  end
  if L==1
    C = zeros(n);
  else
    for i=1:L
      C{i} = zeros(n);
    end
  end
  for i=1:n
    for j=1:n
      [x,y] = TwoProduct(A(i,:),B(:,j)');
      res = SumKL([x y],K,L);
      if L==1
        C(i,j) = res;
      else
        for k=1:L
          C{k}(i,j) = res(k);
        end
      end
    end
  end

  if rndold
    setround(rndold)
  end


function  A = umADEIGSwhole(Ncell,p)

Dhat = zeros(p+1);
for n=0:p
    for m=n+1:2:p
        Dhat(n+1,m+1) =  (2*n+1);
    end
end

% M = int_{-1}^{1} L_n(x)*L_m(x) dx (note dx=1 assumed)
M = diag(2./(2*(0:p)+1));

% D_nm = int_{-1}^{1}  L_n(x)*L_m(x)*dx
D = M*Dhat;

% F_nm = L_n(-1)*L_m(-1)
F = (-1).^[0:p]'*(-1).^[0:p];

% G_nm = -L_n(-1)*L_m(1)
G = -(-1).^[0:p]'*ones(1,p+1);

% build entire matrix
Ntotal = Ncell*(p+1);
A    = zeros(Ntotal);

for cell = 1:Ncell
    id1 = ((cell-1)*(p+1)+1):cell*(p+1);
    A(id1,id1) = (1/2)*M\(D+F);

    if(cell~=1)
        id2 = id1-(p+1);
    else
        id2 = id1+(Ncell-1)*(p+1);
    end

    A(id1, id2) = (1/2)*M\G;
end

eA = eig(A);

plot(-real(eA), -imag(eA), 'bo');


function [rf, vf] = twobody2 (mu, tau, ri, vi)

% solve the two body initial value problem

% Shepperd's method

% input

%  mu  = gravitational constant (km**3/sec**2)
%  tau = propagation time interval (seconds)
%  ri  = initial eci position vector (kilometers)
%  vi  = initial eci velocity vector (kilometers/second)

% output

%  rf = final eci position vector (kilometers)
%  vf = final eci velocity vector (kilometers/second)

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tolerance = 1.0d-12;

u = 0;

imax = 20;

umax = +realmax;

umin = -realmax;

orbits = 0;

tdesired = tau;

threshold = tolerance * abs (tdesired);

r0 = norm(ri);

n0 = dot(ri, vi);

beta = 2 * (mu / r0) - dot(vi, vi);

if (beta ~= 0)
    umax = +1 / sqrt(abs(beta));

    umin = -1 / sqrt(abs(beta));
end

if (beta > 0)
    orbits = beta * tau - 2 * n0;

    orbits = 1 + (orbits * sqrt(beta)) / (pi * mu);

    orbits = floor (orbits / 2);
end

for i = 1:1:imax

    q = beta * u * u;

    q = q / (1 + q);

    n =  0;

    r = 1;

    l = 1;

    s = 1;

    d = 3;

    gcf = 1;

    k = -5;

    gold = 0;

    while (gcf ~= gold)

        k = -k;

        l = l + 2;

        d = d + 4 * l;

        n = n + (1 + k) * l;

        r = d / (d - n * r * q);

        s = (r - 1) * s;

        gold = gcf;

        gcf  = gold + s;

    end

    h0 = 1 - 2 * q;

    h1 = 2 * u * (1 - q);

    u0 = 2 * h0 * h0 - 1;

    u1 = 2 * h0 * h1;

    u2 = 2 * h1 * h1;

    u3 = 2 * h1 * u2 * gcf / 3;

    if (orbits ~= 0)
        u3 = u3 + 2 * pi * orbits / (beta * sqrt(beta));
    end

    r1 = r0 * u0 + n0 * u1 + mu * u2;

    dt = r0 * u1 + n0 * u2 + mu * u3;

    slope = 4 * r1 / (1 + beta * u * u);

    terror = tdesired - dt;

    if (abs (terror) < threshold)
        break;
    end;

    if ((i > 1) && (u  ==  uold) )
        break;
    end

    if ((i > 1) && (dt == dtold) )
        break;
    end

    uold  = u;

    dtold = dt;

    ustep = terror / slope;

    if (ustep > 0)
        umin = u;

        u = u + ustep;

        if (u > umax)
            u = (umin + umax) / 2;
        end
    else
        umax = u;

        u = u + ustep;

        if (u < umin)
            u = (umin + umax) / 2;
        end
    end

    if (i == imax)
        fprintf('\n\nmax iterations in twobody2 function');
    end

end

usaved = u;

f = 1.0 - (mu / r0) * u2;

gg = 1.0 - (mu / r1) * u2;

g  =  r0 * u1 + n0 * u2;

ff = -mu * u1 / (r0 * r1);

% final position and velocity vectors

for i = 1:1:3
    rf(i) = f  * ri(i) + g  * vi(i);

    vf(i) = ff * ri(i) + gg * vi(i);
end


function value = p27_exact ( dim_num )

%*****************************************************************************80
%
%% P27_EXACT returns the exact integral for problem 27.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  r = 0.0;
  r = p27_r8 ( 'G', 'R', r );

  c = [];
  c = p27_r8vec ( 'G', 'C', dim_num, c );

  value = 2.0^dim_num * ...
    cos ( 2.0 * pi * r + 0.5 * sum ( c(1:dim_num) ) ) * ...
    prod ( sin ( 0.5 * c(1:dim_num) ) ./ c(1:dim_num ) );

  return
end


function [G,H] = mass_spring_gradient_hessian(V,E,k,U)

  % Compute the gradient and hessian of a mass-spring system defined for (V,E)
  % with the stiffness k and displacement U
  % Inputs:
  %   V  #V by 2 list of rest curve vertex positions
  %   E  #E by 2 list of edge indices
  %   k  scalar spring stiffness parameter
  %   U  #V by 2 list of deformed curve vertex displacements from the rest pose
  % Outputs:
  %   G  #V*2 list of gradient vectors per vertex defined at the current deformed position
  %   H  #V*2 by #V*2 hessian matrix defined at the current deformed position

  assert(size(E,2) == 2);
  U = reshape(U,size(V));

  % rest edge lengths
  LV = edge_lengths(V,E);
  LU = edge_lengths(U,E);
  f = k*0.5*sum((LV-LU).^2);

  if nargout==0
    return;
  end

  I = E(:,2);
  J = E(:,1);
  T0 = U(I,:) - U(J,:);
  t1 = normrow(T0);

  GI = ((t1-LV)./t1).*T0;
  GJ = -GI;

  dim = size(V,2);
  G = k*full(sparse([repmat(I,1,dim) repmat(J,1,dim)],repmat(1:dim,size(E,1),2),[GI GJ],size(V,1),dim));
  G = G(:); % to match the dimension: not sure if correct

  if nargout==1
    return;
  end

  % Outer product
  vec = @(X) X(:);
  T2 = T0(:,vec(repmat(1:dim,dim,1))').*T0(:,vec(repmat(1:dim,1,dim))');
  t3 = t1 - LV;

  HIJ = (t3./(t1.^3) - 1./(t1.^2)).*T2 - t3./t1.*(vec(eye(dim))');
  HII = -HIJ;
  HJJ = -HIJ;
  HJI = HIJ;
  n = size(V,1);
  H = sparse(n*dim,n*dim);
  for ii = 1:dim
    for jj = 1:dim
      oi = (ii-1)*dim+jj;
      Hiijj = sparse( ...
        [I I J J],[I J I J],[HII(:,oi) HIJ(:,oi) HJI(:,oi) HJJ(:,oi)],n,n);
      H(((ii-1)*n)+(1:n),((jj-1)*n)+(1:n)) = Hiijj;
    end
  end
  H = k*H;

end


function net = glm(nin, nout, outfunc, prior, beta)
%GLM	Create a generalized linear model.
%
%	Description
%
%	NET = GLM(NIN, NOUT, FUNC) takes the number of inputs and outputs for
%	a generalized linear model, together with a string FUNC which
%	specifies the output unit activation function, and returns a data
%	structure NET. The weights are drawn from a zero mean, isotropic
%	Gaussian, with variance scaled by the fan-in of the output units.
%	This makes use of the Matlab function RANDN and so the seed for the
%	random weight initialization can be  set using RANDN('STATE', S)
%	where S is the seed value. The optional argument ALPHA sets the
%	inverse variance for the weight initialization.
%
%	The fields in NET are
%	  type = 'glm'
%	  nin = number of inputs
%	  nout = number of outputs
%	  nwts = total number of weights and biases
%	  actfn = string describing the output unit activation function:
%	      'linear'
%	      'logistic'
%	      'softmax'
%	  w1 = first-layer weight matrix
%	  b1 = first-layer bias vector
%
%	NET = GLM(NIN, NOUT, FUNC, PRIOR), in which PRIOR is a scalar, allows
%	the field  NET.ALPHA in the data structure NET to be set,
%	corresponding  to a zero-mean isotropic Gaussian prior with inverse
%	variance with value PRIOR. Alternatively, PRIOR can consist of a data
%	structure with fields ALPHA and INDEX, allowing individual Gaussian
%	priors to be set over groups of weights in the network. Here ALPHA is
%	a column vector in which each element corresponds to a  separate
%	group of weights, which need not be mutually exclusive.  The
%	membership of the groups is defined by the matrix INDEX in which the
%	columns correspond to the elements of ALPHA. Each column has one
%	element for each weight in the matrix, in the order defined by the
%	function GLMPAK, and each element is 1 or 0 according to whether the
%	weight is a member of the corresponding group or not.
%
%	NET = GLM(NIN, NOUT, FUNC, PRIOR, BETA) also sets the  additional
%	field NET.BETA in the data structure NET, where beta corresponds to
%	the inverse noise variance.
%
%	See also
%	GLMPAK, GLMUNPAK, GLMFWD, GLMERR, GLMGRAD, GLMTRAIN
%

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

net.type = 'glm';
net.nin = nin;
net.nout = nout;
net.nwts = (nin + 1)*nout;

outtfns = {'linear', 'logistic', 'softmax'};

if sum(strcmp(outfunc, outtfns)) == 0
  error('Undefined activation function. Exiting.');
else
  net.outfn = outfunc;
end

if nargin > 3
  if isstruct(prior)
    net.alpha = prior.alpha;
    net.index = prior.index;
  elseif size(prior) == [1 1]
    net.alpha = prior;
  else
    error('prior must be a scalar or structure');
  end
end

net.w1 = randn(nin, nout)/sqrt(nin + 1);
net.b1 = randn(1, nout)/sqrt(nin + 1);

if nargin == 5
  net.beta = beta;
end


% Nonnegative Matrix Factorization with Quadratic Programming
% The function iptrnr(Y,X,no_iter,lambdaA,rho,eta,epsil_A) returns
% the matrix A for the constrained system of linear equations: AX = Y

function A = iptrnr(Y,X,no_iter,lambdaA,rho,eta,epsil_A)

% INPUTS:
% > Y:        observation matrix [I by K] of mixed signals (images)
% > X:        source component matrix [J by K]
% > no_iter:  number of inner iterations
% > lambdaA:  Tikhonov regularization parameter
% > rho:      auxiliary parameter for defining the log barrier parameter
% > eta:      parameter in stopping rule for iterations
% > epsil_A:  threshold for active-set variables

% OUTPUT:
% > A:        mixing matrix [I by J]

[J,K] = size(X); [I,Ky] = size(Y); M = I*J;

% Initialization
A = ones(I,J);
Q_bar = spalloc(M,M,M*J); Q_tilde = spalloc(M,M,M*J);

B = X*X'; C_bar = X*Y';
Q_bar = kron(speye(I),B); % Hessian of D(Y||AX) with respect to A
Go = (C_bar - B*A'); % Gradient of D(Y||AX) with respect to A^T
At = A';
theta = -(rho/(M))*abs(Go(:)'*At(:)); % initialization for
                                      %log barrier parameter
l = 0;
while l < no_iter % inner iterations

    l = l + 1;
    a = At(:);
    active = find(a < epsil_A); % active set
    inactive = find(a >= epsil_A); % inactive set

    if ~isempty(active)
        a_tilde = a;             a_tilde(active) = [];
        N = length(a_tilde);
        c_tilde = C_bar(:);      c_tilde(active) = [];
        c_tilde = 2*theta*1./a_tilde - c_tilde;
        Q_tilde = Q_bar;
        Q_tilde(:,active) = [];  Q_tilde(active,:) = [];
        Q_tilde = Q_tilde - theta*spdiags(1./a_tilde.^2,0,N,N);
    else
        a_tilde = a;
        Q_tilde = Q_bar - theta*spdiags(1./a.^2,0,M,M) + lambdaA*speye(M);
        c_tilde = 2*theta*(1./a) - C_bar(:);
    end

     [Q,R] = qr(Q_tilde,-c_tilde); % Q-less QR factorization
     z_tilde = R\Q; % Gaussian elimination
     h_tilde = z_tilde - a_tilde;
     beta = min(1, 0.9995*min(a_tilde./abs(h_tilde)));
     a_tilde = a_tilde + beta*h_tilde;

     theta_tilde = theta*(2*(1./a_tilde) - (1./a_tilde.^2).*z_tilde);
     theta = ( theta_tilde'*a_tilde);

     if ~isempty(active)
        an = zeros(M,1);   an(inactive) = a_tilde;   a = an;
     else
        a = a_tilde;
     end

     if abs(theta) < eta*norm(a) % stopping rule
        break
     end
     theta = (rho/sqrt(M))*theta;
     At = reshape(a,J,I);

end
A = At';


function f = dtlz4a(x)

alpha = 10;

%% g function
sum = 0;
for i = 3:8
sum = sum + (x(:,i)-0.5)^2;
end
g = sum;

%% cost functions
f(:,1) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*cos(((x(:,2)^alpha)*pi)/2);
f(:,2) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*sin(((x(:,2)^alpha)*pi)/2);
f(:,3) = (1+g)*sin(((x(:,1)^alpha)*pi)/2);
end


function PQ = paddedsize(AB,CD,PARAM)
%PADDEDSIZE Computes padded sizes useful for FFT-based filtering.
%   PQ = PADDEDSIZE(AB), where AB is a two-element vector, computes the
%   two-element vector PQ = 2*AB.
%
%   PQ = PADDEDSIZE(AB,'PWR2') computes the vector PQ such that PQ(1) =
%   PQ(2) = 2^nextpow2(2*m), where m is MAX(AB).
%
%   PQ = PADDEDSIZE(AB,CD), where AB and CD are two-element size vectors,
%   computes the two-element size vector PQ. The elements of PQ are the
%   smallest even integers greater than or equal to AB + CD - 1.
%
%   PQ = PADDEDSIZE(AB,CD,'PWR2') computes the vector PQ such that PQ(1)
%   = PQ(2) = 2^nextpow2(2*m), where m is MAX([AB,CD]).
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

if nargin == 1
   PQ  = 2*AB;
elseif nargin == 2 && ~ischar(CD)
   PQ = AB + CD - 1;
   PQ = 2*ceil(PQ/2);
elseif nargin == 2
   m = max(AB); % Maximum dimension.

   % Find power-of-2 at least twice m.
   P = 2^nextpow2(2*m);
   PQ = [P,P];
elseif (nargin == 3) && strcmpi(PARAM,'pwr2')
   m = max([AB,CD]); % Maximum dimension.
   P = 2^nextpow2(2*m);
   PQ = [P,P];
else
   error('Wrong number of inputs.')
end


function [weight] = anticor_expert(data, weight_o, w)
% This program generates portfolio for a specified parameter setting.
% Anticor expert
% while the original version is based on loop, which is slow in matlab,
% I replace the code using matrix operations
% Most previous experiments run based on the commented version, at the end
% of the file.
%
% function [weight] = anticor_expert(data, weight_o, w)
%
% weight: experts portfolio, used for next rebalance/combination
%
% data: market sequence vectors
% weight_o: last portfolio, also can be last price relative adjusted.
% w: window size for the experts
%
% Example: [weight] = anticor_expert(data, weight_o, 30)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[T, N]=size(data);
weight = weight_o;

% 1. Return the current portfolio if t < 2w
if (T >= 2*w),
    % 2. Compute LX1, LX2
    LX1 = log(data((T-2*w+1):(T-w),:));
    LX2 = log(data((T-w+1):T,:));

    % 2. Compute averages of LX1 and LX2
    mu1 = mean(LX1);
    mu2 = mean(LX2);

    % M_cor = corr(LX1, LX2);
    M_cov = zeros(N, N);
    M_cor = zeros(N, N);

    % I replace the following loop using matrix operations
    n_LX1 = LX1(:, :) - repmat(mu1, [w, 1]);
    n_LX2 = LX2(:, :) - repmat(mu2, [w, 1]);

    Std1 = diag(n_LX1'*n_LX1)/(w-1);
    Std2 = diag(n_LX2'*n_LX2)/(w-1);
    Std12 = Std1*Std2';
    M_cov = n_LX1'*n_LX2/(w-1);

    M_cor(Std12==0) = 0;
    M_cor(Std12~=0) = M_cov(Std12~=0)./sqrt(Std12(Std12~=0));

    % claim varialbe, matrixization
    claim = zeros(N);
    w_mu2 = repmat(mu2', [1, N]);
    w_mu1 = repmat(mu2, [N, 1]);
    s_12 = (w_mu2 >= w_mu1) & (M_cor > 0);
    claim(s_12) = claim(s_12) + M_cor(s_12);

    diag_M_cor = diag(M_cor);
    cor1 = max(0, repmat(-diag_M_cor, [1, N]));
    cor2 = max(0, repmat(-diag_M_cor', [N, 1]));
    claim(s_12) = claim(s_12) + cor1(s_12) + cor2(s_12);

    transfer = zeros(N);
    sum_claim = repmat(sum(claim, 2), [1, N]);
    s_1 = abs(sum_claim) > 0;
    w_weight_o = repmat(weight_o, [1, N]);
    transfer(s_1) = w_weight_o(s_1).*claim(s_1)./sum_claim(s_1);

    % Use matrix operations, replacing the following loop
    transfer_ij = transfer'-transfer;
    weight = weight - sum(transfer_ij)';
end
end

%     for i=1:N,
%         for j = 1:N,
%             M_cov(i, j) = (LX1(:, i)-mu1(:, i))'*(LX2(:, j) - mu2(:, j))/(w-1);
%
%             % Std1 = Std(LX1(:, i)
%             Std1 = (LX1(:, i) - mu1(:, i))'*(LX1(:, i) - mu1(:, i))/(w-1);
%             Std2 = (LX2(:, j) - mu2(:, j))'*(LX2(:, j) - mu2(:, j))/(w-1);
%
%             if (Std12(i, j) == 0),
%                 M_cor(i, j) = 0;
%             else
%                 M_cor(i, j) = M_cov(i, j) / sqrt(Std1(i, 1)*Std2(j, 1));
%             end
%         end
%     end

%
%     for i = 1:N,
%         for j = 1:N,
%             if ((mu2(i) >= mu2(j)) && (M_cor(i, j) > 0)),
%                 claim(i, j) = claim(i, j) + M_cor(i, j);
% %                 claim(i, j) = claim(i, j) + max(0, -M_cor(i, i));
% %                 claim(i, j) = claim(i, j) + max(0, -M_cor(j, j));
%             end
%         end
%     end
%     for i = 1:N,
%         sum_claim_i = sum(claim(i, :));
%         if (abs(sum_claim_i) > 0)
%             for j = 1:N,
%                 transfer(i, j) = weight_o(i)*claim(i, j)/sum_claim_i;
%             end
%         end
%     end
%     for i = 1:N,
%         for j = 1:N,
%             weight(i) = weight(i) - transfer(i, j) + transfer(j, i);
%         end
%     end


function y = vl_sigmoid(x)
% VL_SIGMOID Sigmoid function
%   Y = VL_SIGMOID(X) returns
%
%    Y = 1 ./ (1 + EXP(X)) ;
%
%   Remark::
%     Useful properties of the sigmoid function are:
%
%     -  1 - VL_SIGMOID(X) = VL_SIGMOID(-X)
%     -  Centered sigmoid: 2 * VL_SIGMOID(X) - 1 ;
%     -  VL_SIGMOID(X) = (EXP(X/2) - EXP(X/2)) / (EXP(X/2) + EXP(X/2))
%
%   See also: VL_DSIGMOID(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

y = 1 ./ (1 + exp(-x)) ;


function [xi,yi] = snakeinterp(x,y,dmax,dmin)
%----------This function is used for interpolating the snake adaptively

%   dmax: the maximum distance between two snake points
%   dmin: the maximum distance between two snake points
%   d(i,i+1)>dmax, then a new point is added between i and i+1
%   d(i,i+1)<dmin, then either i or i+1 is removed
%

x = x(:); y = y(:);

N = length(x);

d = abs(x([2:N 1])- x(:)) + abs(y([2:N 1])- y(:));

% remove the points which distance to neighbor points is shorter than dmin
IDX = (d<dmin);

idx = find(IDX==0);
x = x(idx);
y = y(idx);

N = length(x);
d = abs(x([2:N 1])- x(:)) + abs(y([2:N 1])- y(:));

IDX = (d>dmax);

z = snakeindex(IDX);

p = 1:N+1;

xi = interp1(p,[x;x(1)],z');
yi = interp1(p,[y;y(1)],z');

N = length(xi);
d = abs(xi([2:N 1])- xi(:)) + abs(yi([2:N 1])- yi(:));

while (max(d)>dmax),

    IDX = (d>dmax);
    z = snakeindex(IDX);

    p = 1:N+1;

    xi = interp1(p,[xi;xi(1)],z');
    yi = interp1(p,[yi;yi(1)],z');

    N = length(xi);
    d = abs(xi([2:N 1])- xi(:)) + abs(yi([2:N 1])- yi(:));
end


function F=dwt_fuse(I1,I2,zt)
%    DWT
%    Input:
%    I1 - input image A
%    I2 - input image B
%    zt - maximum decomposition level
%    Output:
%    F  - fused image
%
%    The code is edited by Yu Liu, 01-09-2014.

%-------------------------------------------------------------------------%
%                                DWT
%-------------------------------------------------------------------------%
I1=double(I1);
I2=double(I2);
tempA=I1;
tempB=I2;

X=cell(zt,4);
Y=cell(zt,4);
Z=cell(zt,4);
for i=1:zt
    [X{i,1},X{i,2},X{i,3},X{i,4}]=dwt2(tempA,'db1','mode','per');
    tempA=X{i,1};
    [Y{i,1},Y{i,2},Y{i,3},Y{i,4}]=dwt2(tempB,'db1','mode','per');
    tempB=Y{i,1};
end

%-------------------------------------------------------------------------%
%                               low-pass fusion
%-------------------------------------------------------------------------%
Z{zt,1}=(X{zt,1}+Y{zt,1})/2;

%-------------------------------------------------------------------------%
%                               high-pass fusion
%-------------------------------------------------------------------------%
for i=zt:-1:1
    for j=2:4
        Z{i,j}=selc(X{i,j},Y{i,j},3);
    end
end

%-------------------------------------------------------------------------%
%                               IDWT
%-------------------------------------------------------------------------%
for i=zt:-1:1
    if i>1
        Z{i-1,1}=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    else
        F=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    end
end


function op = prox_Sl1( lambda )
%PROX_SL1    Sorted/Ordered L1 norm.
%    OP = PROX_L1( lambda ) implements the nonsmooth function
%        OP(X) = sum(lambda.*sort(abs(X),'descend'))
%    where lambda is strictly positive and sorted in decreasing order,
%    in which case this function is a norm (and hence convex).
%    If lambda is a scalar, it will be expanded to all elements, but in this
%    case it is equivalent to the (scaled) usual l1 norm.
%
%    Notice: this function uses mex files. Some pre-compiled binaries
%       for common systems are included; if these do not work for you,
%       then please install yourself. In the mexFiles/ subdirectory,
%       run the file "makeMex.m"
% Reference:
%  http://www-stat.stanford.edu/~candes/OrderedL1/
% "Statistical Estimation and Testing via the Ordered L1 Norm"
% by M. Bogdan, E. van den Berg, W. Su, and E. J. Candès
% 2013
%
% See also prox_l1.m, makeMex.m

if nargin == 0,
	lambda = 1;
elseif ~isnumeric( lambda ) || ~isreal( lambda ) ||  any( lambda(:) <= 0 )
	error( 'Argument lambda must have all positive entries.' );
end
if ~issorted(flipud(lambda(:)))
    error( 'Argument lambda must be sorted in decreasing order.');
end
if numel(lambda)==1
    warning('TFOCS:prox_SL1','When lambda is a scalar, we recommend prox_l1.m instead pf prox_SL1.m');
end

% The mex file is in the child directory mexFiles/
% Check for its existence. First, add the right paths
addpath(fullfile(tfocs_where,'mexFiles'));
if 3 ~= exist('proxAdaptiveL1Mex','file')
    makeMex;
    % check that it worked
    if 3 ~= exist('proxAdaptiveL1Mex','file')
        disp('Compilation of mex files for prox_SL1.m failed; please report this error');
    end
end

f       = @(x) sum( lambda(:) .* sort(abs(x(:)), 'descend') );
prox_f  = @(x,t) proxOrderedL1(x,t.*lambda);

op      = tfocs_prox( f, prox_f , 'vector' ); % Allow vector stepsizes

end

% -- subroutines --
function x = proxOrderedL1(y,lambda)
    % Normalization
    lambda = lambda(:);
    y      = y(:);
    sgn    = sign(y);
    [y,idx] = sort(abs(y),'descend');

    % Simplify the problem
    k = find(y > lambda,1,'last');

    % Compute solution and re-normalize
    n = numel(y);
    x = zeros(n,1);

    if (~isempty(k))
        v1 = y(1:k);
        if numel(lambda) > 1
            v2 = lambda(1:k);
        else
            v2 = lambda*ones(k,1); % if lambda is a scalar, implicity make it lambda*ones(size(y))
        end
        v = proxAdaptiveL1Mex(v1,v2);
        x(idx(1:k)) = v;
    end

    % Restore signs
    x = sgn .* x;
end

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


%% Machine Learning Online Class - Exercise 1: Linear Regression

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear exercise. You will need to complete the following functions
%  in this exericse:
%
%     warmUpExercise.m
%     plotData.m
%     gradientDescent.m
%     computeCost.m
%     gradientDescentMulti.m
%     computeCostMulti.m
%     featureNormalize.m
%     normalEqn.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%
% x refers to the population size in 10,000s
% y refers to the profit in $10,000s
%

%% Initialization
clear ; close all; clc

%% ==================== Part 1: Basic Function ====================
% Complete warmUpExercise.m
fprintf('Running warmUpExercise ... \n');
fprintf('5x5 Identity Matrix: \n');
warmUpExercise()

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ======================= Part 2: Plotting =======================
fprintf('Plotting Data ...\n')
data = load('ex1data1.txt');
X = data(:, 1); y = data(:, 2);
m = length(y); % number of training examples

% Plot Data
% Note: You have to complete the code in plotData.m
plotData(X, y);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =================== Part 3: Gradient descent ===================
fprintf('Running Gradient Descent ...\n')

X = [ones(m, 1), data(:,1)]; % Add a column of ones to x
theta = zeros(2, 1); % initialize fitting parameters

% Some gradient descent settings
iterations = 1500;
alpha = 0.01;

% compute and display initial cost
computeCost(X, y, theta)

% run gradient descent
theta = gradientDescent(X, y, theta, alpha, iterations);

% print theta to screen
fprintf('Theta found by gradient descent: ');
fprintf('%f %f \n', theta(1), theta(2));

% Plot the linear fit
hold on; % keep previous plot visible
plot(X(:,2), X*theta, '-')
legend('Training data', 'Linear regression')
hold off % don't overlay any more plots on this figure

% Predict values for population sizes of 35,000 and 70,000
predict1 = [1, 3.5] *theta;
fprintf('For population = 35,000, we predict a profit of %f\n',...
    predict1*10000);
predict2 = [1, 7] * theta;
fprintf('For population = 70,000, we predict a profit of %f\n',...
    predict2*10000);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ============= Part 4: Visualizing J(theta_0, theta_1) =============
fprintf('Visualizing J(theta_0, theta_1) ...\n')

% Grid over which we will calculate J
theta0_vals = linspace(-10, 10, 100);
theta1_vals = linspace(-1, 4, 100);

% initialize J_vals to a matrix of 0's
J_vals = zeros(length(theta0_vals), length(theta1_vals));

% Fill out J_vals
for i = 1:length(theta0_vals)
    for j = 1:length(theta1_vals)
	  t = [theta0_vals(i); theta1_vals(j)];
	  J_vals(i,j) = computeCost(X, y, t);
    end
end

% Because of the way meshgrids work in the surf command, we need to
% transpose J_vals before calling surf, or else the axes will be flipped
J_vals = J_vals';
% Surface plot
figure;
surf(theta0_vals, theta1_vals, J_vals)
xlabel('\theta_0'); ylabel('\theta_1');

% Contour plot
figure;
% Plot J_vals as 15 contours spaced logarithmically between 0.01 and 100
contour(theta0_vals, theta1_vals, J_vals, logspace(-2, 3, 20))
xlabel('\theta_0'); ylabel('\theta_1');
hold on;
plot(theta(1), theta(2), 'rx', 'MarkerSize', 10, 'LineWidth', 2);