%% Analyzing Neural Time Series Data
% Matlab code for Chapter 33
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 33.1

figure

subplot(121)
plot(normpdf(-4:.001:4))
axis tight

subplot(122)
a = randn(1000,1);
hist(a,50)
% this histogram was also used in figure 33.3

disp([ 'p_n = ' num2str(sum(a>2)/1000) ])
disp([ 'p_z = ' num2str(1-normcdf(2))  ])

%% Figure 33.5/6

% These figures are generated in the code for chapter 34

%% Figure 33.8

% introduction to bwlabeln and bwconncomp

% create 2D smoothing kernel
[xi,yi] = meshgrid(-10:10,-10:10);
zi = xi.^2+yi.^2;
zi = 1-(zi/max(zi(:)));

% create a random smoothed map
map = conv2(randn(100),zi,'same');

% threshold map at an arbitrary value
mapt = map;
mapt(abs(map)<range(map(:))/4) = 0;

% get labeled map via bwlabeln (in image processing toolbox)
[mapl,nblobs] = bwlabeln(mapt);
% mapl is a map of size <map> that contains numbers at each cluster.
% To extract information from clusters:
clustcount = zeros(1,nblobs);
clustsum   = zeros(1,nblobs);
for i=1:nblobs
    clustcount(i) = sum(mapl(:)==i);
    clustsum(i)   = sum(map(mapl(:)==i));
end

% bwconncomp works slightly differently, but will give similar information
blobinfo = bwconncomp(mapt);
% blobinfo is a structure that contains coordinates for each cluster rather than a map
% To extract information from clusters:
clustcount = zeros(1,nblobs);
clustsum   = zeros(1,nblobs);
for i=1:nblobs
    clustcount(i) = numel(blobinfo.PixelIdxList{i});
    clustsum(i)   = sum(map(blobinfo.PixelIdxList{i}));
end
% cluster count can be done faster using cellfun:
clustercount = cellfun(@numel,blobinfo.PixelIdxList);

figure
subplot(131)
imagesc(map), axis square
title('original')

subplot(132)
imagesc(mapt), axis square
title('thresholded')

subplot(133)
imagesc(mapl), axis square
title([ 'labeled (' num2str(nblobs) ' clusters in total)' ])

%% Figure 33.10

% This script calls the function fdr.m, which is included in the online
% code and was downloaded (and slightly modified) in summer 2012 from http://www-personal.umich.edu/~nichols/FDR/FDR.m

nsigs = round(linspace(1,500,80));
nnons = round(linspace(1,500,100));

fdrpvals = zeros(20,length(nsigs),length(nnons));

for iteri=1:20
    for i=1:length(nsigs)
        for j=1:length(nnons)

            pvals = [ rand(1,nsigs(i))*.05 rand(1,nnons(j))*.5+.05 ];
            temp = fdr(pvals,.05);
            if isempty(temp)
                fdrpvals(iteri,i,j) = NaN;
            else
                fdrpvals(iteri,i,j) = temp;
            end
        end
    end
end

fdrpvals = squeeze(nanmean(fdrpvals));

figure
imagesc(fdrpvals)
set(gca,'clim',[0 .05])
xlabel('number of non-significant p-values')
ylabel('number of significant p-values')

figure
subplot(211)
plot(nanmean(fdrpvals,1))
xlabel('number of non-significant p-values')
ylabel('critical p-value')
set(gca,'ylim',[0 .05])

subplot(212)
plot(nanmean(fdrpvals,2))
xlabel('number of significant p-values')
ylabel('critical p-value')
set(gca,'ylim',[0 .05])

%% end.


function new_model = rotate_3D_model_around_z(model,phi)

r = [cosd(phi) -sind(phi) 0 ; sind(phi) cosd(phi) 0 ; 0 0 1];
new_model = transform_3d_rigid(model,r,[0;0;0]);

end


% JCR library for cretaceous chalk
data{3}=[0.079 0.1
0.1 0.02
0.3 0.015
0.54 0.0
0.68 -0.4
0.747 -0.7];
% fit model to data
pc1=data{3}(:,2)*1e5; % Pa
sw1=data{3}(:,1);
sw_plot=linspace(0,1,5000);
% pc_plot=interp1(sw, pc, sw_plot, 'linear', 'extrap');
% plot(sw, pc, 'o', sw_plot, pc_plot)
% plot(diff(pc_plot)./diff(sw_plot))
% log(pc)=log(pce)-(1/labda)*log((sw-sw0)/(1-sw0-sor))
% ind0=find(pc==0, 1);
% p1=polyfit(sw(1:ind0-1), log(pc(1:ind0-1)), 1)
% plot(sw(1:ind0-1), log(pc(1:ind0-1)), 'o')

% pc=@(sw, cwi, coi, awi, aoi, swc, sor)(cwi./((sw-swc)/(1-swc)).^awi+coi./((1-sw-sor)/(1-sor)).^aoi);
% swc0=0.0789;
% sor0=0.2529;
% labda=2.4;
% f=@(x, sw)pc(sw, x(1), x(2), 1/labda, 1/labda, swc0, sor0);
% f2=@(x, sw)pc(sw, x(1), x(2), x(3), x(4), swc0, sor0);
% fw=@(x, sw)pc(sw, x(1), 0.0 , x(3), x(4), swc0, sor0);
% fo=@(x, sw)pc(sw, 0.0, x(2), x(3), x(4), swc0, sor0);
% % x=lsqcurvefit(f2, [1e3, -1e3, 1.0, 1.0],sw1, pc1)
% x=lsqnonlin(@(x)(sum((f2(x, sw1)-pc1).^2)), [0.5e3, -1e3, 1.0, 1.0])
% fit a spline
% first, do an extrapolation to assign values to sw=0 and sw=1
sw_end = [0,1];
pc_end = interp1(sw1, pc1, sw_end, 'linear', 'extrap');
pc_pp = makima([sw_end(1); sw1; sw_end(2)], [pc_end(1); pc1; pc_end(2)]);
pc = @(sw)ppval(pc_pp, sw);
pcder = @(sw)ppval(fnder(pc_pp, 1), sw);
plot(sw1, pc1, 'o', sw_plot, pc(sw_plot), '--' )%, ...
% sw_plot, fw(x, sw_plot), '--', ...
% sw_plot, fo(x, sw_plot), '-.');
grid;

% Note:
% Looking at the above data, it is better to go with interpolation of the
% data
% note 2: fnder requires spline toolbox
% see this: https://se.mathworks.com/matlabcentral/answers/95194-how-do-i-find-the-derivative-of-a-spline-curve-in-matlab-7-9-r2009b


function rte = pred2path(P,s,t)
%PRED2PATH Convert predecessor indices to shortest paths from 's' to 't'.
%   rte = pred2path(P,s,t)
%     P = |s| x n matrix of predecessor indices (from DIJK)
%     s = FROM node indices
%       = [] (default), paths from all nodes
%     t = TO node indices
%       = [] (default), paths to all nodes
%   rte = |s| x |t| cell array of paths (or routes) from 's' to 't', where
%         rte{i,j} = path from s(i) to t(j)
%                  = [], if no path exists from s(i) to t(j)
%
% (Used with output of DIJK)

% Copyright (c) 1994-2006 by Michael G. Kay
% Matlog Version 9 13-Jan-2006 (http://www.ie.ncsu.edu/kay/matlog)

% Input Error Checking ****************************************************
narginchk(1,3);

[~,n] = size(P);

if nargin < 2 || isempty(s), s = (1:n)'; else, s = s(:); end
if nargin < 3 || isempty(t), t = (1:n)'; else, t = t(:); end

if any(P < 0 | P > n)
   error(['Elements of P must be integers between 1 and ',num2str(n)]);
elseif any(s < 1 | s > n)
   error(['"s" must be an integer between 1 and ',num2str(n)]);
elseif any(t < 1 | t > n)
   error(['"t" must be an integer between 1 and ',num2str(n)]);
end
% End (Input Error Checking) **********************************************

rte = cell(length(s),length(t));

[~,idxs] = find(P==0);

for i = 1:length(s)
%    if rP == 1
%       si = 1;
%    else
%       si = s(i);
%       if si < 1 | si > rP
%          error('Invalid P matrix.')
%       end
%    end
   si = find(idxs == s(i));
   for j = 1:length(t)
      tj = t(j);
      if tj == s(i)
         r = tj;
      elseif P(si,tj) == 0
         r = [];
      else
         r = tj;
         while tj ~= 0
            if tj < 1 || tj > n
               error('Invalid element of P matrix found.')
            end
            r = [P(si,tj) r];
            tj = P(si,tj);
         end
         r(1) = [];
      end
      rte{i,j} = r;
   end
end

if length(s) == 1 && length(t) == 1
   rte = rte{:};
end

%rte = t;
while 0%t ~= s
   if t < 1 || t > n || round(t) ~= t
      error('Invalid "pred" element found prior to reaching "s"');
   end
   rte = [P(t) rte];
   t = P(t);
end


function x = grid2 ( i1, i2, ndim, nstep, x1, x2 )

%*****************************************************************************80
%
%% GRID2 computes grid points between X1 and X2 in N dimensions.
%
%  Discussion:
%
%    However, X1 need not be the first point computed, nor X2 the last.
%    The user must specify the steps on which X1 and X2 are passed
%    through.  These steps may even be outside the range of 1 through NSTEP.
%
%    We assume that a set of equally spaced points have
%    been drawn on the line through X1 and X2, and that
%    they have been numbered, with X1 labeled I1 and X2
%    labeled I2.  I1 or I2 may be between 1 and NSTEP,
%    in which case X1 or X2 will actually be returned in the
%    X array, but there is no requirement that I1 or I2
%    satisfy this condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I1, I2.  I1 specifies the step on which
%    X1 would be computed, and similarly for I2.
%    I1 and I2 must be distinct.
%
%    Input, integer NDIM, the dimension of the points X1 and X2.
%
%    Input, integer NSTEP, this is the number of equally
%    spaced points that are to be generated.
%    NSTEP should be at least 1.
%
%    Input, real X1(NDIM), X2(NDIM), the points that define
%    the line along which the equally spaced points are generated, and
%    which may or may not be included in the set of computed points.
%
%    Output, real X(NDIM,NSTEP), the set of equally spaced points.
%    Each column of X represents one point.
%    If 1 <= I1 <= NSTEP, then X(*,I1) = X1, and similarly for I2.
%
  if ( i1 == i2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GRID2 - Fatal error!\n' );
    fprintf ( 1, '  I1 = I2, leading to zero denominator.\n' );
    fprintf ( 1, '  I1 = %d\n', i1 );
    fprintf ( 1, '  I2 = %d\n', i2 );
    error ( 'GRID2 - Fatal error!' );
  end

  for i = 1 : nstep
    for j = 1 : ndim
      x(j,i) = ( ( i2 - i      ) * x1(j)   ...
               + (      i - i1 ) * x2(j) ) ...
               / ( i2     - i1 );
    end
  end

  return
end


function [O] = forwardcomp(X,W,L)
%FORWARD COMPUTATION.
%
%	Syntax
%     [O] = forwardcomp(X,W,L)
%
%	Description
%
%	  FORWARDCOMP computes the outputs for a multiple perceptron Neural Network
%	  from its net input.
%
%	  FORWARDCOMP(X,W,L) takes:
%	    X - matrix of input patterns including one row of bias = 1.
%	    W - matrix of weights of the Network (cell vectors).
%	    L - vector of hidden layers and neurons per layer [m1 m2...mL].
%	  and returns
%       O - The output of the network
%
%
% Paul Acquatella, 08-02-2009
% Revised 08-02-2009
% Copyright Paul Acquatella
% $Revision: 1.1.6.2 $  $Date: 2009/08/02 18:21:07 $

%Calcular Numero de Salidas y de Capas Ocultas
Q = size(W{end},1);         %Numero de Salidas
H = size(L,2);              %Numero de Capas Ocultas "Depth"

%Inicializar los campos locales inducidos 'v'
v = cell(1,H+1);
v{end} = zeros(Q,1);
for i=1:H;
    v{i} = zeros(L(:,i),1);
end

%Inicializar las salidas 'y' de cada capa
y = cell(1,H+1);
y{end} = zeros(Q,1);
for i=1:H;
    y{i} = [zeros(L(:,i),1); 1];
end

for j = 1:(H+1)
    if j == 1
        v{j} = W{j}*X;                  %Compute the induced local field
        y{j}(1:end-1) = logsig(v{j});   %Compute the layer output
    elseif j == (H+1)
        v{j} = W{j}*y{j-1};             %Compute the induced local field
        y{j} = logsig(v{j});            %Compute the layer output
        O = y{j};
    else
        v{j} = W{j}*y{j-1};             %Compute the induced local field
        y{j}(1:end-1) = logsig(v{j});   %Compute the layer output
    end
end


function p = vonMises_prob( x, m, k, use_log )
%VONMIS_PROB Calculates the probability of x coming from a Von Mises
%distribution with mean mu and concentration parameter k.
%   p = vonMises_prob( x, m, k, use_log )

if nargin < 4, use_log = 0; end
[d N] = size(x);

m = m(:);
M = m*ones(1,N);

denom = (2*pi)*besseli(0,k);
mahal = (k*cos(x-M));

if use_log
    p = mahal-log(denom);
else
    p = exp(mahal)/denom;
end


%{
    Demonstration of computing the kernel function matrix.
%}

clc
clear all
close all
addpath(genpath(pwd))

% data
X = rand(33, 2);
Y = rand(23, 2);

% kernel setting
kernel1 = Kernel('type', 'gaussian', 'gamma', 0.01);
kernel2 = Kernel('type', 'polynomial', 'degree', 3);
kernel3 = Kernel('type', 'linear');
kernel4 = Kernel('type', 'sigmoid', 'gamma', 0.01);
kernel5 = Kernel('type', 'laplacian', 'gamma', 0.1);

% compute the kernel function matrix
matrix{1} = kernel1.computeMatrix(X, Y);
matrix{2} = kernel2.computeMatrix(X, Y);
matrix{3} = kernel3.computeMatrix(X, Y);
matrix{4} = kernel4.computeMatrix(X, Y);
matrix{5} = kernel5.computeMatrix(X, Y);


function y=logsum(x,d,k)
%LOGSUM logsum(x,d,k)=log(sum(k.*exp(x),d))
%
% Usage: (1) y=logsum(x) % log(sum(exp(x)))
%        (2) f=0.1*log(10); y=logsm(x*f)/f;  % add powers in dB
%
% Inputs:  X(M,N,...) data matrix to sum
%          D          optional dimension to sum along [1st non-singular dimension]
%          K(M,N,...) optional scaling matrix. It must either be idential
%                     in size to X, or else be a vector whose length is
%                     equal to the size of dimension D of X
%
% Outputs: Y(1,N,...) = log(sum(exp(X).*K,D))
%
% This routine evaluates the given expression for Y but takes care to avoid
% overflow or underflow.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: logsum.m 8178 2016-07-12 06:57:25Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin==1 || ~numel(d)
    d=[find(size(x)-1) 1];
    d=d(1);
end
n=size(x,d);
if n<=1,            % use efficient computation if only one term in the sum
    if nargin<3
        y=x;
    else
        y=x+log(k);
    end
    return;
end
s=size(x);
p=[d:ndims(x) 1:d-1];
z=reshape(permute(x,p),n,prod(s)/n);
q=max(z,[],1);              % we subtract y from each row to avoid dynamic range problems
a=(q==Inf)|(q==-Inf);       % check for infinities
if nargin<3
    y=q+log(sum(exp(z-q(ones(n,1),:)),1));
elseif numel(k)==n
    y=q+log(sum(exp(z-q(ones(n,1),:)).*repmat(k(:),1,prod(s)/n),1));
else
    y=q+log(sum(exp(z-q(ones(n,1),:)).*reshape(permute(k,p),n,prod(s)/n),1));
end
y(a)=q(a);                  % correct any column whose max is +-Inf
s(d)=1;                     % update the dimension of the summed component
y=ipermute(reshape(y,s(p)),p);


function A = spm_mesh_area(M,PF)
% Compute the surface area of a triangle mesh
% M        - patch structure: vertices and faces must be mx3 and nx3 arrays
%            or 3xm array of edge distances
% PF       - logical indicating whether to return surface area as a whole
%            or per face [default: false]
%
% A        - surface area
%__________________________________________________________________________
%
% Computed using numerically stable version of Heron's formula:
% See https://www.wikipedia.org/wiki/Heron%27s_formula
%__________________________________________________________________________
% Copyright (C) 2010-2018 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_area.m 7367 2018-07-06 16:04:27Z guillaume $

if isnumeric(M)
    A = M;
else
    A = M.vertices(M.faces',:);
    A = permute(reshape(A',3,3,[]),[2 1 3]);
    A = squeeze(sqrt(sum((A([1 2 3],:,:) - A([2 3 1],:,:)).^2,2)));
end
A = sort(A,1,'descend');
A = ( A(1,:) + ( A(2,:) + A(3,:) ) ) .* ...
    ( A(3,:) - ( A(1,:) - A(2,:) ) ) .* ...
    ( A(3,:) + ( A(1,:) - A(2,:) ) ) .* ...
    ( A(1,:) + ( A(2,:) - A(3,:) ) );
A(A<0) = 0;
A = 1/4 * sqrt(A);

if nargin < 2 || ~PF
    A = sum(A);
end


function y = ngaussian(x)

% NGAUSSIAN Compute a Gaussian with mean 0 and variance 1.
% FORMAT
% DESC computes a the likelihood of a normalised Gaussian
% distribution, i.e. with mean 0 and variance 1.
% ARG x : input value(s) for which to compute the distribution.
% RETURN y : probability of the input values under the Gaussian.
%
% SEEALSO : cumGaussian, gaussOverDiffcumGaussian
%
% COPYRIGHT : Neil D. Lawrence, 2004

% NDLUTIL

x2 = x.*x;
y = exp(-.5*x2);
y = y/sqrt(2*pi);


function ex7bvp
%EX7BVP  Example 7 of the BVP tutorial.
%   This is the first example problem for D02HBF from the NAG library.
%   The problem is y'' = (y^3 - y')/(2x), y(0) = 0.1, y(16) = 1/6.  The
%   singularity at the origin is handled by using a series to represent
%   the solution and its derivative at a "small" distance d > 0, namely
%
%      y(d)  = 0.1 + y'(0)*sqrt(d)/10 + d/100
%      y'(d) = y'(0)/(20*sqrt(d)) + 1/100
%
%   The value y'(0) is treated as an unknown parameter p. The problem is
%   solved numerically on [d, 16].  Two boundary conditions are that the
%   computed solution and its first derivative agree with the values from
%   the series at d.  The remaining boundary condition is y(16) = 1/6.
%
%   The results from the documentation for D02HBF are here compared to
%   curves produced by bvp4c.

% Copyright 2004, The MathWorks, Inc.

  options = bvpset('stats','on');

  d = 0.1;  % The known parameter, visible in nested functions.
  solinit = bvpinit(linspace(d,16,5),[1 1],0.2);

  sol = bvp4c(@ex7ode,@ex7bc,solinit,options);

  % Augment the solution array with the values y(0) = 0.1, y'(0) = p
  % to get a solution on [0, 16].
  x = [0 sol.x];
  y = [[0.1; sol.parameters] sol.y];

  % Solution obtained using D02HBF, augmented with the values y(0), y'(0).
  nagx = [0.00 0.10 3.28 6.46 9.64 12.82 16.00]';
  nagy = [0.1000  4.629e-2
          0.1025  0.0173
          0.1217  0.0042
          0.1338  0.0036
          0.1449  0.0034
          0.1557  0.0034
          0.1667  0.0035];

  figure
  plot(x,y(1,:),x,y(2,:),'--',nagx,nagy,'*')
  axis([-1 16 0 0.18])
  title('Problem with singular behavior at the origin.')
  xlabel('x')
  ylabel('y, dy/dx (--), and D02HBF (*) solutions')

  % --------------------------------------------------------------------------
  % Nested functions
  %

  function dydx = ex7ode(x,y,p)
  %EX7ODE  ODE function for Example 7 of the BVP tutorial.
    dydx = [ y(2)
             (y(1)^3 - y(2))/(2*x) ];
  end % ex7ode

  % --------------------------------------------------------------------------

  function res = ex7bc(ya,yb,p)
  %EX7BC  Boundary conditions for Example 7 of the BVP tutorial.
  %   The boundary conditions at x = d are that y and y' have
  %   values yatd and ypatd obtained from series expansions.
  %   The unknown parameter p = y'(0) and known parameter d are
  %   used in the expansions.
    yatd =  0.1 + p*sqrt(d)/10 + d/100;
    ypatd = p/(20*sqrt(d)) + 1/100;
    res = [ ya(1) - yatd
            ya(2) - ypatd
            yb(1) - 1/6 ];
  end % ex7bc

  % --------------------------------------------------------------------------

end  % ex7bvp


% check latlon2local

origin = [30.5298813668333; 114.350355321667; 20.8869];
lla = [30.5298266666667; 114.350367833333; 20.8];
lla = [30.5299376666667; 114.35528; 17.6];
% matlab functions
[e1, n1, u1] = geodetic2enu(lla(1), lla(2), lla(3), origin(1),...
    origin(2), origin(3), wgs84Ellipsoid, 'degrees');
[e2, n2, u2] = latlon2local(lla(1), lla(2), lla(3), origin);

assert(abs(e1 - e2) < 1e-8);
assert(abs(n1 - n2) < 1e-8);
assert(abs(u1 - u2) < 1e-8);

% our own implementation
Ce2n0=llh2dcm_v000(origin(1:2) * pi/ 180,[0;1]);
qs02e=rotro2qr(Ce2n0');
origin_xyz = ecef2geo_v000([origin(1:2) * pi / 180; origin(3)],1);
xyz = ecef2geo_v000([lla(1:2) * pi / 180; lla(3)],1);
ned = quatrot_v000(qs02e, xyz - origin_xyz ,1)';

assert(abs(e1 - ned(2)) < 1e-8);
assert(abs(n1 - ned(1)) < 1e-8);
assert(abs(u1 + ned(3)) < 1e-8);


function weight = gradAscent
%%
%
%   Description : LogisticRegression using gradAsscent
%   Author : Liulongpo
%   Time：2015-4-18 10:57:25
%
%%
clc
close all
clear
%%

data = load('testSet.txt');
[row , col] = size(data);
dataMat = data(:,1:col-1);
dataMat = [ones(row,1) dataMat] ;
labelMat = data(:,col);
alpha = 0.001;
maxCycle = 500;
weight = ones(col,1);
for i = 1:maxCycle
    h = sigmoid((dataMat * weight)');
    error = (labelMat - h');
    weight = weight + alpha * dataMat' * error;
end

figure
scatter(dataMat(find(labelMat(:) == 0),2),dataMat(find(labelMat(:) == 0),3),3);
hold on
scatter(dataMat(find(labelMat(:) == 1),2),dataMat(find(labelMat(:) == 1),3),5);
hold on
x = -3:0.1:3;
y = (-weight(1)-weight(2)*x)/weight(3);
plot(x,y)
hold off

end

function returnVals = sigmoid(inX)
    % 注意这里的sigmoid函数要用点除
    returnVals = 1.0./(1.0+exp(-inX));
end


function besselk_test ( )

%*****************************************************************************80
%
%% BESSELK_TEST examines the besselk correlation.
%
%  Discussion:
%
%    This code is based substantially on a document by Toby Driscoll.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Toby Driscoll,
%    Mercer's theorem and the Karhunen-Loeve expansion,
%    Oxford University Mathematical Institute,
%    http://www2.maths.ox.ac.uk/chebfun/examples/stats/pdf/MercerKarhunenLoeve.pdf
%
  rmpath ( '/usr/local/matlab/toolbox/datafeed/datafeed' )
  addpath ( '../chebfun' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSELK_TEST:\n' );
  fprintf ( 1, '  Demonstrate Mercer''s theorem and the KL expansion\n' );
  fprintf ( 1, '  for the besselk kernel.\n' );
%
%  Set the interval.
%
  a = 0.0;
  b = 10.0;
  fprintf ( 1, '  Using interval [%g,%g]\n', a, b );
  s_num = 21;
  s_vec = linspace ( a, b, s_num );
%
%  FRED is a function from the CHEBFUN library.
%
%  It constructs a "chebop" representing the Fredholm integral operator
%  with kernel K for functions in domain [A,B].
%
  F = fred ( @besselk_correlation, domain ( [ a, b ] ) );
%
%  EIGS has been extended to be able to compute the eigenvalues Lambda
%  and eigenfunctions Psi of the Fredholm integral operator represented by F.
%
%  The "LM" switch requests that we return the eigenvalues of largest magnitude.
%
%  Each Psi is a CHEBFUN, that is, it takes a real number argument.
%
  eigen_num = 20;
  [ Psi, Lambda ] = eigs ( F, eigen_num, 'lm' );

  eigen_found = length ( Lambda );
  fprintf ( 1, '  Requested %d eigenmodes, computed %d\n', eigen_num, eigen_found );
  eigen_num = min ( eigen_num, eigen_found );
%
%  Print the eigenvalues.
%
  lambda_vec = diag ( Lambda );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I       Lambda(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %2d  %10g\n', i, lambda_vec(i) );
  end
%
%  Plot the eigenvalues.
%
  figure ( 1 )
  clf
  plot ( lambda_vec, 'Linewidth', 2 );
  hold on
  plot ( lambda_vec, 'b.', 'Markersize', 20 );
  title ( 'besselk: Mercer eigenvalues' );
  xlabel ( '<--- N --->')
  grid on
  print -dpng 'besselk_figure1.png'
%
%  Plot selected eigenfunctions.
%
  figure ( 2 )
  subplot ( 4, 1, 1 )
  plot ( Psi(:,1), 'Linewidth', 2 );
  title ( 'besselk: Mercer eigenfunction PSI(1)' )
  grid on
  subplot ( 4, 1, 2 )
  plot ( Psi(:,2), 'Linewidth', 2 );
  title ( 'besselk: Mercer eigenfunction PSI(2)' )
  grid on
  subplot ( 4, 1, 3 )
  plot ( Psi(:,3), 'Linewidth', 2 );
  title ( 'besselk: Mercer eigenfunction PSI(3)' )
  grid on
  subplot ( 4, 1, 4 )
  plot ( Psi(:,4), 'Linewidth', 2 );
  title ( 'besselk: Mercer eigenfunction PSI(4)' )
  grid on
  print -dpng 'besselk_figure2.png'
%
%  Orthonormality check.
%
  ptp = Psi' * Psi;
  error_frobenius = r8mat_is_identity ( eigen_num, ptp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of I - Psi'' * Psi = %g\n', error_frobenius );
%
%  K(S,S) should be exactly 1.
%  Because we are using a truncated representation of K, our estimate of K(S,S)
%  will be smaller than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Truncated estimate of K(s,s) = 1 for S in the interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        S        K(s,s) estimate\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 21
    s = s_vec(i);
    ptlp = Psi(s,:) * Lambda * Psi(s,:)';
    fprintf ( 1, '  %10g  %14g\n', s, ptlp );
  end
%
%  Look at eigenvalue decay.
%
  x = diff ( log ( ( 1:eigen_num ) ) );
  y = diff ( log ( lambda_vec ) )';
  c = y ./ x;
  figure ( 3 );
  clf
  plot ( c, 'Linewidth', 2 );
  grid on
  hold on
  plot ( c, 'b.', 'Markersize', 20 );
  xlabel ( '<-- N -->' )
  title ( 'besselk: Eigenvalue decay rate' );
  print -dpng 'besselk_figure3.png'
%
%  Look at eigenvalue sum.
%
%  The trace of K(s,s) over [a,b] should be the integral of 1 over [a,b],
%  that is, b - a.
%
%  We compare the trace to the partial sums of the lambda's to see how much
%  of the variance of the process we have captured.
%
  trace_K = b - a;
  lambda_cum = cumsum ( lambda_vec ) / trace_K;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index    Cumulative Eigenvalue sum\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %5d  %10g\n', i, lambda_cum(i) );
  end
%
%  We decide to use no more than 10 eigenfunctions.
%
  eigen_use = min ( 10, eigen_num );
%
%  Find 400 realizations of the process by selecting, for each realization,
%  10 random parameters Z in the truncated KL expansion.
%
  Z = randn ( eigen_use, 400 );
  X = Psi(:,1:eigen_use) * ( sqrt ( Lambda(1:eigen_use,1:eigen_use) ) * Z );
%
%  Plot 40 of the realizations;
%  Plot their mean, computed from all 400.
%
  figure ( 4 )
  clf
  plot ( X(:,1:40) )
  mu = sum ( X, 2 ) / 400;
  hold on;
  plot ( mu, 'k', 'linewidth', 3 );
  title ( 'besselk: 40 Random Realizations X(t,omega), and their Mean.' )
  print -dpng 'besselk_figure4.png'
%
%  Estimate the covariance from the data.
%
  figure ( 5 )
  clf
  [ S, T ] = meshgrid ( s_vec, s_vec );
  C = cov ( X(s_vec,:)' );
  mesh ( S, T, C );
  hold on;
  D = besselk_correlation ( S, T );
  plot3 ( S, T, D, 'k.', 'markersize', 10 )
  title ( 'besselk: Covariance K(S,T) (dots), and Estimate from Realizations (mesh)' )
  print -dpng 'besselk_figure5.png'
%
%  Using just 10 functions in the expansion,
%  reduce the correlation length,
%  and examine the sum of the lambda's.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a fixed number of eigenfunctions = %d\n', eigen_use );
  fprintf ( 1, '  but vary the correlation length RHOBAR.\n' );
  fprintf ( 1, '  (We used RHOBAR = 1 above.)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sum of the eigenvalues, divided by (B-A),\n' );
  fprintf ( 1, '  discloses the relative amount of the total variation\n' );
  fprintf ( 1, '  that is captured by the truncated expansion.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      RHOBAR      VARSUM\n' );
  fprintf ( 1, '\n' );
  rhobar = 4.0;
  for i = 1 : 10
    F = fred ( @besselk_correlation, domain ( [ a, b ] ) );
    lambda_vec = eigs ( F, eigen_use, 'lm' );
    varsum = sum ( lambda_vec(1:eigen_use) ) / ( b - a );
    fprintf ( 1, '  %10g  %10g\n', rhobar, varsum );
    rhobar = rhobar / 2.0;
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSELK_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../chebfun' )

  return
end


function x = normal_ms_cdf_inv ( cdf, mu, sigma )

%*****************************************************************************80
%
%% NORMAL_MS_CDF_INV inverts the Normal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Input, real MU, SIGMA, the parameters of the PDF.
%    0.0 < SIGMA.
%
%    Output, real X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NORMAL_MS_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'NORMAL_MS_CDF_INV - Fatal error!' );
  end

  x2 = normal_01_cdf_inv ( cdf );

  x = mu + sigma * x2;

  return
end


function th_img = bayesian_th(inp,s,n)
[row col]=size(inp);
th_img=zeros(row,col);
th_img(1:(row/2^n),1:(col/2^n))=inp(1:(row/2^n),1:(col/2^n));
for i=n:-1:1
    a=inp((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1))); %diagonal coefficients
    th_img((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1)))=th(a,s,n);
    a=inp(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)));   %horizontal coefficients
    th_img(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)))=th(a,s,n);
    a=inp((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)));     %vertical coefficients
    th_img((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)))=th(a,s,n);
end
end
function th_subband=th(inp_mtx,s,n)
N=numel(inp_mtx);
c=inp_mtx.^2;
sigmasq_y=(1/N)*sum(c(:));
sigma_x=sqrt(max((sigmasq_y-s),0));
% Tb1=sqrt(log10(N/n))*(s/(sigma_x));
Tb1=(s/(sigma_x));
% th_subband=sign(inp_mtx).*max(0,abs(inp_mtx)-Tb1);
th_subband=inp_mtx.*(abs(inp_mtx)>Tb1);
end


%% Tire comparison
% Comparison between tire models.
%
% <html>
% <script src='https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML'></script>
% </html>
%
%% Description
% The typical relation between the lateral force and the slip angle can be observed in the figure below (Adapted from [1]). Besides, its possible to verify the definition of slip angle.
%
% <<CurvaCaracteristica.svg>>
%
%% Equivalence
% Given a reference Pacejka tire model it is possible to obtain an equivalent linear and polynomial model. The cornering stiffness of all models must be equal and the maximal lateral force of the Pacejka and Polynomial models must be the same.
%
% The Pacejka model depends on the parameters \(a_0\), \(a_1\), \(a_2\), \(a_3\), \(a_4\), \(a_5\), \(a_6\) e \(a_7\) that defines the constants \(B\), \(C\), \(D\) and \(E\) wich can be used to define the constants of the equivalent models.
%
% The equivalent linear tire model has cornering stiffness \(K\) given by
%
% \[ K = B C D \]
%
% The equivalent polynomial tire model has coeficients \(k_1\) and \(k_2\) given by
%
% \[ k_1 = B C D \]
%
% \[ k_2 = (4 k_1^3)/(27 F_{y, Max}^2) \]
%
% where \(F_{y, Max}\) is the maximal lateral force of the reference characteristic curve.
%
%% Model comparison
%

% Code start

clear ; close all ; clc

deriva = (0:0.1:15)*pi/180;         % ngulo de deriva [rad]

a0  = 1.3;
a1  = 2.014156;
a2  = 710.5013;
a3  = 5226.341;
a4  = 78.87699;
a5  = 0.01078379;
a6  = -0.004759443;
a7  = -1.8572;
a8  = 0;
a9  = 0;
a10 = 0;
a11 = 0;
a12 = 0;
a13 = 0;

TirePac = VehicleDynamicsLateral.TirePacejka();

Fz = 4e+03;
camber = 0;
TirePac.a0 = a0;
TirePac.a1 = a1;
TirePac.a2 = a2;
TirePac.a3 = a3;
TirePac.a4 = a4;
TirePac.a5 = a5;
TirePac.a6 = a6;
TirePac.a7 = a7;
TirePac.a8 = a8;
TirePac.a9 = a9;
TirePac.a10 = a10;
TirePac.a11 = a11;
TirePac.a12= a12;
TirePac.a13 = a13;

muy0 = TirePac.a1 * Fz/1000 + TirePac.a2;
D = muy0 * Fz/1000;
BCD = TirePac.a3 * sin(2 * atan(Fz/1000/TirePac.a4))*(1-TirePac.a5 * abs(camber));

% Linear tire model

K = BCD * 180/pi;

TireLin = VehicleDynamicsLateral.TireLinear();
TireLin.k = K;

% Polynomial tire MODEL

k1 = BCD * 180/pi;
k2 = (4 * k1^3)/(27 * D^2);

TirePol = VehicleDynamicsLateral.TirePolynomial();
TirePol.k1 = k1;
TirePol.k2 = k2;

% Lateral force
FyPac = TirePac.Characteristic(deriva, Fz, muy0/1000);
FyLin = TireLin.Characteristic(deriva);
FyPol = TirePol.Characteristic(deriva);

% Graphics
g = VehicleDynamicsLateral.Graphics(TirePac);

figure(1)
ax = gca;
set(ax, 'NextPlot', 'add', 'Box', 'on', 'XGrid', 'on', 'YGrid', 'on')
p = plot(deriva * 180/pi,-FyLin, 'Color', 'g', 'Marker', 's', 'MarkerFaceColor', 'g', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
p = plot(deriva * 180/pi,-FyPol, 'Color', 'b', 'Marker', '^', 'MarkerFaceColor', 'b', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
p = plot(deriva * 180/pi,-FyPac, 'Color', 'r', 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
xlabel('\(\alpha\) [grau]', 'Interpreter', 'Latex')
ylabel('\(F_y\) [N]', 'Interpreter', 'Latex')
l = legend('Linear', 'Polynomial', 'Pacejka');
set(l, 'Interpreter', 'Latex', 'Location', 'NorthWest')

%%
% In the above figure the 3 characteristic curves are plotted. For small slip angles all models are equivalents.
%
%% Comparison treatment slip angle
%

deriva180 = (0:0.1:180)*pi/180;     % ngulo de deriva de 0  180 graus [rad]

% Sem tratamento
ALPHA = deriva180 * 180/pi;
C = a0;
muy = muy0;
E = a6 * Fz/1000 + a7;
B = BCD/(C * D);
Sh = a8 * camber + a9 * Fz/1000 + a10;
Sv = a11 * Fz/1000 * camber + a12 * Fz/1000 + a13;
ALPHAeq = muy0/muy*(ALPHA + Sh);
% Reference characteristics
fy = D * sin(C * atan(B * ALPHAeq - E*(B * ALPHAeq - atan(B * ALPHAeq))));
% Lateral force
FyPacSem180 = -muy/muy0*(fy + Sv);

% Com tratamento
FyPacCom180 = TirePac.Characteristic(deriva180, Fz, muy0/1000);

figure(2)
ax = gca;
set(ax, 'NextPlot', 'add', 'Box', 'on', 'XGrid', 'on', 'YGrid', 'on')
p = plot(deriva180 * 180/pi,-FyPacSem180, 'Color', 'm', 'Marker', 'd', 'MarkerFaceColor', 'm', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
p = plot(deriva180 * 180/pi,-FyPacCom180, 'Color', 'r', 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
plot([90 90],[0 3000],'--k')    % Linha vertical de simetria
xlabel('\(\alpha\) [grau]', 'Interpreter', 'Latex')
ylabel('\(F_y\) [N]', 'Interpreter', 'Latex')
l = legend('Pacejka without treatment', 'Pacejka with treatment ');
set(l, 'Interpreter', 'Latex', 'Location', 'SouthEast')

%%
% In the above figure we can see that the curve is symmetric to a vertical line positioned at \(\alpha = 90 [graus]\).
%
%% References
% [1] GILLESPIE, T. D. Fundamentals of vehicle dynamics. [S.l.]: Society of Automotive Engineers Warrendale, PA, 1992.
%
%% See Also
%
% <../../../index.html Home>
%


function asa006_test02 ( )

%*****************************************************************************80
%
%% TEST02 demonstrates the use of CHOLESKY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2008
%
%  Author:
%
%    John Burkardt
%
  n_max = 15;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02:\n' );
  fprintf ( 1, '  CHOLESKY computes the Cholesky factorization\n' );
  fprintf ( 1, '  of a positive definite symmetric matrix.\n' );
  fprintf ( 1, '  A compressed storage format is used.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Here we look at the Hilbert matrix\n' );
  fprintf ( 1, '  A(I,J) = 1 / ( I + J - 1 )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We expect errors to grow quickly with N.\n' );

  for n = 1 : n_max

    nn = ( n * ( n + 1 ) ) / 2;
%
%  Set A to the Hilbert matrix.
%
    k = 0;
    for i = 1 : n
      for j = 1 : i
        k = k + 1;
        a(k) = 1.0 / ( i + j - 1 );
      end
    end

    [ u, nullty, ifault ] = cholesky ( a, n, nn );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Matrix order N = %d\n', n );
    fprintf ( 1, '  Maxtrix nullity NULLTY = %d\n', nullty );

    k = 0;
    for j = 1 : n
      for i = 1 : j
        k = k + 1;
        ufull(i,j) = u(k);
      end
      for i = j + 1 : n
        ufull(i,j) = 0.0;
      end
    end
%
%  Compute U' * U and compare to A.
%
    k = 0;
    diff = 0.0;
    for i = 1 : n
      for j = 1 : i
        k = k + 1;
        utu = 0.0;
        for l = 1 : n
          utu = utu + ufull(l,i) * ufull(l,j);
        end
        diff = diff + ( a(k) - utu ) * ( a(k) - utu );
      end
    end

    diff = sqrt ( diff );

    fprintf ( 1,  '  RMS ( A - U''*U ) = %f\n', diff );
  end

  return
end


function geometry_test063 ( )

%*****************************************************************************80
%
%% TEST063 tests PLANE_NORMAL2EXP_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST063\n' );
  fprintf ( 1, '  PLANE_NORMAL2EXP_3D puts a plane defined by\n' );
  fprintf ( 1, '    point, normal form into explicit form.\n' );

  pp(1:3,1) = [ -1.0;   0.0; -1.0 ];
  normal(1:3,1) = [ -0.2672612; -0.5345225; -0.8017837 ];

  r8vec_print ( 3, pp, '  The point PP:' );

  r8vec_print ( 3, normal, '  Normal vector:' );

  [ p1, p2, p3 ] = plane_normal2exp_3d ( pp, normal );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  P1, P2, P3:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %12f  %12f  %12f\n', p1(1:3,1) );
  fprintf ( 1, '  %12f  %12f  %12f\n', p2(1:3,1) );
  fprintf ( 1, '  %12f  %12f  %12f\n', p3(1:3,1) );

  return
end


% QUATERNION DIVISION
function [qDiv] = qDivide(q,r)
% The function compute the division of quaternion q, by
% quaterion v.
% Associated block:
% "Quaternion Division"

qDiv = zeros(4,1);
% Normalise the second vector
qDiv(1) =  q(1)*r(1) + q(2)*r(2) + q(3)*r(3) + q(4)*r(4);
qDiv(2) = -q(1)*r(2) + q(2)*r(1) + q(3)*r(4) - q(4)*r(3);
qDiv(3) =  q(1)*r(3) + q(2)*r(4) + q(3)*r(1) + q(4)*r(2);
qDiv(4) = -q(1)*r(4) + q(2)*r(3) - q(3)*r(2) + q(4)*r(1);
qDiv = qDiv./(norm(r));
end


function y = TpFerwerda(x)
%
%        y = TpFerwerda(x)
%
%
%       The gamma function used in Ferwerda TMO for Photopic levels
%
%       Input:
%           -x: a value
%
%       Output:
%           -y: application of the gamma function
%
%
%     Copyright (C) 2010-15 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

t = log10(x);

if(t <= -2.6)
    y = -0.72;
else
    if(t >= 1.9)
        y = t - 1.255;
    else
        y = (0.249 * t + 0.65).^2.7 - 0.72;
    end
end

y = 10.^y;

end


%% Machine Learning Online Class
%  Exercise 5 | Regularized Linear Regression and Bias-Variance
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     linearRegCostFunction.m
%     learningCurve.m
%     validationCurve.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  The following code will load the dataset into your environment and plot
%  the data.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

% Load from ex5data1:
% You will have X, y, Xval, yval, Xtest, ytest in your environment
load ('ex5data1.mat');

% m = Number of examples
m = size(X, 1);

% Plot training data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');

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

%% =========== Part 2: Regularized Linear Regression Cost =============
%  You should now implement the cost function for regularized linear
%  regression.
%

theta = [1 ; 1];
J = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Cost at theta = [1 ; 1]: %f '...
         '\n(this value should be about 303.993192)\n'], J);

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

%% =========== Part 3: Regularized Linear Regression Gradient =============
%  You should now implement the gradient for regularized linear
%  regression.
%

theta = [1 ; 1];
[J, grad] = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Gradient at theta = [1 ; 1]:  [%f; %f] '...
         '\n(this value should be about [-15.303016; 598.250744])\n'], ...
         grad(1), grad(2));

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

%% =========== Part 4: Train Linear Regression =============
%  Once you have implemented the cost and gradient correctly, the
%  trainLinearReg function will use your cost function to train
%  regularized linear regression.
%
%  Write Up Note: The data is non-linear, so this will not give a great
%                 fit.
%

%  Train linear regression with lambda = 0
lambda = 0;
[theta] = trainLinearReg([ones(m, 1) X], y, lambda);

%  Plot fit over the data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
hold on;
plot(X, [ones(m, 1) X]*theta, '--', 'LineWidth', 2)
hold off;

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

%% =========== Part 5: Learning Curve for Linear Regression =============
%  Next, you should implement the learningCurve function.
%
%  Write Up Note: Since the model is underfitting the data, we expect to
%                 see a graph with "high bias" -- slide 8 in ML-advice.pdf
%

lambda = 0;
[error_train, error_val] = ...
    learningCurve([ones(m, 1) X], y, ...
                  [ones(size(Xval, 1), 1) Xval], yval, ...
                  lambda);

plot(1:m, error_train, 1:m, error_val);
title('Learning curve for linear regression')
legend('Train', 'Cross Validation')
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 150])

fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

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

%% =========== Part 6: Feature Mapping for Polynomial Regression =============
%  One solution to this is to use polynomial regression. You should now
%  complete polyFeatures to map each example into its powers
%

p = 8;

% Map X onto Polynomial Features and Normalize
X_poly = polyFeatures(X, p);
[X_poly, mu, sigma] = featureNormalize(X_poly);  % Normalize
X_poly = [ones(m, 1), X_poly];                   % Add Ones

% Map X_poly_test and normalize (using mu and sigma)
X_poly_test = polyFeatures(Xtest, p);
X_poly_test = bsxfun(@minus, X_poly_test, mu);
X_poly_test = bsxfun(@rdivide, X_poly_test, sigma);
X_poly_test = [ones(size(X_poly_test, 1), 1), X_poly_test];         % Add Ones

% Map X_poly_val and normalize (using mu and sigma)
X_poly_val = polyFeatures(Xval, p);
X_poly_val = bsxfun(@minus, X_poly_val, mu);
X_poly_val = bsxfun(@rdivide, X_poly_val, sigma);
X_poly_val = [ones(size(X_poly_val, 1), 1), X_poly_val];           % Add Ones

fprintf('Normalized Training Example 1:\n');
fprintf('  %f  \n', X_poly(1, :));

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

%% =========== Part 7: Learning Curve for Polynomial Regression =============
%  Now, you will get to experiment with polynomial regression with multiple
%  values of lambda. The code below runs polynomial regression with
%  lambda = 0. You should try running the code with different values of
%  lambda to see how the fit and learning curve change.
%

lambda = 1;
[theta] = trainLinearReg(X_poly, y, lambda);

% Plot training data and fit
figure(1);
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
plotFit(min(X), max(X), mu, sigma, theta, p);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
title (sprintf('Polynomial Regression Fit (lambda = %f)', lambda));

figure(2);
[error_train, error_val] = ...
    learningCurve(X_poly, y, X_poly_val, yval, lambda);
plot(1:m, error_train, 1:m, error_val);

title(sprintf('Polynomial Regression Learning Curve (lambda = %f)', lambda));
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 100])
legend('Train', 'Cross Validation')

fprintf('Polynomial Regression (lambda = %f)\n\n', lambda);
fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

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

%% =========== Part 8: Validation for Selecting Lambda =============
%  You will now implement validationCurve to test various values of
%  lambda on a validation set. You will then use this to select the
%  "best" lambda value.
%

[lambda_vec, error_train, error_val] = ...
    validationCurve(X_poly, y, X_poly_val, yval);

close all;
plot(lambda_vec, error_train, lambda_vec, error_val);
legend('Train', 'Cross Validation');
xlabel('lambda');
ylabel('Error');

fprintf('lambda\t\tTrain Error\tValidation Error\n');
for i = 1:length(lambda_vec)
	fprintf(' %f\t%f\t%f\n', ...
            lambda_vec(i), error_train(i), error_val(i));
end

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


% function [IDX_SC, Uk, Dk, time_SC] = SC(G,lap_type)
%
% This is an implementation of the classical Spectral CLustering algorithm
% using either:
% - the combinatorial Laplacian if lap_type='combinatorial' (in this case,
% the features are not normalized);
% - or the normalized Laplacian if lap_type='normalized' (in this case,
% the features are normalized).
% See the paper cited below for many references on this algorithm.
%
% Copyright (C) 2016 Nicolas Tremblay, Gilles Puy.
% This file is part of the CSCbox (Compressive Spectral Clustering toolbox)
%
% The CSCbox 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.
%
% The CSCbox 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 you use this toolbox please kindly cite
%     N. Tremblay, G. Puy, R. Gribonval and P. Vandergheynst.
%     Compressive Spectral Clustering.
%     ArXiv e-prints:1602.02018 Feb. 2016.

function [IDX_SC, Uk, Dk, time_SC] = SC(G,lap_type)

normBOOL=strcmp(lap_type,'normalized');
G = gsp_create_laplacian(G,lap_type);

% eigs
if isfield(G, 'G.Dk'), % if it was already calculated, do not do it again
    Dk=G.Dk;
    Uk=G.Uk;
    time_SC.eigs=G.time_SC_eigs;
else % run eigs
    tic;
    opts.isreal=1;opts.issym=1;opts.maxit=10000;
    [Uk,Dk]=eigs(G.L,G.k,'SA',opts);Dk=diag(Dk);
    time_SC.eigs=toc;
end

if normBOOL % kmeans on Yk
    tic;
    Yk=Uk./repmat(sqrt(sum(Uk.^2,2)),1,G.k);
    IDX_SC = kmeans(Yk, G.k,'Replicates',20);
    time_SC.kmeans=toc;
else % kmeans on Uk
    tic;
    IDX_SC = kmeans(Uk, G.k,'Replicates',20);
    time_SC.kmeans=toc;
end

time_SC.total=time_SC.eigs+time_SC.kmeans;


% StackExchange Signal Processing Q75231
% https://dsp.stackexchange.com/questions/75231
% Image Denoising Using Total Variation (TV) Minimization with ADMM
% References:
%   1.
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.000     18/05/2021
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

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

generateFigures = ON;

CONV_SHAPE_FULL     = 1;
CONV_SHAPE_SAME     = 2;
CONV_SHAPE_VALID    = 3;

%% Simulation Parameters

imageFileName = 'Lena256.png';

paramLambda = 0.025; %<! TV Norm

numIterations   = 250;
noiseStd        = 10 / 255;

%% Generate Data

mI = im2double(imread(imageFileName));

numRows = size(mI, 1);
numCols = size(mI, 2);

mY = mI + (noiseStd * randn(numRows, numCols));
vY = mY(:);

vXInit  = zeros(numRows * numCols, 1);
% Generate the Diff Operator (2D Gradient) by Finite Differences
mD      = CreateGradientOperator(numRows, numCols);

% Objective Function
hObjFun = @(vX) (0.5 * sum( (vX - vY) .^ 2)) + (paramLambda * sum(abs(mD * vX)));

% Analysis
solverIdx       = 0;
cMethodString   = {};

mObjFunValMse   = zeros(numIterations, 1);
mSolMse         = zeros(numIterations, 1);

%% Display Data

figureIdx = figureIdx + 1;

hFigure = figure('Position', [100, 100, 542, 642]);
hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [010, 326, 256, 256]);
hImgObj = image(repmat(mI, [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Reference Image']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [276, 326, 256, 256]);
hImgObj = image(repmat(mY, [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Input Image (Noisy)']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

%% Solution by CVX

solverString = 'CVX';

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

hRunTime = tic();

cvx_begin('quiet')
    % cvx_precision('best');
    variable vX(numRows * numCols);
    minimize( (0.5 * sum_square(vX - vY)) + (paramLambda * norm(mD * vX, 1)) );
cvx_end

runTime = toc(hRunTime);

DisplayRunSummary(solverString, hObjFun, vX, runTime, cvx_status);

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

%% Display Result

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [010, 010, 256, 256]);
hImgObj = image(repmat(reshape(vX, numRows, numCols), [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Denoised Image - CVX']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

%% Solution by ADMM
%{
Solving:

$$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| x - y \right|}_{2}^{2} + \lambda {\left\| D x \right\|}_{1} $$
%}

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['ADMM'];

hRunTime = tic();

[vX, mX] = SolveProxTvAdmm(vXInit, vY, mD, paramLambda, 'numIterations', numIterations);

runTime = toc(hRunTime);

DisplayRunSummary(cLegendString{solverIdx}, hObjFun, vX, runTime);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Display Results

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [276, 010, 256, 256]);
hImgObj = image(repmat(reshape(vX, numRows, numCols), [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Denoised Image - ADMM']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

figureIdx = figureIdx + 1;

hFigure = DisplayComparisonSummary(numIterations, mObjFunValMse, mSolMse, cLegendString, figPosLarge, lineWidthNormal, fontSizeTitle, fontSizeAxis);

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

%% Restore Defaults

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


function f = laplacian(f)
%LAPLACIAN   Scalar laplacian of a SPHEREFUN.
%   L = LAPLACIAN(F) computes the spherical Laplacian of the SPHEREFUN F.
%
% This is gvien, in intrinsic spherical coordinates as
%
% laplacian(F) = d_{theta} d_{theta} F + tan(theta) d_{theta} F +
%                1/sin(theta) + d_{lambda} d_{lambda} F
%
% where 0 <= theta <= pi is the co-latitude variable and
% -pi <= lambda <= pi  is the longitude variable.
%
% See also GRADIENT, DIVERGENCE.

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

% We implement the computation of the Laplacian using the spherefun/diff
% command.  This function computes tangential derivatives in the x, y, and
% z directions, so that the Laplacian can be implemented as we usually
% think of it in 3D, i.e.
% laplacian(f) = f_xx + f_yy + f_zz.

% Compute the derivatives
fxx = diff(f, 1, 2);
fyy = diff(f, 2, 2);
fzz = diff(f, 3, 2);

% Determine which derivative has the most coefficients in its
% representation for the columns and rows.
[mxx,nxx] = length(fxx);
[myy,nyy] = length(fyy);
[mzz,nzz] = length(fzz);

m = max([mxx myy mzz]);
n = max([nxx nyy nzz]);

% Ensure m and n are even:
m = m + mod(m, 2);
n = n + mod(n, 2);

% Construct a spherefun by sampling the derivatives on the same size grid.
f = spherefun(sample(fxx, m, n/2) + sample(fyy, m, n/2) + ...
               sample(fzz, m, n/2));

end


function G = rand_inverse_wishart(m, v, H_inv_upper_chol)

% function G = rand_inverse_wishart(m, v, H_inv_upper_chol)
% rand_inverse_wishart  Pseudo random matrices drawn from an
% inverse Wishart distribution
% G = rand_inverse_wishart(m, v, H_inv_upper_chol)
% Returns an m-by-m matrix drawn from an inverse-Wishart distribution.
%
% INPUTS:
%     m:          dimension of G and H_inv_upper_chol.
%     v:          degrees of freedom, greater or equal than m.
%     H_inv_chol: upper cholesky decomposition of the inverse of the
%                 matrix parameter.
%                 The upper cholesky of the inverse is requested here
%                 in order to avoid to recompute it at every random draw.
%                 H_inv_upper_chol = chol(inv(H))
% OUTPUTS:
%     G:          G ~ IW(m, v, H) where H = inv(H_inv_upper_chol'*H_inv_upper_chol)
%                 or, equivalently, using the correspondence between Wishart and
%                 inverse-Wishart: inv(G) ~ W(m, v, S) where
%                 S = H_inv_upper_chol'*H_inv_upper_chol = inv(H)
%
% SPECIAL REQUIREMENT
%     none
%

% Copyright (C) 2003-2009 Dynare Team
%
% This file is part of Dynare.
%
% Dynare 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.
%
% Dynare 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 Dynare.  If not, see <http://www.gnu.org/licenses/>.

X = randn(v, m) * H_inv_upper_chol;

% At this point, X'*X is Wishart distributed
% G = inv(X'*X);

% Rather compute inv(X'*X) using the SVD
[U,S,V] = svd(X, 0);
SSi = 1 ./ (diag(S) .^ 2);
G = (V .* repmat(SSi', m, 1)) * V';


function fx1 = p01_fx1 ( x )

%*****************************************************************************80
%
%% P01_FX1 evaluates the derivative of the function for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX1, the first derivative of the function at X.
%
  fx1 = cos ( x ) - 0.5;

  return
end


function [ p, l, u ] = plu_plu ( n, pivot )

%*****************************************************************************80
%
%% PLU_PLU returns the PLU factors of the PLU matrix.
%
%  Example:
%
%    Input:
%
%      N = 5
%      PIVOT = ( 1, 3, 3, 5, 5 )
%
%    Output:
%
%      P:
%
%          1             0            0             0            0
%          0             0            1             0            0
%          0             1            0             0            0
%          0             0            0             0            1
%          0             0            0             1            0
%
%      L:
%
%         1              0            0             0            0
%         0.25           1            0             0            0
%         0.125          0.375        1             0            0
%         0.0625         0.1875       0.3125        1            0
%         0.03125        0.09375      0.15625       0.21875      1
%
%      U:
%
%        11             12           13            14           15
%         0             22           23            24           25
%         0              0           33            34           35
%         0              0            0            44           45
%         0              0            0             0           55
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer PIVOT(N), the list of pivot rows.  PIVOT(I)
%    must be a value between I and N, reflecting the choice of
%    pivot row on the I-th step.  For no pivoting, set PIVOT(I) = I.
%
%    Output, real P(N,N), L(N,N), U(N,N), the P, L and U factors
%    of A, as defined by Gaussian elimination with partial pivoting.
%    P is a permutation matrix, L is unit lower triangular, and U
%    is upper trianguler.
%

%
%  Check that the pivot vector is legal.
%
  for i = 1 : n

    if ( pivot(i) < i )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PLU_PLU - Fatal error!\n' );
      fprintf ( 1, '  PIVOT(%d) = %d\n', i, pivot(i) );
      fprintf ( 1, '  but PIVOT(I) must be no less than I!\n' );
      error ( 'PLU_PLU - Fatal error!' )
    elseif ( n < pivot(i) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PLU_PLU - Fatal error!\n' );
      fprintf ( 1, '  PIVOT(%d) = %d\n', i, pivot(i) );
      fprintf ( 1, '  but PIVOT(I) must be no greater than N\n' );
      fprintf ( 1, '  and N = %d\n', n );
      error ( 'PLU_PLU - Fatal error!' )
    end

  end
%
%  Compute U.
%
  u = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n

      if ( i <= j )
        u(i,j) = 10 * i + j;
      else
        u(i,j) = 0.0;
      end

    end
  end
%
%  Compute L.
%
  l = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n

      if ( i < j )
        l(i,j) = 0.0;
      elseif ( j == i )
        l(i,j) = 1.0;
      else
        l(i,j) = ( 2 * j - 1 ) / 2^i;
      end

    end
  end
%
%  Compute P.
%
  p = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        p(i,j) = 1.0;
      else
        p(i,j) = 0.0;
      end
    end
  end
%
%  Apply the pivot permutations, in reverse order.
%
  for i = n : -1 : 1

    if ( pivot(i) ~= i )
      for j = 1 : n
        temp          = p(i,j);
        p(i,j)        = p(pivot(i),j);
        p(pivot(i),j) = temp;
      end
    end

  end

  return
end


function B = bound2im(b, M, N, x0, y0)
%BOUND2IM Converts a boundary to an image.
%   B = BOUND2IM(b) converts b, an np-by-2 or 2-by-np array
%   representing the integer coordinates of a boundary, into a binary
%   image with 1s in the locations defined by the coordinates in b
%   and 0s elsewhere.
%
%   B = BOUND2IM(b, M, N) places the boundary approximately centered
%   in an M-by-N image. If any part of the boundary is outside the
%   M-by-N rectangle, an error is issued.
%
%   B = BOUND2IM(b, M, N, X0, Y0) places the boundary in an image of
%   size M-by-N, with the topmost boundary point located at X0 and
%   the leftmost point located at Y0. If the shifted boundary is
%   outside the M-by-N rectangle, an error is issued. XO and X0 must
%   be positive integers.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.6 $  $Date: 2003/06/14 16:21:28 $

[np, nc] = size(b);
if np < nc
   b = b'; % To convert to size np-by-2.
   [np, nc] = size(b);
end

% Make sure the coordinates are integers.
x = round(b(:, 1));
y = round(b(:, 2));

% Set up the default size parameters.
x = x - min(x) + 1;
y = y - min(y) + 1;
B = false(max(x), max(y));
C = max(x) - min(x) + 1;
D = max(y) - min(y) + 1;

if nargin == 1
   % Use the preceding default values.
elseif nargin == 3
   if C > M | D > N
      error('The boundary is outside the M-by-N region.')
   end
   % The image size will be M-by-N. Set up the parameters for this.
   B = false(M, N);
   % Distribute extra rows approx. even between top and bottom.
   NR = round((M - C)/2);
   NC = round((N - D)/2); % The same for columns.
   x = x + NR; % Offset the boundary to new position.
   y = y + NC;
elseif nargin == 5
   if x0 < 0 | y0 < 0
      error('x0 and y0 must be positive integers.')
   end
   x = x + round(x0) - 1;
   y = y + round(y0) - 1;
   C = C + x0 - 1;
   D = D + y0 - 1;
   if C > M | D > N
      error('The shifted boundary is outside the M-by-N region.')
   end
   B = false(M, N);
else
   error('Incorrect number of inputs.')
end

B(sub2ind(size(B), x, y)) = true;


function [lng] = spm_lg_gamma(p,b)
% Log of generalised gamma function
% FORMAT [lng] = spm_lg_gamma(p,b)
%
% p       - dimension parameter
% b       - degrees of freedom type parameter
%__________________________________________________________________________
%
% References:
% * Bayesian Inference in Statistical Analysis, Box & Tiao, 1992, p. 427.
% * Aspects of Multivariate Statistical Theory, R.J. Muirhead, p. 62.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_lg_gamma.m 2784 2009-02-24 19:11:20Z guillaume $

if b <= (p-1)/2
   warning('Parameter out of range');
   lng = NaN;
   return
end

lng = (p*(p-1)/2) * gammaln(0.5);
for alpha = 1:p
  lng = lng + gammaln(b+0.5*(alpha-1));
end


function [Xnorm] = L12norm(X)
% ||X||_{1,2} = sum_i||X^i||_2
Xnorm = sum(sqrt(sum(X.^2,2)));


function [xd yd] = mapping_homography(xs, ys, mode, Theta)

% [xd yd] = mapping_homography(xs, ys, mode, Theta)
%
% DESC:
% Apply the homography mapping to the source points (xs, ys)
%
% AUTHOR
% Marco Zuliani - marco.zuliani@gmail.com
%
% VERSION:
% 1.0.0
%
% INPUT:
% xs, ys            = source points
% mode              = true for forward mapping, false for backward mapping
% Theta             = parameters of the mapping function
%
% OUTPUT:
% xd, yd            = destination points
%
% SEE ALSO: imcanvas

% HISTORY
% 1.0.0             - 11/28/08 - Initial version

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check for the input parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% preliminary operations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
H = reshape(Theta, 3, 3);

xs = transpose(xs(:));
ys = transpose(ys(:));
Xs = [xs; ys];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% mapping
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mode
    % forward mapping
    Xd = homo2cart(H*cart2homo(Xs));
else
    % backward mapping
    Xd = homo2cart(H\cart2homo(Xs));
end;

% make sure the points are returned as row vectors
xd = Xd(1,:);
yd = Xd(2,:);

return;


% Author: Jai Juneja
% Date: 12/02/2013
%
% Function transforms a point p_r from robot's local frame to the global
% frame.
%
% Inputs:
%   r = [x y alpha]'	:   Robot frame
%   p_r                 :	Point in robot's frame (2*n matrix for n points)
%
% Outputs:
%   p_g                 :   Point in global frame (2*n matrix)
%   Optional:
%   PG_r                :   Jacobian of p_g wrt. robot frame
%   PG_pr               :   Jacobian of p_g wrt. p_r
function [p_g, PG_r, PG_pr] = transToGlobal(r, p_r)

    r_pos = r(1:2);     % Robot's position
    r_ang = r(3);       % Robot's orientation

    % Transformation matrix:
    R = [cos(r_ang), -sin(r_ang) ; ...
        sin(r_ang), cos(r_ang)];

    % Hence point p_r tranformed to global co-ordinate frame is:
    p_g = R * p_r;
    p_g = [p_g(1, :) + r_pos(1); p_g(2, :) + r_pos(2)];

    if nargout > 1      % Compute Jacobians
        pr_x = p_r(1); pr_y = p_r(2);

        PG_r = [...
            1,  0,  - pr_y*cos(r_ang) - pr_x*sin(r_ang); ...
            0,  1,  pr_x*cos(r_ang) - pr_y*sin(r_ang)];

        PG_pr = R;
    end
end


% SOLVE_LINEAR_ELASTICITY: Solve a linear elasticity problem on a NURBS domain.
%                          For a planar domain it is the plane strain model.
%
% The function solves the linear elasticity problem
%
%      - div (sigma(u)) = f    in Omega = F((0,1)^n)
%      sigma(u) \cdot n = g    on Gamma_N
%                     u = h    on Gamma_D
%
% with   sigma(u) = mu*(grad(u) + grad(u)^t) + lambda*div(u)*I.
%
%   u:          displacement vector
%   sigma:      Cauchy stress tensor
%   lambda, mu: Lame' parameters
%   I:          identity tensor
%
% USAGE:
%
%  [geometry, msh, space, u] = solve_linear_elasticity (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - press_sides:  sides with pressure boundary condition (may be empty)
%    - symm_sides:   sides with symmetry boundary condition (may be empty)
%    - lambda_lame:  first Lame' parameter
%    - mu_lame:      second Lame' parameter
%    - f:            source term
%    - h:            function for Dirichlet boundary condition
%    - g:            function for Neumann condition (if nmnn_sides is not empty)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry: geometry structure (see geo_load)
%  msh:      mesh object that defines the quadrature rule (see msh_cartesian)
%  space:    space object that defines the discrete basis functions (see sp_vector)
%  u:        the computed degrees of freedom
%
% See also EX_LIN_ELAST_HORSESHOE for an example.
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
%    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/>.

function [geometry, msh, sp, u] = ...
              solve_linear_elasticity (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
geometry = geo_load (geo_name);
degelev  = max (degree - (geometry.nurbs.order-1), 0);
nurbs    = nrbdegelev (geometry.nurbs, degelev);
[rknots, zeta, nknots] = kntrefine (nurbs.knots, nsub-1, nurbs.order-1, regularity);

nurbs    = nrbkntins (nurbs, nknots);
geometry = geo_load (nurbs);

% Construct msh structure
rule     = msh_gauss_nodes (nquad);
[qn, qw] = msh_set_quad_nodes (geometry.nurbs.knots, rule);
msh      = msh_cartesian (geometry.nurbs.knots, qn, qw, geometry);

% Construct space structure
space_scalar = sp_nurbs (nurbs, msh);
scalar_spaces = repmat ({space_scalar}, 1, msh.rdim);
sp = sp_vector (scalar_spaces, msh);
clear space_scalar scalar_spaces

% Assemble the matrices
mat    = op_su_ev_tp (sp, sp, msh, lambda_lame, mu_lame);
rhs    = op_f_v_tp (sp, msh, f);

% Apply Neumann boundary conditions
for iside = nmnn_sides
% Restrict the function handle to the specified side, in any dimension, gside = @(x,y) g(x,y,iside)
  gside = @(varargin) g(varargin{:},iside);
  dofs = sp.boundary(iside).dofs;
  rhs(dofs) = rhs(dofs) + op_f_v_tp (sp.boundary(iside), msh.boundary(iside), gside);
end

% Apply pressure conditions
for iside = press_sides
  msh_side = msh_eval_boundary_side (msh, iside);
  sp_side  = sp_eval_boundary_side (sp, msh_side);

  x = cell (msh_side.rdim, 1);
  for idim = 1:msh_side.rdim
    x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
  end
  pval = reshape (p (x{:}, iside), msh_side.nqn, msh_side.nel);

  rhs(sp_side.dofs) = rhs(sp_side.dofs) - op_pn_v (sp_side, msh_side, pval);
end

% Apply symmetry conditions
symm_dofs = [];
for iside = symm_sides
  if (~strcmpi (sp.transform, 'grad-preserving'))
    error ('The symmetry condition is only implemented for spaces with grad-preserving transform')
  end
  msh_side = msh_eval_boundary_side (msh, iside);
  for idim = 1:msh.rdim
    normal_comp(idim,:) = reshape (msh_side.normal(idim,:,:), 1, msh_side.nqn*msh_side.nel);
  end

  parallel_to_axes = false;
  for ind = 1:msh.rdim
    ind2 = setdiff (1:msh.rdim, ind);
    if (all (all (abs (normal_comp(ind2,:)) < 1e-10)))
      symm_dofs = union (symm_dofs, sp.boundary(iside).dofs(sp.boundary(iside).comp_dofs{ind}));
      parallel_to_axes = true;
      break
    end
  end
  if (~parallel_to_axes)
    error ('solve_linear_elasticity: We have only implemented the symmetry condition for boundaries parallel to the axes')
  end

end

% Apply Dirichlet boundary conditions
u = zeros (sp.ndof, 1);
[u_drchlt, drchlt_dofs] = sp_drchlt_l2_proj (sp, msh, h, drchlt_sides);
u(drchlt_dofs) = u_drchlt;

int_dofs = setdiff (1:sp.ndof, [drchlt_dofs, symm_dofs]);
rhs(int_dofs) = rhs(int_dofs) - mat (int_dofs, drchlt_dofs) * u_drchlt;

% Solve the linear system
u(int_dofs) = mat(int_dofs, int_dofs) \ rhs(int_dofs);

end


function [xmin, fmin] = golden(npd, f, ax, bx, cx, tol, varargin)
%
%GOLDEN   Minimize function of one variable using golden section search
%
%   [xmin, fmin] = golden(npd, f, ax, bx, cx, tol) computes a local minimum
%   of f. xmin is the computed local minimizer of f and fmin is
%   f(xmin). xmin is computed to an relative accuracy of TOL.
%
%   The parameters ax, bx and cx must satisfy the following conditions:
%   ax < bx < cx, f(bx) < f(ax) and f(bx) < f(cx).
%
%   xmin satisfies ax < xmin < cx. golden is guaranteed to succeed if f
%   is continuous between ax and cx
%
%   Roman Geus, ETH Zuerich, 9.12.97
%
%   ATI -- added "npd" argument & made private to KDE class
C = (3-sqrt(5))/2;
R = 1-C;

x0 = ax;
x3 = cx;
if (abs(cx-bx) > abs(bx-ax)),
  x1 = bx;
  x2 = bx + C*(cx-bx);
else
  x2 = bx;
  x1 = bx - C*(bx-ax);
end
f1 = feval(f,x1,npd,varargin{:});
f2 = feval(f,x2,npd,varargin{:});

k = 1;
while abs(x3-x0) > tol*(abs(x1)+abs(x2)),
%  fprintf(1,'k=%4d, |a-b|=%e\n', k, abs(x3-x0));
  if f2 < f1,
    x0 = x1;
    x1 = x2;
    x2 = R*x1 + C*x3;   % x2 = x1+c*(x3-x1)
    f1 = f2;
    f2 = feval(f,x2,npd,varargin{:});
  else
    x3 = x2;
    x2 = x1;
    x1 = R*x2 + C*x0;   % x1 = x2+c*(x0-x2)
    f2 = f1;
    f1 = feval(f,x1,npd,varargin{:});
  end
  k = k+1;

%  [x0,x1,x2,x3,f1,f2]
end

if f1 < f2,
  xmin = x1;
  fmin = f1;
else
  xmin = x2;
  fmin = f2;
end


function asa103_test01 ( )

%*****************************************************************************80
%
%% TEST01 demonstrates the use of DIGAMMA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01:\n' );
  fprintf ( 1, '  DIGAMMA computes the Digama or Psi function.\n' );
  fprintf ( 1, '  Compare the result to tabulated values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          X         ' );
  fprintf ( 1, 'FX                        FX2\n' );
  fprintf ( 1, '                    ' );
  fprintf ( 1, '(Tabulated)               (DIGAMMA)               DIFF\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = psi_values ( n_data );

    if ( n_data == 0 )
      break
    end

    [ fx2, ifault ] = digamma ( x );

    fprintf ( 1, '  %12.4f  %24.16e  %24.16e  %10.4e\n', ...
    x, fx, fx2, abs ( fx - fx2 ) );

  end

  return
end


function p=get_required_precision(d,epsilon)
% Finds the number of decimal positions required to print all data
% elements.
%
% P=GET_REQUIRED_PRECISION(D) returns the least number of decimals
% 'after the period' to represent all numbers D without loss of precision.
% D should be an array with real numbers and can be of any size.
%
% P=GET_REQUIRED_PRECISION(D,EPSILON) allows for an imprecision of
% at most EPSILON.
%
% In the current implementation, this function always returns a value
% between 0 and 7 (inclusive).
%
% Examples: GET_REQUIRED_PRECISION([2 5])       returns 0
%           GET_REQUIRED_PRECISION([2 5.532])   returns 3
%
% NNO Jan 2010

if nargin<2
    epsilon=0; % we want high precision of course
end

% max precision is 7, because that seems more or less the limit of 32 bit
% floats

for p=0:7
    if max(abs(rem(d(:),10^(-p))))<=epsilon
        return
    end
end


%%
% Simple example of gradient flow in metric space.

rep = '../results/gradflow-metric/';
[~,~] = mkdir(rep);

f = @(x,y)x.^2 + y.^2;
Gx = @(x,y)2*x;
Gy = @(x,y)2*y;

% background image
N = 256;
tx = linspace(-.05,1,N);
ty = linspace(-.08,.5,N);
[Y,X] = meshgrid(ty,tx);
F = f(X,Y);
clf; hold on;
imagesc(tx,ty,F');
contour(tx,ty,F', 15, 'k');

% for the implicit stepping
N1 = 2048;
tx1 = linspace(min(tx),max(tx),N1);
ty1 = linspace(min(ty),max(ty),N1);
[Y1,X1] = meshgrid(ty1,tx1);

plist = [1 1];
plist = [1 1.3 1.5 1.8 2 3 5 10 Inf];

gmode = 'explicit';
gmode = 'implicit';

tau = .25; niter = 70;

lgd = {};
for ip=1:length(plist)
    p=plist(ip);

    % conjugate exponent
    q = conjexp(p);
    ri = @(r,p)sign(r).*abs(r).^(1/(p-1));

    grad = @(gx,gy)ri([gx, gy],p);
    if p==1
        grad = @(gx,gy)[gx.*(gx==max(gx,gy)), gy.*(gy==max(gx,gy))];
    elseif p==Inf
        grad = @(gx,gy)[sign(gx),sign(gy)];
    end
    grad = @(gx,gy)grad(gx,gy)*norm([gx,gy],q)/norm(grad(gx,gy),q);

    % initial point
    x = .95; y = .45;
    U = []; V = [];
    % do the descent
    for i=1:niter
        U(end+1) = x; V(end+1) = y;
        switch gmode
            case 'explicit'
                g = grad(Gx(x,y), Gy(x,y));
                x = x - tau*g(1);
                y = y - tau*g(2);
            case 'implicit'
                E = ( abs(X1-x).^p + abs(Y1-y).^p ).^(2/p) + tau * ( X1.^2 + Y1.^2 );
                if p==Inf
                    E = max(abs(X1-x),abs(Y1-y)).^2 + tau * ( X1.^2 + Y1.^2 );
                end
                [tmp,i] = min(E(:));
                x = X1(i); y = Y1(i);
        end
    end

    lgd{end+1} = ['p=' num2str(p)];
    c = (ip-1)/(length(plist)-1);
    plot(U,V, '.-', 'Color', [c 0 1-c], 'LineWidth', 2, 'MarkerSize', 23);
    colormap parula(256);
    axis image; axis off;
    axis([min(tx) max(tx) min(ty) max(ty)]);
    drawnow;

end

saveas(gcf, [rep 'gradflow-' gmode '.eps'], 'epsc');


function [wc,w0,a0,ak,bk,c0,ck]=get_harmonics(y,pas)
%[wc,w0,a0,ak,bk,c0,ck]=get_harmonics(y,pas)
% given a signal x(t),y=x(t)
% the function get_harmoniques returns the pulse w0(rd/s) of the signal y(t)
% the coefficients ak (a1,a2,...)  and bk (b1,b2,...)  of the trigonomical
% representation  of the  signal y(t):
% y(t)= a0/2+ a1.cos(w0t)+b1.sin(w0t)+a2.cos(2w0t) +b2*sin(t*w0)+...
% it also returns the coefficients c0 and ck of the complex representation
% y(t)= ...+c(-1).exp(-jw0t)+c(0)+c(1).exp(jw0t)+c(2).exp(2jw0t)+...
% wc is the interval pulse;
% to plot the amplitude spectrum: stem(wc,abs(ck))
%[wc,w0,a0,ak,bk,c0,ck]=get_harmoniques(y,pas)
% be a signal x(t).
% For more theorical details look at www.azzimalik.com
N=length(y);
Y=fft(y);
if size(Y,2)==1;
    Y=Y';
end
w=2*(0:(N-1))*pi/(N*pas);
w0=w(2);
nc2=fix(N/2);
if nc2~=N/2;
    nc3=nc2+1;
    wc=[fliplr(-w(2:nc2+1)) w(1:nc2+1)];
    Yc=[Y(nc2+2:N) Y(1:nc2+1)];Yc=Yc/N;
else
    nc3=nc2;
    wc=[fliplr(-w(2:nc2+1)) w(1:nc2+1)];
    Yc=[Y(nc2+1:N) Y(1:nc2+1)];Yc=Yc/N;
end
%-------------------------------------------------------------
N1=length(Yc);
for l=2:nc2+1
    ak(l-1)=real(Y(l)+Y(N-l+2))/N;
    bk(l-1)=-imag(Y(l)-Y(N-l+2))/N;
    ckp(l-1)=(ak(l-1)-j*bk(l-1))/2;
    ckm(l-1)=(ak(l-1)+j*bk(l-1))/2;
end
a0=2*Y(1)/N;
c0=a0/2;ck=[fliplr(ckm) c0 ckp];


function h = p31_h ( n, x )

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

  m = 5;
  a = [ 4.0, 4.0, 4.0, 4.0; ...
        1.0, 1.0, 1.0, 1.0; ...
        8.0, 8.0, 8.0, 8.0; ...
        6.0, 6.0, 6.0, 6.0; ...
        3.0, 7.0, 3.0, 7.0 ]';

  c = [ 0.1, 0.2, 0.2, 0.4, 0.6 ]';

  for ii = 1 : n
    for jj = 1 : n
      for j = 1 : m
        d = c(j) + sum ( ( x(1:n) - a(1:n,j) ).^2 );
        h(ii,jj) = h(ii,jj) - 8.0 * ( x(ii) - a(ii,j) ) * ( x(jj) - a(jj,j) ) / d^3;
        if ( ii == jj )
          h(ii,jj) = h(ii,jj) + 2.0 / d^2;
        end
      end
    end
  end

  return
end


function phi = basis_brick8 ( n, p )

%*****************************************************************************80
%
%% BASIS_BRICK8: BRICK8 basis functions at natural coordinates.
%
%  Discussion:
%
%      8------7        t  s
%     /|     /|        | /
%    5------6 |        |/
%    | |    | |        0-------r
%    | 4----|-3
%    |/     |/
%    1------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), natural coordinates of evaluation
%    points.
%
%    Output, real PHI(8,N), the basis function values.
%
  phi(1,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(2,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(3,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(4,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(5,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(6,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(7,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(8,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) );

  phi(1:8,1:n) = phi(1:8,1:n) / 8.0;

  return
end


function f = p02_f ( n, x )

%*****************************************************************************80
%
%% P02_F evaluates the objective function for problem 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = 0.0;

  for i = 1 : 13

    c = - i / 10.0;

    fi = x(3)     * exp ( c * x(1) )         - x(4) * exp ( c * x(2) ) ...
       + x(6)     * exp ( c * x(5) )         -        exp ( c ) ...
       + 5.0  * exp ( 10.0 * c ) - 3.0  * exp ( 4.0 * c );

    f = f + fi * fi;

  end

  return
end


classdef kMeans < handle
%
% k-means++: The Advantages of Careful Seeding", by David Arthur and
% Sergei Vassilvitskii, SODA 2007.

  properties
    n      % number of clusters
    center % cluster center
  end

  methods

    function km = kMeans(n)
      if nargin == 1, km.n = n; end
    end

    function initSeeds(km,obj)

      % Choose one center uniformly at random from among the data points.
      km.center = obj(randi(length(obj)));
      D = inf(length(obj),1);

      for k = 2:km.n

        %For each data point x, compute D(x), the distance between x and
        %the nearest center that has already been chosen.
        D = min(D,angle(obj,km.center(k-1)));

        %Choose one new data point at random as a new center, using a
        %weighted probability distribution where a point x is chosen with
        %probability proportional to D(x)2.
        id = discretesample(D.^2,1);
        km.center(k) = obj(id);

      end

    end

    function [label, center] = doClustering(km,obj)

      km.initSeeds(obj);

      label = find(km.center,obj);
      label_old = nan(size(label));

      % computes new centers until the labels do not change anymore
      while any(label(:) ~= label_old(:))

        % compute new centers
        for i = 1:km.n
          km.center(i) = mean(obj(label == i));
        end

        % store old labels
        label_old = label;

        % assign new labels
        label = find(km.center,obj);

      end

      center = km.center;

    end

  end

end


function [a,b,alpha,p,chiopt,Cab,Calphap]=...
    wtls_line(xin,yin,uxin,uyin,guck);
% function [a,b,alpha,p,chiopt,Cab,Calphap]=...
%   wtls_line(xin,yin,uxin,uyin,guck);
%
% weighted total least squares (wtls) fit of a straigth line
% to a set of points with uncertainties in both coordinates
%
% input:        xin     abscissa vector
%               yin     ordinate vector
%               uxin    (standard) uncertainties of xin, same size as xin
%               uyin    (standard) uncertainties of yin, same size as yin
%               guck    flag, if >0, chisquare is plotted over whole
%                       alpha-interval, optimum alpha is shown as red +
%
% output:       a, b    usual straight line parameters
%                       y=a*x+b
%               alpha,p more stable parametrisation
%                       y*cos(alpha)-x*sin(alpha)-p=0
%                       alpha: slope angle in radians
%                       p: distance of straight line from (0,0)
%                       conversion: a=tan(alpha),b=p/cos(alpha)
%               chiopt  minimum chisquare found
%               Cab     covariances, [var(a),var(b),cov(a,b)]
%               Calphap covariances, [var(alpha),var(p),cov(alpha,p)]
%
% algorithm:    M. Krystek & M. Anton
%               Physikalisch-Technische Bundesanstalt Braunschweig, Germany
%               Meas. Sci. Tech. 18 (2007), pp3438-3442
%
% tested for Matlab 6 and Matlab 7
% testdata: script file pearson_york_testdata.m
%
% 2007-03-08

tol=1e-6; %"tolerance" parameter of fnimbnd, see there

global ux uy x y
% inputs
if nargin<5,guck=0;end
% force column vectors
x=xin(:);
y=yin(:);
ux=uxin(:);
uy=uyin(:);
% "initial guess":
p0=polyfit(x,y,1);
alpha0=atan(p0(1));
% one-dimensional search, use p=p^
[alphaopt,chiopt,exitflag] = ...
    fminbnd(@chialpha,alpha0-pi/2,alpha0+pi/2,optimset('TolX',tol));
% get optimum p from alphaopt
alpha=alphaopt;
uk2=ux.^2*sin(alpha)^2+uy.^2*cos(alpha)^2;
u2=1./mean(1./uk2);
w=u2./uk2;
xbar=mean(w.*x);
ybar=mean(w.*y);
p=ybar*cos(alpha)-xbar*sin(alpha);
% convert to a, b parameters y=a*x+b
a=sin(alpha)/cos(alpha);
b=p/cos(alpha);
% --- uncertainty calculation, covariance matrix = 2*inv(Hessian(chi2)) ---
n=length(x);
vk=y.*cos(alpha)-x*sin(alpha)-p;
vka=-y*sin(alpha)-x*cos(alpha);
vkaa=-vk-p;
fk=vk.*vk;
fka=2*vk.*vka;
fkaa=2*(vka.^2+vk.*vkaa);
gk=uk2;
gka=2*sin(alpha)*cos(alpha)*(ux.^2-uy.^2);
gkaa=2*(ux.^2-uy.^2)*(cos(alpha)^2-sin(alpha)^2);
Hpp=2*n/u2;
Halphap=-2*sum((vka.*gk-gka.*vk)./gk.^2);
Halphaalpha=...
    sum(fkaa./gk-2*fka.*gka./gk.^2+2*gka.^2.*fk./gk.^3-gkaa.*fk./gk.^2);
NN=2/(Hpp*Halphaalpha-Halphap^2);
var_p=NN*Halphaalpha;
var_alpha=NN*Hpp;
cov_alphap=-NN*Halphap;
Calphap=[var_alpha,var_p,cov_alphap];
% ------ convert to a & b covariance matrix, following DIN 1319 (4)
var_a=var_alpha/cos(alpha)^4;
var_b=(var_alpha*p*p*sin(alpha)^2+var_p*cos(alpha)^2+...
    2*cov_alphap*p*sin(alpha)*cos(alpha))/cos(alpha)^4;
cov_ab=(var_alpha*p*sin(alpha)+cov_alphap*cos(alpha))/cos(alpha)^4;
Cab=[var_a,var_b,cov_ab];
% ------ end of uncertainty calculation -----------------------------------
%
%-------------------- plotting section ------------------------------------
if guck~=0
    alphatest=linspace(alpha-pi/2,alpha+pi/2,1000);
    for k=1:length(alphatest),chitest(k)=chialpha(alphatest(k));end
    plot(alphatest,chitest,alphaopt,chiopt,'+r')
    xlabel('alpha')
    ylabel('\chi^2')
    grid on
    disp([mfilename,' :: paused, hit any key to continue'])
    pause
end
% ------------------ end of plotting section ------------------------------
return
% ------------------ subfunction ------------------------------------------
function chi=chialpha(alpha)
    global ux uy x y
    uk2=ux.^2*sin(alpha)^2+uy.^2*cos(alpha)^2;
    u2=1./mean(1./uk2);
    w=u2./uk2;
    xbar=mean(w.*x);
    ybar=mean(w.*y);
    p=ybar*cos(alpha)-xbar*sin(alpha);
    chi=sum((y*cos(alpha)-x*sin(alpha)-p).^2./uk2);
return
% ----------- end of subfunction ------------------------------------------


function [theta,b,QMin]=LTSRegression(x,y,h)
%%LTSREGRESSION Perform least trimmed squares (LTS) regression using the
%               fast, approximate algorithm of [1] that is identified as
%               best for <600 data points. This means find theta and b that
%               minimizes
%               sum_{i=1}^h r_i^2
%               where r_i is the ith largest value of
%               abs(y(i)-theta*x(:,i)-b)
%               This type of regression is robust to outliers.
%
%INPUTS: x,y These are the real samples of data. x is an xDimXN set of N
%            vectors and y is a set of corresponding scalar values. The
%            regression searches for theta and b to fit a line such that
%            y=theta'*x(:,i)+b.
%          h The trim value of the estimator. This must be between
%            fix((N+xDim+2)/2) and n. The default if omitted or an empty
%            matrix is passed is the value that allows for the highest
%            breakdown value, which is fix((N+xDim+2)/2).
%
%OUTPUTS: theta An xDimX1 vector. The multivariate slope of the line.
%             b The scalar y intercept of the data.
%          QMin The residual of the data.
%
%The algorithm described in [1] without the y intercept adjustment is used.
%
%EXAMPLE:
%This is similar to an example in [1]. There is a distribution that one
%might expect to be well approximated with a line corrupted by a minority
%of samples of a different distribution at the side. The plot is zoomed in
%so not all samples are shown. The standard linear regression result is
%shown for comparison.
% x=100*randn(1,800/2);
% y=x+1+randn(1,800/2);
%
% xy=GaussianD.rand(200/2,[50;0],25*eye(2,2));
% x=[x,xy(1,:)];
% y=[y,xy(2,:)];
%
% figure(1)
% clf
% hold on
% scatter(x,y,'b','filled')
%
% coeffs=multilinRegress([x;y]);
% numPoints=100;
% xEst=linspace(-200,200,numPoints);
% yEst=xEst*coeffs(1)+coeffs(2);
% plot(xEst,yEst,'--k','linewidth',2)
% [theta,b]=LTSRegression(x,y);
% yEst=theta*xEst+b;
% plot(xEst,yEst,'-r','linewidth',2)
% axis([-30-20,70+20,-30-20,50+20])%Zoom in.
% legend('Samples','Least Squares Fit','LTS Fit','location','NorthWest')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] P. J. Rousseeuw and V. Katrien, "Computing LTS regression for large
%    data sets," Data Mining and Knowledge Discovery, vol. 12, no. 1, pp.
%    26-45, Jan. 2006.
%
%August 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(x,2);

x=[x;ones(1,n)];

y=y(:);

p=size(x,1);%p>=2

if(nargin<3||isempty(h))
    h=fix((n+p+1)/2);
end

if(h<fix((n+p+1)/2)||h>n)
   error('Invalid value of h provided.')
end

totalStarts=binomial(n,p);
Q3Heap=BinaryHeap(10,false);
for k=1:min(totalStarts,500)
    if(totalStarts<500)
        %Do all possible initializations.
        idx=unrankCombination(k-1,n,p)+1;
    else
        %Get a random subset of points in x.
        idx=randCombination(n,p)+1;
    end

    xCur=x(:,idx);
    yCur=y(idx);
    %If what we get is not full rank, then we just use a
    %pseudoinverse. This skips issues of determining which and how
    %many additional columns to add.
    if(rank(xCur)<p)
        theta0=pinv(xCur)'*yCur;
    else
        theta0=xCur'\yCur;
    end

    for i=1:3
        r0=y'-sum(bsxfun(@times,x,theta0),1);
        [~,idx]=sort(abs(r0),'ascend');
        idx=idx(1:h);

        xCur=x(:,idx);
        yCur=y(idx);
        theta0=pinv(xCur)'*yCur;
    end

    r2=(y'-sum(bsxfun(@times,x,theta0),1)).^2;
    Q3=sum(mink(r2,h));

    if(Q3Heap.heapSize()<10)
        Q3Heap.insert(Q3,idx);
    else
        topEl=Q3Heap.getTop();
        if(topEl.key>Q3)
            Q3Heap.deleteTop();
            Q3Heap.insert(Q3,idx);
        end
    end
end

%For the 10 Q3 values in the heap, iterate the C-step in Section 2
%until convergence.
theta0List=zeros(p,10);
QList=zeros(10,1);
for k=1:10
    topEl=Q3Heap.getTop();
    Q3Heap.deleteTop();

    idx=topEl.value;
    xCur=x(:,idx);
    yCur=y(idx);
    theta0=pinv(xCur)'*yCur;
    idxOld=zeros(h,1);

    %It was noted that convergence should be for fewer than 10
    %iterations, so we make the maximum 10.
    for curIter=1:10
        if(all(idx(:)==idxOld(:)))
            break;
        end
        r0=y'-sum(bsxfun(@times,x,theta0),1);
        [~,idx]=sort(abs(r0),'ascend');
        idx=idx(1:h);

        xCur=x(:,idx);
        yCur=y(idx);
        theta0=pinv(xCur)'*yCur;
    end

    r2=(y'-sum(bsxfun(@times,x,theta0),1)).^2;
    Q=sum(mink(r2,h));

    QList(k)=Q;
    theta0List(:,k)=theta0;
end

[QMin,idx]=min(QList);
theta0Min=theta0List(:,idx);

theta=theta0Min(1:(p-1));
b=theta0Min(p);
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 rbfBasis = rbfComputeBasis( X, k, cluster, scale, show )
% Get locations and sizes of radial basis functions for use in rbf network.
%
% Radial basis function are a simple, fast method for universal function
% approximation / regression. The idea is to find a mapping X->y where X
% and y are continuous real variables. The mapping is linear over features
% of X: y=rbfWeight*features(X). The features are of the form:
%  f_j(x) = exp(-||x-mu_j||_2^2 / 2 sig_j^2 ).
% The number of basis functions controls the complexity of the network. Too
% many basis functions can lead to overfitting, too few to bad
% interpolation. Rbf networks are trained in two phases.  First, the radial
% basis functions are found in an unsupervised manner (using only Xtrain),
% for example by clustering Xtrain.  Next, given the basis functions,
% Xtrain and ytrain, the basis weights are found by solving the system:
%  rbfWeight * features(Xtrain) = ytrain
% At this point, to interpolate any new points, Xtest, use:
%  ytest = rbfWeight * features(Xtest)
% The code below achieves all three steps:
%  rbfBasis  = rbfComputeBasis( Xtrain, k, cluster, scale, show );
%  rbfWeight = rbfComputeFtrs(Xtrain,rbfBasis) \ ytrain;
%  ytest     = rbfComputeFtrs(Xtest,rbfBasis) * rbfWeight;
% Note, in the returned rbfBasis struct there are a number of flags that
% control how the rbf features are computed. These can be altered to
% achieve the desired effect.
%
% For an in depth discussion of rbf networks see:
%  Christopher M. Bishop. "Neural Networks for Pattern Recognition"
%
% USAGE
%  rbfBasis = rbfComputeBasis( X, k, [cluster], [scale], [show] )
%
% INPUTS
%  X           - [N x d] N points of d dimensions each
%  k           - number of basis functions to use
%  cluster     - [1]: Computes cluster centers for use as rbf functions.
%              - 0: Evenly centered basis functions (ok for small d)
%  scale       - [5] Alter computed value of sigma by given factor
%                set larger for smoother results, too small -> bad interp
%  show        - [0] will display results in figure(show)
%                if show<0, assumes X is array Nxs^2 of N sxs patches
%
% OUTPUTS
%  rfbBasis
%   .d          - feature vector size
%   .k          - number of basis functions actually used
%   .mu         - [d x k] rbf centers
%   .vars       - [1 x k] rbf widths
%   .var        - rbf average width
%   .globalVar  - [1] if true use single average var for rbfs
%   .constant   - [0] if true include extra basis with constant activation
%   .normalize  - [0] if true normalize overall rbf response to sum to 1
%
% EXAMPLE
%
% See also RBFDEMO, RBFCOMPUTEFTRS
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.50
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<2 || isempty(k)); error('k not specified'); end
if( nargin<3 || isempty(cluster)); cluster=1;  end
if( nargin<4 || isempty(scale)); scale=5;  end
if( nargin<5 || isempty(show)); show=0;  end
[N, d] = size(X);

if( cluster )
  %%% CLUSTERS subsample, run kmeans
  maxN=5000; if( N>maxN );  X=X(randSample(N,maxN),:);  N=maxN;  end
  prm.nTrial=5; prm.display=0;
  [IDX,mu] = kmeans2( X, k, prm );
  mu = mu'; k = size(mu,2);
else
  %%% GRID generate locations evenly spaced on grid
  if( d>4 ); error('d too high. curse of dimensionality..'); end
  nBPer = round( k ^ (1/d) );  k = nBPer ^ d; rg=[min(X)' max(X)'];
  del=(rg(:,2)-rg(:,1))/(nBPer-1); rg=rg+[-del del]/2;
  loc=cell(1,d);  for i=1:d; loc{i}=linspace(rg(i,1),rg(i,2),nBPer); end
  grid=cell(1,d); if(d>1); [grid{:}]=ndgrid(loc{:}); else grid=loc; end
  mu=zeros(d,k);   for i=1:d; mu(i,:) = grid{i}(:); end
end

%%% Set var to be equal to average distance of neareast neighbor.
dist = pdist2( mu', mu' );
dist = dist + realmax * eye( k );
vars = min(dist)* scale;
var  = mean(vars);
vars = max( vars, var/100 );

%%% store results
rbfBasis = struct('d',d, 'k',k, 'mu',mu, 'vars',vars, 'var',var, ...
  'globalVar',1, 'constant',0, 'normalize',0);

%%% optionally display
if( abs(show) )
  if( show<0 ) % if images can display
    siz = sqrt(d);
    I = clusterMontage( reshape(X,siz,siz,N), IDX, 25, 1 );
    figure(-show); clf; montage2( I );
    figure(-show+1); clf; montage2(reshape(mu,siz,siz,[]));
  elseif( d==1 ) % 1D data
    figure(show); clf; hold on;
    minX = min(X,[],1 );  maxX = max(X,[],1 );
    xs = linspace( minX, maxX, 500 )';
    for i=1:k
      ys = exp( -(xs-mu(i)).^2 / 2 / var  );
      plot( xs, ys );
    end
  elseif( d==2 ) % 2D data
    figure(show); clf;
    minX = min(X,[],1 );  maxX = max(X,[],1 );
    xs1 = linspace(minX(1),maxX(1),25);
    xs2 = linspace(minX(2),maxX(2),25);
    [xs1,xs2] = ndgrid( xs1, xs2 );
    xs = [xs1(:) xs2(:)]; n = size(xs,1);
    for i=1:k
      mui = repmat(mu(:,i),[1 n])';
      ys = exp( - sum( ((xs - mui)).^2, 2 ) / 2 / var );
      surf( xs1, xs2, reshape(ys,size(xs1)) );
      hold on;
    end;
  elseif( d==3 ) % 3D data (show data+centers)
    figure(show); clf; hold on;
    scatter3( X(:,1),X(:,2),X(:,3),12,'filled');
    scatter3( mu(1,:),mu(2,:),mu(3,:),30,'filled');
  end
end