function [afixed,sqnorm,ierr] = lsearch (afloat,L,D,Chi2,ncands)
%LSEARCH: Integer ambiguity resolution, search
%
% This routine finds the integer vector which is closest to a given
% float vector, in a least squares sence. This is the search-step in
% integer ambiguity resolution. It is best to perform this search only
% on ambiguities which have been decorrelated using LAMBDA.
%
% Input arguments:
%    afloat : Float ambiguities (\hat{a})
%    L      : LtDL-decomposition of the decorrelated
%    D        variance-covariance matrix of the ambiguities
%    Chi2   : Size of the search ellipsoid
%    ncands : Number of requested candidates
%
% Output arguments:
%    afixed : Estimated integers (matrix)
%    sqnorm : Corresponding squared norms (vector, sorted)
%    ierr   : Error code: 0: No errors found
%                         1: Not enough candidates found

% ----------------------------------------------------------------------
% File.....: lsearch.m
% Date.....: 19-MAY-1999
% Author...: Peter Joosten
%            Mathematical Geodesy and Positioning
%            Delft University of Technology
% ----------------------------------------------------------------------

% -------------------------------
% --- Initializing statements ---
% -------------------------------

Linv      = inv(L);
Dinv      = 1./D;

True      = 1;
False     = 0;

n         = max(size(afloat));

right     = [zeros(n,1) ; Chi2];
left      = [zeros(n+1,1)];
dq        = [Dinv(2:n)./Dinv(1:n-1) 1/Dinv(n)];

cand_n    = False;
c_stop    = False;
endsearch = False;

ncan      = 0;

i         = n + 1;
iold      = i;
ierr      = 0;

afixed = zeros(n,ncands);
sqnorm = zeros(1,ncands);

dist = [];

% ----------------------------------
% --- Start the main search-loop ---
% ----------------------------------

while ~ (endsearch);

   i = i - 1;

   if iold <= i
      lef(i) = lef(i) + Linv(i+1,i);
   else
      lef(i) = 0;
      for j = i+1:n;
         lef(i) = lef(i) + Linv(j,i)*dist(j,1);
      end;
   end;
   iold = i;

   right(i)  = (right(i+1) - left(i+1)) * dq(i);
   reach     = sqrt(right(i));
   delta     = afloat(i) - reach - lef(i);
   dist(i,1) = ceil(delta) - afloat(i);

   if dist(i,1) > reach - lef(i)

%     ----------------------------------------------------
%     --- There is nothing at this level, so backtrack ---
%     ----------------------------------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;
         if dist(i) < endd(i);
            dist(i) = dist(i) + 1;
            left(i) = (dist(i) + lef(i)) ^ 2;
            c_stop = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   else

%     ----------------------------
%     --- Set the right border ---
%     ----------------------------

      endd(i) = reach - lef(i) - 1;
      left(i) = (dist(i,1) + lef(i)) ^ 2;

   end

   if i == 1;

%     -------------------------------------------------------------------
%     --- Collect the integer vectors and corresponding               ---
%     --- squared distances, add to vectors "afixed" and "sqnorm" if: ---                             ---
%     --- * Less then "ncands" candidates found so far                ---
%     --- * The squared norm is smaller than one of the previous ones ---
%     -------------------------------------------------------------------

      t       = Chi2 - (right(1)-left(1)) * Dinv(1);
      endd(1) = endd(1) + 1;

      while dist(1) <= endd(1);

         if ncan < ncands;

            ncan             = ncan + 1;
            afixed(1:n,ncan) = dist + afloat;
            sqnorm(ncan)     = t;

         else

            [maxnorm,ipos] = max(sqnorm);
            if t < maxnorm;
               afixed(1:n,ipos) = dist + afloat;
               sqnorm(ipos)     = t;
            end;

         end;

         t       = t + (2 * (dist(1) + lef(1)) + 1) * Dinv(1);
         dist(1) = dist(1) + 1;

      end;

%     -------------------------
%     --- And backtrack ... ---
%     -------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;

         if dist(i) < endd(i);
            dist(i) = dist(i) + 1;
            left(i) = (dist(i) + lef(i)) ^ 2;
            c_stop = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   end;

end;

% ----------------------------------------------------------------------
% --- Sort the resulting candidates, according to the norm
% ----------------------------------------------------------------------

tmp    = sortrows ([sqnorm' afixed']);
sqnorm = tmp(:,1)';
afixed = tmp(:,2:n+1)';

% ------------------------
% --- Check for errors ---
% ------------------------

if ncan < ncands; ierr = 1; end;

% ----------------------------------------------------------------------
% End of routine: lsearch
% ----------------------------------------------------------------------


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Problem 11-  System response to discrete time sinusoidal input

syms n
w0=2;
Hw0=(3*j*w0+2)/(w0+1);

mag=abs(Hw0);
phas=angle(Hw0);

y=3*mag*sin(w0*n+1+ phas);

n=0:50;
y=subs(y,n);
stem(n,y);
legend('y[n]');


function [D, alpha, E, A] = inexact_alm_rosl_subsampling(X, K, L, lambda, tol, maxIter)

% This matlab code implements the inexact augmented Lagrange multiplier
% method for Robust Orthogonal Subspace Learning with random sampling.
%
% X - m x n matrix of observations/data (required input)
%
% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%
% maxIter - maximum number of iterations
%
%
% min \sum_{1<i<T}\|\alpha_i\|_2 + \lambda\|E\|_1, s.t. \|D_i\|_2= 1 ;
%
% The laplacian function:
% L(A,E,Y,u) =  \sum_1^T |alpha_i|_2 + \lambda*|E|_1 + \mu/2* |X- \sum_1^T D_i*alpha_i-E+Y/mu|_F^2;
%
% Xianbiao Shu (xshu2@illinois.edu)
% Copyright: Mitsubishi Electric Research Lab
% Reference: X. Shu, F. Porikli, N. Ahujia "Robust Orthonormal Subspace Learning: Efficient Recovery of Corrupted Low-rank Matrices". CVPR 2014;

[m, n] = size(X);

% Random subsampling (using randsample or randperm)

row_sampled = randsample(m,L);
row_notsampled = [1:m]';
row_notsampled(row_sampled) =[];
row_ind = [row_sampled; row_notsampled];
column_sampled = randsample(n,L);
column_notsampled = [1:n]';
column_notsampled(column_sampled) =[];
column_ind = [column_sampled; column_notsampled];

X_perm = X(row_ind, :);
X_perm = X_perm(:, column_ind);

% % solve the simplified RSL
% [Dict_hat1, alpha] = inexact_alm_rsl(X_perm(1:L,:), K, lambda, tol, maxIter);
%
% % robust linear regression
% [Dict_hat2] = inexact_alm_rlr(X_perm(L+1:m,1:L), K, alpha(:, 1:L), tol, maxIter) ;

% solve the simplified RSL
[D, alpha_1] = inexact_alm_rosl(X_perm(:,1:L), K, lambda, tol, maxIter);

% robust linear regression
[alpha] = inexact_alm_rlr(X_perm(1:L, :), K, D(1:L, :), tol, maxIter) ;

% compute the low-rank matrix
%  D = [Dict_hat1; Dict_hat2];
 A = D*alpha;

 %permute back
 A(:, column_ind) = A;
 A(row_ind, :) = A;

 E = X -A;

end


% spher() - return the sphering matrix for given input data
%
% Usage:
%
%        >> sphere_matrix = spher(data);
%
% Reference: T. Bell (1996) - - -
%

% S. Makeig CNL / Salk Institute, La Jolla CA 7-17-97

function sphere = spher(data)

if nargin<1 | size(data,1)<1
  help spher
  return
end

sphere = 2.0*inv(sqrtm(cov(data'))); % return the "sphering" matrix


function [g,h] = compute_spline_filter(s,N)

% compute_spline_filter - compute the spline filter of a wavelete transform
%
% function [g,h] = compute_spline_filter(s,N);
%
%    'N' is the length of the filter (must be even).
%    's' is the order of the spline.
%    'g' is the low pass filter.
%    'h' is the high pass filter.
%

x = (0:N-1)*2*pi/(N-1);
gg = ; % compute here the fourier transform
g = ifft(gg);
g = [g(N/2+1:end),g(1:N/2)];
h = (-1).^(0:N-1) .* g;


%% This file is used to generate the simulation data of the Schwarzian-Kroteweg-de Vires equation.
% Coded By: K
% Last Updated: 2019/06/17
%% Clear all
clc;clear;close all;

%% Create a folder
addpath('Functions')
%
[fld_status, fld_msg, fld_msgID]=mkdir('Datas');
addpath('Datas')
%
[fld_status, fld_msg, fld_msgID]=mkdir('TempFunctions');
addpath('TempFunctions')
%% Define the parameters for the SKdV equation
% Define the time span
dt=0.01;T=20;
tspan=0:dt:T;

% Define the spatial domain
L=50;

% Define the discretization point
n=256;

% Get the index of each point
x2=linspace(-L/2,L/2,n+1);
x=x2(1:n);
dx=x(2)-x(1);

% Get the frequency
k=(2*pi/L)*[0:n/2-1 -n/2:-1].';
k2=fftshift(k);

% Get the initial value
a1=0.5;
u1=(a1/2*(sech(0.5*sqrt(1)*(x+10))).^2 ).';

a2=0.5;
u2=(a2/2*(sech(0.5*sqrt(1)*(x-10))).^2 ).';

u0=u1+u2;

% Transfer the initial point to spectram domain
ut0=fft(u0);

% Define parameter for system
% Omega: This term should be positive, and it will determine the
% dissapation of the system
Omega = 0.1;

alpha = 1;

% g0: This term should be positive, and it determines the gain of the
% system.
g0 =[0;0.0001;0.001;0.01;0.1;1];

%% Loop through the g0
for kkk=1:size(g0,1)
    % Solve for the PDE
    tic
    opts = odeset('RelTol',1e-12,'AbsTol',1e-13);
    [time,utsol]=ode45(@(time,u0)mkdv_rhs(time,u0,k,x,Omega,g0(kkk,1),alpha),tspan,ut0,opts);
    toc

    % Back to time domain
    for j=1:length(tspan)
        usol(:,j)=ifft(utsol(j,1:n).');
    end

    u=zeros(size(usol));
    ut=zeros(size(usol));
    ux=zeros(size(usol));
    uxx=zeros(size(usol));
    uxxx=zeros(size(usol));

    % Use the previous utsol to get the derivative
    for j=1:length(tspan)
        [~,ut(:,j),u(:,j),ux(:,j),uxx(:,j),uxxx(:,j)]=mkdv_rhs(0,utsol(j,1:n)',k,x,Omega,g0(kkk,1),alpha);
    end

    u=u';ut=ut';ux=ux';uxx=uxx';uxxx=uxxx';

    name=strcat('Datas/','SimulationData_V_',num2str(kkk),'.mat');
    save(name,'u','ut','ux','uxx','uxxx','x','tspan','dt','T','L','n');

end

%% Plot the result
figure(1)
waterfall(x,tspan,real(u))
colormap([0 0 0]);
view(42,55)

figure(2)
surfl(x,tspan,real(u))
colormap(gray)
shading interp
view(42,55)

figure(3)
pcolor(x,tspan,real(u))
shading interp


function imdisp(I)
% balance the gray level range to display the image
x = (0:255)./255;
grey = [x;x;x]';
minI = min(min(I));
maxI = max(max(I));
I = (I-minI)/(maxI-minI)*255;
image(I);
axis('square','off');
colormap(grey);


function I = surfacearea(f , varargin )
%SURFACEAREA    Surface area of a SEPARABLEAPPROX.
%
%   SURFACEAREA(F) computes the surface area of the SEPARABLEAPPROX in the domain of F.

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

if ( nargin > 1 )
    if ( isa(varargin{1}, 'double') )
        if ( length(varargin{1}) == 4 )
            % Restricted surface area over rectangular region.
            dom = varargin{1};
            f = restrict( f, dom );
        else
            error('CHEBFUN:SEPARABLEAPPROX:surfacearea:domain', 'Bad domain.');
        end
    elseif ( isa(varargin{1}, 'chebfun') )
        f = restrict( f, varargin{1} );
        % Surface area is now just the arc length.
        I = sum( sqrt( 1 + diff( f ).^2 ) );
        return
    else
        error('CHEBFUN:SEPARABLEAPPROX:surfacearea:domain','Bad restricting domain.');
    end
end

% First order derivatives:
fx = diff(f, 1, 2);
fy = diff(f, 1, 1);

% Integrand:
G = 1 + fx.^2 + fy.^2;
S = sqrt( G );

% Surface area:
I = integral2( S );

end


function distr = Create1DDistribution(values)
%
%
%        distr = Create1DDistribution(values)
%
%
%        Input:
%           -values: values
%
%        Output:
%           -distr: a 1D distribution
%
%     Copyright (C) 2011  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/>.
%

sumPDF = sum(values);

if(sumPDF <= 0)
    PDF = zeros(size(values));
    sumPDF = 1.0;
else
    PDF = values;
end

CDF = cumsum(PDF);
maxCDF = max(CDF);

if(maxCDF > 0)
    CDF = CDF / maxCDF;
end

distr = struct('PDF', PDF / sumPDF, 'CDF', CDF, 'maxCDF', maxCDF);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXERCISE: WRITE THE LAGRANGE FORMULATION FOR A 2DOF PLANAR ROBOT ARM
%   fext are expressed in the base reference system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function tau = exercise_lagrange_2dofplanar(robot, q, qd, qdd, g, fext)
a = eval(robot.DH.a);
m=robot.dynamics.masses(1);

%DEFINE M

%DEFINE V

%DEFINE G

%compute tau!
%tau = M*Qdd + V + G


function h=entropy(p,dim,cond,arg,step)
%ENTROPY calculates the entropy of discrete and sampled continuous distributions H=(P,DIM,COND,ARG,STEP)
%
%  Inputs:  P        is a vector or matrix of probabilities - one dimension per variable
%           DIM      lists dimensions along which to evaluate the entropy [default: 1st non singleton dimension]
%           COND     lists dimensions to use as conditional variables [default - none]
%           ARG      lists dimensions to use as parameters in the ouput [default - none]
%           STEP     for continuous distributions STEP gives the sample increment for each dimension of P
%                    if STEP is a scalar, the increment is assumed to be the same for each dimension
%
% Outputs:  H        is the entropy. It will have the same number of dimensions as the length of the ARG input.
%                    If the STEP argument is specified then this will be the differential entropy.
%
% Example: Suppose P(W,X,Y,Z) represents the joint probability of four correlated random variables
%
%               (a) H(W,X,Y,Z) = entropy(P,[1 2 3 4]).
%               (b) H(W) = entropy(P), or equivalently entropy(P,1)
%               (c) H(W,Z | X,Y) = entropy(P,[1 4],[2 3])
%               (d) H(W | X, Z=z) = entropy(P,1,2,4); this is a function of z and will be a column vector
%
% As a special case, if the dimensions included in DIM are all singletons, the entries in P are treated
% as Bernoulli variable probabilities.

%	   Copyright (C) Mike Brookes 2006
%      Version: $Id: entropy.m,v 1.3 2007/05/04 07:01:38 dmb Exp $
%
%   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<5
    stp=zeros(ndims(p),1);
else
    stp=repmat(step(1),ndims(p),1);
    stp(1:length(step))=step(:);
    stp=log2(stp);
end
if nargin<4
    arg=[];
else
    arg=arg(arg>0);
end
if nargin<3
    cond=[];
else
    cond=cond(cond>0);
end
if ~length(cond)
    s=size(p);
    if nargin<2
        dim=find(s>=min(2,max(s)));
        dim=dim(1);
    else
        dim=dim(dim>0);
    end
    st=prod(s);
    sd=prod(s(dim));
    sa=prod(s(arg));
    marg=1:length(s);
    marg(arg)=0;
    marg(dim)=0;
    marg=marg(marg>0);
    sm=st/sd/sa;
    if sm>1
        ip=[arg dim(:)' marg(:)'];
        sp=[s([arg dim(:)']) prod(s(marg))];
        q=sum(reshape(permute(p,[arg(:)' dim(:)' marg]),sa,sd,sm),3);
    else
        q=reshape(permute(p,[arg(:)' dim(:)' marg]),sa,sd);
    end
    if sd==1
        h=-log2(q+(q==0)).*q-log2(1-q+(q==1)).*(1-q);   % special treatment for bernoulli variables
    else
        sq=sum(q,2);
        h=sum(-log2(q+(q==0)).*q,2)./sq+log2(sq);
    end
    if length(arg)>1
        h=reshape(h,s(arg));
    end
    h=h+sum(stp(dim));
else
    % we could probably make this more efficient by avoiding the recursive call
    h=entropy(p,[dim(:); cond(:)],0,arg)-entropy(p,cond,0,arg);
end


function [ x, w ] = spquad ( dim, ord, bpt )
%SPQUAD
%   Computes the sparse grid quadrature abscissae and weights
%   on an orthotope/hyperrectangle using the Clenshaw-Curtis rule.
%
%   [X,W]=SPQUAD(DIM,ORD,AB)
%
%   Input Parameters:
%   DIM - number of dimensions
%   ORD - order of the integration rule
%   BPT - boundary points (Optional. Defaults to [-1,1]^dim)
%
%   If used, BPT should be a 2 by DIM matrix containing the
%   endpoints of the hyperrectangle in each column.
%
%   Example usage:
%
%   f=@(x) (1+x(:,1)).*exp(x(:,2).*x(:,3))+(1+x(:,2)).*exp(x(:,3).*x(:,1))
%   [x,w]=spquad(3,4,[-1 0 2; 1 1 3]);
%   Q=w'*f(x);
%
%   written by Greg von Winckel - March 3, 2008
%   Contact: gregory(dot)von-winckel(at)uni-graz(dot)at

%
% Check that the number of dimensions agree.
%
if nargin>2
    if size(bpt,2)~=dim
        error('dimension mismatch');
    end
else
    bpt=repmat([-1,1]',1,dim);
end
%
% Length and midpoint in each dimension.
%
len=diff(bpt); mpt=mean(bpt);
%
% zeroth order case is just the midpoint rule
%
if ord==0
    x=mpt; w=prod(len);
%
%  1D case is special.
%
elseif ord~=0 && dim==1
    x=mpt+len*cos(pi*2^(-ord)*(0:(2^ord))')/2;
    w=clencurt(2^(ord)+1)*len/2;
else
    x0=mpt;
    w0=prod(len);
    for j=1:ord
        [x,w]=sparsegridnd(dim,j,mpt,len);
        [C,I]=intersect(x,x0,'rows');
        w(I)=w(I)+w0; x0=x; w0=w;
    end

end

%% Multidimensional nodes and difference weights.

function [ x, w ] = sparsegridnd ( n, ord, mpt, len )

% Generates the n-dimensional sparse grid of order ord on the
% hyperrectangle with centroid mpt and side lengths len, based
% on Chebyshev-Gauss-Lobatto points.

%
% Define 1D grid points
%
p=@(i) unique((i~=0)*cos(pi*2^(-i)*(0:(2^i)))');
%
% Get configurations of all possible subgrids
%
v=genindex(n,ord);
vmax=1+v(1,n);
%
% Compute all orders of one-dimensional quadrature rules
%
P=uaf(@(j) p(j), (0:vmax-1));
Q=uaf(@(j) diffweight(j), (0:vmax));
%
% Take the union of all possible subgrids
%
m=size(v,1);
xw=uaf(@(k) getpts(P,Q,v(k,:),mpt,len), (1:m)',1);
x=xw(:,1:n);
w=xw(:,n+1);
%
% Kludge to deal with small errors introduced by ndgrid
% need a better way to do this
%
roundn=@(a,n) round(a*10^n)/10^n;
x=roundn(x,15);
%
% Get unique points and weights
%
[x,ii,jj]=unique(x,'rows');
rows=length(ii);
cols=length(jj);
%
% Do node condensation for combining weights
%
w=sparse(jj,(1:cols)',ones(cols,1),rows,cols)*w;

%% One dimensional difference weights

function dw = diffweight ( ii )

if ii==0
    dw=2;
elseif ii==1
    dw=[1;-2;1]/3;
else
    q=@(i) clencurt(2^(i)+1);
    dw=q(ii); dw(1:2:end)=dw(1:2:end)-q(ii-1);
end

%% Compute the subset grid points and difference weights.

function [XW] = getpts ( P, Q, v, mpt, len )

n=size(v,2);
[x{1:n}]=ndgrid(P{1+v});
[w{1:n}]=ndgrid(Q{1+v});
d=size(v,2);

X=uaf(@(k) mpt(k)+x{k}(:)*len(k)/2, (1:d),1);
W=prod(uaf(@(k) w{k}(:), (1:d),1),2);
XW=[X,W];

%% Find all possible combinations of bases

function v = genindex ( n, L1, head )

if n==1
    v = L1;
else
    v = uaf(@(j) genindex(n-1, L1-j, j),(0:L1)',1);
end

if nargin>=3
    v = [head+zeros(size(v,1),1) v];
end

%% Compute 1D Clenshaw-Curtis weights
% Reference: Jörg Waldvogel,
%  "Fast construction of the Fejér and Clenshaw-Curtis quadrature rules,"
%  BIT Numerical Mathematics 43 (1), p. 001-018 (2004).

function w = clencurt ( N1 )

if N1==1
     w=2;
else
    N=N1-1;  c=zeros(N1,1);
    c(1:2:N1,1)=(2./[1 1-(2:2:N).^2 ])';
    f=real(ifft([c(1:N1,:);c(N:-1:2,:)]));
    w=2*([f(1,1); 2*f(2:N,1); f(N1,1)])/2;
end

%% Shorthand array function with uniform output
% Optional third argument converts output to matrix

function result = uaf ( arg, dex, varargin )

result=arrayfun(arg,dex,'UniformOutPut',false);

if nargin>2
    result=cell2mat(result);
end


  function [xs, wlsden] = wls_gcd(A, W, yy, xhat, niter, ng, chat)
%|function [xs, wlsden] = wls_gcd(A, W, yy, xhat, niter, ng, chat)
%| weighted least squares minimized by
%| grouped coordinate descent algorithm
%| in
%|	A	[nn np]			system matrix
%|	W	[nn nn]			inverse data covariance
%|	yy	[nn 1]			noisy data
%|	nder1	[nn 1]
%|	xhat	[np 1] or [nx ny]	initial estimate
%|	niter		# total iterations
%|	ng		# groups, or, if array, groups
%| out
%|	xs	[np niter]		estimates each iteration
%|
%| Copyright July 1996, Jeff Fessler, University of Michigan

if nargin < 6, help(mfilename), error(' '), end

if 1 ~= exist('chat'), chat = 1; end
%chat = (1==exist('mask'));

[nx,ny] = size(xhat);

if 1~=exist('mask') || isequal(mask, 1)
	mask = ones(nx,ny);
end

np = ncol(A);

if numel(xhat) ~= np
	xhat = xhat(mask(:));
end

if ~isempty(nder1)
	back_nder1 = A' * nder1(:);
else
	back_nder1 = zeros(np,1);
end
yy = yy(:);
xs = zeros(np, niter);
xs(:,1) = xhat;

if chat
	im(321, embed(xhat,mask), 'x0')
end

% build groups
if numel(ng) == 1
	groups	= zeros(np, ng);
	for ig = 1:ng
		gg = [ig:ng:np]';
		groups(gg,ig) = ones(size(gg));
	end
elseif numel(ng) == 2
	groups	= group2d(nx, ny, ng(1), ng(2));
	ng	= ncol(groups);
else
	groups	= ng;
	ng	= ncol(groups);
end

% overlapping groups?
if any(sum(groups')) > 1, error 'overlap groups', end

if chat
	im(322, embed(groups * [1:ng]',mask), 'Groups')
end

% precompute quadratic part of denominator
% change '1' to 'ig' for overlapping groups
wlsden	= zeros(np, 1);
if 1
	for ig = 1:ng
		gg = groups(:,ig);
		printm('precompute %g, assuming nonnegative A', ig)

%		wlsden(gg,1) = abs(A(:,gg))' * (W * sum(abs(A(:,gg)'),1)');
%		wlsden(gg,1) = A(:,gg)' * (W * sum(A(:,gg)')',1);

		t = A * gg;	% faster than: sum(A(:,gg)',1)';
		t = W * t;	% only slightly slower than: diag(W) .* t;
		%	faster than either: A(:,gg)'*t or A' * t !
		t = t' * A;
		wlsden(gg,1) = t(:,gg)';
	end
else
	wlsden	= ones(np, 1);	% for testing
end

if chat
	im(323, embed(wlsden,mask), 'WLS Denominator')
end

% compare GCA and SCA denominators (for WLS part)
if chat && (np < 1000)
	t = full(sum(A .* (W * A))'); % SCA den
	t = t(wlsden ~= 0) ./ wlsden(wlsden ~= 0);
	if min(t(:)) < 1-10*eps
		printm(['Under-relax. range:' sprintf(' %g', minmax(t))])
		t = embed(t,wlsden);
		im(324, embed(t,mask), 'Under-relaxer')
	else
		printm 'Note: GCA with ideal denominator!'
	end
end

% 'depierro' factors for penalty? (GCA vs SCA)
for ig = 1:ng
	gg = groups(:,ig);
	t = abs(C(:,gg))' * sum(abs(C(:,gg)'),1)';
	t = sum(C(:,gg).^2)' ./ t; % SCA / GCA
	t = minmax(t);
	if min(t) < 1-10*eps
		printm(['Virtual under-relax. range:' sprintf(' %g', t)])
		error 'depierro not done'
	end
	clear t
end

if chat
	im(325, embed(sum(C.^2)',mask), 'sum(C.^2)')
	input('hit key');
end

res = A * xhat - yy;

for ii = 2:niter

	for ig = 1:ng
		gg = groups(:,ig);

		xn = xhat(gg);
		xw = xn;

		ader = ( (W * res)' * A(:,gg) )' + back_nder1(gg);

		for jj = 1:nsub
			dx = C * xhat;
			eval(sprintf(wstring, 'wt', 'dx'));
			denom = wlsden(gg) + (C(:,gg).^2)' * wt;
			num = ader + wlsden(gg).*(xw-xn) ...
				+ C(:,gg)' * (wt .* dx);
			xw = xw - num ./ denom;
			xhat(gg) = xw;
		end

		res = res + A(:,gg) * (xw - xn);
	end

	xs(:,ii) = xhat;
	if chat && (ii < 10 || rem(ii,10) == 0)
		im(326, embed(xhat,mask), 'x hat')
		if ny == 1
			im(324, wt, 'wts')
		end
	end
	if chat
		printm('max %g %g', max(xhat), max(abs(xhat-xs(:,ii-1)))/max(xhat)*100)
	end
end


function scimat = scimat_surface2seg(scimat, tri, x)
% scimat_surface2seg  Convert triangular mesh into segmentation.
%
%   scimat_surface2seg sets to "1" those voxels in a segmentation volume
%   that contain or are intersected by the triangles in a triangular mesh.
%   The method works by sampling each triangle in the mesh uniformly, using
%   barycentric coordinates. Samples are close enough to each other so that
%   each voxel will have at least two samples. Then sample coordinates are
%   rounded to the closest voxel centres, and the voxels are set.
%
% SCIMAT2 = scimat_surface2seg(SCIMAT, TRI, X)
%
%   SCIMAT is a struct to hold the segmentation (see "help scimat" for
%   details). SCIMAT.axis must be provided. SCIMAT.axis is important
%   because it tells us where the segmentation volume begings and ends, and
%   the voxel size. SCIMAT.data is not necessary, and if it is not
%   provided, a new one will be created. If it is provided, previously set
%   voxels will not be cleared.
%
%   (TRI, X) describe the triangular mesh.
%
%   TRI is a 3-column matrix. Each row contains the 3 nodes that form one
%   triangular facet in the mesh.
%
%   X is a 2-column matrix. X(i, :) contains the xy-coordinates of the
%   i-th node in the mesh.
%
%   SCIMAT2 is the output segmentation.
%
% See also: surface_tridomain, surface_param, surface_interp.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(3, 3);
nargoutchk(0, 1);

% smallest dimension of the voxel size
lmin = min([scimat.axis.spacing]);

% if the scimat struct has no data field, create one
if (~isfield(scimat, 'data'))
    scimat.data = zeros([scimat.axis.size], 'uint8');
end

% loop triangles
Ntri = size(tri, 1);
for I = 1:Ntri

    % vertices of current triangle
    v = x(tri(I, :), :);

    % sample triangle
    xi = sample_triangle(v, lmin);

    % convert real world coordinates to indices
    idx = round(scimat_world2index(xi, scimat));

    % linearize indices
    idx = sub2ind(size(scimat.data), idx(:, 1), idx(:, 2), idx(:, 3));

    % set voxels that belong to the triangle to 1
    scimat.data(idx) = 1;

end

end

% given a triangle, sample it uniformly using barycentric coordinates
function xi = sample_triangle(x, lmin)

% nomenclature
v1 = x(1, :);
v2 = x(2, :);
v3 = x(3, :);

% compute number of points between triangle vertices. We want to have at
% least 2 samples within each voxel
dmax = max([norm(v1-v2) norm(v2-v3) norm(v1-v3)]);
N = ceil(1 + 2 * dmax / lmin);

% sample the triangle
a1 = linspace(0, 1, N);
a2 = linspace(0, 1, N);

% barycentric coordinate variables
[a1, a2] = meshgrid(a1, a2);
a1 = a1(:);
a2 = a2(:);
idx = a1 + a2 <= 1;
a1 = a1(idx);
a2 = a2(idx);
a3 = 1 - a1 - a2;

% point coordinates
xi = [a1 a1 a1] .* repmat(v1, length(a1), 1) ...
    + [a2 a2 a2] .* repmat(v2, length(a2), 1) ...
    + [a3 a3 a3] .* repmat(v3, length(a3), 1);

% % plot sampled triangle
% hold off
% plot3(xi(:, 1), xi(:, 2), xi(:, 3), '.')
% hold on
% plot3(x([1:end 1], 1), x([1:end 1], 2), x([1:end 1], 3))

end


function f=gflux(u,i)
if nargin<2 || (i==0)
	% give the flux
	f=-(1-u).*u;
elseif (i==1)
    % the derivative
	f=-(1-2*u);
else
	error(' Unknown option in gflux ');
end
% More advanced model of two-phase gravity flow
% un = u.^2;
% uw = (1-u).^2;
% if nargin<2 || (i==0)
% 	% give the flux
% 	f=-un.*uw./(un + uw);
% elseif (i==1)
%     % the derivative
% 	f=(-2*u.*uw.^2 + 2*un.^2.*(1-u))./(un+uw).^2;
% else
% 	error(' Unknown option in fflux ');
% end


function [in3] = cm32in3(cm3)
% Convert volume from cubic centimeters to cubic inches.
% Chad Greene 2012
in3 = cm3*0.061023744095;


function out = std(f)
%STD   Standard deviation of a CHEBFUN.
%   STD(F) is the standard deviation of the CHEBFUN F.

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

% The standard deviation is the square root SQRT() of the variance VAR():
out = sqrt(var(f));

end


function A = unfold(T, rowDims, colDims)
%UNFOLD   Unfold (flatten or matricize) a discrete tensor.
%   A = UNFOLD(T, ROWDIMS, COLDIMS) returns an unfolding of T, where
%   ROWDIMS and COLDIMS are vectors of integers denoting the dimensions of
%   T that are merged into the rows and columns of the matrix A,
%   respectively.
%
%   A = UNFOLD(T, ROWDIMS) chooses COLDIMS automatically to contain all
%   dimensions not contained in ROWDIMS. The entries of COLDIMS are
%   arranged in the ascending order.
%
%   Note that UNFOLD(T, 1:ndims(T)) is the same as T(:).
%
%   Example:
%   T = randn(2, 3, 4, 5);
%   A = unfold(T, [2 4]); % Returns a matrix of size 15 x 8.
%
% See also CHEBFUN3/FOLD.

% The structure of this code is similar to `matricize.m` from the HTUCKER
% toolbox of Tobler and Kressner.

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

if ( nargin == 3 )
    rowDims = rowDims(rowDims <= ndims(T));
    colDims = colDims(colDims <= ndims(T));
elseif ( nargin == 2 )
    % Put remaining modes onto columns.
    colDims = setdiff(1:ndims(T), rowDims);
else
    error('CHEBFUN:CHEBFUN3:unfold', ...
        'COL_DIMS must be a positive integer vector without duplicate entries.')
end

% Size of tensor T
sizeT = size(T);
if ( ndims(T) < 3 )
    % To work properly also with scalars, vectors and matrices.
    sizeT(3) = 1;
end

% Permute dimensions of T
T = permute(T, [rowDims, colDims]);

% Flatten the tensor T
A = reshape(T, prod(sizeT(rowDims)), prod(sizeT(colDims)) );

end


% VL_DEMO_SIFT_VS_UBC Compare VLFeat SIFT with Lowe's original

% --------------------------------------------------------------------
%                            Load a figure and original SIFT keypoints
% --------------------------------------------------------------------

im = imread(fullfile(vl_root, 'data', 'box.pgm')) ;
[f1,d1] = vl_ubcread(fullfile(vl_root, 'data', 'box.sift')) ;

% --------------------------------------------------------------------
%                                    Compare with VLFeat SIFT detector
% --------------------------------------------------------------------

f2 = vl_sift(im2single(im), ...
             'firstoctave', -1, ...
             'peakthresh', .01, ...
             'edgethresh', 10, ...
             'windowsize', 2, ...
             'verbose') ;

D = sqrt(vl_alldist2(f1(1:2,:), f2(1:2,:))) ;
[d12,m12] = min(D,[],2) ;
[d21,m21] = min(D,[],1) ;

matches = [1:size(f1,2), m21 ; m12',  1:size(f2,2)] ;
proxim  = [d12', d21] ;

h = histc(proxim, [0 .01 .05 +inf]) ;
h = h / sum(h) * 100 ;
h = h(1:end-1) ;

figure(1) ; clf ;
imagesc(im) ; colormap(gray(256)) ; hold on ;
vl_plotframe(f1, 'linewidth', 3, 'color', 'r') ;
vl_plotframe(f2, 'linewidth',  1, 'color', 'b') ;
axis image off ;
vl_demo_print('sift_vs_ubc_1', 0.7) ;

figure(2) ; clf ;
pie(h) ;
colormap(hot(3)) ;
legend({'0.01 pixels', '0.05 pixels', 'others'}, ...
       'location', 'northeastoutside') ;
set(findobj(2, '-property', 'fontsize'), 'fontsize', 11) ;
vl_demo_print('sift_vs_ubc_2') ;

% --------------------------------------------------------------------
%                                  Compare with VLFeat SIFT descriptor
% --------------------------------------------------------------------

[drop,d2]=vl_sift(im2single(im), 'frames', f1, 'verbose', 'firstoctave', -1) ;

D = sqrt(double(vl_alldist2(d1, d2, 'l2'))) ;
[d12,m12] = min(D,[],2) ;
[d21,m21] = min(D,[],1) ;

matches = [1:size(d1,2), m21 ; m12',  1:size(d2,2)] ;
proxim  = [d12', d21] ;

meanDist = mean(D(:)) ;

h = histc(proxim, meanDist * [0 .05 .10 .20 +inf]) ;
h = h / sum(h) * 100 ;
h = h(1:end-1) ;

figure(3) ; clf ;
pie(h) ;
colormap(hot(4)) ;
legend({'5% differnce', '10% difference', '20% difference', 'others'}, ...
       'location', 'northeastoutside') ;
set(findobj(3, '-property', 'fontsize'), 'fontsize', 11) ;
vl_demo_print('sift_vs_ubc_3') ;


% function [Ps,Pb] = pb_qam_koh_ray (EbN0db,M,L,map);
% ------------------------------------------------------------------------------
% EINGABE:
%   EbN0db: Signal/Noise pro Infobit [db]
%           (Skalar, Spalten- oder Zeilenvektor)
%        M: Stufigkeit der Modulation (default=4)
%           (Skalar)
%        L: Anzahl der Pfade (default=1)
%           (Skalar)
%      map: Art des Mappings
%           'gray' (default)
%             Gray-Codierung
%           'nat'
%             natuerliches Mapping
%           (string)
%
% AUSGABE:
%   Ps: Symbolfehlerwahrscheinlichkeit (Spaltenvektor)
%   Pb: Bitfehlerwahrscheinlichkeit (Spaltenvektor)
%
% ANMERKUNGEN:
%  - benoetigt pb_pam_ray_co.m
%
% AUTOR: Juergen Rinas,  08.12.1998
% Erweitert um Symbolfehlerraten und Mapping von Volker Kuehn, 04.01.01
% ------------------------------------------------------------------------------

function [Ps,Pb] = pb_qam_koh_ray (EbN0db,M,L,map)

if (sscanf(version,'%d')<5)
  disp(['# Achtung: ',mfilename,' wurde unter Matlab 5.1 entwickelt.']);
end;
if (nargin<2) M=4; end;        % default: M=4
if (nargin<3) L=1; end;        % default: L=1
if (nargin<4) map='gray'; end; % default: map='gray'
K=log2(M);
if (K~=fix(K))
  disp(['# Achtung(',mfilename,') M ist keine Potenz von 2.']);
end;
sqrtM=sqrt(M);
if (sqrtM~=fix(sqrtM))
  disp(['# Achtung(',mfilename,') sqrt(M) ist keine ganze Zahl.']);
end;

[tmp,Pb]=pb_pam_ray(EbN0db,sqrt(M),L,map);
Ps=2*tmp-tmp.^2;

% EOF --------------------------------------------------------------------------


function [A, B] = kafbox_covNoise(logtheta, x, z);

% Independent covariance function, ie "white noise", with specified variance.
% The covariance function is specified as:
%
% k(x^p,x^q) = s2 * \delta(p,q)
%
% where s2 is the noise variance and \delta(p,q) is a Kronecker delta function
% which is 1 iff p=q and zero otherwise. The hyperparameter is
%
% logtheta = [ log(sqrt(s2)) ]
%
% For more help on design of covariance functions, try "help covFunctions".
%
% (C) Copyright 2006 by Carl Edward Rasmussen, 2006-03-24.

if nargin == 0, A = '1'; return; end              % report number of parameters

s2 = exp(2*logtheta);                                          % noise variance

if nargin == 2                                      % compute covariance matrix
  A = s2*eye(size(x,1));
elseif nargout == 2                              % compute test set covariances
  A = s2;
  B = 0;                               % zeros cross covariance by independence
else                                                % compute derivative matrix
  A = 2*s2*eye(size(x,1));
end


function [W_G, IDX2, S, IDX_vote, S_vote] = RBF_affnity_knn_fast_outliers(data_noise,params,f_outliers)
%mydisp('Computing RBF affinity')
if nargin < 2
    neibours = 20;
else
    neibours = params.n_neighbors;
end
Sigma = params.Sigma;
N=size(data_noise,2);

%W_G=sparse(zeros(N,N));
W_G=spalloc(N,N,300*N);
%W_G=(zeros(N,N));
affinity_type = params.affinity_type;
retain = params.m*params.n_neighbors;

%S = pdist2imp(data_noise',data_noise','sqeuclidean');
switch  affinity_type
    case 'cosine'
    %S = pdist2imp(data_noise',data_noise','cosine');
    [IDX_vote,S]=knnsearch(data_noise',data_noise','k',retain+1,'distance','cosine');
    IDX_vote=IDX_vote(:,2:end);
    S=S(:,2:end);
    S_vote=S;
    IDX_vote=IDX_vote';
    S=S';
    S_vote=S;
    S = S(1:neibours,:);
    otherwise
%   S = single(pdist2imp(data_noise',data_noise','sqeuclidean'));
%   S = (pdist2imp(data_noise',data_noise','sqeuclidean'));
   %[IDX_vote,S]=knnsearch(data_noise',data_noise','k',neibours+1,'distance','euclidean');
    [IDX_vote,S]=knnsearch(data_noise',data_noise','k',retain+1,'distance','euclidean');

     IDX_vote=IDX_vote(:,2:end);
     S=S(:,2:end);
     IDX_vote=IDX_vote';
     S=S';

end
%% Clipping NN

IDX2 = IDX_vote(1:neibours,:);
S_vote=S;
S = S(1:neibours,:);
a1=1:N;
a2=repmat(a1,neibours);
a2=a2(1:neibours,1:N);
a3=a2(:);
I=a3;
J=IDX2(:);
% s = sqrt( sum( ( data_noise(:,I)-data_noise(:,J) ).^2, 1) );
% s=S(:);
% s = sqrt( sum( ( data_noise(:,I)-data_noise(:,J) ).^2, 1) );

Sigma=0.1;
% s = (1.1./ (1.1+ (f_outliers(I)-f_outliers(J)).^2)).^2;
% s1= ( sum( ( data_noise(:,I)-data_noise(:,J) ).^2, 1) )
% s2= (1.0./ (1+ s1)).^2;
% s2=s2';

s = (f_outliers(I)-f_outliers(J) ).^2;
w_temp=exp(-(s) ./ Sigma );
%s=s.*s2;
%w_temp=s;
waff = sparse(I,J,w_temp,N,N);
W_k=waff;
%W_k= (W_k + W_k')./2 ;
W_k= ((W_k + W_k')+ abs(W_k - W_k'))./2;
W_G=sparse(W_k);


function [x0, exitflag] = vertex_untangle(tri, x, idx0)
% VERTEX_UNTANGLE  Find 2D coordinates for a free vertex surrounded
% by a polygon of fixed counter-clockwise sorted vertices such that the
% free vertex is untangled.
%
% This function is an enhanced implementation of the 2D case (i.e. each
% simplex is a triangle) of the "Optimization-based mesh untangling"
% method proposed by Freitag and Plassmann (2000).
%
% Note that the original paper contains some errors in the formulation of
% the linear programming problem. The correct problem formulation is
%
%     max b^T \pi (equivalenty: min -b^T \pi)
%     such that -A^T \pi <= c
%
% where b = [0 0 1], A^T = [ax ay -2*ones(N, 1)], N=number of
% triangles, and ax, ay, c as given in the paper.
%
% We check whether the free vertex is already untangled. In that case, we
% do not relocate it (this is an untangling algorithm, not a mesh
% improvement or mesh smoothing algorithm).
%
% X0 = vertex_untangle(TRI, X);
%
%   Let TRI, X describe a closed fan triangular mesh. That is, we have a
%   central or free vertex connected to N neighbours. The neighbours are
%   connected between them to form a closed perimeter. The perimeter is
%   assumed to be oriented in counter-clockwise orientation (that is, if a
%   solution exists, the areas of the triangles will all be positive).
%
%   We want to find the coordinates X0 for the free vertex, such that the
%   edges that connect vertices don't overlap. The N neighbours remain
%   fixed. If the free vertex was already producing any overlaps (i.e. the
%   mesh was entangled), this can be seen as an untangling algorithm.
%
%   TRI is a 3-column matrix. Each row contains the 3 nodes that form one
%   triangular facet in the mesh. The orientation provided by the triangles
%   will be assumed by the algorithm to the counter-clockwise, i.e. the
%   algorithm will produce a solution where all triangles have positive
%   signed areas.
%
%   X is a 2-column matrix. X(i, :) contains the xy-coordinates of the
%   i-th node in the mesh. Note that Freitag and Plassmann (2000) also
%   provided the solution for the 3D case (i.e. each simplex is a
%   tetrahedron), but we haven't implemented it here yet.
%
%   X0 is a 2-vector with the optimal coordinates of the central (a.k.a.
%   free) vertex, in a linear programming sense,
%
% [X0, EXITFLAG] = vertex_untangle(...)
%
%   EXITFLAG is the exit condition of the algorithm.
%
%     2: Free vertex was not tangled, no relocation performed.
%     1: Linear programming algorithm converged to a relocation solution.
%     0: Linear programming algorithm stopped because of maximum number of
%        iterations reached.
%    -1: Linear programming algorithm found no valid solution exists.
%
% L. A. Freitag and P. Plassmann, "Local optimization-based simplicial mesh
% untangling and improvement", International Journal for Numerical Methods
% in Engineering, 49(1):109-125, 2000.
%
% See also: sphtri_untangle, linprog, surfreorient.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.2.3
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

% check arguments
narginchk(3, 3);
nargoutchk(0, 3);

if (size(tri, 2) ~= 3)
    error('TRI must have 3 columns')
end
if (size(x, 2) ~= 2)
    error('X must have 2 columns')
end
if (~isscalar(idx0))
    error('IDX must be a scalar')
end

% number of vertices in the neighbourhood
N = size(x, 1);

% adjacency matrix for the mesh. Edges are directed, thus the matrix is not
% symmetric. We do it this way to preserve the counter-clockwise
% orientation of the neighbourhood, i.e. the positive sign of the areas
d = sparse(N, N);
d(sub2ind([N, N], tri(:, 1), tri(:, 2))) = 1;
d(sub2ind([N, N], tri(:, 2), tri(:, 3))) = 1;
d(sub2ind([N, N], tri(:, 3), tri(:, 1))) = 1;

% number of neighbours for each vertex
nn = full(sum((d | d') ~= 0));

% valid neighbourhood check: the central vertex is connected to every other
% vertex
if (nn(idx0) ~= size(x, 1)-1)
    error(['Invalid neighbourhood: central vertex connected to ' ...
        num2str(nn(idx0)) ' vertices instead of ' num2str(size(x, 1) - 1)])
end
% valid neighbourhood check: non-central vertices must be connected to the
% central vertex, and to another two vertices each
if (any(nn([1:idx0-1 idx0+1:end]) ~= 3))
    error('Invalid neighbourhood: at least one boundary vertex not connected to another 3 vertices')
end

% get sorted list of vertices that form the perimeter
v = sort_perim_vertices(d, idx0);

% % DEBUG: plot boundary edges, marking the first segment
% subplot(2, 2, 1)
% hold off
% gplot(d, x)
% hold on
% plot(x(v([1:end 1]), 1), x(v([1:end 1]), 2), 'r')
% plot(x(v(1), 1), x(v(1), 2), 'ro')
% plot(x(v(2), 1), x(v(2), 2), 'rp')

% compute the optimal coordinates
% [x1, triarea, exitflag, output] ...
[x1, ~, exitflag] ...
    = linear_programming_coordinates(x, idx0, v);

% % DEBUG: plot solution of current orientation
% subplot(2, 2, 2)
% aux = x;
% aux(idx0, :) = x1;
% hold off
% gplot(d, aux)
% hold on
% plot(aux(v([1:end 1]), 1), aux(v([1:end 1]), 2), 'r')
% plot(aux(v(1), 1), aux(v(1), 2), 'ro')
% plot(aux(v(2), 1), aux(v(2), 2), 'r*')

% % DEBUG: plot solution of opposite orientation
% subplot(2, 2, 4)
% aux = x;
% aux(idx0, :) = x2;
% hold off
% gplot(d, aux)
% hold on
% plot(aux(v([1:end 1]), 1), aux(v([1:end 1]), 2), 'r')
% plot(aux(v(1), 1), aux(v(1), 2), 'ro')
% plot(aux(v(2), 1), aux(v(2), 2), 'r*')

%% choose the best solution

% if unsuccessful, we return NaN as a solution
if (exitflag < 0)

    x0 = nan(1, 2);
    exitflag = -1;

% else, we return the new location
else

    x0 = x1;

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Auxiliary functions

% d: the sparse adjacency matrix with the directed connections between the
% perimeter vertices
%
% idx0: index of the central vertex
%
% v: ordered list of perimeter vertices in counter-clockwise orientation
% (so that the signed area is positive)
function v = sort_perim_vertices(d, idx0)

% clear up the connections from the perimeter to the central vertex
d(idx0, :) = 0;
d(:, idx0) = 0;

% Dijkstra's shortest paths from one arbitrary vertex in the perimeter to
% all the others
if (idx0 == 1)
    va = 2;
else
    va = 1;
end
[l, p] = dijkstra(d, va);

% furtherst vertex from origin vertex
l(isinf(l)) = 0;
[~, vb] = max(l);

% path from va to vb
v = graphpred2path(p, vb);

% invert path
v = v(end:-1:1);

end

% set up the Freitag and Plassmann (2000) linear programming problem and
% compute the solution. Note that the formulation in the original paper is
% wrong, as has been corrected here.
%
% x: 2D vertex coordinates
%
% v0: index of the free vertex
%
% v: indices of the perimeter vertices, sorted in counter-clockwise
% orientation (so that the signed area is positive)
%
% lb, ub: lower and upper bounds for the linear programming algorithm
function [x0, triarea, exitflag, output] ...
    = linear_programming_coordinates(x, v0, v)

% for convenience, express the boundary as a list of edges
e = [v(1:end)' v([2:end 1])'];

% number of triangles
Ntri = size(e, 1);

% terms for the linear programming problem
xi = x(e(:, 1), 1);
xj = x(e(:, 2), 1);
yi = x(e(:, 1), 2);
yj = x(e(:, 2), 2);
ax = yi - yj;
ay = xj - xi;
c = xi .* yj - xj .* yi;
A = [ax'; ay'; -2*ones(1, Ntri)];

% compute triangle areas (this formula comes from expanding the determinant
% form of a triangle's area)
triarea = 0.5 * (ax * x(v0, 1) + ay * x(v0, 2) + c);

% if all the areas are positive, then there's no tangling, and we won't
% relocate the free vertex (this is not a smoothing algorithm, thus the
% free vertex will only be moved if it's tangled)
if all(triarea > 0)
    x0 = x(v0, :);
    exitflag = 2;
    output = [];
    return
end

% the solution cannot be outside the convex hull of the boundary vertices.
% Thus, it cannot be outside of a rectangular box containing the boundary
% vertices. We can use this as the lower and upper bounds for the linear
% programming solution
lb = min(x(v, :));
ub = max(x(v, :));

% the third component of the linear programming solution vector = minimum
% area. We bound the area value between 0 and infinite. We could use as the
% upper bound the convex hull's area, but it's not worth wasting time
% computing it.
lb = [lb 0]';
ub = [ub Inf]';

% otherwise, we solve linear programming problem to try to untangle the
% free vertex
options = optimset('Display', 'off', 'Simplex', 'on', 'LargeScale', 'off');
[x0, ~, exitflag, output] = linprog([0 0 -1]', -A', c, [], [], ...
    lb, ub, [], options);

% the two first elements are the target coordinates of the central vertex
x0 = x0(1:2)';

end


function [FEVD, VAR] = VARfevd(VAR,VARopt)
% =======================================================================
% Compute FEVDs for a VAR model estimated with VARmodel. Three
% identification schemes can be specified: zero short-run restrictions,
% zero long run restrictions, and sign restrictions
% =======================================================================
% [FEVD, VAR] = VARfevd(VAR,VARopt)
% -----------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel function
%   - VARopt: options of the VAR (see VARopt from VARmodel)
% -----------------------------------------------------------------------
% OUTPUT
%	- FEVD(t,j,k): matrix with 't' steps, the FEVD due to 'j' shock for
%       'k' variable
%   - VAR: structure including VAR estimation results
%       * VAR.invA: identified contemporaneous A matrix
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% I thank Dora Xia for pointing out a typo in the above description.

%% Check inputs
%===============================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
if ~exist('VARopt','var')
    error('You need to provide VAR options (VARopt from VARmodel)');
end

%% Retrieve and initialize variables
%===============================================
nsteps = VARopt.nsteps;
ident  = VARopt.ident;

S     = VAR.S;
Fcomp = VAR.Fcomp;
nvar  = VAR.nvar;
sigma = VAR.sigma;

MSE   = zeros(nvar,nvar,nsteps);
MSE_j = zeros(nvar,nvar,nsteps);
PSI   = zeros(nvar,nvar,nsteps);
FEVD  = zeros(nsteps,nvar,nvar);
SE    = zeros(nsteps,nvar);

%% Compute the multipliers
%===============================================
VARjunk = VAR;
VARjunk.sigma = eye(nvar);
IRFjunk = VARir(VARjunk,VARopt);

% this loop is to save the multipliers for each period
for mm = 1:nvar
    PSI(:,mm,:) = reshape(IRFjunk(:,:,mm)',1,nvar,nsteps);
end

%% Calculate the contribution to the MSE for each shock (i.e, FEVD)
%===============================================

for mm = 1:nvar % loop for the shocks

    % Calculate Total Mean Squared Error
    MSE(:,:,1) = sigma;

    for kk = 2:nsteps;
       MSE(:,:,kk) = MSE(:,:,kk-1) + PSI(:,:,kk)*sigma*PSI(:,:,kk)';
    end;

    % Get the matrix invA containing the structural impulses
    if strcmp(ident,'oir')
        [out, chol_flag] = chol(sigma);
        if chol_flag~=0; error('VCV is not positive definite'); end
        invA = out';
    elseif strcmp(ident,'bq')
        Finf_big = inv(eye(length(Fcomp))-Fcomp);   % from the companion
        Finf     = Finf_big(1:nvar,1:nvar);
        D        = chol(Finf*sigma*Finf')'; % identification: u2 has no effect on y1 in the long run
        invA = Finf\D;
    elseif strcmp(ident,'sr')
        [out, chol_flag] = chol(sigma);
        if chol_flag~=0; error('VCV is not positive definite'); end
        if isempty(S); error('Rotation matrix is not provided'); end
        invA = (out')*(S');
    end

    % Get the column of invA corresponding to the mm_th shock
    column = invA(:,mm);

    % Compute the mean square error
    MSE_j(:,:,1) = column*column';
    for kk = 2:nsteps
        MSE_j(:,:,kk) = MSE_j(:,:,kk-1) + PSI(:,:,kk)*(column*column')*PSI(:,:,kk)';
    end

    % Compute the Forecast Error Covariance Decomposition
    FECD = MSE_j./MSE;

    % Select only the variance terms
    for nn = 1:nsteps
        for ii = 1:nvar
            FEVD(nn,mm,ii) = FECD(ii,ii,nn);
            SE(nn,:) = sqrt( diag(MSE(:,:,nn))' );
        end
    end
end

% Update VARopt
VAR.invA = invA;


function particles = prediction_step(particles, u, noise)
% Updates the particles by drawing from the motion model
% Use u.r1, u.t, and u.r2 to access the rotation and translation values
% which have to be pertubated with Gaussian noise.
% The position of the i-th particle is given by the 3D vector
% particles(i).pose which represents (x, y, theta).

% noise parameters
% Assume Gaussian noise in each of the three parameters of the motion model.
% These three parameters may be used as standard deviations for sampling.
r1Noise = noise(1);
transNoise = noise(2);
r2Noise = noise(3);

numParticles = length(particles);

for i = 1:numParticles

  % append the old position to the history of the particle
  particles(i).history{end+1} = particles(i).pose;

  % sample a new pose for the particle
  r1 = normrnd(u.r1, r1Noise);
  r2 = normrnd(u.r2, r2Noise);
  trans = normrnd(u.t, transNoise);
  particles(i).pose(1) = particles(i).pose(1) + trans*cos(particles(i).pose(3) + r1);
  particles(i).pose(2) = particles(i).pose(2) + trans*sin(particles(i).pose(3) + r1);
  particles(i).pose(3) = normalize_angle(particles(i).pose(3) + r1 + r2);
end

end


function [Model, Info] = linear_sparse_stepwise_reg(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     regularization version
%
% Notice !!
%     Delay embedding is done in this module,
%     then input vector should be original input data without embedding
%
%   [Model, Info] = linear_sparse_stepwise_reg(X,Y,Model,parm)
%     Scalar iteration version
% --- Input
%  Y  : Output data ( N x T x Ntrial )
%  X  : Input data  ( M x (T + (Dtau-1)*Tau)  x Ntrial)
%  N  =  # of output
%  M  =  # of input (original input space dimension)
%  T  =  # of time sample
%
%  Estimate the following model
%    Y(t) = W * [X(:, t + (Dtau-1)*Tau ); ...; X(:, t)]
%
%  Model : Structure for estimated model
%  if Model is empty, initialization is done before training
%  if Model is previous training result, re-training us done
%
%  parm  : Structure for learning parameter
%  parm.Npre_train :  # of VB-update in initial training
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
%  parm.Tau       = Lag time
%  parm.Dtau      = Number of embedding dimension
%    Total input dimension in embedding space is (M * parm.Dtau)
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.W   :  Weight matrix          ( N x M*D ) , D = parm.Dtau
%  Model.A   :  Prior weight variance  ( 1 x M*D )
%  Model.ix_act : Active index for W after pruning
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = Log likelihood
%  Info.H   = - Model entropy
%
% 2009-10-18 Made by M. Sato

MINVAL = 1.0e-15;

fprintf('linear_sparse_stepwise (regularization) start\n')

% Dimension
[N ,T  ,Ntrial]  = size(Y); % N = # of output
[M ,Tx ,Ntrialx] = size(X); % M = # of input without embedding

Tall = T * Ntrial;

if Ntrial~=Ntrialx, error('# of trial is different in X and Y'); end;

Nskip  = 100;   % skip steps for display info
a_min = 1e-14;	% Minimum value for weight pruning
Fdiff = 1e-12; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode
space_ARD=1;	% if space_ARD=1, ARD is done only for space dimension

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'space_ARD'),space_ARD  = parm.space_ARD ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end
% Parameters
Ntrain = parm.Ntrain;

if Ntrain < 1, Info = []; return; end;

if isfield(parm,'Nupdate')
	Nupdate = parm.Nupdate;
else
	Nupdate = 1;
end

fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Ntrial)
fprintf('--- Number of training sample = %d\n',T)
fprintf('--- Total update iteration    = %d \n',Ntrain)

% original input dimension
Xdim  = M;
M = Xdim*D;
M_ALL = M;

%
% --- Initialization
%

% Input/Output variance
sx = mean((X(:) - mean(X(:))).^2);
sy = mean((Y(:) - mean(Y(:))).^2);

SY0 = mean(sy);
A0  = 1./mean(sx);

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

% Input covariance
XX  = sum(sum(X.^2,3),2)/(Tx*Ntrial);
XX  = repmat(XX', [1 D]);% 1 x M

if isfield(Model,'ix_act')
	SY  = mean(Model.SY);  % 1 x 1

	% Active index
	ix_act = Model.ix_act;
	A = zeros(1,M_ALL);
	W = zeros(N,M_ALL);

	A(ix_act)   = sum(Model.A,1);
	W(:,ix_act) = Model.W;

	if M_ALL ~= Xdim*D,
		fprintf('M_ALL=%d,Xdim=%d,D=%d\n',M_ALL ,Xdim ,D)
		error('M_ALL ~= Xdim*D')
	end

	% Active index in input space without embedding
	IX_dim = find( sum(reshape(A,[Xdim,D]),2) > 0);

	IX_act = repmat( (0:(D-1))* Xdim ,[length(IX_dim) 1]) ...
	       + repmat(IX_dim, [1 D]);
	IX_act = IX_act(:);

	W  = W(:,IX_act) ;  % N x (M*D)
	A  = A(IX_act) ;  % 1 x (M*D)
	X  = X(IX_dim,:,:);
	XX = XX(IX_act);

	M     = length(IX_act);
	Xdim  = length(IX_dim);
elseif isfield(Model,'A')
	A = Model.A;
	if size(A,1)==1 && size(A,2)==1
		A = repmat(A, [1,M_ALL]);
	end
	W = zeros(N,M_ALL);
	SY  = SY0;
	% Save original input for pruning
	IX_act  = 1:M_ALL;
	IX_dim  = 1:Xdim;
else
	A = repmat(A0, [1,M_ALL]);
	W = zeros(N,M_ALL);
	SY  = SY0;
	% Save original input for pruning
	IX_act  = 1:M_ALL;
	IX_dim  = 1:Xdim;
end

ix_act  = 1:M;
ix_dim  = 1:Xdim;
M_all   = M;

% Delay embedding index for W
Wid = [(0:(D-1))'* Xdim + 1 , (1:D)'* Xdim];

% Working variable
G_A = zeros(N,M);       % N x M

if size(A,1)==Xdim && size(A,2)==1
	A   = repmat(A',[1 D]);
elseif size(A,1)==1 && size(A,2)==Xdim
	A   = repmat(A,[1 D]);
elseif size(A,1)==1 && size(A,2)==1
	A   = repmat(A,[1 M_ALL]);
end

SW = SY./( Tall*XX + SY./A );

fprintf('a_min = %g\n', a_min)
fprintf('SY0 = %g\n', SY0)
fprintf('SY  = %g\n', SY)

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
Err = zeros(Ntrain,1);
Mhist = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') && ~isempty(parm.Debug) && parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(N*M_ALL, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

% recover all component
A_all  = zeros(1,M_all);       % N x M

[dY] = error_delay_time(X,Y,W,tau);

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% Ainv = alpha , A = 1/alpha
	Ainv    = SY./A;

	% E = (Y-W*X)^2/SY +  W^2/A
	%   = ( (Y-W*X)^2 +  W^2 * (SY/A) )/SY
    [W]  = weight_update_embed(X, dY, W, Tall*XX, Ainv, tau);
	[dY] = error_delay_time(X, Y, W, tau);

    dYY = sum(dY(:).^2)/(N*Tall);
	WW  = sum(W.^2,1);

    % Noise variance update
    SY  = dYY + sum(WW .* Ainv)/(N*Tall);
    % Prevent zero variance
    SY  = max( SY, MINVAL);

    % Log variance
    SWA     = max( SW .* Ainv , MINVAL);
    log_sw  = N*(sum( log(SWA) - SWA + 1 ));
    log_sy  = N*( log(SY) );
    log_a   = Ta0*(sum(log(Ainv) - a0.*Ainv + 1));

    % Free energy
    LP(k)  = - (0.5*Tall) * log_sy ;
    H(k)   = 0.5*( log_sw + log_a - sum(WW .*Ainv) );
    FE(k)  = LP(k) + H(k);
    Err(k) = sum(dYY)/(N*SY0);

    % Weight variance

    SW = SY./( Tall*XX + Ainv );

    % Hyper parameter for weight variance (ARD)
	if mod(k,Nupdate) == 0,
	    % A = Alpha^-1 * SY
		% Ainv = SY./A;

		% N*A = N * (Alpha * SY) =  (W.^2) + N * SW  ;
		% VB update rule (Stable)
		A  = (WW + N*SW + 2*Ta0*a0)/( N + 2*Ta0 );

		% regularization parameter
		Am = mean(A);
		A  = repmat(Am, 1,M);
		A = max(A,MINVAL);

	end
	% END of if mod(k,Nupdate) == 0

	Mhist(k) = M;
	SW  = SW /SY;

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, err = %g, F = %g, SY= %g, H = %g\n', ...
               k, Err(k), FE(k), SY, - 2*H(k)/log(T));
    end

    % Convergence check
	if k > Ncheck,
		Fdif = (FE(k) - FE(k-Fstep))/(abs(FE(k))+eps);
	else
		Fdif = Fdiff + 1;
	end

	if (Fdiff > abs(Fdif)),
		fprintf('Converged : Free energy change = %g\n',Fdif)
		break;
	end;
end

ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save output variable
Model.A    = A ;
Model.W    = W ;
Model.SY   = SY;
Model.method = 'linear_sparse_stepwise';
Model.mode   = 'scalar';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = Mhist(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

% recover all component
%W_all   = zeros(N,M_all);
%SW_all  = zeros(N,M_all);
%
%A_all(:,ix_act)  = A;
%W_all(:,ix_act)  = W ;  % N x M
%SW_all(:,ix_act) = SW;  % N x M

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


%WAVELET  1D Wavelet transform with optional singificance testing
%
%   [WAVE,PERIOD,SCALE,COI,DJ, PARAMOUT, K] = contwt(Y,DT,PAD,DJ,S0,J1,MOTHER,PARAM)
%
%   Computes the continuous wavelet transform of the vector Y (length N),
%   with sampling rate DT.
%
%   By default, the Morlet wavelet (k0=6) is used.
%   The wavelet basis is normalized to have total energy=1 at all scales.
%
%
% INPUTS:
%
%    Y = the time series of length N.
%    DT = amount of time between each Y value, i.e. the sampling time.
%
% OUTPUTS:
%
%    WAVE is the WAVELET transform of Y. This is a complex array
%    of dimensions (N,J1+1). FLOAT(WAVE) gives the WAVELET amplitude,
%    ATAN(IMAGINARY(WAVE),FLOAT(WAVE) gives the WAVELET phase.
%    The WAVELET power spectrum is ABS(WAVE)^2.
%    Its units are sigma^2 (the time series variance).
%
%
% OPTIONAL INPUTS:
%
% *** Note *** setting any of the following to -1 will cause the default
%               value to be used.
%
%    PAD = if set to 1 (default is 0), pad time series with enough zeroes to get
%         N up to the next higher power of 2. This prevents wraparound
%         from the end of the time series to the beginning, and also
%         speeds up the FFT's used to do the wavelet transform.
%         This will not eliminate all edge effects (see COI below).
%
%    DJ = the spacing between discrete scales. Default is 0.25.
%         A smaller # will give better scale resolution, but be slower to plot.
%
%    S0 = the smallest scale of the wavelet.  Default is 2*DT.
%
%    J1 = the # of scales minus one. Scales range from S0 up to S0*2^(J1*DJ),
%        to give a total of (J1+1) scales. Default is J1 = (LOG2(N DT/S0))/DJ.
%
%    MOTHER = the mother wavelet function.
%             The choices are 'MORLET', 'PAUL', or 'DOG'
%
%    PARAM = the mother wavelet parameter.
%            For 'MORLET' this is k0 (wavenumber), default is 6.
%            For 'PAUL' this is m (order), default is 4.
%            For 'DOG' this is m (m-th derivative), default is 2.
%
%
% OPTIONAL OUTPUTS:
%
%    PERIOD = the vector of "Fourier" periods (in time units) that corresponds
%           to the SCALEs.
%
%    SCALE = the vector of scale indices, given by S0*2^(j*DJ), j=0...J1
%            where J1+1 is the total # of scales.
%
%    COI = if specified, then return the Cone-of-Influence, which is a vector
%        of N points that contains the maximum period of useful information
%        at that particular time.
%        Periods greater than this are subject to edge effects.
%        This can be used to plot COI lines on a contour plot by doing:
%
%              contour(time,log(period),log(power))
%              plot(time,log(coi),'k')
%    DJ = actual scale spacing used
%
%    PARAMOUT = actual parameter value used for mother wavelet
%
%   K       =  frequencies at which the transform was computed
%----------------------------------------------------------------------------
%   Copyright (C) 1995-2004, Christopher Torrence and Gilbert P. Compo
%
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made. This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%
% Notice: Please acknowledge the use of the above software in any publications:
%    ``Wavelet software was provided by C. Torrence and G. Compo,
%      and is available at URL: http://paos.colorado.edu/research/wavelets/''.
%
% Reference: Torrence, C. and G. P. Compo, 1998: A Practical Guide to
%            Wavelet Analysis. <I>Bull. Amer. Meteor. Soc.</I>, 79, 61-78.
%
% Please send a copy of such publications to either C. Torrence or G. Compo:
%  Dr. Christopher Torrence               Dr. Gilbert P. Compo
%  Research Systems, Inc.                 Climate Diagnostics Center
%  4990 Pearl East Circle                 325 Broadway R/CDC1
%  Boulder, CO 80301, USA                 Boulder, CO 80305-3328, USA
%  E-mail: chris[AT]rsinc[DOT]com         E-mail: compo[AT]colorado[DOT]edu
%----------------------------------------------------------------------------
function [wave,period,scale,coi, dj, paramout, k] = ...
	contwt(Y,dt,pad,dj,s0,J1,mother,param);

if (nargin < 8), param = -1;, end
if (nargin < 7), mother = -1;, end
if (nargin < 6), J1 = -1;, end
if (nargin < 5), s0 = -1;, end
if (nargin < 4), dj = -1;, end
if (nargin < 3), pad = 0;, end
if (nargin < 2)
	error('Must input a vector Y and sampling time DT')
end

n1 = length(Y);

if (s0 == -1), s0=2*dt;, end
if (dj == -1), dj = 1./4.;, end
if (J1 == -1), J1=fix((log(n1*dt/s0)/log(2))/dj);, end
if (mother == -1), mother = 'MORLET';, end

%....construct time series to analyze, pad if necessary
x(1:n1) = Y - mean(Y);
if (pad == 1)
	base2 = fix(log(n1)/log(2) + 0.4999);   % power of 2 nearest to N
	x = [x,zeros(1,2^(base2+1)-n1)];
end
n = length(x);

%....construct wavenumber array used in transform [Eqn(5)]
k = [1:fix(n/2)];
k = k.*((2.*pi)/(n*dt));
k = [0., k, -k(fix((n-1)/2):-1:1)];

%....compute FFT of the (padded) time series
f = fft(x);    % [Eqn(3)]

%....construct SCALE array & empty PERIOD & WAVE arrays
scale = s0*2.^((0:J1)*dj);
period = scale;
wave = zeros(J1+1,n);  % define the wavelet array
wave = wave + i*wave;  % make it complex

% loop through all scales and compute transform
for a1 = 1:J1+1
	[daughter,fourier_factor,coi,dofmin, paramout]=wave_bases(mother,k,scale(a1),param);
	wave(a1,:) = ifft(f.*daughter);  % wavelet transform[Eqn(4)]
end

period = fourier_factor*scale;
coi = coi*dt*[1E-5,1:((n1+1)/2-1),fliplr((1:(n1/2-1))),1E-5];  % COI [Sec.3g]
wave = wave(:,1:n1);  % get rid of padding before returning

return

% end of code


function A = dh_calc(a,alpha,d,theta)
% DH_CALC
%
% Calculates the homogeneous transformation between two consecutive joint frames
%
% Input:  [a, alpha, d, theta] - classical DH parameters (not modified DH)
% Output: A - homogeneous transformation between two consecutive joint frames

v = [ a*cos(theta), a*sin(theta), d ];
Xx = cos(theta); Yx = -sin(theta) * cos(alpha); Zx =  sin(theta) * sin(alpha);
Xy = sin(theta); Yy =  cos(theta) * cos(alpha); Zy = -cos(theta) * sin(alpha);
Xz = 0.0;        Yz =  sin(alpha);              Zz =  cos(alpha);
A = [Xx, Yx, Zx, v(1);
     Xy, Yy, Zy, v(2);
     Xz, Yz, Zz, v(3);
     0,  0,  0,  1];
end

github-repos/MATLAB/xuhuairuogu-V-REP-Simulation-Projects/V-REP-Simulation-Projects-841b944af4ea3a8fb250578d36434515f577f411/admittance control(adapted from an example in the book--A Systematic Approach to Learning Robot Programming with ROS)/iiwa14_kinematics/dh_calc.m


function Smooth = my_conv_local(S1, sig)
NN = size(S1,1);
NT = size(S1,2);
dt = -4*sig:1:4*sig;
gaus = exp( - dt.^2/(2*sig^2));
gaus = gaus'/sum(gaus);
Smooth = filter(gaus, 1, [S1' ones(NT,1); zeros(4*sig, NN+1)]);
Smooth = Smooth(1+4*sig:end, :);
Smooth = Smooth(:,1:NN) ./ (Smooth(:, NN+1) * ones(1,NN));
Smooth = Smooth';


 function ob = Gdown(Nd, varargin)
%function ob = Gdown(Nd, varargin)
%|
%| Construct Gdown object, which performs downsampling.
%| This is useful perhaps for iterative zooming methods.
%| See Gdown_test.m for example usage.
%|
%| in
%|	Nd	[1 D]		input signal dimensions
%|
%| options
%|	mask	logical			default: true(Nd)
%|	down	1|2|3|...		down sampling factor (default 2)
%|	type	func|Gsparse|...	not done.  default: 'func'
%| out
%|	ob			fatrix2 object
%|
%| Copyright 2006-8-25, Jeff Fessler, University of Michigan

if nargin == 1 && streq(Nd, 'test'), Gdown_test, return, end
if nargin < 1, ir_usage, end

% defaults
arg.mask = [];
arg.idim = Nd;
arg.down = 2;
arg.type = 'func';
arg.class = 'fatrix2';

% options
arg = vararg_pair(arg, varargin);

arg.odim = arg.idim ./ arg.down;
tmp = round(arg.odim) * arg.down;
if any(tmp ~= arg.idim)
	error 'only integer multiple image size supported'
end

if length(arg.idim) > 2
	error 'only 2d implemented due to downsample2() limitation'
end

if isempty(arg.mask)
	arg.mask = true(arg.idim);
end

arg.up_scale = 1 / arg.down^numel(arg.idim);

switch arg.class
case 'Fatrix'
	arg.np = sum(arg.mask(:));
	arg.dim = [prod(arg.odim) arg.np]; % nd x np
	ob = Fatrix(arg.dim, arg, ...
		'abs', @(ob) ob, ...
		'forw', @Gdown_forw_Fatrix, 'back', @Gdown_back_Fatrix);
case 'fatrix2'
	forw = @(arg, x) downsample2(x, arg.down);
	back = @(arg, y) fatrix2_maskit(arg.mask, ...
			upsample_rep(arg.up_scale * y, arg.down));
	ob = fatrix2('arg', arg, 'forw', forw, 'back', back, ...
		'abs', @(ob) ob);

otherwise
	fail 'not done'
end

% Gdown_forw_Fatrix(): y = A * x
% in
%	x	[np L] or [(Nd) L]
% out
%	y	[M L]
%
function y = Gdown_forw_Fatrix(arg, x)

[x ei] = embed_in(x, arg.mask, arg.np);

LL = size(x, 1+length(arg.idim)); % *L
if LL == 1
	y = downsample2(x, arg.down);
else
	y = [];
	for ll=1:LL
		y(:,:,ll) = downsample2(x(:,:,ll), arg.down); % fix: generalize
	end
end

y = ei.shape(y);

% Gdown_back_Fatrix(): x = A' * y
% in
%	y	[M L]
% out
%	x	[np L]
%
function x = Gdown_back_Fatrix(arg, y)

[y eo] = embed_out(y, arg.odim);

LL = size(y, 1+length(arg.odim)); % *L
if LL == 1
	x = upsample_rep(y, arg.down);
else
	for ll=1:LL
		x(:,:,ll) = upsample_rep(y(:,:,ll), arg.down); % fix: generalize
	end
end
x = x * arg.up_scale;

x = eo.shape(x, arg.mask, arg.np);

%
% Gdown_test
%
function Gdown_test

dim = [4 6];
down = 2;
A = Gdown(dim, 'down', down);

x = reshape(1:prod(dim), dim);
y1 = downsample2(x, down);
y2 = A * x;
jf_equal(y1, y2)

y = y1;
x1 = upsample_rep(y, down) * A.arg.up_scale;
x2 = A' * y;
jf_equal(x1, x2)

mask = true(dim);
if isa(A, 'Fatrix')
	Fatrix_test_basic(A, mask)
else
	fatrix2_tests(A)
end

test_adjoint(A);


%% Tutorial for Inscribed_Rectangle Package
% *By Jarek Tuszynski*
%
% Inscribed_Rectangle package provides 2 low level computer vision / image
% analysis functions able to locate largest square or rectangle inscribed
% inside arbitrary shape defined by a binary mask (black and white image).
% Only rectangles with vertical/horizontal edges are considered. The
% functions proved can be used as tools for larger image segmentation
% problems.
%% Change History
% * 2010-07-07 - original version

%% Licence
% The package is distributed under BSD License
format compact; % viewing preference
clear variables; close all;
type('license.txt')

%% Create a mask defining a circle
[X,Y] = meshgrid(-200:200, -200:200);
BW = (X.^2 + Y.^2)<180^2;
figure(1); imshow(BW)

%% Run FindLargestSquares and return results.
% S stores for each pixel the size of the largest all-white square
% with its upper-left corner at that pixel
S = FindLargestSquares(BW);
imagesc(S); axis off

%% Use S to find the largest Square inscribed in the circle
imshow(BW)
[~, pos] = max(S(:));
[r c] = ind2sub(size(S), pos);
rectangle('Position',[c,r,S(r,c),S(r,c)], 'EdgeColor','r', 'LineWidth',3);

%% Run FindLargestRectangles and return results.
% C stores for each pixel the area of the largest all-white rectangle
% with its upper-left corner at that pixel. W and H store width and height
% of those rectangles
[C H W] = FindLargestRectangles(BW, [0 0 1]);
subplot(2,2,1); imagesc(BW); axis off; axis equal; colormap gray
title('Circle mask');
subplot(2,2,2); imagesc(H);  axis off; axis equal; colormap gray
title('Height of the rectangles');
subplot(2,2,3); imagesc(W);  axis off; axis equal; colormap gray
title('Width of the rectangles');
subplot(2,2,4); imagesc(C);  axis off; axis equal; colormap gray
title('Area of the rectangles');

%% Find the largest Rectangle inscribed in the circle, with size measured by area
close all; imshow(BW)
[~, pos] = max(C(:));
[r c] = ind2sub(size(S), pos);
rectangle('Position',[c,r,W(r,c),H(r,c)], 'EdgeColor','r', 'LineWidth',3);

%% Find the largest Rectangle inscribed in the circle with size measured as rectangle circumference with vertical edges having twice the weight of the horizontal edges
[C H W] = FindLargestRectangles(BW, [2 1 0]);
imshow(BW)
[~, pos] = max(C(:));
[r c] = ind2sub(size(S), pos);
rectangle('Position',[c,r,W(r,c),H(r,c)], 'EdgeColor','r', 'LineWidth',3);

%% Find the largest Rectangle inscribed in the circle with size measured as rectangle circumference with horizontal edges having 3 times the weight of the vertical edges
[C H W] = FindLargestRectangles(BW, [1 3 0]);
imshow(BW)
[tmp pos] = max(C(:));
[r c] = ind2sub(size(S), pos);
rectangle('Position',[c,r,W(r,c),H(r,c)], 'EdgeColor','r', 'LineWidth',3);

%% Load an image of Jenga tower and create a boolean mask of the tower shape.
% Image by "Jason7825" was copied from http://commons.wikimedia.org/wiki/File:Jenga_arrangement.jpg and is distributed under CC-BY-SA-3.0 & GDFL licenses
I = imread('Jenga_arrangement.jpg');
imshow(I)
BW = rgb2gray(I)>115;
BW = imfill(BW,'holes');
figure(2); imshow(BW)

%% Use FindLargestRectangles to locate rectangular blocks in the image
imshow(I)
for i = 1:10
  [C H W] = FindLargestRectangles(BW, [0 10 1]);
  [tmp pos] = max(C(:));
  [r c] = ind2sub(size(C), pos);
  rectangle('Position',[c,r,W(r,c),H(r,c)], 'EdgeColor','r', 'LineWidth',3);
  BW( r:(r+H(r,c)-1), c:(c+W(r,c)-1) ) = 0;
end


function [r, c, hnew] = houghpeaks(h, numpeaks, threshold, nhood)
%HOUGHPEAKS Detect peaks in Hough transform.
%   [R, C, HNEW] = HOUGHPEAKS(H, NUMPEAKS, THRESHOLD, NHOOD) detects
%   peaks in the Hough transform matrix H.  NUMPEAKS specifies the
%   maximum number of peak locations to look for.  Values of H below
%   THRESHOLD will not be considered to be peaks.  NHOOD is a
%   two-element vector specifying the size of the suppression
%   neighborhood.  This is the neighborhood around each peak that is
%   set to zero after the peak is identified.  The elements of NHOOD
%   must be positive, odd integers.  R and C are the row and column
%   coordinates of the identified peaks.  HNEW is the Hough transform
%   with peak neighborhood suppressed.
%
%   If NHOOD is omitted, it defaults to the smallest odd values >=
%   size(H)/50.  If THRESHOLD is omitted, it defaults to
%   0.5*max(H(:)).  If NUMPEAKS is omitted, it defaults to 1.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.5 $  $Date: 2003/11/21 13:34:50 $

if nargin < 4
   nhood = size(h)/50;
   % Make sure the neighborhood size is odd.
   nhood = max(2*ceil(nhood/2) + 1, 1);
end
if nargin < 3
   threshold = 0.5 * max(h(:));
end
if nargin < 2
   numpeaks = 1;
end

done = false;
hnew = h; r = []; c = [];
while ~done
   [p, q] = find(hnew == max(hnew(:)));
   p = p(1); q = q(1);
   if hnew(p, q) >= threshold
      r(end + 1) = p; c(end + 1) = q;

      % Suppress this maximum and its close neighbors.
      p1 = p - (nhood(1) - 1)/2; p2 = p + (nhood(1) - 1)/2;
      q1 = q - (nhood(2) - 1)/2; q2 = q + (nhood(2) - 1)/2;
      [pp, qq] = ndgrid(p1:p2,q1:q2);
      pp = pp(:); qq = qq(:);

      % Throw away neighbor coordinates that are out of bounds in
      % the rho direction.
      badrho = find((pp < 1) | (pp > size(h, 1)));
      pp(badrho) = []; qq(badrho) = [];

      % For coordinates that are out of bounds in the theta
      % direction, we want to consider that H is antisymmetric
      % along the rho axis for theta = +/- 90 degrees.
      theta_too_low = find(qq < 1);
      qq(theta_too_low) = size(h, 2) + qq(theta_too_low);
      pp(theta_too_low) = size(h, 1) - pp(theta_too_low) + 1;
      theta_too_high = find(qq > size(h, 2));
      qq(theta_too_high) = qq(theta_too_high) - size(h, 2);
      pp(theta_too_high) = size(h, 1) - pp(theta_too_high) + 1;

      % Convert to linear indices to zero out all the values.
      hnew(sub2ind(size(hnew), pp, qq)) = 0;

      done = length(r) == numpeaks;
   else
      done = true;
   end
end


function [y,deriv] = vectorized_function(w,f,m,direction)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% This template vectorizes the given function F: R^m -> R as follows:
%   k = length(w)/m;
%   If direction=1, X = reshape(w,m,k), y(j) = F(X(:,j)), or
%   if direction=2, X = reshape(w,k,m), y(i) = F(X(i,:)),
%   so that length(y) = k.
%
% Input parameters:
%   w: As with every MV2DF, w can be [], a vector, or a function handle to
%      another MV2DF.
%   f: is a function handle to an m-file that represents the function
%      F: R^m -> R, as well as its first and second derivatives.
%
%   m: The input dimension to F.
%      (optional, default m = 1)
%
%   direction: is used as explained above to determine whether columns,
%              or rows of X are processed by F.
%              (optional, default  direction = 2)
%
%   Function f works as follows:
%     (Note that f, f1 and f2 have to know the required direction, it is
%      not passed to them.)
%   [y,f1] = f(X), where X and y are as defined above.
%
%   Function f1 works as follows:
%   [J,f2] = f1(), where size(J) = size(X).
%                  Column/row i of J is the gradient of y(i) w.r.t.
%                  column/row i of W.
%                  f2 is a function handle to 2nd order derivatives.
%                     If 2nd order derivatives are 0, then f2 should be [].
%
%   Function f2 works as follows:
%   H = f2(dX), where size(dX) = size(X).
%               If direction=1, H(:,j) = H_i * dX(:,j), or
%               if direction=2, H(i,:) = dX(i,:)* H_i, where
%               H_i is Hessian of y(i), w.r.t. colum/row i of X.
%
%

if nargin==0
    test_this();
    return;
end

if ~exist('m','var')
    m = 1;
    direction = 2;
end

if isempty(w)
    y = @(w)vectorized_function(w,f,m,direction);
    return;
end

if isa(w,'function_handle')
    outer = vectorized_function([],f,m,direction);
    y = compose_mv(outer,w,[]);
    return;
end

if direction==1
    W = reshape(w,m,[]);
elseif direction==2
    W = reshape(w,[],m);
else
    error('illegal direction %i',direction);
end

if nargout==1
    y = f(W);
else
    [y,f1] = f(W);
    deriv = @(dy) deriv_this(dy,f1,direction);
end
y = y(:);

end

function [g,hess,linear] = deriv_this(dy,f1,direction)
if direction==1
    dy = dy(:).';
else
    dy = dy(:);
end
if nargout==1
  J = f1();
  g = reshape(bsxfun(@times,J,dy),[],1);
else
  [J,f2] = f1();
  linear = isempty(f2);
  g = reshape(bsxfun(@times,J,dy),[],1);
  hess = @(d) hess_this(d,f2,J,dy,direction);
end

end

function [h,Jv] = hess_this(dx,f2,J,dy,direction)

dX = reshape(dx,size(J));
if isempty(f2)
    h = [];
else
    h = reshape(bsxfun(@times,dy,f2(dX)),[],1);
end
if nargout>1
    Jv = sum(dX.*J,direction);
    Jv = Jv(:);
end

end

%%%%%%%%%%%%%%%%%%%%  Example function: z = x^2 + y^3 %%%%%%%%%%%%%%%%%%%%

% example function: z = x^2 + y^3
function [z,f1] = x2y3(X,direction)
    if direction==1
        x = X(1,:);
        y = X(2,:);
    else
        x = X(:,1);
        y = X(:,2);
    end
    z = x.^2+y.^3;
    f1 = @() f1_x2y3(x,y,direction);
end

% example function 1st derivative: z = x^2 + y^2
function [J,f2] = f1_x2y3(x,y,direction)
if direction==1
    J = [2*x;3*y.^2];
else
    J = [2*x,3*y.^2];
end
f2 = @(dxy) f2_x2y3(dxy,y,direction);
end

% example function 2nd derivative: z = x^2 + y^2
function H = f2_x2y3(dxy,y,direction)
if direction==1
    H = dxy.*[2*ones(size(y));6*y];
else
    H = dxy.*[2*ones(size(y)),6*y];
end
end

%%%%%%%%%%%%%%%%%%%%  Example function: z = x*y^2 %%%%%%%%%%%%%%%%%%%%

% example function: z = x*y^2
function [z,f1] = xy2(X,direction)
    if direction==1
        x = X(1,:);
        y = X(2,:);
    else
        x = X(:,1);
        y = X(:,2);
    end
    y2 = y.^2;
    z = x.*+y2;
    f1 = @() f1_xy2(x,y,y2,direction);
end

% example function 1st derivative: z = x*y^2
function [J,f2] = f1_xy2(x,y,y2,direction)
if direction==1
    J = [y2;2*x.*y];
else
    J = [y2,2*x.*y];
end
f2 = @(dxy) f2_xy2(dxy,x,y,direction);
end

% example function 2nd derivative: z = x*y^2
function H = f2_xy2(dxy,x,y,direction)
if direction==1
  dx = dxy(1,:);
  dy = dxy(2,:);
  H = [2*y.*dy;2*y.*dx+2*x.*dy];
else
  dx = dxy(:,1);
  dy = dxy(:,2);
  H = [2*y.*dy,2*y.*dx+2*x.*dy];
end
end

function test_this()

k = 5;
m = 2;

dr = 1;
fprintf('Testing x^2+y^2 in direction %i:\n\n',dr);
f = vectorized_function([],@(X)x2y3(X,dr),2,dr);
test_MV2DF(f,randn(k*m,1));

dr = 2;
fprintf('\n\n\n\nTesting x*y^2 in direction %i:\n\n',dr);
f = vectorized_function([],@(X)xy2(X,dr),2,dr);
test_MV2DF(f,randn(k*m,1));

end


function [b rho ypred matrix xval] = find_spatial_hist_fast(x)
% Function to find the spatial histogram for IMAGE and then compute Corr
% and fit 3-parameter logistic. returns logistic fitting value.
[nrows ncolms] = size(x);
 im = x;
 nbins = 100;
 max_x = 25;
 ss_fact = 4;
 [hval bins] = hist(x(:),nbins);
 x = map_matrix_to_closest_vec(im,bins);
for j = 1:ss_fact:max_x
    matrix = zeros(nbins);

    for i = 1:nbins

        [r c] = find(x==bins(i));

        if(isempty(r)~=1)
           h  = zeros(1,nbins);

            ctemp = c-j;
            rtemp = r;rtemp(ctemp<1) = [];
            ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            ctemp = c+j; rtemp = r;rtemp(ctemp>ncolms) = [];
            ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c;ctemp(rtemp<1) = [];
            rtemp(rtemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;
            ctemp = c;ctemp(rtemp>nrows) = [];
            rtemp(rtemp>nrows) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;ctemp = c;ctemp(rtemp>nrows) = [];rtemp(rtemp>nrows) = [];
            ctemp = ctemp+j;rtemp(ctemp>ncolms) = []; ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;ctemp = c;ctemp(rtemp>nrows) = [];rtemp(rtemp>nrows) = [];
            ctemp = ctemp-j; rtemp(ctemp<1) = [];ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c;ctemp(rtemp<1) = [];rtemp(rtemp<1) = [];
            ctemp = ctemp+j;rtemp(ctemp>ncolms) = []; ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c; ctemp(rtemp<1) = []; rtemp(rtemp<1) = [];
            ctemp = ctemp-j;rtemp(ctemp<1) = []; ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            matrix(i,:) = h;
        end

    end
    X = [bins]'; Y = X; matrix = matrix/sum(sum(matrix));
    px = sum(matrix,2); py = sum(matrix,1)';
    mu_x = sum(X.*px); mu_y = sum(Y.*py);
    sigma2_x = sum((X-mu_x).^2.*px);    sigma2_y = sum((Y-mu_x).^2.*py);
    [xx yy] = meshgrid(X,Y);
    rho(j) = (sum(sum(xx.*yy.*matrix))-mu_x.*mu_y)/(sqrt(sigma2_x)*sqrt(sigma2_y));

end

rho = [rho(1:ss_fact:end)];
xval = [1:ss_fact:max_x];
% b = nlinfit([0 1:4:100],rho,@fit_spatial_corr,ones(1,5));
pp = polyfit([1:ss_fact:max_x],rho,3);
%  ypred = fit_spatial_corr(b,1:4:100);
ypred = polyval(pp,[1:ss_fact:max_x]);
err = sum((ypred-rho).^2);
b = [pp err];
%
% figure
% plot(rho,'r','LineWidth',3); hold on
% plot(ypred,'k'); title(['Error:',num2str( sum((ypred-rho).^2))]);
% toc
% subplot(1,3,1)
% imagesc(log(matrix+1));colormap(gray);axis xy
% subplot(1,3,2)
% bar(bins,sum(matrix,1)); axis([1 255 0 max(sum(matrix,1))])
% subplot(1,3,3)
% bar(bins,sum(matrix,2));axis([1 255 0 max(sum(matrix,2))])


% This demo efficiently computes the emd_hat between
% histograms where the ground distance is non
% symmetric and the histograms are of non equal size.
% emd_hat is described in the paper:
% A Linear Time Histogram Metric for Improved SIFT Matching
% Ofir Pele, Michael Werman
%  ECCV 2008
% The efficient algorithm is described in the paper:
%  Fast and Robust Earth Mover's Distances
%  Ofir Pele, Michael Werman
%  ICCV 2009
clc; close all; clear all;

D= [0 3 5;
    10 0 30;
    90 80 0;
   80 80 80];

P= [1; 0; 0; 2];
Q= [0; 1; 1];

[dist F]= emd_hat_mex_nes(P,Q,D,0,3)
[dist F]= emd_hat_mex_nes(Q,P,D',0,3)

% Copyright (c) 2009-2012, Ofir Pele
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright
%    notice, this list of conditions and the following disclaimer in the
%    documentation and/or other materials provided with the distribution.
%    * Neither the name of the The Hebrew University of Jerusalem nor the
%    names of its contributors may be used to endorse or promote products
%    derived from this software without specific prior written permission.

% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


%tstoolbox/mex/boxcount
%   boxcount is a fast algorithm that partitions a data set of points into
%   equally spaced and sized boxes. The algorithm is based on Robert
%   Sedgewick's Ternary Search Trees which offer a fast and efficient way
%   to create and search a multidimensional histogram. Empty boxes require
%   no storage space, therefore the maximum number of boxes (and memory)
%   used can not exceed the number of points in the data set, regardless
%   of the data set's dimension and the number of partitions per axis.
%
%   During processing, data values are scaled to be within the range
%   [0,1]. All columns of the input matrix are scaled by the same factor,
%   so no skewing is introduced into the point set.
%
%   Syntax:
%
%     * [a,b,c] = boxcount(point_set, partitions)
%
%   Input arguments:
%
%     * pointset - a N by D double matrix containing the coordinates of
%       the point set, organized as N points of dimension D. D is limited
%       to 128.
%     * partitions - number of partitions per axis, limited to 16384. For
%       convenience, if a vector is given, boxcount will iterate over all
%       values of this vector.
%
%   Output arguments:
%
%     * a - vector of size D with: log2(sum(Number of nonempty boxes))
%     * b - vector of size D with: sum(p * log2(p)) , where p is the
%       relative frequency of points falling into a box
%     * c - vector of size D with: log2(sum(p*p)), where p is the relative
%       frequency of points falling into a box
%
%   Example:
%
% p = rand(50000, 4);
% p = p - min(min(p));
% p = p ./ max(max(p));
% [a,b,c] = boxcount(p, 16)
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt


%% This method normalizes the features to fall in the range [-1, 1]

function testX = testFeatNormalize(testX, minimums, ranges)

	testX = (testX - repmat(minimums, size(testX, 1), 1)) ./ repmat(ranges, size(testX, 1), 1);

end


function [stoi_PercCorr] = stoi_d2percCorr(stoi_d)
% Converts the stoi measure, d, to percent words correct unit
%
% Syntax:	[stoi_PercCorr] = stoi_d2percCorr(stoi_d)
%
% Inputs:
% 	stoi_d - The STOI measure in the raw units
%
% Outputs:
% 	stoi_PercCorr - The corressponding STOI value in Percent of Words
% 	Correct as defined for the IEEE English Library
%
%
%    References:
%      C. H. Taal, R. C. Hendriks, R. Heusdens, and J. Jensen. A Short-Time
%      Objective Intelligibility Measure for Time-Frequency Weighted Noisy
%      Speech. In Acoustics Speech and Signal Processing (ICASSP), pages
%      4214-4217. IEEE, 2010.
%
%      C. H. Taal, R. C. Hendriks, R. Heusdens, and J. Jensen. An Algorithm
%      for Intelligibility Prediction of Time-Frequency Weighted Noisy Speech.
%      IEEE Transactions on Audio, Speech and Language Processing,
%      19(7):2125-2136, 2011.

% Author: Jacob Donley
% University of Wollongong
% Email: jrd089@uowmail.edu.au
% Date: 05 August 2015
% Revision: 0.1
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

f_IEEE_a = -17.4906;
f_IEEE_b = 9.6921;

stoi_PercCorr = 100 / (1 + exp(f_IEEE_a * stoi_d + f_IEEE_b ) );

end


%Image descriptor based on Histogram of Orientated Gradients for gray-level images. This code
%was developed for the work: O. Ludwig, D. Delgado, V. Goncalves, and U. Nunes, 'Trainable
%Classifier-Fusion Schemes: An Application To Pedestrian Detection,' In: 12th International IEEE
%Conference On Intelligent Transportation Systems, 2009, St. Louis, 2009. V. 1. P. 432-437. In
%case of publication with this code, please cite the paper above.

function H=HOG(Im)
nwin_x=3;%set here the number of HOG windows per bound box
nwin_y=3;
B=9;%set here the number of histogram bins
[L,C]=size(Im); % L num of lines ; C num of columns
H=zeros(nwin_x*nwin_y*B,1); % column vector with zeros
m=sqrt(L/2);
if C==1 % if num of columns==1
    Im=im_recover(Im,m,2*m);%verify the size of image, e.g. 25x50
    L=2*m;
    C=m;
end
Im=double(Im);
step_x=floor(C/(nwin_x+1));
step_y=floor(L/(nwin_y+1));
cont=0;
hx = [-1,0,1];
hy = -hx';
grad_xr = imfilter(double(Im),hx);
grad_yu = imfilter(double(Im),hy);
angles=atan2(grad_yu,grad_xr);
magnit=((grad_yu.^2)+(grad_xr.^2)).^.5;
for n=0:nwin_y-1
    for m=0:nwin_x-1
        cont=cont+1;
        angles2=angles(n*step_y+1:(n+2)*step_y,m*step_x+1:(m+2)*step_x);
        magnit2=magnit(n*step_y+1:(n+2)*step_y,m*step_x+1:(m+2)*step_x);
        v_angles=angles2(:);
        v_magnit=magnit2(:);
        K=max(size(v_angles));
        %assembling the histogram with 9 bins (range of 20 degrees per bin)
        bin=0;
        H2=zeros(B,1);
        for ang_lim=-pi+2*pi/B:2*pi/B:pi
            bin=bin+1;
            for k=1:K
                if v_angles(k)<ang_lim
                    v_angles(k)=100;
                    H2(bin)=H2(bin)+v_magnit(k);
                end
            end
        end

        H2=H2/(norm(H2)+0.01);
        H((cont-1)*B+1:cont*B,1)=H2;
    end
end


function element_num = sphere_grid_q16_element_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% SPHERE_GRID_Q16_ELEMENT_NUM counts the elements in a Q16 sphere grid.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NUM = NELEMX * NELEMY = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  element_num = nelemx * nelemy;

  return
end


%  Figure 10.18      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%  fig10_18.m is a script to generate Fig 10.18,
%  the symmetric  rootlocus for the LQR symmetric rootlocus
%  compensator of the satellite position control, non-collocated case

% parameter values
m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
% call model
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
% call model
[f1,g,h,j] = twomass(m,k1,d);
% Form G(s)G(-s) in state-space
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
c=[0*h, g'];
d=[0];

hold off; clf;
sysGG=ss(a,b,c,d);
rlocus(sysGG);
grid;
v=[-2 2 -1.5 1.5];axis(v);
title('Fig. 10.18 Symmetric rootlocus for the satellite')


% PDETIMEDEMO
% This program creates the left preconditionied time-dependent PDE example
% from Chapter 3
%
%
% The example has the form
%
% u_t = - L(u) + f, 0 < x,y < 1
% with homogeneous Dirichlet BC
%
% In terms of the program below, the equation is
% Lu = - (u_xx + u_yy) + 20*u*(u_x + u_y)
%
% Within the nonlinear map, pdetime.m, we apply fish2d.m as
% a preconditioner.
%
% The action of the Laplacian, partial wrt x, and partial wrt y
% are computed in the routines lapmf, dxmf, and dymf. Here mf
% stands for MATRIX FREE.
%
global rhsf uold dt;
m=63; n=m*m;
h=1/(m+1);
h2=(m+1)*(m+1);
h1=(m+1);
tolh=1/h2;
mres=3;
%
% set up the equation
%
x=1:m;
e=x';
x=x'*h;
%
% Form the steady-state solution: sol(x,y)= 10 x y (1-x) (1-y) exp(x^4.5)
%
zsoly=x.*(1-x);
zsolx=zsoly.*exp(x.^4.5);
solt=10*zsolx*zsoly';
sol=solt(:);
clear rhs; clear axt; clear at; clear ayt; clear solt;
%
% Fix the right side so that the steady-state solution is known
%
rhsf = lapmf(sol) + 20*sol.*(dxmf(sol)+dymf(sol));
%
% Set the initial data to zero.
%
u0=zeros(m*m,1);
uold=u0;
uinit=uold;
dt=.1;
nt=10;
%
%
% Use nsoli.m with GMRES as the solver.
% Converged result from previous time step is initial iterate.
%
for it=1:nt+1
    norm(uold-sol)
    tol=[1.d-1,1.d-1]*tolh;
    x=zeros(n,1); parms = [40,40,-.1,1];
    [unew, it_histg, ierr] = nsoli(uinit,'pdetime',tol,parms);
    uinit=uold;
    uold=unew;
end
norm(uold-sol)