function [X,proc]=genMarkedPointProc(lambda,dt,ts,tf,X0,mark,eventDist,timeDist,staycell)
%%GENMARKEDPOINTPROC A function to simulate a marked point process with
%                    rate parameter lambda, initial value X0, and given
%                    time parameters. Using the defaults for the last 3
%                    parameters will generate a compound Poisson process
%                    provided the mark distribution is properly defined
%                    (see note below). The null mark is assumed to be zero.
%                    See [1] for more details on general marked point
%                    processes.
%
%INPUTS: lambda The unit rate parameter for the default Poisson
%               distribution. This may be arbitrarily defined if eventDist
%               is given.
%            dt Time step for event draws.
%            ts Start time for process.
%            tf End time for process.
%            X0 The initial value for the process. Defaults to 0 if
%               omitted.
%          mark An optional function handle f(x,t,j,proc) to be used for
%               sampling mark values. This must accept three scalar inputs
%               and a structure input:
%               x The process value at the beginning of a dt time step.
%               t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%               j The index corresponding to the ordinal number of the
%                 jump since the beginning of the current time step.
%               proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%               If mark is not given or is empty, it defaults to the unity
%               function for all potential values (jumps will have mark 1
%               regardless of the variables).
%     eventDist An optional function handle for the distribution of events.
%               This must output a scalar value in the interval [0,inf). If
%               not given, it defaults to a Poisson distribution with rate
%               parameter lambda*dt. This function handle must take as
%               input:
%               x The process value at the beginning of a dt time step.
%               t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%               i The index corresponding to the ordinal number of the
%                 time step since the beginning of the process at time
%                 ts.
%               proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%     timeDist An optional function handle for the distribution of
%              times during a step. This must output a vector of values in
%              the interval (0,1). If not given, it defaults to a uniform
%              random distribution on (0,1). This function handle must take
%              as input:
%              x The process value at the beginning of a dt time step.
%              t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%              i The index corresponding to the ordinal number of the
%                 time step since the beginning of the process at time
%                 ts.
%              proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%     staycell A logical value. If this is equivalent to true, then X is
%              returned as a cell array. Otherwise, X is returned as a
%              vector. Defaults to false.
%
%OUTPUTS: X Defaults to a 1Xnsteps+1 vector of the process values after
%           each step. X(1) = X0. If the staycell variable evaluates to
%           true, then X is returned as a 1Xnsteps+1 cell array.
%      proc A structure containing information which can be used to
%           reconstruct the exact process X. Members are:
%           nEvents A 1Xnsteps+1 vector of the number of jumps which
%                   occured during each step prior to the index. So
%                   nEvents(1) = 0 and nEvents(i) will be the number of
%                   jumps which occured during the (i-1) time step
%                   corresponding to the change X{i}-X{i-1}.
%               mks A 1Xnsteps cell array where each cell contains a row
%                   vector with the mark values generated for a step.
%              times A 1X((tf-ts)/dt) cell array with each cell
%                   containing a row vector containing the jump times for
%                   the process at the corresponding time step.
%
%Note: If the mark distribution is defined such that all marks are
%independently and identically distributed and the events are Poisson
%distributed, then the generated process will be a compound Poisson
%process. For more on compound Poisson processes, see pages 10-12 of [2].
%
%WARNING: No check is performed to ensure that eventDist and timeDist
%output reasonable values. This function could technically work with marks
%which are not numbers, provided some addition operation is defined for
%them. However, read  the documentation above to ensure your inputs will
%generate a proper process as intended.
%
%EXAMPLE 1: Generating several realizations of a time dependent marked
%           Poisson process and comparing it with the expected path.
% %Set path parameters
% ts = 40; % Start time
% tf = 50; % End time
% dt = 1; % Time step
% numpaths = 100; % Number of simulated paths
% t = ts:dt:tf;
% lambda1 = 5; % Poisson rate parameter for unit time
% lambda2 = 6; % Exponential rate parameter
% X0 = 100; % Initial value for the process
%
% close all
% figure
% hold on
%
% pathends = zeros([1,numpaths]);
% %Generate paths
% for i = 1:numpaths
%     [cp,proc] = genMarkedPointProc(lambda1,dt,ts,tf,X0,@(x,t,j,proc)(tf-t)*ExponentialD.rand(1,lambda2));
%     plot(t,cp)
%     pathends(i) = cp(end);
% end
% avgend = mean(pathends);
% stdend = std(pathends);
%
% %Compute the expected path
% expected = X0+[0,cumsum(lambda1*dt*(tf-(t(1:end-1)+t(2:end))/2)/lambda2)];
% expectedstd = sqrt(cumsum(lambda1*dt*((tf-(t(1:end-1)+t(2:end))/2)).^2*2/(lambda2)^2));
% plot(t,expected,'--k','LineWidth',5)
% xlabel('time (t)')
%
% %Compare simulated path statistics to the expected value
% fprintf("The average path value at t = %0.0f is : %0.5f\n",tf,avgend)
% fprintf("The path standard deviation at t = %0.0f is : %0.5f\n",tf,stdend)
% fprintf("The expected path value at t = %0.0f is : %0.5f\n",tf,expected(end))
% fprintf("The expected standard deviation at t = %0.0f is : %0.5f\n",tf,expectedstd(end))
%
%REFERENCES:
%[1] Stover, Christopher. "Marked Point Process." From MathWorld--A
%    Wolfram Web Resource, created by Eric W. Weisstein.
%    http://mathworld.wolfram.com/MarkedPointProcess.html
%[2] Platen, Eckhard, and Nicola Bruti-Liberati. Numerical solution of
%    stochastic differential equations with jumps in finance. Vol. 64.
%    Springer Science & Business Media, 2010.
%
%July 2019 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(~exist('X0','var')||isempty(X0))
    X0 = 0;
end
if(~exist('mark','var')||isempty(mark))
    mark = @(x,t,j,proc) 1;
end
if(~exist('eventDist','var')||isempty(eventDist))
    eventDist = @(x,t,i,proc) PoissonD.rand(1,lambda*dt);
end
if(~exist('timeDist','var')||isempty(timeDist))
    timeDist = @(x,t,j,proc) rand([1,proc.nEvents(j)]);
end
if(~exist('staycell','var'))
    staycell = false;
elseif(~islogical(staycell))
    error('The value in staycell is not a logical value.')
end

nsteps = floor((tf-ts)/dt);
X = cell(1,nsteps+1);
X{1} = X0;

proc.nEvents = zeros([1,length(X)]);
proc.mks = cell([1,length(X)]);
proc.times = cell([1,length(X)]);

for i = 1:nsteps
    t = (i-1)*dt+ts;
    proc.nEvents(i) = eventDist(X{i},t,i,proc);
    stepmks = cell([1,proc.nEvents(i)]);
    steptimes = sort(timeDist(X{i},t,i,proc)*dt+t);
    proc.times{i} = steptimes;

    stepIntensity = 0;
    for j = 1:proc.nEvents(i)
        stepmks{j} = mark(X{i},steptimes(j),j,proc);
        proc.mks{i}{j} = stepmks{j};
        stepIntensity = stepIntensity+stepmks{j};
    end
    X{i+1} = X{i}+stepIntensity;
end

if(~staycell)
    X = cell2mat(X);
end
end

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


function [date, doy, dow] = gps2date(gps_week, gps_sow)

% SYNTAX:
%   [date, doy, dow] = gps2date(gps_week, gps_sow);
%
% INPUT:
%   gps_week = GPS week
%   gps_sow  = GPS seconds of week
%
% OUTPUT:
%   date = date [year month day hour min sec]
%   doy  = day of year
%   dow  = day of week
%
% DESCRIPTION:
%   Conversion from GPS time to calendar date and day of year (DOY).

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

gps_start_datenum = 723186; %This is datenum([1980,1,6,0,0,0])

gps_dow = fix(gps_sow/86400);                             %day of week
date = datevec(gps_start_datenum + 7*gps_week + gps_dow); %calendar date up to days
gps_sod = gps_sow - gps_dow*86400;                        %seconds of day
date(:,4) = floor(gps_sod/3600);                          %hours
date(:,5) = floor(gps_sod/60 - date(:,4)*60);             %minutes
date(:,6) = gps_sod - date(:,4)*3600 - date(:,5)*60;      %seconds

%day of year (DOY)
if (nargout > 1)
    doy = date2doy(datenum(date));
    doy = floor(doy);
end

%day of week
if (nargout > 2)
    dow = gps_dow;
end


function binomial_test ( )

%*****************************************************************************80
%
%% BINOMIAL_TEST tests BINOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BINOMIAL_TEST:\n' );
  fprintf ( 1, '  A demonstration of the binomial method\n' );
  fprintf ( 1, '  for option valuation.\n' );

  s0 = 2.0;
  e = 1.0;
  r = 0.05;
  sigma = 0.25;
  t1 = 3.0;
  m = 256;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The asset price at time 0, S0    = %f\n', s0 );
  fprintf ( 1, '  The exercise price         E     = %f\n', e );
  fprintf ( 1, '  The interest rate          R     = %f\n', r );
  fprintf ( 1, '  The asset volatility       SIGMA = %f\n', sigma );
  fprintf ( 1, '  The expiry date            T1    = %f\n', t1 );
  fprintf ( 1, '  The number of intervals    M     = %d\n', m );

  c = binomial ( s0, e, r, sigma, t1, m );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The option value is %f\n', c );

  return
end


function y = sfb3D(lo, hi, sf1, sf2, sf3)

% 3D Synthesis Filter Bank
%
% USAGE:
%   y = sfb3D(lo, hi, sf1, sf2, sf3);
% INPUT:
%   lo, hi - lowpass subbands
%   sfi - synthesis filters for dimension i
% OUPUT:
%   y - output array
% See afb3D
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

if nargin < 4
   sf2 = sf1;
   sf3 = sf1;
end

LLL = lo;
LLH = hi{1};
LHL = hi{2};
LHH = hi{3};
HLL = hi{4};
HLH = hi{5};
HHL = hi{6};
HHH = hi{7};

% filter along dimension 3
LL = sfb3D_A(LLL, LLH, sf3, 3);
LH = sfb3D_A(LHL, LHH, sf3, 3);
HL = sfb3D_A(HLL, HLH, sf3, 3);
HH = sfb3D_A(HHL, HHH, sf3, 3);

% filter along dimension 3
L = sfb3D_A(LL, LH, sf2, 2);
H = sfb3D_A(HL, HH, sf2, 2);

% filter along dimension 1
y = sfb3D_A(L, H, sf1, 1);

% LOCAL FUNCTION

function y = sfb3D_A(lo, hi, sf, d)

% 3D Synthesis Filter Bank
% (along single dimension only)
%
% y = sfb3D_A(lo, hi, sf, d);
% sf - synthesis filters
% d  - dimension of filtering
% see afb2D_A

lpf = sf(:, 1);     % lowpass filter
hpf = sf(:, 2);     % highpass filter

% permute dimensions of lo and hi so that dimension d is first.
p = mod(d-1+[0:2], 3) + 1;
lo = permute(lo, p);
hi = permute(hi, p);

[N1, N2, N3] = size(lo);
N = 2*N1;
L = length(sf);
y = zeros(N+L-2, N2, N3);

for k = 1:N3
   y(:, :, k) = upfirdn(lo(:, :, k), lpf, 2, 1) + upfirdn(hi(:, :, k), hpf, 2, 1);
end
y(1:L-2, :, :) = y(1:L-2, :, :) + y(N+[1:L-2], :, :);
y = y(1:N, :, :);
y = cshift3D(y, 1-L/2, 1);

% permute dimensions of y (inverse permutation)
y = ipermute(y, p);


function N = tangentspacefactory(M, x)
% Returns a manifold structure representing the tangent space to M at x.
%
% N = tangentspacefactory(M, x)
%
% N defines a (linear) manifold that is the tangent space to M at x. Points
% are represented as tangent vectors to M at x. Tangent vectors are also
% represented as tangent vectors to M at x.
%
% This is chiefly useful to solve optimization problems involving tangent
% vectors to M at x, which notably comes up when solving linear systems
% involving, for example, the Hessian of the cost on M at x (think of the
% Newton equations.) The Riemannian (actually, Euclidean) structure on N is
% that of the tangent space to M, that is, the inner product is inherited.
%
% See also: preconhessiansolve

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 9, 2015.
% Contributors:
% Change log:
%
%   Jan. 25, 2017 (NB):
%       Following a comment by Jesus Briales on the Manopt forum, the
%       functions N.egrad2rgrad, N.ehess2rhess and N.tangent now include a
%       projection (they were formerly identities.)
%
%   Feb. 2, 2017 (NB):
%       Following a comment by Jesus Briales on the Manopt forum, the
%       function N.proj now calls M.proj(x, .) instead of M.proj(y, .).
%       Furthermore, N.ehess2rhess was corrected in the same way.
%
%   Dec. 14, 2019 (NB):
%       Fixed N.tangent so that it should now work with factories that
%       have a non-identity tangent2ambient, e.g., fixedrankembeddedfactory
%       and rotationsfactory.

    % N is the manifold we build. y will be a point on N, thus also a
    % tangent vector to M at x. This is a typical Euclidean space, hence it
    % will be easy to describe in terms of the tools available for M.
    N = struct();

    % u, u1 and u2 will be tangent vectors to N at y. The tangent space to
    % N at y is the tangent space to M at x, thus u, u1 and u2 are also
    % tangent vectors to M at x.

    if isfield(M, 'name')
        N.name  = @() ['Tangent space to ' M.name()];
    end
    N.dim   = @() M.dim();
    N.inner = @(y, u1, u2) M.inner(x, u1, u2);
    N.norm  = @(y, u) M.norm(x, u);
    N.proj  = @(y, u) M.proj(x, u);
    N.typicaldist = @() sqrt(N.dim());
    if isfield(M, 'tangent2ambient')
        N.tangent = @(y, u) M.proj(x, M.tangent2ambient(x, u));
    else
        N.tangent = N.proj;
    end

    N.egrad2rgrad = N.proj;
    N.ehess2rhess = @(y, eg, eh, d) M.proj(x, eh);
    N.exp = @exponential;
    N.retr = @exponential;
    N.log = @(y1, y2) M.lincomb(x, 1, y2, -1, y1);
    N.pairmean = @(y1, y2) M.lincomb(x, 0.5, y1, 0.5, y2);
    N.rand = @() M.randvec(x);
    N.randvec = @(y) M.randvec(x);
    N.zerovec = M.zerovec;
    N.lincomb = M.lincomb;
    N.transp = @(y1, y2, u) u;
    N.hash = @(y) ['z' hashmd5(M.vec(x, y))];

    % In a Euclidean space, the exponential is merely the sum: y + tu.
    function yy = exponential(y, u, t)
        if nargin == 2
            t = 1;
        end
        yy = M.lincomb(x, 1, y, t, u);
    end

end


clear;
clc;
close all;
format

%% 参考: https://x-io.co.uk/oscillatory-motion-tracking-with-x-imu/

% 利用速度，位置高通滤波来获得短时位移曲线

%load filter_vel.mat
% load example_ins4.mat

data = ch_data_import('data_20210609_163153.csv');
acc = data.imu.acc*9.8;
gyr = deg2rad(data.imu.gyr);
%% 读取数据
acc = acc';
gyr = gyr';

q= [1 0 0 0]';

Fs = 100;
dt = 1 / Fs;
N = length(acc);

fprintf("共%d数据，用时:%.3fs\n", N, N/Fs);

% 惯导解算, 初始化
p = zeros(3, 1);
v = zeros(3, 1);
linVel = zeros(N,3);

for i=1:N
    [p , v , q] = ch_nav_equ_local_tan(p, v, q, acc(i,:)', gyr(i,:)', 1 / Fs, [0, 0, -9.8]');

    % 获得惯性系下加速度并扣除重力
    linAcc(i,:) = ch_qmulv(q, acc(i,:));
    linAcc(i,3) = linAcc(i,3) -9.8;

%   linVel(i,:) = v;
end

% 速度积分

for i = 2:N
    linVel(i,:) = linVel(i-1,:) + linAcc(i,:) * dt;
end

order = 1;
filtCutOff = 0.01;
[b, a] = butter(order, (2*filtCutOff)/(1/dt), 'high');
%linVelHP = filtfilt(b, a, linVel);
linVelHP = filter(b, a, linVel);

% 位置积分
linPos = zeros(size(linVelHP));
for i = 2:length(linVelHP)
    linPos(i,:) = linPos(i-1,:) + linVelHP(i,:) * dt;
end

order = 1;
filtCutOff = 0.5;
[b, a] = butter(order, (2*filtCutOff)/(1/dt), 'high');
%linPosHP = filtfilt(b, a, linPos);
linPosHP = filter(b, a, linPos);

%% Plot
figure('NumberTitle', 'off', 'Name', '速度');
subplot(2,1,1);
plot(linVel);
title('速度');
legend('X', 'Y', 'Z');
subplot(2,1,2);
plot(linVelHP);
title('HP后速度');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', '位置');
subplot(2,1,1);
plot(linPos);
title('位置');
legend('X', 'Y', 'Z');
subplot(2,1,2);
plot(linPosHP);
title('HP后位置');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', '原始数据');
subplot(2,1,1);
plot(acc);
legend("X", "Y", "Z");
title("加速度");
subplot(2,1,2);
plot(gyr);
title("陀螺");
legend("X", "Y", "Z");

figure('NumberTitle', 'off', 'Name', '3D位置');
plot3(linPosHP(1,1), linPosHP(1,2), linPosHP(1,3), '-ks');
hold on;
plot3(linPosHP(:,1), linPosHP(:,2), linPosHP(:,3), '.b');
axis equal
xlabel('X(m)');  ylabel('Y(m)');   zlabel('Z(m)');
title('3D位置');
legend('起始', '3D');

linPosHP(end,:)

% SamplePlotFreq = 4;
%
% SixDOFanimation(linPosHP, R, ...
%                 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'Off', ...
%                 'Position', [9 39 400 400], ...
%                 'AxisLength', 0.1, 'ShowArrowHead', false, ...
%                 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)', 'ShowLegend', false, 'Title', 'Unfiltered',...
%                 'CreateAVI', false, 'AVIfileNameEnum', false, 'AVIfps', ((1/dt) / SamplePlotFreq));


function qr = rightQuaternionProductMatrix(q)
% q 4x1 w,x,y,z
% p * q = rightQuaternionProductMatrix(q) * p
% eq 18 in Sola Quaternion kinematics for the error-state Kalman filter
w = q(1);
x = q(2);
y = q(3);
z = q(4);
qr = [w, -x, -y, -z;
    x, w, z, -y;
    y, -z, w, x;
    z, y, -x, w];
end


function [lat2,lon2,derr,aerr]=vdistinv(lat1,lon1,dist,azim)
% VDISTINV - Invert VDIST function using numerical inversion
%
% Usage:
%
% [lat2,lon2] = vdistinv(lat1,lon1,dist,azim)
% [lat2,lon2,derr,aerr] = vdistinv(lat1,lon1,dist,azim)
%
% Variables:
%
% lat1, lon1 = coordinates of intial point in degrees
% dist       = geodesic distance in meters
% azim       = geodesic azimuth in degrees clockwise from north
% lat2,lon2  = destination coordinates in degrees
% derr       = optional output: error between input distance and the
%              calculated length of the path to the computed endpoint
%              (as computed by VDIST)
% aerr       = optional output: error between the provided azimuth and the
%              calculated azimuth (as computed by VDIST)
%
% Notes: (1) This "quick and dirty" approach was written in response to
%            a user request. The use of a numerical optimization to invert
%            VDIST is a relatively slow and crude approach. (It would be
%            better to implement an algorithm written specifically for that
%            purpose by Vincenty or others.)
%        (2) The distance between essentially antipodal points on an
%            ellipsoid is very sensitive to small deviations in azimuth,
%            so such points should be avoided. (A warning is given.)
%        (3) For other cases, precision is set to about one part in 10^12
%        (3) Tested but no warranty; use at your own risk.
%        (4) Written by Michael Kleder, April 2006
%
% Example:
% >> [dist,azim]=vdist(10,20,30,40)
% dist =    3035728.95690893
% azim =    40.3196402221127
% >> [lat2,lon2]=vdistinv(10,20,dist,azim)
% lat2 =    29.9999999999981
% lon2 =    39.9999999999979
% >>

% initial guess for path endpoint is computed using spherical earth trig:
t1=lat1*0.0174532925199433; % degrees to radians
n1=lon1*0.0174532925199433; % degrees to radians
a=azim*0.0174532925199433; % degrees to radians
d=dist*1.56961230576048e-007; % meters to radians
lat2 = asin(sin(t1)*cos(d)+cos(t1)*sin(d)*cos(a));
lon2 = n1+atan2(sin(d)*sin(a),cos(t1)*cos(d)-sin(t1)*sin(d)*cos(a));
X=[lat2;lon2]*57.2957795130823; % radians to dgrees
% other parameters (start point, arc length, and azimuth) are fixed:
params=[lat1;lon1;dist;azim];
% optimization control settings:
opt=optimset('MaxFunEvals',5000,'TolFun',1e-12);
% solve for accurate end point:
X=fminsearch(@tryone,X,opt,params);
% recover coordinates of endpoint:
lat2=X(1);
lon2=X(2);
if nargout > 2 % if error data is requested
    % compute distance and azimuth from startpoint to endpoint
    [d,a] = vdist(lat1,lon1,lat2,lon2);
    % error in distance:
    derr=abs(dist-d);
    % error in azimuth:
    azim = mod(azim,360);
    a = mod(a,360);
    aerr = abs(azim-a);
    aerr = min(aerr,abs(360-aerr));
end
return
function err=tryone(X,params)
lat2=X(1);
lon2=X(2);
lat1=params(1);
lon1=params(2);
dist=params(3);
azim=params(4);
% crude catch for out-of-bounds attempts:
if lat1<-90 || lat1 > 90 || lat2 < -90 || lat2 > 90
    err = 1e6;
    return
end
[trydist,tryazim]=vdist(lat1,lon1,lat2,lon2);
% compute overall error. First convert distance to approximate arc length
% in degrees so as to weight the terms reasonably closely. (Both move to
% zero in the optimization, but reasonably balancing the units can help.)
err = sqrt((9e-6*(dist-trydist))^2 + (azim-tryazim)^2);
return
function varargout = vdist(lat1,lon1,lat2,lon2)
% VDIST - Using the WGS-84 Earth ellipsoid, compute the distance between
%         two points within a few millimeters of accuracy, compute forward
%         azimuth, and compute backward azimuth, all using a vectorized
%         version of Vincenty's algorithm.
%
% s = vdist(lat1,lon1,lat2,lon2)
% [s,a12] = vdist(lat1,lon1,lat2,lon2)
% [s,a12,a21] = vdist(lat1,lon1,lat2,lon2)
%
% s = distance in meters (inputs may be scalars, vectors, or matrices)
% a12 = azimuth in degrees from first point to second point (forward)
% a21 = azimuth in degrees from second point to first point (backward)
%       (Azimuths are in degrees clockwise from north.)
% lat1 = GEODETIC latitude of first point (degrees)
% lon1 = longitude of first point (degrees)
% lat2, lon2 = second point (degrees)
%
%  Original algorithm source:
%  T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid
%  with Application of Nested Equations", Survey Review, vol. 23, no. 176,
%  April 1975, pp 88-93.
%  Available at: http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
%
% Notes: (1) lat1,lon1,lat2,lon2 can be any (identical) size/shape. Outputs
%            will have the same size and shape.
%        (2) Error correcting code, convergence failure traps, antipodal
%            corrections, polar error corrections, WGS84 ellipsoid
%            parameters, testing, and comments: Michael Kleder, 2004.
%        (3) Azimuth implementation (including quadrant abiguity
%            resolution) and code vectorization, Michael Kleder, Sep 2005.
%        (4) Vectorization is convergence sensitive; that is, quantities
%            which have already converged to within tolerance are not
%            recomputed during subsequent iterations (while other
%            quantities are still converging).
%        (5) Vincenty describes his distance algorithm as precise to within
%            0.01 millimeters, subject to the ellipsoidal model.
%        (6) For distance calculations, essentially antipodal points are
%            treated as exactly antipodal, potentially reducing accuracy
%            slightly.
%        (7) Distance failures for points exactly at the poles are
%            eliminated by moving the points by 0.6 millimeters.
%        (8) The Vincenty distance algorithm was transcribed verbatim by
%            Peter Cederholm, August 12, 2003. It was modified and
%            translated to English by Michael Kleder.
%            Mr. Cederholm's website is http://www.plan.aau.dk/~pce/
%        (9) Distances agree with the Mapping Toolbox, version 2.2 (R14SP3)
%            with a max relative difference of about 5e-9, except when the
%            two points are nearly antipodal, and except when one point is
%            near the equator and the two longitudes are nearly 180 degrees
%            apart. This function (vdist) is more accurate in such cases.
%            For example, note this difference (as of this writing):
%            >>vdist(0.2,305,15,125)
%            18322827.0131551
%            >>distance(0.2,305,15,125,[6378137 0.08181919])
%            0
%       (10) Azimuths FROM the north pole (either forward starting at the
%            north pole or backward when ending at the north pole) are set
%            to 180 degrees by convention. Azimuths FROM the south pole are
%            set to 0 degrees by convention.
%       (11) Azimuths agree with the Mapping Toolbox, version 2.2 (R14SP3)
%            to within about a hundred-thousandth of a degree, except when
%            traversing to or from a pole, where the convention for this
%            function is described in (10), and except in the cases noted
%            above in (9).
%       (12) No warranties; use at your own risk.

% reshape inputs
keepsize = size(lat1);
lat1=lat1(:);
lon1=lon1(:);
lat2=lat2(:);
lon2=lon2(:);
% Input check:
if any(abs(lat1)>90 | abs(lat2)>90)
    error('Input latitudes must be between -90 and 90 degrees, inclusive.')
end
% Supply WGS84 earth ellipsoid axis lengths in meters:
a = 6378137; % definitionally
b = 6356752.31424518; % computed from WGS84 earth flattening coefficient
% preserve true input latitudes:
lat1tr = lat1;
lat2tr = lat2;
% convert inputs in degrees to radians:
lat1 = lat1 * 0.0174532925199433;
lon1 = lon1 * 0.0174532925199433;
lat2 = lat2 * 0.0174532925199433;
lon2 = lon2 * 0.0174532925199433;
% correct for errors at exact poles by adjusting 0.6 millimeters:
kidx = abs(pi/2-abs(lat1)) < 1e-10;
if any(kidx);
    lat1(kidx) = sign(lat1(kidx))*(pi/2-(1e-10));
end
kidx = abs(pi/2-abs(lat2)) < 1e-10;
if any(kidx)
    lat2(kidx) = sign(lat2(kidx))*(pi/2-(1e-10));
end
f = (a-b)/a;
U1 = atan((1-f)*tan(lat1));
U2 = atan((1-f)*tan(lat2));
lon1 = mod(lon1,2*pi);
lon2 = mod(lon2,2*pi);
L = abs(lon2-lon1);
kidx = L > pi;
if any(kidx)
    L(kidx) = 2*pi - L(kidx);
end
lambda = L;
lambdaold = 0*lat1;
itercount = 0;
notdone = logical(1+0*lat1);
alpha = 0*lat1;
sigma = 0*lat1;
sinsigma=nan*lat1;
cossigma=nan*lat1;
cos2sigmam = 0*lat1;
C = 0*lat1;
warninggiven = false;
while any(notdone)  % force at least one execution
    %disp(['lambda(21752) = ' num2str(lambda(21752),20)]);
    itercount = itercount+1;
    if itercount > 50
        if ~warninggiven
            warning('VDIST:antipodal',['Essentially antipodal points ' ...
                'encountered. Precision may be reduced.']);
        end
        lambda(notdone) = pi;
        break
    end
    lambdaold(notdone) = lambda(notdone);
    sinsigma(notdone) = sqrt((cos(U2(notdone)).*sin(lambda(notdone)))...
        .^2+(cos(U1(notdone)).*sin(U2(notdone))-sin(U1(notdone)).*...
        cos(U2(notdone)).*cos(lambda(notdone))).^2);
    cossigma(notdone) = sin(U1(notdone)).*sin(U2(notdone))+...
        cos(U1(notdone)).*cos(U2(notdone)).*cos(lambda(notdone));
    % eliminate rare imaginary portions at limit of numerical precision:
    sinsigma(notdone)=real(sinsigma(notdone));
    cossigma(notdone)=real(cossigma(notdone));
    sigma(notdone) = atan2(sinsigma(notdone),cossigma(notdone));
    alpha(notdone) = asin(cos(U1(notdone)).*cos(U2(notdone)).*...
        sin(lambda(notdone))./sin(sigma(notdone)));
    cos2sigmam(notdone) = cos(sigma(notdone))-2*sin(U1(notdone)).*...
        sin(U2(notdone))./cos(alpha(notdone)).^2;
    C(notdone) = f/16*cos(alpha(notdone)).^2.*(4+f*(4-3*...
        cos(alpha(notdone)).^2));
    lambda(notdone) = L(notdone)+(1-C(notdone)).*f.*sin(alpha(notdone))...
        .*(sigma(notdone)+C(notdone).*sin(sigma(notdone)).*...
        (cos2sigmam(notdone)+C(notdone).*cos(sigma(notdone)).*...
        (-1+2.*cos2sigmam(notdone).^2)));
    %disp(['then, lambda(21752) = ' num2str(lambda(21752),20)]);
    % correct for convergence failure in the case of essentially antipodal
    % points
    if any(lambda(notdone) > pi)
        warning('VDIST:antipodal',['Essentially antipodal points ' ...
            'encountered. Precision may be reduced.']);
        warninggiven = true;
        lambdaold(lambda>pi) = pi;
        lambda(lambda>pi) = pi;
    end
    notdone = abs(lambda-lambdaold) > 1e-12;
end
u2 = cos(alpha).^2.*(a^2-b^2)/b^2;
A = 1+u2./16384.*(4096+u2.*(-768+u2.*(320-175.*u2)));
B = u2./1024.*(256+u2.*(-128+u2.*(74-47.*u2)));
deltasigma = B.*sin(sigma).*(cos2sigmam+B./4.*(cos(sigma).*(-1+2.*...
    cos2sigmam.^2)-B./6.*cos2sigmam.*(-3+4.*sin(sigma).^2).*(-3+4*...
    cos2sigmam.^2)));
varargout{1} = reshape(b.*A.*(sigma-deltasigma),keepsize);
if nargout > 1
    % From point #1 to point #2
    % correct sign of lambda for azimuth calcs:
    lambda = abs(lambda);
    kidx=sign(sin(lon2-lon1)) .* sign(sin(lambda)) < 0;
    lambda(kidx) = -lambda(kidx);
    numer = cos(U2).*sin(lambda);
    denom = cos(U1).*sin(U2)-sin(U1).*cos(U2).*cos(lambda);
    a12 = atan2(numer,denom);
    kidx = a12<0;
    a12(kidx)=a12(kidx)+2*pi;
    % from poles:
    a12(lat1tr <= -90) = 0;
    a12(lat1tr >= 90 ) = pi;
    varargout{2} = reshape(a12 * 57.2957795130823,keepsize); % to degrees
end
if nargout > 2
    a21=NaN*lat1; %#ok this variable won't be computed if not needed
    % From point #2 to point #1
    % correct sign of lambda for azimuth calcs:
    lambda = abs(lambda);
    kidx=sign(sin(lon1-lon2)) .* sign(sin(lambda)) < 0;
    lambda(kidx)=-lambda(kidx);
    numer = cos(U1).*sin(lambda);
    denom = sin(U1).*cos(U2)-cos(U1).*sin(U2).*cos(lambda);
    a21 = atan2(numer,denom);
    kidx=a21<0;
    a21(kidx)= a21(kidx)+2*pi;
    % backwards from poles:
    a21(lat2tr >= 90) = pi;
    a21(lat2tr <= -90) = 0;
    varargout{3} = reshape(a21 * 57.2957795130823,keepsize); % to degrees
end
return


function [Xt,Yt] = elec_3d_2d(X,Y,Z,Zrad)

% elec_3d_2d - Project Cartesian 3D coordinates to a 2D plane.
%
% [Xt,Yt] = elec_3d_2d(X,Y,Z,Zrad)
%
% Project 3D electrode positions onto a 2D plane, using
% gnomonic projection, which draws a line from a point
% halfway between equator and south pole, through electrode,
% to a plane tangential to north pole.
%
% Given:  Set of 3D Cartesian coordinates (X,Y,Z with Z > 0)
%         X,Y midpoint at 0
%         Zrad is radius in Z
%
% See also elec_2d_3d.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:54 $

% Licence:  GNU GPL, no implied or express warranties
% History:  10/1999, Chris Harvey
%           07/2001, Darren.Weber_at_radiology.ucsf.edu
%                    - using matrix algebra rather than indexed looping
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rad = Zrad / 2;

t = (Z + rad) * ( 1/rad );

Xt = X .* t;
Yt = Y .* t;

return


function R = gqr3 (A)
%GQR3 QR factorization, based on Givens rotations
%
% Example:
%   R = gqr3 (A)
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;

% parent = cs_etree (sparse (A), 'col') ;

for i = 2:m
    % i
    for k = 1:min(i-1,n)
    % k
        % Givens rotation to zero out A(i,k) using A(k,k)
        G = givens2 (A(k,k), A(i,k)) ;
        A ([k i],k:n) = G * A ([k i],k:n) ;
        A (i,k) = 0 ;
        % fprintf ('A(21,25)=%g\n', A(21,25)) ;
        % if (A(21,25) ~= 0)
            % pause
        % end
    end
end
R = A ;


function [v1, v2] = lambertBattinVector(r1, r2, dt, numRevs, gmu)
%lambertBattin Summary of this function goes here
%   Detailed explanation goes here
    if(size(r1,1) ~= 3)
        error('r1 not of length 3');
    end

    if(size(r2,1) ~= 3)
        error('r2 not of length 3');
    end

    r1Mag = sqrt(sum(abs(r1).^2,1));
    r2Mag = sqrt(sum(abs(r2).^2,1));

    tm = zeros(size(dt));
    if(any(dt(dt<0)))
        B = dt<0;
        tm(B) = -1.0;
    end
    if(any(dt(dt>0)))
        B = dt>0;
        tm(B) = 1.0;
    end
    if(any(dt(dt==0)))
        error('dt = 0; lambert cannot compute');
    end
    dt = abs(dt);

    cosDeltaTA = dot(r1,r2,1)./(r1Mag.*r2Mag);
    sinDeltaTA = tm .* sqrt(1 - cosDeltaTA.^2);
    deltaTA = atan2(sinDeltaTA,cosDeltaTA);
    deltaTA = AngleZero2Pi(deltaTA);

    c = sqrt(r1Mag.^2 + r2Mag.^2 - 2.*r1Mag.*r2Mag.*cosDeltaTA);
    s = (r1Mag + r2Mag + c)./2;
    epsilon = (r2Mag - r1Mag)./r1Mag;

    intemed1 = r2Mag./r1Mag;
    TanSqr2w = (epsilon.^2/4) ./ (sqrt(intemed1) + intemed1.*(2 + sqrt(intemed1)));

    sinSqrDeltaTAOver4 = (sin(deltaTA/4)).^2;
    cosSqrDeltaTAOver4 = (cos(deltaTA/4)).^2;
    rop = sqrt(r1Mag.*r2Mag) .* (cosSqrDeltaTAOver4 + TanSqr2w);

    l = zeros(size(tm));
    if(any(tm(tm==1.0)))
        bool = tm==1.0;
        l(bool) = (sinSqrDeltaTAOver4(bool) + TanSqr2w(bool))./(sinSqrDeltaTAOver4(bool) + TanSqr2w(bool) + cos(deltaTA(bool)/2));
    end
    if(any(tm(tm==-1.0)))
        bool = tm==-1.0;
        l(bool) = (cosSqrDeltaTAOver4(bool) + TanSqr2w(bool) - cos(deltaTA(bool)/2)) ./ (cosSqrDeltaTAOver4(bool) + TanSqr2w(bool));
    end

    m = (gmu .* dt.^2)./(8*rop.^3);

    x = l;
    x_change = 1;
    loops=0;
    while(any(x_change > 1E-6) && loops <= 30)
            ksi = computeKsi(x, 20);

            h1 = ((l+x).^2.*(1 + 3.*x + ksi)) ./ ((1+2.*x+l).*(4.*x + ksi.*(3+x)));
            h2 = (m.*(x - l + ksi)) ./ ((1+2.*x+l).*(4.*x + ksi.*(3+x)));

            B=27*h2./(4*(1+h1).^3);
            U=B./(2*(sqrt(1+B)+1));
            K_U=Kay(U);
            y=(1+h1)./3.*(2+sqrt(1+B)./(1+2*U.*K_U.^2));

            x_new = sqrt(((1-l)/2).^2 + m./(y.^2)) - (1+l)/2;
            x_change=abs(x-x_new);
            x = x_new;
            loops=loops+1;
    end
    a = (gmu .* dt.^2)./(16*(rop.^2).*x.*(y.^2));

    sinBetaEOver2 = zeros(size(a));
    betaE = zeros(size(a));
    amin = zeros(size(a));
    tmin = zeros(size(a));
    alphaE = zeros(size(a));
    deltaE = zeros(size(a));
    alphaH = zeros(size(a));
    betaH = zeros(size(a));
    deltaH = zeros(size(a));
    f = zeros(size(a));
    g = zeros(size(a));
    g_dot = zeros(size(a));
    if(any(a(a > 0.0)))
        bool = a > 0.0;

        sinBetaEOver2(bool) = sqrt((s(bool)-c(bool))./(2*a(bool)));
        betaE(bool) = real(2*asin(sinBetaEOver2(bool))); %real needed to prevent complex numbers from appearing

        if(any(deltaTA(bool) > pi))
            bool2 = bool & deltaTA > pi;
            betaE(bool2) = -betaE(bool2);
        end

        amin(bool) = s(bool)./2;
        tmin(bool) = sqrt(amin(bool).^3./gmu(bool)) .* (pi - betaE(bool) + sin(betaE(bool)));

        alphaE(bool) = real(2*asin(sqrt(s(bool)./(2*a(bool))))); %real needed to prevent complex numbers from appearing

        if(any(dt > tmin))
            bool2 = bool & dt > tmin;
            alphaE(bool2) = 2*pi - alphaE(bool2);
        end

        deltaE(bool) = alphaE(bool) - betaE(bool);

        f(bool)     = real(1 - (a(bool)./r1Mag(bool)) .* (1 - cos(deltaE(bool)))); %real() necessary to prevent complex doubles from forming
        g(bool)     = real(dt(bool) - sqrt(a(bool).^3./gmu(bool)) .* (deltaE(bool) - sin(deltaE(bool)))); %real() necessary to prevent complex doubles from forming
        g_dot(bool) = real(1 - (a(bool)./r2Mag(bool)) .* (1- cos(deltaE(bool))));  %real() necessary to prevent complex doubles from forming
    end
    if(any(a(a < 0.0)))
        bool = a < 0.0;

        alphaH(bool) = 2*asinh(sqrt(s(bool)./(-2*a(bool))));
        betaH(bool)  = 2*asinh(sqrt((s(bool)-c(bool))./(-2*a(bool))));
        deltaH(bool) = alphaH(bool)-betaH(bool);

        f(bool)     = real(1 - (a(bool)./r1Mag(bool)) .* (1 - cosh(deltaH(bool)))); %real() necessary to prevent complex doubles from forming
        g(bool)     = real(dt(bool) - sqrt(-(a(bool).^3)./gmu(bool)) .* (sinh(deltaH(bool)) - deltaH(bool))); %real() necessary to prevent complex doubles from forming
        g_dot(bool) = real(1 - (a(bool)./r2Mag(bool)) .* (1 - cosh(deltaH(bool)))); %real() necessary to prevent complex doubles from forming
    end
    if(any(a(a == 0.0)))
        error('a = 0.0');
    end

    v1 = bsxfun(@rdivide,r2 - bsxfun(@times,r1,f),g);
    v2 = bsxfun(@rdivide,bsxfun(@times,r2,g_dot) - r1,g);
end

function ksi = computeKsi(x, numLevels)
    eta = x./(sqrt(1+x) + 1).^2;
    num = 8.*(sqrt(1+x) + 1);
    denom = 1;

    if(numLevels>0)
        denom = 3 + 1./(eta + computeKsi(eta, numLevels-1));
    end
    ksi = num ./ denom;
%     disp(ksi);
end

function [K_U]=Kay(U)
    % setup the C variable
    c(1)=4/27;   %@n=0
    c(2)=8/27; %@n=1
    c(3)=208/891; %n=2
    c(4)=340/1287; %@n=3
    c(5)=700/2907;
    c(6)=928/3591;
    c(7)=296/1215;
    c(8)=1804/7047;
    c(9)=2548/10395;
    c(10)=2968/11655;
    c(11)=3904/15867;
    c(12)=884/3483;
    c(13)=5548/22491;
    c(14)=6160/24327;
    c(15)=7480/30267;
    c(16)=8188/32391;
    c(17)=1940/7839;
    c(18)=10504/41607;
    c(19)=12208/49275;
    c(20)=13108/51975;

    % sum up all the intermediate variables
    Z=1+c(20).*U;
    Z=1+c(19).*U./Z;
    Z=1+c(18).*U./Z;
    Z=1+c(17).*U./Z;
    Z=1+c(16).*U./Z;
    Z=1+c(15).*U./Z;
    Z=1+c(14).*U./Z;
    Z=1+c(13).*U./Z;
    Z=1+c(12).*U./Z;
    Z=1+c(11).*U./Z;
    Z=1+c(10).*U./Z;
    Z=1+c(9).*U./Z;
    Z=1+c(8).*U./Z;
    Z=1+c(7).*U./Z;
    Z=1+c(6).*U./Z;
    Z=1+c(5).*U./Z;
    Z=1+c(4).*U./Z;
    Z=1+c(3).*U./Z;
    Z=1+c(2).*U./Z;
    Z=1+c(1).*U./Z;

    K_U=1./(3*Z);
end


function b = dsisl ( a, lda, n, kpvt, b )

%*****************************************************************************80
%
%% DSISL solves a real symmetric system factored by DSIFA.
%
%  Discussion:
%
%    To compute inverse(A) * C where C is a matrix with P columns
%
%      call dsifa ( a, lda, n, kpvt, info )
%
%      if ( info == 0 ) then
%        do j = 1, p
%          call dsisl ( a, lda, n, kpvt, c(1,j) )
%        end do
%      end if
%
%    A division by zero may occur if the inverse is requested
%    and DSICO has set RCOND == 0.0D+00 or DSIFA has set INFO /= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output from DSIFA.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer KPVT(N), the pivot vector from DSIFA.
%
%    Input, real B(N), the right hand side.
%
%    Output, real B(N), the solution.
%

%
%  Loop backward applying the transformations and D inverse to B.
%
  k = n;

  while ( 0 < k )

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )

        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-1) = daxpy ( k-1, b(k), a(1:k-1,k), 1, b(1:k-1), 1 );

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / a(k,k);
      k = k - 1;

    else
%
%  2 x 2 pivot block.
%
      if ( k ~= 2 )

        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k-1 )
          temp = b(k-1);
          b(k-1) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-2) = daxpy ( k-2, b(k), a(1:k-2,k), 1, b(1:k-2), 1 );
        b(1:k-2) = daxpy ( k-2, b(k-1), a(1:k-2,k-1), 1, b(1:k-2), 1 );

      end
%
%  Apply D inverse.
%
      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = b(k) / a(k-1,k);
      bkm1 = b(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;

  while ( k <= n )

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + b(1:k-1)' * a(1:k-1,k);
        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      k = k + 1;

    else
%
%  2 x 2 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + b(1:k-1) * a(1:k-1,k);
        b(k+1) = b(k+1) + b(1:k-1) * a(1:k-1,k+1);
        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      k = k + 2;

    end

  end

  return
end


function enlarged_tt = add_non_essential_dims(small_tt, new_n, old_vars_pos)
    % enlarged_tt = add_non_essential_dims(small_tt, new_n, old_vars_pos)
    % creates enlarged version of tt_tensor, expanding it by ones.
    % Mode sizes of the enlarged_tt are equal to new_n.
    % First dimension of the small_tt is old_vars_pos(1) dimension of the enlarged_tt.
    % This procedure doesn't increase the maximal TT-rank.
    %
    % Input:
    % 	small_tt     -- P-dimensional tensor in the TT-format.
    % 	new_n        -- Q by 1 vector with enlarged_tt mode sizes (Q > P).
    % 	old_vars_pos -- P by 1 vector in ascending order. Consists of positions
    % 							of the small_tt dimensions in the enlarged_tt tensor.
    %
    % Output:
    % 	enlarged_tt  -- Q-dimensional tensor in the TT format.
    % 			enlarged_tt(i1, ..., iQ)
    % 			i1 take values from {1, ..., new_n(1)}
    % 			...
    % 			iQ take values from {1, ..., new_n(Q)}
    %
    % Example:
    %       m = rand(3, 4);
    %       small_tt = tt_tensor(m); % small_tt(i2, i4)
    %       enlarged_tt = add_non_essential_dims(small_tt, [2, 3, 6, 4, 4], [2, 4]);
    %       % enlarged_tt(i1, i2, i3, i4, i5, i6) == small_tt(i2, i4) for any i1, i3, i5, i6
    %

    if (~issorted(old_vars_pos))
        error('Dimensions (third argument) must be in ascending order.');
    end

    if ~isvector(new_n) | ~isvector(old_vars_pos) | small_tt.d ~= length(old_vars_pos)
    	error('Wrong usage, see help tt_tensor.add_non_essential_dims.');
    end

    d = length(new_n);
    enlarged_tt = tt_tensor;
    enlarged_tt.d = d;
    enlarged_tt.r = zeros(d + 1, 1);
    enlarged_tt.r(1) = 1;
    enlarged_tt.n = new_n(:);
    enlarged_tt.core = [];
    enlarged_tt.ps = zeros(d + 1, 1);
    enlarged_tt.over = 0;
    curr_var_i = 0;
    vars_length = length(old_vars_pos);
    before_first_var = true;
    after_last_var = false;
    for dimension_i = 1:d
        if curr_var_i == vars_length
            after_last_var = true;
        end

        if curr_var_i < vars_length && old_vars_pos(curr_var_i + 1) == dimension_i
            curr_var_i = curr_var_i + 1;
            before_first_var = false;
            if new_n(dimension_i) ~= small_tt.n(curr_var_i)
                error('Dimensions must agree!');
            end
        end

        if before_first_var || after_last_var
            % Dimension_i is before old_vars_pos(1) or after old_vars_pos(end).
            curr_core = ones(new_n(dimension_i), 1);
            curr_rank = 1;
        elseif old_vars_pos(curr_var_i) == dimension_i
            % It's real dimension.
            curr_core = core(small_tt, curr_var_i);
            curr_rank = small_tt.r(curr_var_i);
        else
            % Non-essential dimension between two real ones.
            curr_rank = small_tt.r(curr_var_i + 1);
            curr_core = core_eye(curr_rank, new_n(dimension_i));
        end
        enlarged_tt.r(dimension_i) = curr_rank;
        enlarged_tt.ps(dimension_i) = length(enlarged_tt.core) + 1;
        enlarged_tt.core = [enlarged_tt.core; curr_core(:)];
    end
    enlarged_tt.r(end) = 1;
    enlarged_tt.ps(end) = length(enlarged_tt.core) + 1;
end

function core = core_eye(r, n)
    % Return 3-dimensional array filled with eye matrices,
    % which corresponds to non-essential dimension core.
    core = zeros(r, n, r);
    for i = 1:n
        core(:, i, :) = eye(r);
    end
end


function [I,J] = stam_order(F,A)
  % STAM_ORDER For a regular patch of 13 faces and 12 vertices of a mesh (V,F)
  % around a "center" facet f determined by all its corners having valence 6,
  % determine the reordering of vertices, so that the reordered mesh
  % (V(I,:),J(F)) will fit Jos Stam's ordering in "Evaluation of Loop
  % Subdivision Surfaces". The facet F(f,:) is mapped to [4 7 8] in Stam's
  % patch.
  %
  % [I,J] = stam_order(F)
  % [I,J] = stam_order(F,A)
  %
  % Input:
  %   F  13 by 3 list of triangle indices into some V
  %   A  max(F) by max(F) adjacency matrix {[]}
  % Output:
  %   I  12 long list of indices such that V(I,:) are the reorderd vertices.
  %   J  12 long list of indices such that J(F) are the reordered faces,
  %     {J=full(sparse(I,1,1:12))}
  %
  % Example:
  %   % Canonical regular patch
  %   WV = [-1 0;0 -1;-1 1;0 0;1 -1;-1 2;0 1;1 0;2 -1;0 2;1 1;2 0];
  %   F = [ ...
  %     1 3 4;4 2 1;4 5 2; ...
  %     6 7 3;7 4 3;4 7 8;8 5 4;8 9 5; ...
  %     6 10 7;10 11 7;7 11 8;11 12 8;12 9 8];
  %   % Scramble patch vertex order and face order
  %   R = randperm(size(WV,1));
  %   G = R(F(randperm(end),:));
  %   U = full(sparse([R R],repmat([1 2],size(WV,1),1),WV));
  %   % Uncover order
  %   [I,J] = stam_order(G);
  %   % plot before and after
  %   subplot(1,2,1);
  %   tsurf(G,U,'VertexIndices',1);
  %   set(gca,'YDir','reverse');
  %   subplot(1,2,2);
  %   tsurf(J(G),U(I,:),'VertexIndices',1);
  %   set(gca,'YDir','reverse');
  %

  % There should be 13 face in a regular patch
  assert(size(F,1)==13,'F should have 13 faces to be a regular patch');

  if nargin<2 || isempty(A)
    A = adjacency_matrix(F);
  end
  assert(nnz(any(A))  == 12,'F should reference 12 vertices');
  % valences
  C6 = sum(A,2) == 6;
  % Locate the center face:
  f = find(all(C6(F),2),1);
  if isempty(f)
    error('F is not a regular patch');
  end
  % Stams 4,7,8 vertices
  I = zeros(12,1);
  I(4) = F(f,1);
  I(7) = F(f,2);
  I(8) = F(f,3);
  A(:,F(f,:)) = 0;
  I(5)  = find(A(I(4),:) & A(I(8),:));
  I(3)  = find(A(I(7),:) & A(I(4),:));
  I(11) = find(A(I(8),:) & A(I(7),:));
  A(:,I(I~=0)) = 0;
  I(12) = find(A(I(11),:) & A(I(8),:));
  I( 9) = find(A(I(12),:) & A(I(8),:));
  I( 2) = find(A(I( 5),:) & A(I(4),:));
  I( 1) = find(A(I( 2),:) & A(I(4),:));
  I( 6) = find(A(I( 3),:) & A(I(7),:));
  I(10) = find(A(I( 6),:) & A(I(7),:));
  J = full(sparse(I,1,1:12));
end


function G = gsp_jtv_graph(G,T,fs,param)
% GSP_JTV_GRAPH Add time information to the graph structure
%   Usage:  G = gsp_jtv_graph(G);
%           G = gsp_jtv_graph(G,T);
%           G = gsp_jtv_graph(G,T,fs);
%           G = gsp_jtv_graph(G,T,fs,param);
%
%   Input parameters:
%         G          : Graph structure
%         T          : Time length
%         fs         : Sampling frequency (default 1)
%         param      : Structure of optional parameters
%
%   Output parameters:
%         G          : Time-Vertex Graph structure
%
%   This function adds the time domain information to the structure G.
%   The fields stored inside G.jtv are:
%   - T         : Time length
%   - fs        : Sampling frequency
%   - NFFT      : Number of frequency point (default [])
%   - Transform : Time Fourier basis: 'dft' or 'dct'
%   - Extension : Signal will be zero padded in time to take into account negative lag
%   - Lag       : Lag axis
%   - omega     : Frequency axis
%   - DiffT     : Time Gradient
%   - LT        : Time Laplacian
%
%   Additional parameters
%   ---------------------
%
%   * *param.transform*  : Fourier basis: 'dft' or 'dct'. (default 'dft')
%   * *param.approx*     : Stencil approx for the gradient: 'forward' or 'backward'. (default 'forward')
%   * *param.extension*  : Signal will be zero padded in time when needed. (default '0')
%   * *param.NFFT*       : Number of frequency point. (default [])
%

% Author :  Francesco Grassi
% Date   : September 2016

if nargin<4
    param = struct;
end

if ~isstruct(G)
    error('G is not a valid graph');
end

if nargin<2 || ~isnumeric(T) || T<1
    error('Time length T must be a numeric value greater than 0');
end

if nargin<3 || isempty(fs)
    fs=1;
end

if ~isnumeric(fs) || fs<0
    error('Sampling frequency fs must be a numeric value greater than 0');
end

if ~isfield(param,'NFFT'),      param.NFFT = []; end
if ~isfield(param,'transform'), param.transform = 'dft'; end
if ~isfield(param,'approx'),    param.approx = 'forward'; end
if ~isfield(param,'extension'), param.extension = 0; end

%% JTV PARAMETERS
G.jtv = struct;
G.jtv.T = T;
G.jtv.fs = fs;
G.jtv.NFFT = param.NFFT;
G.jtv.transform = param.transform;
G.jtv.extension = param.extension;
G.jtv.omega = gsp_jtv_fa(G);

if G.jtv.extension
    G.jtv.lag = 2*G.jtv.T-1;
else
    G.jtv.lag = T;
end

%% DIFFERENTIAL OPERATORS
z = ones(T,1);

% G.jtv.LT = spdiags([-z 2*z -z], [-1 0 1], T, T);

G.jtv.DiffT = spdiags([-z z], [0 1], T, T);

switch param.transform
    case 'dft'
        G.jtv.DiffT(T,1) = 1;
    case 'dct'
        G.jtv.DiffT(T,T-1) = 1;
    otherwise
        error('Unknown transform');
end

G.jtv.LT = G.jtv.DiffT'*G.jtv.DiffT;

if strcmpi(param.approx,'backward')
    G.jtv.DiffT = -G.jtv.DiffT.';
end

% toeplitz([-1 zeros(T-2,1) 1],[-1 1 zeros(T-2,1)]); %periodic forward
% toeplitz([1 -1 zeros(T-2,1)],[1 zeros(T-2,1) -1]); %periodic backward (= -transpose(periodic forward))

end


  function [sino, hn, nn, Hk] = fbp2_sino_hilbert(sino, varargin)
%|function [sino, hn, nn, Hk] = fbp2_sino_hilbert(sino, [options])
%|
%| Apply band-limited Hilbert-transform filter to 2D sinogram.
%| Frequency response: H(u) = -1i * sign(u) * rect(u/2/umax)
%|
%| in
%|	sino	[nb (L)] sinogram(s)
%|
%| options
%|	dr | ds	(real)	sample spacing (in distance units, e.g., cm) (default 1)
%|	npad		# of padded samples. (default: 0, means next power of 2)
%|	decon1		deconvolve effect of linear interpolator? (default: 0)
%|	window	[npad]	samples of apodization window function
%|			for [-np/2,...,np/2-1]. (default: '' = plain ramp)
%|			or a string like 'hann' for some predefined windows
%| out
%|	sino	[nb (L)] filtered sinogram rows
%|	hn	[npad]	samples of band-limited Hilbert transform filter
%|	nn	[npad]	[-np/2,...,np/2-1] vector for convenience
%|	Hk	[npad]	spectral samples on [0 ... np-1]
%|
%| Copyright 2011-07-16, Jeff Fessler, University of Michigan

if nargin == 1 && streq(sino, 'test'), fbp2_sino_hilbert_test, clear, return, end
if nargin < 1, ir_usage, end

arg.dr = 1;
arg.window = '';
arg.npad = 0;
arg.decon1 = false;
arg = vararg_pair(arg, varargin, 'subs', {'ds', 'dr'});

dims = size(sino);
sino = reshape(sino, dims(1), []);
[sino hn nn Hk] = fbp2_sino_hilbert_do(sino, ...
	arg.dr, arg.window, arg.npad, arg.decon1);
sino = reshape(sino, [size(sino, 1) dims(2:end)]);

% fbp2_sino_hilbert_do()
function [sino, hn, nn, Hk] ...
	= fbp2_sino_hilbert_do(sino, dr, window, npad, decon1);

[nb na] = size(sino);
if ~npad
	npad = 2^ceil(log2(2*nb-1)); % padded size
end
sino = [sino; zeros(npad-nb,na)]; % padded sinogram

[hn nn] = fbp2_filter_hilbert_make(npad, dr);

Hk = 1i * imag_check(fft(fftshift(hn))); % trick: pure imaginary!

Hk = Hk .* fbp2_window(npad, window);

Hk = dr * Hk; % differential for discrete-space convolution vs integral

% linear interpolation is like blur with a triangular response,
% so we can compensate for this approximately in frequency domain
if decon1
	Hk = Hk ./ fftshift(nufft_sinc(nn / npad).^2);
end

sino = ifft_sym( fft(sino, [], 1) .* repmat(Hk, [1 na]), [], 1); % apply filter

% fbp2_filter_hilbert_make()
function [hn, nn] = fbp2_filter_hilbert_make(n, dr)
nn = [-(n/2):(n/2-1)]';
u0 = 1/2/dr;
hn = 2 * u0 * nufft_sinc(nn/2) .* sin(pi/2 * nn);

% imag_check()
% take imaginary part but check that real part is negligible
function out = imag_check(in, varargin)
out = reale(-1i * in, varargin{:});

% fbp2_sino_hilbert_test()
function fbp2_sino_hilbert_test
nb = 2^5;
dr = 0.1;
[sino1 h1 nn H1] = fbp2_sino_hilbert(zeros(nb,2), 'dr', dr);
h2 = 1 ./ (pi * nn * dr); % samples of ideal non-bandlimited

if im
	clf, subplot(121)
	plot(nn, h1, '.-', nn, h2, '-')
	xlabel 'n', ylabel 'h[n]'
	legend('band-limited', 'ideal')

	subplot(122)
	plot(nn, imag_check(fftshift(H1)), '.-', nn, -sign(nn), '-')
	xlabel 'k', ylabel 'imag(H[k])'
	legend('band-limited', 'ideal', 'location', 'north')
end


function fom = figuremerit(ideal,detected)

    Ni = numel(find(ideal == 1));
    Nd = numel(find(detected == 1));

    [h w c] = size(ideal);

    alpha = 1/9;
    sum = 0.0;
    for r = 1:h
        for c = 1:w
            if (detected(r,c) == 1)
                dist = distance(ideal,detected,r,c)^2;
                sum = sum + (1/(1+alpha*dist));
            end
        end
    end

    fom = (1/max(Ni,Nd))*sum;

function d = distance(ideal,detected,rind,cind)

    [h w c] = size(ideal);

    diff  = 1;
    found = 0;
    d = 10.0^100;

    if (ideal(rind,cind) == detected(rind,cind))
        d     = 0.0;
        found = 1;
    end

    left   = cind - diff;
    right  = cind + diff;
    top    = rind - diff;
    bottom = rind + diff;
    while (~found)

        for c = left:right
            if ((top >= 1) && (c >= 1) && (c <= w))
                if (ideal(top,c) == detected(rind,cind))
                    tmp = sqrt((top-rind)^2 + (c-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for c = left:right
            if ((bottom <= h) && (c >= 1) && (c <= w))
                if (ideal(bottom,c) == detected(rind,cind))
                    tmp = sqrt((bottom-rind)^2 + (c-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for r = top+1:bottom-1
            if ((left >= 1) && (r >= 1) && (r <= h))
                if (ideal(r,left) == detected(rind,cind))
                    tmp = sqrt((r-rind)^2 + (left-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for r = top+1:bottom-1
            if ((right <= w) && (r >= 1) && (r <= h))
                if (ideal(r,right) == detected(rind,cind))
                    tmp = sqrt((r-rind)^2 + (right-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        diff = diff + 1;
        left   = cind - diff;
        right  = cind + diff;
        top    = rind - diff;
        bottom = rind + diff;
        if ((left < 1) && (right > w) && (top < 1) && (bottom > h))
            found = 1;
        end
    end


function [W_RF,W_BB]=OMP_Combining(F_RF,F_BB)
global  Nr Ns Nrf H Vn Codebook_w
W_RF = [];
W_MMSE = ((F_BB' * F_RF' * H' * H * F_RF * F_BB + Vn * Ns * eye(Ns))^(-1) * F_BB' * F_RF' * H')';
Wres = W_MMSE;
n = 1 / Ns * H * F_RF * F_BB * F_BB' * F_RF' *H' + Vn * eye(Nr);
for i = 1 : Nrf
    y = Codebook_w' * n * Wres;
    k = find(diag(y * y')==max(diag(y * y')));
    W_RF(:,i) = Codebook_w(:,k);
    W_BB = (W_RF' * n * W_RF)^(-1) * W_RF' * n * W_MMSE;
    Wres = (W_MMSE - W_RF * W_BB) / norm(W_MMSE - W_RF * W_BB,'fro');
end


function [H] = scale(f)

% SCALE returns the homogenous coordinate transformation matrix
% corresponding to a scaling along the x, y and z-axis
%
% Use as
%   [H] = translate(S)
% where
%   S       [sx, sy, sz] scaling along each of the axes
%   H   corresponding homogenous transformation matrix

% Copyright (C) 2000-2005, Robert Oostenveld
%
% 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 should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if numel(f)~=3
  ft_error('incorrect input vector');
end

H = [
  f(1) 0    0    0
  0    f(2) 0    0
  0    0    f(3) 0
  0    0    0    1
  ];


function [h, compUpVV, compUpVP, compUp] =  lfmComputeH3VV(gamma1_p, gamma1_m, sigma2, t1, ...
    t2, preFactor, mode)

% LFMCOMPUTEH3VV Helper function for computing part of the LFMVXLFMV kernel.
% FORMAT
% DESC computes a portion of the LFMVXLFMV kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG preFactor : precomputed constants.
% ARG mode: indicates the correct precomputations.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% Evaluation of h

if nargout>1
    [compUpVV{1}, compUpVP{1}, compUp{1}] = lfmvvComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode);
    [compUpVV{2}, compUpVP{2}, compUp{2}] = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
    h = preFactor(1)*compUpVV{1} + preFactor(2)*compUpVV{2};
else
    h = preFactor(1)*lfmvvComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode) ...
        + preFactor(2)*lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);

%     %h = preFactor(2)*lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%
%      h = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%
%      epsilon = 1e-6;
%      valg = gamma1_m;
%      gamma1_m = valg + epsilon;
%      h1 = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%      gamma1_m = valg - epsilon;
%      h2 = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%      gamma1_m = valg;
%      numerics = 0.5*(h1-h2)/epsilon;
%      theo = lfmvvGradientUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);

end


function coef = sandia_sgmgg_coef_naive ( dim_num, point_num, sparse_index )

%*****************************************************************************80
%
%% SANDIA_SGMGG_COEF_NAIVE returns the combinatorial coefficients.
%
%  Discussion:
%
%    The coefficient of point I is calculated as follows:
%
%    *) point J is a "neighbor" of point I if every entry of the sparse
%       index for point J is either equal to, or 1 greater than, the
%       corresponding entry of the sparse index of point I.
%
%    *) If point J is a neighbor of point I, then it contributes
%       (-1)^D to the coefficient, where D is the sum of the differences
%       between the sparse indices of point I and point J.
%
%    This is a completely naive implementation of the calculation,
%    intended simply as a demonstration for small examples.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the vector.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer SPARSE_INDEX(DIM_NUM,POINT_NUM),
%    the indices that define the points.
%
%    Output, integer COEF(POINT_NUM), the coefficients.
%
  coef = zeros ( point_num, 1 );

  for j1 = 1 : point_num

    for j2 = 1 : point_num

      neighbor = 1;
      term = + 1;

      for i = 1 : dim_num

        dif = sparse_index(i,j2) - sparse_index(i,j1);

        if ( dif == 0 )

        elseif ( dif == 1 )
          term = - term;
        else
          neighbor = 0;
          break;
        end

      end

      if ( neighbor )
        coef(j1) = coef(j1) + term;
      end

    end

  end

  return
end


function [ R t s ]=computeOrientation(x,xp,method,varargin)
% Compute Absolute or Exterior Orientation (Pose Estimation)
%
% Absolute is : given x and xp (2 arrays of 3D coordinates), find the best
% transformation such that x=s*R*xp+t
%
% Exterior is : 1 array of 3D position x is known and its 2D orthographic
% projection xp.
% Find the best transformation such that xp=projection*(s*R*x+t)
% (same as Pose Estimation, ePNP). Projection is ortho for now
%
% The routines below are only for the orthographic case for now
%
% USAGE
%  [R,t,s]=computeOrientation(x,xp,method)
%
% INPUTS
%  x,xp       - 3xN or 2xN array of points or right/left.
%               For 'exteriorSequence', it can be of size
%               2 x nPoint x nFrame and 3 x nPoint x nFrame
%  method     - 'absolute', 'absoluteHard' (try more if Horn's fails)
%               or 'exterior' and 'exteriorSequence'
%  varargin   - list of paramaters in quotes alternating with their values
%       - 'RIni' initial rotation from which the optimization will be
%       started (only for 'exterior' and 'exteriorSequence')
%
% OUTPUTS
%  R       - rotation matrix
%  t       - translation vector
%  s       - scale factor (if not requested, s=1)
%
% EXAMPLE
%
% See also
%
% Vincent's Structure From Motion Toolbox      Version 3.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

nPoint=size(x,2);

switch method
  case { 'absolute', 'absoluteHard' }
    % Computes the absolute orientation between 2 sets of 3D points
    % Xright=s*R*Xleft + t   . We want to recover s,R,T
    % Ref: B.K.P. Horn, H.M. Hilden, and S. Negahdaripour, Closed-Form
    % Solution of Absolute Orientation Using Orthonormal Matrices
    rr=normalizePoint(x,4); rl=normalizePoint(xp,4);
    rrBar=mean(rr,2); rlBar=mean(rl,2);
    rrp=rr-rrBar(:,ones(1,nPoint)); rlp=rl-rlBar(:,ones(1,nPoint));

    M=zeros(3); for i=1:nPoint; M=M+rrp(:,i)*rlp(:,i)'; end
    [V,D]=eig(M'*M);
    V=real(V);

    R=1/sqrt(D(2,2))*V(:,2)*V(:,2)' + 1/sqrt(D(3,3))*V(:,3)*V(:,3)';
    temp=V(:,1)*V(:,1)';
    if D(1,1)>0
      R=R+1/sqrt(D(1,1))*temp;
    else
      if det(R+temp)>0; R=R+temp; else R=R-temp; end
    end
    R=M*R;

    R=rotationMatrix(R);

    if det(R)<0 && strcmp(method,'absoluteHard') % if Horn's method fails
      warning('Horn''s method failed. Trying GloptiPoly3 solution');
      RHorn=R;
      try
        % Sedumi and GloptiPoly3 must be installed and in the path !
        mpol R 3 3;

        g0=rlp-R*rrp;
        g0=g0(1,:)*g0(1,:)'+g0(2,:)*g0(2,:)'+g0(3,:)*g0(3,:)';

        % define the rotation constraints
        K = [ R(1,:)*R(1,:)' == 1, R(2,:)*R(2,:)'==1, R(3,:)*R(3,:)'==1,...
          R(1,:)*R(2,:)' == 0, R(1,:)*R(3,:)' == 0, R(2,:)*R(3,:)' == 0,...
          det(R)>=0 ];

        % define the problem and solve it
        P=msdp(min(g0),K);
        [ status obj ] = msol(P);

        R=rotationMatrix( double(R) );
      catch %#ok<CTCH>
        R = RHorn;
      end
    end

    % Figure out s and t
    if nargout==2
      s=1;
    else
      s=norm(rrp,'fro')/norm(rlp,'fro');
      if norm( rrp+s*R*rlp, 'fro' ) < norm( rrp-s*R*rlp, 'fro' ); s=-s; end
    end

    t=rrBar-s*R*rlBar;
  case 'exterior'
    if ~isempty(varargin); RIni = varargin{0};
    else RIni=[];
    end

    if isempty(RIni)
      try
        % Sedumi and GloptiPoly3 must be installed and in the path !
        mpol R 2 3;
        if nargout<2; t=0; else mpol t 2 1; end
        if nargout<3; s=1; else mpol s; end

        g0=0;
        for i=1:nPoint
          tmp=xp(:,i) - s*(R*x(:,i)+t); %#ok<NODEF>
          g0 = g0 + tmp'*tmp;
        end

        % define the rotation constraints
        K = [ R(1,:)*R(1,:)' == 1, R(2,:)*R(2,:)'==1, R(1,:)*R(2,:)' == 0];

        % define the problem and solve it
        mset('verbose',false);
        P=msdp(min(g0),K);

        [ status obj ] = msol(P);

        R=rotationMatrix( double(R) );

        if nargout>=2; t=[ double(t); 0]; end
        if nargout==3; s=double(s); end
      catch %#ok<CTCH>
        warning(['GloptiPoly3 not installed or Gloptypoly crashed,' ...
          'using ePnP']);
        try
          mset clear;
        catch %#ok<CTCH>
          [ R t ] = efficient_pnp( x', xp', eye(3,3) );
          t=t(1:2);
        end
      end
      if any(isnan(R))
        warning(['GloptiPoly failed, using ePnP']);
        [ R t ] = efficient_pnp( x', xp', eye(3,3) );
        t=t(1:2);
      end
    else
      R=RIni;
    end

    % perform gradient descent to optimize the rotation
    Q = refineExteriorOrientation(x,xp,quaternion(R));
    R=quaternion( Q );
  case 'exteriorSequence'
    RIni=getPrmDflt( varargin, {'RIni' [] }, 1 );
    if length(RIni)>1
      Q=quaternion(RIni);
    else
      Q=2*rand(4,size(xp,3))-1;
    end
    [ Q err ]=refineExteriorOrientation(x,xp,Q);

    for i = 1 : 10
      [ QNew errNew ]=refineExteriorOrientation(x,xp,2*rand(4,size(xp,3))-1);
      temp = errNew<err;
      Q(:,temp) = QNew(:,temp);
    end

    R=quaternion( Q );
end
end


function out = DN_Moments(y,theMom)
% DN_Moments    A moment of the distribution of the input time series.
%
% Normalizes by the standard deviation
% Uses the moment function from Matlab's Statistics Toolbox
%
%---INPUTS:
% y, the input data vector
% theMom, the moment to calculate (a scalar)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

out = moment(y,theMom) / std(y); % normalized

end


function [cx, cy, w, h] = get_axis_aligned_BB(region)
% GETAXISALIGNEDBB extracts an axis aligned bbox from the ground truth REGION with same area as the rotated one
nv = numel(region);
assert(nv==8 || nv==4);

if nv==8
    cx = mean(region(1:2:end));
    cy = mean(region(2:2:end));
    x1 = min(region(1:2:end));
    x2 = max(region(1:2:end));
    y1 = min(region(2:2:end));
    y2 = max(region(2:2:end));
    A1 = norm(region(1:2) - region(3:4)) * norm(region(3:4) - region(5:6));
    A2 = (x2 - x1) * (y2 - y1);
    s = sqrt(A1/A2);
    w = s * (x2 - x1) + 1;
    h = s * (y2 - y1) + 1;
else
    x = region(1);
    y = region(2);
    w = region(3);
    h = region(4);
    cx = x+w/2;
    cy = y+h/2;
end


% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% Spontaneous imbibition in a core, IMPES, works fine
% not as fast as expected, can be improved
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
%% define the geometry
Nx = 20; % number of cells in x direction
Ny = 50; % number of cells in y direction
W = 0.02; % [m] length of the domain in x direction
H = 0.07; % [m] length of the domain in y direction
% m = createMesh1D(Nx, W);
m = createMeshCylindrical2D(Nx, Ny, W, H); % creates a 2D mesh
%% define the physical parametrs
krw0_ww = 0.3;
krw0_ow = 1.0;
kro0_ww = 0.6;
kro0_ow = 0.76;
nw = 2.4;
no = 2.0;
sor_ww=0.1;
sor_ow=0.12;
swc_ww=0.09;
swc_ow=0.09;
SF=createFaceVariable(m, 0.0); % 1 is water wet, 0 is oil wet
krw0=krw0_ww*SF+krw0_ow*(1-SF);
kro0=kro0_ww*SF+kro0_ow*(1-SF);
sor=sor_ww*SF+sor_ow*(1-SF);
swc=swc_ww*SF+swc_ow*(1-SF);
sws=@(sw, sor, swc)((sw>swc).*(sw<1-sor).*(sw-swc)./(1-sor-swc)+(sw>=1-sor));
kro=@(sw, kro0, sor, swc)((sw>=swc).*kro0.*(1-sws(sw, sor, swc)).^no+(sw<swc).*(1+(kro0-1)./swc.*sw));
krw=@(sw, krw0, sor, swc)((sw<=1-sor).*krw0.*sws(sw, sor, swc).^nw+(sw>1-sor).*(-(1-krw0)./sor.*(1.0-sw)+1.0));
dkrwdsw=@(sw, krw0, sor, swc)((sw<=1-sor).*nw.*krw0.*(1./(1-sor-swc)).*sws(sw, sor, swc).^(nw-1)+(sw>1-sor).*((1-krw0)./sor));
dkrodsw=@(sw, kro0, sor, swc)((sw>=swc).*(-kro0.*no.*(1-sws(sw, sor, swc)).^(no-1))./(-swc-sor+1)+(sw<swc).*((kro0-1)./swc));
p0 = 100e5; % [bar] pressure
pin = 150e5; % [bar] injection pressure at the left boundary
u_in= 1.0/(24*3600); % [m/s] equal to 1 m/day
sw0 = swc_ww+0.01;
% sw0(10:end-10, 10:end-10)=swc+0.2;
% sw0 = swc+0.1; % initial water saturation
sw_in = 1;
mu_oil = 2e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 0.005e-12; % [m^2] average reservoir permeability
phi0 = 0.45; % average porosity
teta_ow=deg2rad(30);
gama_ow=0.03; % N/m
labda=2.4;
eps1=1e-6;
clx=0.05;
cly=0.05;
V_dp=0.01; % Dykstra-Parsons coef.
perm_val= k0; %field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
pce=0.1*gama_ow*cos(teta_ow)*(phi./k).^0.5;
pce_face=0.05*gama_ow*cos(teta_ow)*(arithmeticMean(phi)./geometricMean(k)).^0.5;
sco=swc+0.01;
pc=@(sw, sor, swc)(pce.*(sws(sw, sor, swc)+eps).^(-1/labda)); % it can also be defined for each block
dpc=@(sw, sor, swc)((-1/labda)*(1./(1-sor-swc)).*pce_face.*(sws(sw, sor, swc)+eps).^(-1/labda-1));
dpcdk=@(sw, sor, swc)(0.5*gama_ow*cos(teta_ow)*(geometricMean(k)./arithmeticMean(phi)).^0.5.*(sws(sw, sor, swc)+eps).^(-1/labda));
grad_phik=gradientTerm(phi./k);
% sw_plot=linspace(0,1, 10000);
% plot(sw_plot, pc(sw_plot))
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries: all fixed Sw=1, fixed pressure everywhere(?)
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% left boundary pressure gradient
% BCp.left.a(:)=(krw(sw_in)*lw.xvalue(1,:)+kro(sw_in)*lo.xvalue(1,:)); BCp.left.b(:)=0; BCp.left.c(:)=-u_in;
% change the right boandary to constant pressure (Dirichlet)
% BCp.left.a(:)=0; BCp.left.b(:)=1; BCp.left.c(:)=p0;
BCp.right.a(:)=0; BCp.right.b(:)=1; BCp.right.c(:)=p0;
BCp.top.a(:)=0; BCp.top.b(:)=1; BCp.top.c(:)=p0;
BCp.bottom.a(:)=0; BCp.bottom.b(:)=1; BCp.bottom.c(:)=p0;
% change the left boundary to constant saturation (Dirichlet)
% BCs.left.a(:)=0; BCs.left.b(:)=1; BCs.left.c(:)=1.0-sor;
BCs.right.a(:)=0; BCs.right.b(:)=1; BCs.right.c(:)=1.0;
BCs.top.a(:)=0; BCs.top.b(:)=1; BCs.top.c(:)=1.0;
BCs.bottom.a(:)=0; BCs.bottom.b(:)=1; BCs.bottom.c(:)=1.0;
%% define the time step and solver properties
% dt = 1000; % [s] time step
% dt=(W/Nx)/u_in/20; % [s]
dt=1;
t_end = 10*3600*24; % [s] final time
eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-4; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
oil_init=domainInt(1-sw_old);
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)
rec_fact=0;
t_day=0;
t = 0;
dt0=dt;
dsw_alwd= 0.01;
dp_alwd= 100; % Pa
while (t<t_end)
% for i=1:5
    error_p = 1e5;
    error_sw = 1e5;
    % Implicit loop
%     while ((error_p>eps_p) || (error_sw>eps_sw))
    while(1)
        % calculate parameters
        pgrad = gradientTerm(p);
%         pcgrad=gradientTerm(pc(sw));
        sw_face = upwindMean(sw, -pgrad); % average value of water saturation
        sw_grad=gradientTerm(sw);
        sw_ave=arithmeticMean(sw);
        pcgrad=dpc(sw_ave, sor, swc).*sw_grad+dpcdk(sw_ave, sor, swc).*grad_phik;
        % solve for pressure at known Sw
        labdao = lo.*funceval(kro, sw_face, kro0, sor, swc);
        labdaw = lw.*funceval(krw, sw_face, krw0, sor, swc);
        labda = labdao+labdaw;
        % compute [Jacobian] matrices
        Mdiffp1 = diffusionTerm(-labda);
        RHSpc1=divergenceTerm(labdao.*pcgrad);
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        RHS1 = RHSpc1+RHSbcp; % with capillary
        p_new=solvePDE(m, Mdiffp1+Mbcp, RHS1);

        % solve for Sw
        pgrad = gradientTerm(p_new);
        uw=-labdaw.*pgrad;
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        RHS_sw=-divergenceTerm(uw);
        sw_new=solveExplicitPDE(sw_old, dt, RHS_sw, BCs, phi);

        error_p = max(abs((p_new.value(:)-p.value(:))./p_new.value(:)))
        error_sw = max(abs(sw_new.value(:)-sw.value(:)))
        dt_new=dt*min(dp_alwd/error_p, dsw_alwd/error_sw);
        % assign new values of p and sw
        if error_sw>dsw_alwd
            dt=dt*(dsw_alwd/error_sw)
        else
            t=t+dt;
            p = p_new;
            sw = sw_new;
            p_old = p;
            sw_old = sw;
            dt=min(dt*(dsw_alwd/error_sw), 10*dt);
            break;
        end
    end

    rec_fact=[rec_fact (oil_init-domainInt(1-sw))/oil_init];
    t_day=[t_day t];
    figure(1);visualizeCells(1-sw); drawnow;
    figure(2); plot(t_day/3600/24, rec_fact)
    xlabel('time [day]');
    ylabel('recovery factor');
    title([num2str(t/3600/24) ' day']); drawnow;
end


% AUSM (Liou-Steffen) scheme for one-dimensional Euler equations

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory in Section 10.5 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program makes Figs. 10.17, 10.20 in the book

% Function called: f
% Programs called: problem_specification, Riemann

clear all

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

		% .....................Input............................
gamma = 1.4; 	% Ratio of specific heats
J = 48;		% Number of grid cells
bouncon = 0;	% bouncon chooses outflow boundary conditions
		% = 0: Nothing happens: infinite domain
		% = 1: Solid wall at x = 1 with direct prescription of uwall
		% = 2: Solid wall at x = 1 with reflection b.c.
		% ....................End of input........................

gammab = 1/(gamma - 1); gam1 = gamma-1; gamgam = gamma^gamma;
problem_specification

h = 1/J;  				% Cell size
dt = lambda*h;				% Time step
n = floor(tend/dt);			% Number of time-steps

% 		Definition of grid numbering
%       x=0    					 x=1
% grid   |---o---|---o---|---o---  ...  --|---o---|
%            1   1   2   2   3           J-1  J

xcenter = h*[1:J] - h/2;		% Location of cell centers

press = zeros(size(xcenter));		% Preallocation of pressure,
rhoold = press; uold = press;		%       density and velocity
rhonew = press; mnew = press;		%       momentum
totenew = press; enthalpy = press;	%	total energy and enthalpy

for j = 1:length(xcenter)		% Initial conditions
  if xcenter(j) < 0.5, press(j) = pleft; rhoold(j) = rholeft;  uold(j) = uleft;
  else,  	     press(j) = pright;  rhoold(j) = rhoright; uold(j) = uright;
  end
end

	% Initialization of cell center variables
totenold = rhoold.*(0.5*uold.*uold + gammab*press./rhoold); % Total energy rho*E
totenleft = totenold(1); totenright = totenold(J);
mold = rhoold.*uold;					    % Momentum m
c = sqrt(gamma*press./rhoold);				    % Sound speed
mach = uold./c;						    % Mach number
enthalpy =  0.5*uold.*uold + gammab*c.^2;		    % Enthalpy

machplus = mach; machminus = mach;		% Preallocation of
presplus = mach; presminus = mach;		%     	split fluxes
flux1 = zeros(J-1,1); flux2 = flux1;		% 	and Liou-Steffen
flux3 = flux1; machhalf = flux1;		%	fluxes
m1 = flux1; m2 = flux1;

t = 0;
for i = 1:n,  t = t + dt;
  for j = 1:J
    if mach(j) > 1
      machplus(j) = mach(j);  machminus(j) = 0;
      presplus(j) = press(j); presminus(j) = 0;
    elseif mach(j) < -1
      machplus(j) = 0; machminus(j) = mach(j);
      presplus(j) = 0; presminus(j) = press(j);
    else
      machplus(j)  =  0.25*(mach(j) + 1)^2;
      machminus(j) = -0.25*(mach(j) - 1)^2;
      presplus(j)  =  0.5*press(j)*(1 + mach(j));
      presminus(j) =  0.5*press(j)*(1 - mach(j));
    end
  end

	% Liou-Steffen fluxes
  for j = 1:J-1,  machhalf(j) = machplus(j) + machminus(j+1); end
  m1 = 0.5*(machhalf + abs(machhalf));  m2 = 0.5*(machhalf - abs(machhalf));
  rhoc = rhoold.*c;
  for j = 1:J-1
    flux1(j) = m1(j)*rhoc(j) + m2(j)*rhoc(j+1);
    flux2(j) = m1(j)*rhoc(j)*uold(j) +  m2(j)*rhoc(j+1)*uold(j+1) +...
      presplus(j) + presminus(j+1);
    flux3(j) =  m1(j)*rhoc(j)*enthalpy(j)  + m2(j)*rhoc(j+1)*enthalpy(j+1);
  end

	% Update of state variables
  rhonew(1)  = rholeft;  	rhonew(J)  = rhoright;
  mnew(1)    = rholeft*uleft;	mnew(J)    = rhoright*uright;
  totenew(1) = totenleft; 	totenew(J) = totenright;
  for j = 2:J-1
    rhonew(j)  = rhoold(j)   - lambda*(flux1(j) - flux1(j-1));
    mnew(j)    = mold(j)     - lambda*(flux2(j) - flux2(j-1));
    totenew(j) = totenold(j) - lambda*(flux3(j) - flux3(j-1));
  end
  uold   = mnew./rhonew;   press = gam1*(totenew - 0.5*mnew.*uold);
  rhoold = rhonew; 	totenold = totenew; 	mold = mnew;
  c = sqrt(gamma*press./rhoold); 	mach = uold./c;
  enthalpy =  0.5*uold.*uold + gammab*c.^2;
end
entropy = log(press./rhoold.^gamma);

figure(1), clf
subplot(2,3,1),hold on,title('DENSITY','fontsize',14),plot(xcenter,rhonew,'o')
subplot(2,3,2),hold on,title('VELOCITY','fontsize',14),plot(xcenter,uold,'o')
subplot(2,3,3),hold on,title('PRESSURE','fontsize',14),plot(xcenter,press,'o')
subplot(2,3,4),hold on,title('MACHNUMBER','fontsize',14),plot(xcenter,mach,'o')
subplot(2,3,5),hold on,title('ENTROPY','fontsize',14),plot(xcenter,entropy,'o')
subplot(2,3,6),axis('off'),title('Liou-Steffen scheme','fontsize',14)

Riemann		% Plot exact solution


%DEMO_REGRESSION_HIER  Hierarchical regression demonstration using
%                      Rats data
%
%  Description
%    The example data is taken from section 6 of Gelfand et al
%    (1990) (also used in WinBUGS/OpenBUGS), and concerns 30 young
%    rats whose weights were measured weekly for five week. This
%    demo demosntrates how to make hierarchical linear and
%    non-linear models.
%
%  Reference
%    Gelfand, A. E., Hills, S. E., Racine-Poon, A. and Smith, A. F.
%    M. (1990) Illustration of Bayesian Inference in Normal Data
%    Models Using Gibbs Sampling. Journal of the American
%    Statistical Association 85(412):972-985.
%

% Copyright (c) 2010 Aki Vehtari

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

S = which('demo_regression_hier');
L = strrep(S,'demo_regression_hier.m','demodata/rats.mat');
data=load(L);
xx = data.x;
yy = data.y;
% Show data : 5 weight measurements per rat for 30 rats
figure
plot(xx,yy,'o-')
axis([7 37 100 400])
title('Data')
drawnow

% Reshape data
ntime = size(xx,2);
nrats = size(yy,1);
% All y's to one vector
y=yy(:);
% Repeat x for each rat
x=reshape(repmat(xx,nrats,1),ntime*nrats,1);
% Add ratid
x=[x repmat([1:nrats]',ntime,1)];
% Now 'x' consist of the inputs (ratid,time) and 'y' of the output (weight).
% Normalize x and y
[xn,xmean,xstd]=normdata(x);
[yn,ymean,ystd]=normdata(y);

% optmization options
opt=optimset('TolFun',1e-4,'TolX',1e-4,'Display','on');

% common Gaussian likelihood with weakly informative prior for variance
lik=lik_gaussian('sigma2',.1,...
                 'sigma2_prior',prior_sinvchi2('s2',0.01,'nu',1));
% common categorical covariance term
cc=gpcf_cat('selectedVariables',2);

% 1) Linear model with intercept and slope wrt time
disp('1) Linear model with intercept and slope wrt time')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfl});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,1)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model')
drawnow

% 2) Linear model with hierarchical intercept
disp('2) Linear model with hierarchical intercept')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,2)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model with hierarchical intercept')
drawnow

% 3) Linear model with hierarchical intercept and slope
disp('3) Linear model with hierarchical intercept and slope')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% linear covariance term for each rat
cfli=gpcf_prod('cf',{cfl cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfli});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,3)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model with hierarchical intercept and slope')
drawnow

% 4) Nonlinear model with hierarchical intercept
% include linear part, too
disp('4) Nonlinear model with hierarchical intercept')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part
cfs=gpcf_sexp('selectedVariables',1);
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfs});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,4)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear model with hierarchical intercept')
drawnow

% 5) Nonlinear model with hierarchical intercept and curve
% include linear part, too
disp('5) Non-linear hierarchical model 1 with MAP')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% linear covariance term for each rat
cfli=gpcf_prod('cf',{cfl cc});
% nonlinear part
cfs=gpcf_sexp('selectedVariables',1);
% nonlinear covariance term for each rat
cfsi=gpcf_prod('cf',{cfs cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfli cfs cfsi},...
          'jitterSigma2',1e-6);
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,5)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 1 with MAP')
drawnow

% 6) With increasing flexibility of the modeling function
%    we need to integrate over the parameteres
% integrate over parameters
disp('6) Non-linear hierarchical model 1 with IA')
[gps,pth,th]=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,6)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 1 with IA')
drawnow

% 7) Nonlinear model with hierarchical intercept and curve
%    Same as 5, but with no linear and product covariances
disp('7) Non-linear hierarchical model 2 with MAP')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with delta distance for ratid
cfs=gpcf_sexp('metric',metric_euclidean('components',{[1] [2]},...
                                        'deltadist', [0 1], ...
                                        'lengthScale_prior',prior_t()));
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfs});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,7)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 2 with MAP')
drawnow

% 8) With increasing flexibility of the modeling function
%    we need to integrate over the parameteres
% integrate over parameters
disp('8) Non-linear hierarchical model 2 with IA')
gps=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,8)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 2 with IA')
drawnow

% 9) With neuralnetwork covariance and integration over the parameters
disp('9) Non-linear hierarchical model 3 with IA')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with neuralnetwork covariance
cfnn=gpcf_neuralnetwork('selectedVariables',1,'biasSigma2_prior',prior_t(),...
                        'weightSigma2_prior',prior_t());
% nonlinear covariance term for each rat
cfnni=gpcf_prod('cf',{cfnn cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfnn cfnni});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% integrate over parameters
gps=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,9)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 3 with IA')
drawnow

%*** Missing Data Example ***
% In the original paper (Gelfand et al, 1990) data was also to
% demonstrate missing data handling, by removing 1-4 weeks of data
% for part of the rats. Handling missing data in this case is trivial
% for GPs, too

disp('10) Missing data example')
S = which('demo_regression_hier');
L = strrep(S,'demo_regression_hier.m','demodata/rats.mat');
data=load(L);
xx = data.x;
yy = data.y;
yymiss = data.ymiss;
% Show data : 5 weight measurements per rat for 30 rats
figure
plot(xx,yymiss,'o-')
axis([7 37 100 400])
title('Data')
% Reshape data
ntime = size(xx,2);
nrats = size(yy,1);
% All y's to one vector
ym=yymiss(:);
% Repeat x for each rat
xm=reshape(repmat(xx,nrats,1),ntime*nrats,1);
% Add ratid
xm=[xm repmat([1:nrats]',ntime,1)];
% Now 'x' consist of the inputs (ratid,time) and 'y' of the output (weight).
% Normalize x and y
[xmn,xmmean,xmstd]=normdata(xm);
[ymn,ymmean,ymstd]=normdata(ym);
% test x is the complete x
xmnt=xmn;
% remove missing data from the training data
missi=isnan(ym);
ymn(missi,:)=[];
xmn(missi,:)=[];

% 10) neuralnetwork covariance, IA and missing data
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_gaussian());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with neuralnetwork covariance
cfnn=gpcf_neuralnetwork('selectedVariables',1,'biasSigma2_prior',prior_gaussian('s2',10));
% nonlinear covariance term for each rat
cfnni=gpcf_prod('cf',{cfnn cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfnn cfnni});
% optimize
gp=gp_optim(gp,xmn,ymn);
% integrate over parameters
gps=gp_ia(gp,xmn,ymn);
% predict and plot
Ef=gp_pred(gps,xmn,ymn,xmnt);
Eff=reshape(denormdata(Ef,ymmean,ymstd),nrats,ntime);
Effc=Eff;Effc(isnan(ym))=NaN;
plot(xx,Effc,'bo-',xx,Eff,'bo--')
axis([7 37 100 400])
title('Non-linear hierarchical model 3 with IA and missing data')


function perimeterEdges=findPerimeter(mesh)
% function perimeterPoints=findPerimeter(mesh,faceIndexList)
% Finds perimeter points.
% Works by looking at the connection matrix (which is a list of edges)
% The perimeter points are those points that are on edges that are used by only one face

%nVerts=length(mesh.uniqueVertices);

% Create a list of edges...
[edgeList1,edgeList2]=find(triu(mesh.connectionMatrix));

edgeList=[edgeList1,edgeList2];

disp ('Number of edges found:');
numUniqueEdges=length(edgeList);

disp (length(edgeList));

sortedEdgeList=sort(edgeList')';
sortedEdgeList=sub2ind([numUniqueEdges,numUniqueEdges],sortedEdgeList(:,1),sortedEdgeList(:,2));

% Now convert the list of faces into three sets of (sorted!) edges
% And convert the x,y coordinates into unique indices
FaceEdges1=[mesh.uniqueFaceIndexList(:,1),mesh.uniqueFaceIndexList(:,2)];
FaceEdges2=[mesh.uniqueFaceIndexList(:,1),mesh.uniqueFaceIndexList(:,3)];
FaceEdges3=[mesh.uniqueFaceIndexList(:,2),mesh.uniqueFaceIndexList(:,3)];

% Sort them so that [a b] and [b a] are seen as the same edge.
sortedFaceEdges1=sort(FaceEdges1')';
sortedFaceEdges2=sort(FaceEdges2')';
sortedFaceEdges3=sort(FaceEdges3')';

% Make them into unique index numbers to help searching and sorting
[FaceEdges1]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges1(:,1),sortedFaceEdges1(:,2));
[FaceEdges2]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges2(:,1),sortedFaceEdges2(:,2));
[FaceEdges3]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges3(:,1),sortedFaceEdges3(:,2));

% concatenate the list of Face edges into one long list of indices
FaceEdges=[FaceEdges1,FaceEdges2,FaceEdges3]';

% Sort them - should produce doublets of most edges 'cos most edges are part of two faces
sortedFaceEdges=sort(FaceEdges(:));

% SortedFaceEdges is a sorted list of all the edges in the face list. If all the edges are
% members of two faces, it'll just be pairs of numbers.
% Lone edges (part of only one face) will be on their own here as well.
% So the list might look like
% 1,1,2,2,3,3,4,4,5,5,6,6,7,8,9,9,10,10....
% Where edge 7 and edge 8 are lone...
% if n(i)==n(i+1) or n(i)==n(i-1) then n(i) is an internal edge
% So in fact we're looking for cases where the above fails...
diffFromUpper=sortedFaceEdges-shift(sortedFaceEdges,[1,0]);
diffFromLower=sortedFaceEdges-shift(sortedFaceEdges,[-1,0]);
loneEdges=sortedFaceEdges(find(diffFromUpper.*diffFromLower));

[a b c]=unique(sortedFaceEdges(:));
% Just a quick check:
% 'a' tells you what unique edges there are in the faces. This should be the same as edgeList

nFound=length(loneEdges);
if (nFound>0)
   disp(nFound);
   disp ('Perimeter edges found');

% In the test cases, the mesh is closed so there's no perimeter, loneEdges should be empty
% But let's pretend we've found some...
[perimeterEdges(:,1),perimeterEdges(:,2)]=ind2sub([numUniqueEdges,numUniqueEdges],loneEdges);
else
   disp ('No Perimeter edges found - mesh is closed');
   perimeterEdges=[];
end


% NOLM_Bifurcation_Diagram.
% Copyright Springer 2013 A.L. Steele and S. Lynch.
clear
N=49999;halfN=N/2;
lambda=1.55E-6;n2=3.2E-20;Aeff=30E-12;L=80;
E1(1)=0;Pmax=40;phi=0;Gauss(1)=0;
Pin(1:N)=0; Pout(1:N)=0;
phif=0*pi; E6p=0;
kappa1=0.25;kappa2=0.8;kappa3=0.8;
rootk1 = sqrt(kappa1); irootk1 = 1i*sqrt(1-kappa1);
rootk2 = sqrt(kappa2); irootk2 = 1i*sqrt(1-kappa2);
rootk3 = sqrt(kappa3); irootk3 = 1i*sqrt(1-kappa3);
G=sqrt((1-kappa2)*(1-kappa3));
% Ramp the power up and down
for n=1:N

    Ein = sqrt(Pmax*exp(-0.02*((n*Pmax/N-Pmax/2))^2));
    E1 = rootk3*Ein+irootk3*E6p;
    P1 = abs(E1)^2;

    E3 = rootk1*E1;
    E4 = irootk1*E1;

    phic=2*pi*n2*L*(2-kappa1)*P1/(lambda*Aeff);
    phicc=2*pi*n2*L*(1+kappa1)*P1/(lambda*Aeff);

    E3p = E3*exp(-1i*(phi+phic));
    E4p = E4*exp(-1i*(phi+phicc));

    E5 = rootk1*E3p+irootk1*E4p;

    Pout(n) = abs(rootk2*E5)^2;
    Pin(n) = abs(Ein)^2;
    E6 = irootk2*E5;

    E6p = E6*exp(1i*phif);

end

% Plot the bifurcation diagrams
figure(1)
clf
fsize=15;
subplot(2,1,1)
hold on
plot(Pout(1:N),'.','MarkerSize',1)
plot(Pin(1:N),'.','MarkerSize',1);
xlabel('Number of Ring Passes','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off

subplot(2,1,2)
hold on
plot(Pin,Pout,'.','MarkerSize',1);
xlabel('Input Power','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off


% GPCG algorithm adapted for NMF
function X = gpcg_nmf(Y,A,X,MaxIter,CostFun,Alpha)
% Our implementation of GPCG;

[R,T] = size(X); [M,T] = size(Y);
beta = 0.5; mu = 0.01; MaxSearchIter = 10;
Diff_DF_max = 0; gamma_GP = 0.01;

if CostFun == 1
   H = spalloc(R*T,R*T,T*R^2);
   Hx = A'*A;
   H = kron(speye(T),Hx); % Hessian
end

for k = 1:MaxIter

% Step 1
    Xp = X;
    if CostFun ~= 1
       H = CostFunHess(Y,A,X,Alpha); % Hessian
    end
    P = -CostFunGrad(Y,A,X,CostFun,Alpha); % Gradient Matrix
    p = P(:); % Vectorization

% Step 2
     eta = (p'*p)/(p'*(H*p));
     for i = 0:MaxSearchIter % Armijo rule
         eta = eta*beta.^i;
         X = max(X + eta*P,0);
         if (CostFunEval(Y,A,X,CostFun,Alpha) - CostFunEval(Y,A,Xp,CostFun,Alpha)) <=  (norm(X - Xp,'fro')*mu/eta)
             break;
         end
     end

% Step 3
  Z = zeros(size(X));
  Z(X > eps) = 1;
  z = vec(Z);

% Step 4
  Pc = CostFunGrad(Y,A,X,CostFun,Alpha); % Gradient Matrix
  pc = Pc(:); % Vectorization

% Step 5
  pR = z.*pc; % Reduced gradient
  if CostFun ~= 1
     H = CostFunHess(Y,A,X,Alpha); % Hessian
  end

HR = repmat(z,1,R*T).*H.*repmat(z',R*T,1) + speye(R*T) - spdiags(z,0,R*T,R*T); % reduced Hessian

% Step 6
 [pc,flag,relres,iter,resvec] = pcg(HR,-pR);
 P = reshape(pc,R,T); % Matricization

 % Step 7
     DF = CostFunEval(Y,A,X,CostFun,Alpha);
     eta = 1;
     for j = 0:MaxSearchIter % Armijo rule
         eta = eta*beta.^j;
         X = max(X + eta*P,0);
         if (CostFunEval(Y,A,X,CostFun,Alpha) < DF)
             break;
         end
     end

   Fn(k) = CostFunEval(Y,A,X,CostFun,Alpha);

 % Stopping criterion
   DF_old = norm(Y - A*Xp,'fro');  DF = norm(Y - A*X,'fro');
   Diff_DF = DF_old - DF;  Diff_DF_max = max(Diff_DF,Diff_DF_max);
   if (Diff_DF <= gamma_GP*Diff_DF_max) & (k > 1)
       break;
   end

end

% Cost Function
function F = CostFunEval(Y,A,X,CostFun,Alpha);

 switch CostFun

     case 1 % Eucliden distance

         F = norm(Y - A*X,'fro');

     case 2 % Alpha divergence

         Z = A*X + 1E2*eps;
         Y = Y + 1E2*eps;
         if Alpha == 1 % KL divergence
            F = sum(sum(Y.*log(Y./Z) + Z - Y));
         elseif Alpha == 0 % Dual KL divergence
            F = sum(sum(Z.*log(Z./(Y+eps)) + Y - Z));
         else % Alpha-divergence
            F = (1/Alpha)*sum(sum(Y.*( (Y./Z).^(Alpha - 1) - 1)/(Alpha - 1) + Z - Y));
         end
 end

% Gradient of Cost Function
function G = CostFunGrad(Y,A,X,CostFun,Alpha);

 switch CostFun

     case 1 % Eucliden distance

         G = A'*(A*X - Y);

     case 2 % Alpha divergence

         Z = A*X+1E2*eps;
         if ~Alpha % Dual KL divergence
            G = A'*log(Z./(Y + 1E2*eps));
         else
            G = (1/Alpha)*A'*(1 - ((Y+1E2*eps)./Z).^Alpha);
         end
  end

% Hessian of Cost Function
function H = CostFunHess(Y,A,X,Alpha);

[R,T] = size(X);
M = size(Y,1);
H = spalloc(R*T,R*T,T*R^2);
Z = A*X+1E2*eps;
if ~Alpha % Dual KL divergence
    Zx = 1./Z + 1E2*eps;
else
    Zx = ((Y+1E2*eps).^Alpha)./(Z.^(Alpha + 1));
end
 for t = 1:T
     H(((t-1)*R+1):t*R,((t-1)*R+1):t*R) = A'*repmat(Zx(:,t),1,M)*A;
 end