%% FUNCTION bsa_ihb
%    Singular Projection
%
%% OBJECTIVE
%   min 1/2*||x - a||_2^2
%    s.t. b'*x = r, 0<= x <= u,  b > 0
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% Related papers
%
% [1] KC. Kiwiel. On linear-time algorithms for the continuous
%     quadratic knapsack problem, Journal of Optimization Theory
%     and Applications, 2007
%

function [x_star,t_star,iter] = bsa_ihb(a,b,r,u)

% initilization
break_flag = 0;
t_l = a./b; t_u = (a - u)./b;
T = [t_l;t_u];
t_L = -inf; t_U = inf;
g_tL = 0; g_tU = 0;

iter = 0;
while ~isempty(T)
    iter = iter + 1;
    g_t = 0;
    t_hat = median(T);

    U = t_hat < t_u;
    M = (t_u <= t_hat) & (t_hat <= t_l);

    if sum(U)
       g_t = g_t + b(U)'*u(U);
    end
    if sum(M)
        g_t = g_t + sum(b(M).*(a(M) - t_hat*b(M)));
    end

    if g_t > r
        t_L = t_hat;
        T = T(T > t_hat);
        g_tL = g_t;
    elseif g_t < r
        t_U = t_hat;
        T = T(T < t_hat);
        g_tU = g_t;
    else
        t_star = t_hat;
        break_flag = 1;
        break;
    end
end
if ~break_flag
     t_star = t_L - (g_tL -r)*(t_U - t_L)/(g_tU - g_tL);
end
x_star = min(max(0,a - t_star*b),u);
end


function MS = createMesh2D(varargin)
% MeshStructure = createMesh2D(Nx, Ny, Lx, Ly)
% MeshStructure = createMesh2D(facelocationX, facelocationY)
% creates a uniform 2D mesh:
% Nx is the number of cells in x (horizontal) direction
% Ny is the number of cells in y (vertical) direction
% Lx is the domain length in x direction
% Ly is the domain length in y direction
%
% SYNOPSIS:
%   MeshStructure = createMesh2D(Nx, Ny, Lx, Ly)
%
% PARAMETERS:
%   Nx: number of cells in the x direction
%   Lx: domain length in x direction
%   Ny: number of cells in the y direction
%   Ly: domain length in y direction
%
% RETURNS:
%   MeshStructure.
%                 dimensions=2 (2D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 and top to bottom
%                 cellsize: x and y elements of the cell size =[Lx/Nx,
%                 Ly/Ny]
%                 cellcenters.x: location of each cell in the x direction
%                 cellcenters.y: location of each cell in the y direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 facecenters.y: location of interface between cells in the
%                 y direction
%                 numberofcells: [Nx, Ny]
%
%
% EXAMPLE:
%   Nx = 5;
%   Ny = 7;
%   Lx = 10;
%   Ly = 20;
%   m = createMesh2D(Nx, Ny, Lx, Ly);
%   [X, Y] = ndgrid(m.cellcenters.x, m.cellcenters.y);
%   [Xf,Yf]=ndgrid(m.facecenters.x, m.facecenters.y);
%   plot(X, Y, 'or', ...
%        Xf, Yf, '-b', Xf', Yf', '-b');
%
% SEE ALSO:
%     createMesh1D, createMesh3D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==4
  % uniform 1D mesh
  Nx=varargin{1};
  Ny=varargin{2};
  Width=varargin{3};
  Height=varargin{4};
  % cell size is dx
  dx = Width/Nx;
  dy = Height/Ny;
  G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
  CellSize.x= dx*ones(Nx+2,1);
  CellSize.y= dy*ones(Ny+2,1);
  CellSize.z= [0.0];
  CellLocation.x= [1:Nx]'*dx-dx/2;
  CellLocation.y= [1:Ny]'*dy-dy/2;
  CellLocation.z= [0.0];
  FaceLocation.x= [0:Nx]'*dx;
  FaceLocation.y= [0:Ny]'*dy;
  FaceLocation.z= [0.0];
elseif nargin==2
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  facelocationY=varargin{2};
  facelocationX=facelocationX(:);
  facelocationY=facelocationY(:);
  Nx = length(facelocationX)-1;
  Ny = length(facelocationY)-1;
  G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
  CellSize.x= [facelocationX(2)-facelocationX(1); ...
    facelocationX(2:end)-facelocationX(1:end-1); ...
    facelocationX(end)-facelocationX(end-1)];
  CellSize.y= [facelocationY(2)-facelocationY(1); ...
    facelocationY(2:end)-facelocationY(1:end-1); ...
    facelocationY(end)-facelocationY(end-1)];
  CellSize.z= [0.0];
  CellLocation.x= 0.5*(facelocationX(2:end)+facelocationX(1:end-1));
  CellLocation.y= 0.5*(facelocationY(2:end)+facelocationY(1:end-1));
  CellLocation.z= [0.0];
  FaceLocation.x= facelocationX;
  FaceLocation.y= facelocationY;
  FaceLocation.z= [0.0];
end
c=G([1,end], [1,end]);
MS=MeshStructure(2, ...
  [Nx,Ny], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  c(:), ...
  [1]);


function plot_imu_sta(omega, freq, text_st)
% plot_imu_sta: plots static IMU data.
%
% INPUT
%   omega, IMU sensor data (rad/s or m/s^2).%
%   freq, Nx1 IMU sensor sampling frequency (Hz).
%   text_st, title for the figure (string).
%
% OUTPUT
%   a figure.
%
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   M.A. Hopcroft. Allan overlap MATLAB function v2.24.
% https://www.mathworks.com/matlabcentral/fileexchange/13246-allan
%
% Version: 001
% Date:    2021/12/06
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

% Colors
blue    = [0, 0.4470, 0.7410];
orange  = [0.8500, 0.3250, 0.0980];
green   = [0.4660, 0.6740, 0.1880];
yellow  = [0.9290, 0.6940, 0.1250];
light_blue = [0.3010, 0.7450, 0.9330];

% Text size
font_title  = 50;
font_tick   = 15;
font_label  = 20;
font_legend = 15;

% Line width
lw = 3;

M = max(size(omega)) - 1;
dt = 1/freq;

time = (0:dt:dt*M)';

s.linear = polyfit( time(1:length(omega)), omega, 1);

% scale to median for plotting
omega_median=median(omega);
medianfreq=omega-omega_median;

% Screen for outliers using 5x Median Absolute Deviation (MAD) criteria
MAD = median(abs(medianfreq)/0.6745);

% adjust time to remove any starting offset
dtime = time - time(1) + mean(diff(time));

% plot the frequency data, centered on median
% this should not be necessary, but dsplot 1.1 is a little bit brittle
if size(dtime,2) > size(dtime,1), dtime=dtime'; end

% dsplot makes a new figure
if(is_octave)
  hd=plot(dtime,medianfreq);
else
  hd=dsplot(dtime,medianfreq);
end

  set(hd,'Marker','.','LineStyle','none'); % equivalent to '.-'
hold on;

fx = xlim;
% plot([fx(1) fx(2)],[omega_median omega_median],'-k');
plot([fx(1) fx(2)],[0 0],':k','LineWidth', lw);

% show 5x Median Absolute deviation (MAD) values
hm = plot([fx(1) fx(2)],[5*MAD 5*MAD],'-r','LineWidth', lw);
plot([fx(1) fx(2)],[-5*MAD -5*MAD],'-r','LineWidth', lw);

% show linear fit line
hf=plot(xlim,polyval(s.linear,xlim)-omega_median,'-g','LineWidth', lw);
t1 = title(text_st);

hs=plot(xlim,polyval(s.linear,xlim)-omega_median-3*MAD,'--m','LineWidth', lw);
plot(xlim,polyval(s.linear,xlim)-omega_median+3*MAD,'--m','LineWidth', lw);

%set(get(gca,'Title'),'Interpreter','none');
x1=xlabel('Time [sec]');
y1=ylabel('Samples');

if MAD ~= 0.0
    l1=legend([hd hm hs hf],{'data (centered on median)','5x MAD outliers', ...
        '3x MAD outliers', ['Linear Fit (' num2str(s.linear(1),'%g') ')']});
else
    l1=legend([hd hf],{'data (centered on median)', ...
        ['Linear Fit (' num2str(s.linear(1),'%g') ')']});
end

% tighten up
xlim([dtime(1) dtime(end)]);

set(t1,'FontSize', font_title);
set(x1,'FontSize', font_label);
set(y1,'FontSize', font_label);
set(l1,'FontSize', font_legend);
set(gca, 'YTickMode', 'auto', 'FontSize', font_tick);


function model = kpca_train(X,varargin)
% DESCRIPTION
% Kernel principal component analysis (KPCA)
%
%       model = kpca_train(X,varargin)
%
% INPUT
%   X            Training samples (N*d)
%                N: number of samples
%                d: number of features
%
% OUTPUT
%   model        KPCA model
%
%
% Created on 18th April 2019, by Kepeng Qiu.
%-------------------------------------------------------------%

% Default Parameters setting
options.sigma = 10;   % kernel width
options.dims  = 2;    % output dimension (dimensionality reduction)
                      % 2D data is easy to visualize
options.type  = 0;    % 0: dimensionality reduction or feature extraction
                      % 1: fault detection using KPCA
                      % 2: fault detection using dynamic KPCA(DKPCA)
options.beta  = 0.9;  % corresponding probabilities (for fault detection)
                      %
options.pcr   = 0.65; % principal contribution rate (for fault detection)
options.fd    = 0;    % 0: No fault diagnosis (fd)
                      % 1: fault diagnosis (fd)
options.lag   = 4;    % time lag (for DKPCA)
%

if rem(nargin-1,2)
    error('Parameters to kpca_train should be pairs')
end
numParameters = (nargin-1)/2;

for n =1:numParameters
    Parameters = varargin{(n-1)*2+1};
    value	= varargin{(n-1)*2+2};
    switch Parameters
            %
        case 'type'
            options.type = value;
            %
        case 'dims'
            options.dims = value;
            %
        case 'sigma'
            options.sigma = value;
            %
        case 'fd'
            options.fd = value;
            %
        case 'lag'
            options.lag = value;
            %
        case 'pcr'
            options.pcr = value;
            %
        case 'beta'
            options.beta = value;
    end
end

% number of training samples
L = size(X,1);

% DPCA
if options.type == 2
    %  Construct the augmented matrix
    X = constructAM(X,options.lag);
    L = size(X,1);
    model.lag = options.lag; % time lag
end

% Compute the kernel matrix
K = computeKM(X,X,options.sigma);

% Centralize the kernel matrix
unit = ones(L,L)/L;
K_c = K-unit*K-K*unit+unit*K*unit;

% Solve the eigenvalue problem
[V,D] = eigs(K_c/L);
% [V,D] = eig(K_c/L);
lambda = diag(D);

% Normalize the eigenvalue
V_s = V ./ sqrt(L*lambda)';
%  Lower version of MATLAB may report an error at line 70,
%  please replace it with the following code.
% ------------------------------
% for i = 1:size(lambda,1)
%     V_s(:,i) = V(:,i)/sqrt(L*lambda(i,1));
% end
% ------------------------------

% Compute the numbers of principal component
if options.type  == 1 || options.type  == 2  % fault detection
    dims = find(cumsum(lambda/sum(lambda)) >= ...
        options.pcr,1, 'first');
else % dimensionality reduction or feature extraction
    dims = options.dims;
end

% Extract the nonlinear component
mappedX  = K_c* V_s(:,1:dims) ;

% Store the results
model.mappedX =  mappedX ;
model.V_s = V_s;
model.lambda = lambda;
model.K_c = K_c;
model.L = L;
model.dims = dims;
model.X = X;
model.K = K;
model.unit = unit;
model.sigma = options.sigma;
model.diagnosis = options.fd;
model.type = options.type;

% Compute the threshold for fault detection
if options.type  == 1 || options.type  == 2
    model.beta = options.beta; % corresponding probabilities
    [SPE_limit,T2_limit] = comtupeLimit(model);
    model.SPE_limit = SPE_limit;
    model.T2_limit = T2_limit;
end

end


function ye = fL(t,D,nalpha,nbeta,u,h)
alpha = nalpha*pi;
beta = nbeta*pi;
ye = h*cos(beta*u*t) + exp(-D*alpha*alpha*t)*cos(alpha*u*t);


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [ D ] = dctn( A, varargin )
%
% N-D Discrete Cosine Transform
%
% Computes the N-dimensional dct of an array, by applying the
% one-dimnesional dct along all dimensions.
%
% Input:
%
%   A        - full N-dimensional array
%   varargin - (optional flag in which dimensions to apply the dct)
%
% Output:
%
%   D        - discrete cosine transform of A
%
%==============================================================================

function [ D ] = dctn( A, varargin )

if nargin==0
    help(mfilename)
    return;
end

% size if the input array
m = size(A);
dim = length(m);

% default parameters
dimFlag = ones(1,dim);  % apply dct in all dimensions by default

% overwrites default parameter
for k=1:2:length(varargin),
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

m = size(A);
D = A;
md = m;
P = circshift(1:dim,[0 -1]);

for d=1:dim
    if dimFlag(d)
        D = reshape(D,md(1),prod(md(2:end)));
        D = dct(D);
        D = reshape(D,md);
    end
    md = circshift(md,[0 -1]);
    D = permute(D,P);
end


function y = SSST (x, p)
% SSST  Spherical Surface Spline in Tension
%
%   y = SSST (x, p)
%
%   x is cos(theta) in range -1 <= x <= 1
%   p is tension (p >= 0)
%
% Returns the Green's' function for a spherical surface spline
% in tension, following Wessel and Becker [2008].
% If p == 0 or not given then we return Parker's [1994] minimum
% curvature solution instead.

% $Id: SSST.m,v 1.1.1.1 2008/05/09 21:34:52 myself Exp $
% P. Wessel, SOEST, U of Hawaii, April 2008 (pwessel@hawaii.edu)

if nargin == 1
	p = 0;
end
if (p == 0)     % Just do Parker's dilog solution
    y = dilog (0.5 - 0.5 * x);
    return
end

% Here we have tension
v = tension2nu (p);

y = zeros(size(x));     % Initialize output array
A = pi / sin(v*pi);     % Constant scale
k = find (abs(x) < (1-eps));  % Where Pv solution works
if (~isempty(k))
    y(k) = A*Pv(-x(k),v) - log(1-x(k));
end
% Deal with special case x == -1
k = find ((x-eps) <= -1);
if (~isempty(k))
    y(k) = A - log(2);
end
% Deal with special case x == +1
k = find ((x+eps) >= +1);
if (~isempty(k))
    y(k) = pi*cot(v*pi) + 2*(0.577215664901 + psi(1+v)) - log(2);
end
y = real(y);    % Knock off any insignificant imaginary noise

% Sub-functions used by SSST
function P = Pv (x, v)

P = zeros(size(x));
for i = 1:length(x)
    if (x(i) == -1)
        p = inf;
    else
        [p q k] = PvQv (x(i), v);
    end
    P(i) = p;
end

function [Pv Qv iter] = PvQv (x, v)
% Based on recipe in "An Atlas of Functions" by
% Spanier and Oldham, 1987
iter = 0;
if (x == -1)
    Pv = -inf;
    Qv = -inf;
    return
end
if (x == +1)
    Pv = 1;
    Qv = inf;
    return
end
a = 1;
R = 1;
K = 4 * sqrt (abs(v - v^2));
if (abs(1+v) + floor (1+v)) == 0
	a = 1.0e99;
	v = -1 - v;
end
s = sin (0.5*pi*v);
c = cos (0.5*pi*v);
w = (0.5 + v)^2;
while v <= 6.0
	v = v + 2;
	R = R * (v - 1)/v;
end
X = 1.0 / (4 + 4*v);
g = 1 + 5*X*(1 - 3*X*(0.35+6.1*X));
R = R*(1 - X*(1 - g*X/2))/sqrt (8*X);
g = 2*x;
u = g;
f = 1;
t = 1;
k = 0.5;
X = 1 + (1e8/(1 - x.^2));

t = t .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
k = k + 1;
f = f + t;
u = u .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
k = k + 1;
g = g + u;
while (k < K || abs (X*t) > abs(f))
        iter = iter + 1;
	t = t .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
	k = k + 1;
	f = f + t;
	u = u .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
	k = k + 1;
	g = g + u;
end
f = f + (x.^2.*t ./ (1 - x.^2));
g = g + (x.^2.*u ./ (1 - x.^2));
Pv = ((s*g*R) + (c*f/R))/sqrt(pi);
Qv = a*sqrt(pi)*((c*g*R) - (s*f/R))/2;

function [f] = psi(z)
%Psi     Psi (or Digamma) function valid in the entire complex plane.
%
%                 d
%        Psi(z) = --log(Gamma(z))
%                 dz
%
%usage: [f] = psi(z)
%
%        Z may be complex and of any size.
%
%        This program uses the analytical derivative of the
%        Log of an excellent Lanczos series approximation
%        for the Gamma function.
%
%References: C. Lanczos, SIAM JNA  1, 1964. pp. 86-96
%            Y. Luke, "The Special ... approximations", 1969 pp. 29-31
%            Y. Luke, "Algorithms ... functions", 1977
%            J. Spouge,  SIAM JNA 31, 1994. pp. 931
%            W. Press,  "Numerical Recipes"
%            S. Chang, "Computation of special functions", 1996
%
siz = size(z);
z=z(:);
zz=z;

f = zeros(size(z));

%reflection point
p=find(real(z)<0.5);
if ~isempty(p)
   z(p)=1-z(p);
end

%Lanczos approximation for the complex plane

g=607/128; % best results when 4<=g<=5

c = [  0.99999999999999709182;
      57.156235665862923517;
     -59.597960355475491248;
      14.136097974741747174;
      -0.49191381609762019978;
        .33994649984811888699e-4;
        .46523628927048575665e-4;
       -.98374475304879564677e-4;
        .15808870322491248884e-3;
       -.21026444172410488319e-3;
        .21743961811521264320e-3;
       -.16431810653676389022e-3;
        .84418223983852743293e-4;
       -.26190838401581408670e-4;
        .36899182659531622704e-5];

n=0;
d=0;
for k=size(c,1):-1:2
    dz=1./(z+k-2);
    dd=c(k).*dz;
    d=d+dd;
    n=n-dd.*dz;
end
d=d+c(1);
gg=z+g-0.5;
%log is accurate to about 13 digits...

f = log(gg) + (n./d - g./gg) ;

if ~isempty(p)
   f(p) = f(p)-pi*cot(pi*zz(p));
end

p=find(round(zz)==zz & real(zz)<=0 & imag(zz)==0);
if ~isempty(p)
   f(p) = Inf;
end

f=reshape(f,siz);

function y = dilog (x)
% DILOG   The dilogarithm
%
%   y = dilog (x)
%
% Compute dilog(x) (defined for x >= 0) by the method of Parker's
% Appendix A of his Geophysical Inverse Theory.  The function
% is needed for x in the range 0 <= x <= 1 when solving the
% spherical spline interpolation in section 2.07 of Parker.

y = zeros (size (x));

pisqon6 = pi * pi / 6.0;
k = find (x <= 0.0);
if (~isempty(k))
    y(k) = pisqon6;
end
k = find (x > 0.0 & x < 0.5);
if (~isempty(k))
	y(k) = -log (1.0 - x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
	y(k) = pisqon6 - z + y(k) .* log (x(k));
end
k = find (x >= 0.5 & x < 2.0);
if (~isempty(k))
    y(k) = -log (x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
    y(k) = z;
end
k = find (x >= 2.0);
if (~isempty(k))
    y(k) = log (x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
        y(k) = -z - 0.5 * ysq;
end

function nu = tension2nu (p)
nu = (-1 + sqrt (1 - 4*p^2))/2;


function [X]=tt_meshgrid_vert(varargin)
% Analogue of the meshgrid function for the TT-format,
% !!!!!!!!!!!! concatenating tensor trains with ones with tkron !!!!!!!!!!!
% !!!!!!!!!!!! contrarily to tt_meshgrid (which uses kron2)     !!!!!!!!!!!
%   X = TT_MESHGRID_VERT(A,B,C,...) Computes the meshgrid based on "1d"
%       representations
%   X = TT_MESHGRID_VERT(A) Computes the meshgrid based on the cell array A of
%       the representations
%   X = TT_MESHGRID_VERT(T,D) Computes the d-dimensional meshgrid, using T as a
%       one-dimensional grid
%   X = TT_MESHGRID_VERT(..., 'id', M) Expands with M instead of all-ones

vars = varargin;
M = [];
for i=1:numel(vars)
    if (isa(vars{i}, 'char'))&&(strcmp(vars{i},'id'))
        M = vars{i+1};
        vars(i:i+1) = [];
        break;
    end
end

if (numel(vars)==1)&&(isa(vars{1}, 'cell'))
    % We have a cell array of 1d points
    X = vars{1};
    d = numel(X);
    X = reshape(X, 1, d);
elseif (numel(vars)==2)&&(isa(vars{1}, 'tt_tensor')||isa(vars{1}, 'tt_matrix'))&&(isscalar(vars{2}))
    % First argument is a single x, second argument is the dimension
    d = vars{2};
    X = cell(1, d);
    for i=1:d
        X{i} = vars{1}; % make the same format as in the first case
    end
else
    % A set of independent x
    d = numel(vars);
    X = cell(1,d);
    for i=1:d
        if (isa(vars{i}, 'tt_tensor')||isa(vars{i}, 'tt_matrix'))
            X{i} = vars{i};
        else
            error('wrong input %d to tt_meshgrid_vert', i);
        end
    end
end

% Now X contains 1D tt_tensors. Expand them by ones

% Concat all n to the common storage
n = zeros(0,1);
% positions in n where each component starts
pos = ones(d+1,1);
for i=1:d
    n = [n; X{i}.n];
    pos(i+1) = pos(i)+X{i}.d;
end
% Expand
for i=1:d
    if (i>1)
        if (isempty(M))
            expand = tt_ones(n(1:pos(i)-1));
        else
            expand = M;
            if (i>2)
                expand = mtkron(repmat({M},1,i-1));
            end
        end
        X{i} = tkron(expand, X{i});
    end
    if (i<d)
        if (isempty(M))
            expand = tt_ones(n(pos(i+1):pos(d+1)-1));
        else
            expand = M;
            if (i<d-1)
                expand = mtkron(repmat({M},1,d-i));
            end
        end
        X{i} = tkron(X{i}, expand);
    end
end

end


function value = r8_besk0 ( x )

%*****************************************************************************80
%
%% R8_BESK0 evaluates the Bessel function K of order 0 of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the Bessel function K of order 0 of X.
%
  persistent bk0cs
  persistent ntk0
  persistent xmax
  persistent xsml

  if ( isempty ( ntk0 ) )

    bk0cs = [ ...
      -0.353273932339027687201140060063153E-01, ...
      +0.344289899924628486886344927529213, ...
      +0.359799365153615016265721303687231E-01, ...
      +0.126461541144692592338479508673447E-02, ...
      +0.228621210311945178608269830297585E-04, ...
      +0.253479107902614945730790013428354E-06, ...
      +0.190451637722020885897214059381366E-08, ...
      +0.103496952576336245851008317853089E-10, ...
      +0.425981614279108257652445327170133E-13, ...
      +0.137446543588075089694238325440000E-15, ...
      +0.357089652850837359099688597333333E-18, ...
      +0.763164366011643737667498666666666E-21, ...
      +0.136542498844078185908053333333333E-23, ...
      +0.207527526690666808319999999999999E-26, ...
      +0.271281421807298560000000000000000E-29, ...
      +0.308259388791466666666666666666666E-32 ]';

    ntk0 = r8_inits (bk0cs, 16, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( 4.0 * r8_mach ( 3 ) );
    xmax = - log ( r8_mach ( 1 ) );
    xmax = xmax - 0.5 * xmax * log ( xmax ) / ( xmax + 0.5 );

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BESK0 = Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R8_BESK0 = Fatal error!' )
  elseif ( x <= xsml )
    y = 0.0;
    value = - log ( 0.5 * x ) * r8_besi0 ( x ) ...
      - 0.25 + r8_csevl ( 0.5 * y - 1.0, bk0cs, ntk0 );
  elseif ( x <= 2.0 )
    y = x * x;
    value = - log ( 0.5 * x ) * r8_besi0 ( x ) ...
      - 0.25 + r8_csevl ( 0.5 * y - 1.0, bk0cs, ntk0 );
  elseif ( x <= xmax )
    value = exp ( - x ) * r8_besk0e ( x );
  else
    value = 0.0;
  end

  return
end


% ATANDEMO  This program creates the ArcTangent example in Chapter 2.
%
% C. T. Kelley, November 13, 2002.
%
x=10;
tol=[1.d-6,1.d-6];
%
% Solve the problem with Newton's method.
%
params=[40, 1, 0,0];
[an, errsn, ierrn]=nsold(x, 'fatan', tol, params);
%
% Set the default Jacobian updating strategy.
%
params=[40, 1000, .5, 0];
[ac, errsd, ierrd]=nsold(x, 'fatan', tol, params);
inewt=length(errsn(:,1)); cnewt=0:inewt-1;
idefault=length(errsd(:,1)); cdefault=0:idefault-1;
%
% Plot the residual histories.
%
semilogy(cnewt,errsn(:,1),'-',cdefault,errsd(:,1),'--')
legend('Newton','default')


%SurfPointFeature.support Support region of feature
%
% OUT = F.support(IM, W) is an image of the support region of the
% feature F, extracted from the image IM in which the feature appears.
% The support region is scaled to WxW and rotated so that the feature's
% orientation axis is upward.
%
% OUT = F.support(IMAGES, W) as above but if the features were extracted
% from an image sequence IMAGES then the feature is extracted from the
% appropriate image in the same sequence.
%
% [OUT,T] = F.support(IMAGES, W) as above but returns the pose of the feature
% as a 3x3 homogeneous transform in SE(2) that comprises the feature position
% and orientation.
%
% F.support(IM, W) as above but the support region is displayed.
%
% See also SurfPointFeature.

function [out,TT] = support(sf, images, N)

    if nargin < 3
        N = 50;
    end

    if isempty(sf.image_id_)
        im = images(:,:);
    else
        im = images(:,:,sf.image_id_);
    end

    d = 20*sf.scale_;

    [Uo,Vo] = imeshgrid(N, N);

    T = SE2(sf.u_, sf.v_, sf.theta_) * SE2(diag([d/N,d/N,1])) * SE2(-N/2, -N/2);

    UV = T * [Uo(:) Vo(:)]';
    U = reshape(UV(1,:), size(Uo));
    V = reshape(UV(2,:), size(Vo));

    [Ui,Vi] = imeshgrid(im);

    im2 = interp2(Ui, Vi, idouble(im), U, V);

    if nargout == 0
        idisp(im2)
    elseif nargout == 1
        out = im2;
    elseif nargout == 2
        out = im2;
        TT = T;
    end


function [cost, gradf, hessian, hessianinv]=costgradf(px,py,pz,rho,u,param)
% compute the cost function

e=1e-6;

h     = param.h;
dim   = param.dim;
gamma = param.gamma;
A1x   = param.A1x;
A1y   = param.A1y;
A1z   = param.A1z;
A2    = param.A2;
A3    = param.A3;
a     = param.a;
c     = param.c;
F1    = param.F1;
F2    = param.F2;

n  = dim(1);
nx = dim(2);
ny = dim(3);
nz = dim(4);
nt = dim(5);
hx = h(1);
hy = h(2);
hz = h(3);
ht = h(4);

%compute p^2 and u^2

pxsq  = px.^2;
pysq  = py.^2;
pzsq  = pz.^2;
usq   = u.^2;

% inverse of rho
rhoinv  = 1./rho;
rhoinv1 = 1./(F1'*rho(:));
rhoinv2 = 1./(F2'*rho(:));

E=eye(n);
E(n,n)=e;
W1=kron(speye((nx-1)*ny*nz*nt),E);
W2=kron(speye((ny-1)*nx*nz*nt),E);
W3=kron(speye((nz-1)*nx*ny*nt),E);

% cost f
%cost = (A1x*W1*pxsq(:)+A1y*W2*pysq(:)+A1z*W3*pzsq(:))'*(A2*rhoinv(:) + a(:))*hx*hy*hz*ht + usq(:)'*(A3*rhoinv1(:)+A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht*gamma;
cost = (A1x*W1*pxsq(:)+A1y*W2*pysq(:)+A1z*W3*pzsq(:))'*(A2*rhoinv(:) + a(:))*hx*hy*hz*ht + usq(:)'*(A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht*gamma;
% compute gradient of f
if nargout > 1
    % derivative wrt px
    A11xe    = 2*W1*A1x'*(A2*rhoinv(:)+a(:));
    nablapxf = px(:).*A11xe(:);

    % derivative wrt py
    A11ye    = 2*W2*A1y'*(A2*rhoinv(:)+a(:));
    nablapyf = py(:).*A11ye(:);

    % derivative wrt pz
    A11ze    = 2*W3*A1z'*(A2*rhoinv(:)+a(:));
    nablapzf = pz(:).*A11ze(:);

    % derivative wrt rho
    A22e     = A2'*(A1x*W1*pxsq(:) + A1y*W2*pysq(:)+ A1z*W3*pzsq(:));
    nablarhof = -A22e.*rhoinv(:).^2-gamma.*F2*((A3'*usq(:)).*rhoinv2(:).^2);
                %-gamma.*F1*((A3'*usq(:)).*rhoinv1(:).^2);

    % derivative wrt u
    %A33e    = 2*gamma*(A3*(rhoinv2(:)+rhoinv1(:))+c(:));
    A33e    = 2*gamma*(A3*(rhoinv2(:))+c(:));
    nablauf = u(:).*A33e(:);

    % gradient wrt p rho u
    gradf   = [nablapxf; nablapyf; nablapzf; nablarhof; nablauf];

    % compute Hessian
    if nargout > 2
        % Hessian over px
        A11x    = A11xe;

        % Hessian over py
        A11y    = A11ye;

        % Hessian over py
        A11z    = A11ze;

         % hessian over rho
        temp1   = 2.*A22e.*rhoinv(:).^3;
        temp2   = 2.*gamma.*(F2*((A3'*usq(:)).*rhoinv2(:).^3));
        %temp3   = 2.*gamma.*(F1*((A3'*usq(:)).*rhoinv1(:).^3));
        A22     = temp1(:)+temp2(:)...+temp3(:)
        +1e-4*ones(n*nx*ny*nz*(nt-1),1);

        % hessian over u
        A33  = A33e;

        % hessian
        hessian = [A11x(:);A11y(:);A11z(:);A22(:);A33(:)];

        % compute the inverse of Hessian
        if nargout > 3
            % inverse of hessian
            hessianinv = spdiags(1./(hessian),0,numel(hessian),numel(hessian));
        end
    end
end

end


function [A_lon,B_lon,A_lat,B_lat] = compute_ss_model(filename,x_trim,u_trim)
% x_trim is the trimmed state,
% u_trim is the trimmed input

% add stuff here
[A,B,C,D] = linmod(filename, x_trim, u_trim);

E1 = [...
      0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0;...
      0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0;...
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;...
      0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0;...
      0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;...
      ];
E2 = [...
      0, 1, 0, 0;...
      0, 0, 1, 0;...
      ];
A_lat = E1 * A * E1';
B_lat = E1 * B * E2';

E3 = [...
      0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0;...
      0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;...
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0;...
      0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0;...
      0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;...
      ];
E4 = [...
      1, 0, 0, 0;...
      0, 0, 0, 1;...
      ];

A_lon = E3 * A * E3';
B_lon = E3 * B * E4';


function varargout = drawEllipse3d(varargin)
%DRAWELLIPSE3D Draw a 3D ellipse
%
%   Possible calls for the function :
%   drawEllipse3d([XC YC ZC A B THETA PHI])
%   drawEllipse3d([XC YC ZC A B THETA PHI PSI])
%   drawEllipse3d([XC YC ZC A B], [THETA PHI])
%   drawEllipse3d([XC YC ZC A B], [THETA PHI PSI])
%   drawEllipse3d([XC YC ZC A B], THETA, PHI)
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI)
%   drawEllipse3d([XC YC ZC A B], THETA, PHI, PSI)
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI, PSI)
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI)
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI, PSI)
%
%   where XC, YC, ZY are coordinate of ellipse center, A and B are the
%   half-lengths of the major and minor axes of the ellipse,
%   PHI and THETA are 3D angle (in degrees) of the normal to the plane
%   containing the ellipse (PHI between 0 and 360 corresponding to
%   longitude, and THETA from 0 to 180, corresponding to angle with
%   vertical).
%
%   H = drawEllipse3d(...)
%   return handle on the created LINE object
%
%   ------
%   Author: David Legland
%   e-mail: david.legland@grignon.inra.fr
%   Created: 2008-05-07
%   Copyright 2008 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY

%   Possible calls for the function, with number of arguments :
%   drawEllipse3d([XC YC ZC A B THETA PHI])             1
%   drawEllipse3d([XC YC ZC A B THETA PHI PSI])         1
%   drawEllipse3d([XC YC ZC A B], [THETA PHI])          2
%   drawEllipse3d([XC YC ZC A B], [THETA PHI PSI])      2
%   drawEllipse3d([XC YC ZC A B], THETA, PHI)           3
%   drawEllipse3d([XC YC ZC A B], THETA, PHI, PSI)      4
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI)         5
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI, PSI)    6
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI)         7
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI, PSI)    8

% extract drawing options
ind = find(cellfun(@ischar, varargin), 1, 'first');
options = {};
if ~isempty(ind)
    options = varargin(ind:end);
    varargin(ind:end) = [];
end

if length(varargin)==1
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get colatitude of normal
    if size(ellipse, 2)>=6
        theta = ellipse(:,6);
    else
        theta = zeros(size(ellipse, 1), 1);
    end

    % get azimut of normal
    if size(ellipse, 2)>=7
        phi     = ellipse(:,7);
    else
        phi = zeros(size(ellipse, 1), 1);
    end

    % get roll
    if size(ellipse, 2)==8
        psi = ellipse(:,8);
    else
        psi = zeros(size(ellipse, 1), 1);
    end

elseif length(varargin)==2
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get angle of normal
    angle = varargin{2};
    theta   = angle(:,1);
    phi     = angle(:,2);

    % get roll
    if size(angle, 2)==3
        psi = angle(:,3);
    else
        psi = zeros(size(angle, 1), 1);
    end

elseif length(varargin)==3
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get angle of normal and roll
    theta   = varargin{2};
    phi     = varargin{3};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==4
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);

    if size(ellipse, 2)==5
        a  = ellipse(:,4);
        b  = ellipse(:,5);
    end

    theta   = varargin{2};
    phi     = varargin{3};
    psi     = varargin{4};

elseif length(varargin)==5
    % get center and radius
    ellipse = varargin{1};
    xc      = ellipse(:,1);
    yc      = ellipse(:,2);
    zc      = ellipse(:,3);
    a       = varargin{2};
    b       = varargin{3};
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==6
    ellipse = varargin{1};
    xc      = ellipse(:,1);
    yc      = ellipse(:,2);
    zc      = ellipse(:,3);
    a       = varargin{2};
    b       = varargin{3};
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = varargin{6};

elseif length(varargin)==7
    xc      = varargin{1};
    yc      = varargin{2};
    zc      = varargin{3};
    a       = varargin{4};
    b       = varargin{5};
    theta   = varargin{6};
    phi     = varargin{7};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==8
    xc      = varargin{1};
    yc      = varargin{2};
    zc      = varargin{3};
    a       = varargin{4};
    b       = varargin{5};
    theta   = varargin{6};
    phi     = varargin{7};
    psi     = varargin{8};

else
    error('drawEllipse3d: please specify center and radius');
end

% uses 60 intervals
t = linspace(0, 2*pi, 61)';

% polyline approximation of ellipse, centered and parallel to main axes
x       = a * cos(t);
y       = b * sin(t);
z       = zeros(length(t), 1);
base    = [x y z];

% compute transformation from local basis to world basis
trans   = localToGlobal3d(xc, yc, zc, theta, phi, psi);

% transform points composing the ellipse
ellipse = transformPoint3d(base, trans);

% draw the curve
h = drawPolyline3d(ellipse, options{:});

if nargout > 0
    varargout = {h};
end


function x_new = r8sd_cg ( n, ndiag, offset, a, b, x )

%*****************************************************************************80
%
%% R8SD_CG uses the conjugate gradient method on a R8SD linear system.
%
%  Discussion:
%
%    The R8SD storage format is for symmetric matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0, and
%    each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%
%    Assuming there are NDIAG nonzero diagonals (ignoring subdiagonals!),
%    we then create an array B that has N rows and NDIAG columns, and simply
%    "collapse" the matrix A to the left:
%
%    For the conjugate gradient method to be applicable, the matrix A must
%    be a positive definite symmetric matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals that are stored.
%    NDIAG must be at least 1 and no more than N.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8SD matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, real X(N), an estimate for the solution, which may be 0.
%
%    Output, real X_NEW(N), the approximate solution vector.  Note that
%    repeated calls to this routine, with the output X_NEW from the
%    previous call used as the input value of X, MAY improve the solution.
%
  x_new(1:n) = x(1:n);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n) = r8sd_mxv ( n, ndiag, offset, a, x_new );

  r(1:n) = b(1:n) - ap(1:n);
  p(1:n) = b(1:n) - ap(1:n);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP = A*P.
%
    ap(1:n) = r8sd_mxv ( n, ndiag, offset, a, p );
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p(1:n) * ap(1:n)';
    pr =  p(1:n) * r(1:n)';

    if ( pap == 0.0E+00 )
      return;
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x_new(1:n) = x_new(1:n) + alpha * p(1:n);
    r(1:n) = r(1:n) - alpha * ap(1:n);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r(1:n) * ap(1:n)';

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n) = r(1:n) + beta * p(1:n);

  end

  return
end


function spline_test ( )

%*****************************************************************************80
%
%% SPLINE_TEST tests the SPLINE library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 June 2013
%
%  Author
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPLINE_TEST\n' );
  fprintf ( 1, '  MATLAB version:\n' );
  fprintf ( 1, '  Test the SPLINE library.\n' );

  spline_test001 ( );
  spline_test002 ( );
  spline_test003 ( );
  spline_test004 ( );
  spline_test005 ( );
  spline_test006 ( );

  spline_test01 ( );
  spline_test02 ( );
  spline_test03 ( );
  spline_test04 ( );
  spline_test05 ( );
  spline_test06 ( );
  spline_test07 ( );
  spline_test08 ( );
  spline_test09 ( );

  spline_test10 ( );
  spline_test11 ( );
  spline_test115 ( );
  spline_test116 ( );
  spline_test12 ( );
  spline_test125 ( );
  spline_test126 ( );
  spline_test127 ( );
  spline_test13 ( );
  spline_test14 ( );
  spline_test145 ( );
  spline_test15 ( );
  spline_test16 ( );
  spline_test17 ( );
  spline_test18 ( );
  spline_test19 ( );
  spline_test195 ( );

  spline_test20 ( );
  spline_test205 ( );
  spline_test21 ( );
  spline_test215 ( );
  spline_test22 ( );
  spline_test225 ( );
  spline_test23 ( );
  spline_test235 ( );
  spline_test24 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPLINE_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


% homodyne_recon_demo.m
%
% - Demonstrates use of homodyne_recon.m, which implements the homodyne
% reconstruction method outlined in Doug Noll's 1991 IEEE T-MI paper
% "Homodyne detection in magnetic resonance imaging"
% - Figure shows the decrease in RMSE that results from sampling more.
% - Reconstructed image can be rectangular, with odd or even dimensions.
%
% 2012-06-15, Mai Le
% 2012-09-26 Jeff Fessler tweaked

% set up "true" brain image
%f.dir = [path_find_dir('mri') '/../data/mri/'];
%f.xtrue = [f.dir 'brainweb_t1.jpg'];
%mag_true = double(imread(f.xtrue))'; % true image magnitude
mag_true = 25*ellipse_im(256);
mag_true = mag_true(16:end-16,3:end-2); % make it rectangle to stress test
dims = size(mag_true);

% simulate linear phase over object
[xx yy] = ndgrid(-dims(1)/2:dims(1)/2-1,-dims(2)/2:dims(2)/2-1);
ph_max = pi/2;
ph = ph_max*xx/(dims(1)/2) + ph_max*yy/(dims(2)/2);
image = mag_true .* exp(1i*ph); % modulate to make it complex
im_fft = fftshift(fft2(image));

im plc 2 3
im(1, mag_true)

for homodyne_direction = [1 2] % partial k-space can be in direction 1 or 2
	% # of samples in "half" kspace:
	nhalf = ceil((dims(homodyne_direction)+1)/2);
	overlaps = dims(homodyne_direction) - nhalf; % maximum # of extra rows
	overlaps = [0:5:(overlaps-1) overlaps];
	nover = numel(overlaps);

	nrms_im = zeros(nover,1);
	nrms_ph_demod = zeros(nover,1);

	for ii = 1:nover
		overlap = overlaps(ii);
		if (homodyne_direction == 1)
			k1 = 1:min(dims(1), nhalf+overlap);
			partial_kspace = im_fft(k1,:);
		elseif (homodyne_direction == 2)
			k2 = 1:min(dims(2), nhalf+overlap);
			partial_kspace = im_fft(:,k2);
		else
			fail 'bug'
		end

		f.sigma = 800; % complex AWGN std dev
		noisy_partial_kspace = partial_kspace + f.sigma * ...
			(randn(size(partial_kspace)) + 1i*randn(size(partial_kspace)));

		[recon_ph_demod, recon_im, lp_im, full_kspace] = ...
			homodyne_recon(noisy_partial_kspace, dims(1), dims(2), ...
				overlap, 'direction', homodyne_direction);

		nrms_fun = @(x,y) 100 * nrms(x(:), y(:));
		nrms_im(ii) = nrms_fun(abs(recon_im), mag_true);
		nrms_ph_demod(ii) = nrms_fun(abs(recon_ph_demod), mag_true);

		if overlap <= 15
			clim = minmax(mag_true)';
			im(2, abs(recon_im), clim)
			titlef('Conventional, overlap=%d', overlap)
			im(3, abs(recon_ph_demod), clim)
			titlef('Demodulated, overlap=%d', overlap)
			im(4, log(abs(partial_kspace)), clim)
			im(5, abs(recon_im) - mag_true, [-9 9], 'Error')
			im(6, abs(recon_ph_demod) - mag_true, [-9 9], 'Error')
			drawnow
	%		prompt
		end
	end

	f.snr = sqrt(mean(abs(partial_kspace(:)).^2)) / ...
		sqrt(mean(abs(noisy_partial_kspace(:) - partial_kspace(:)).^2));
	f.snr = 20*log10(f.snr);
	pr f.snr

	im subplot 4
	plot(overlaps, nrms_im, 'g-+', overlaps, nrms_ph_demod, 'b-o')
	axis tight
	xtick([0 15 max(overlaps)])
	title('NRMSE (%)')
	legend('without demodulation', 'phase demodulated');
	xlabel('rows acquired beyond 1/2 of kspace');
	ylabel('NRMS error of reconstructed image (%)');
prompt
end


%File Description:
%  Unknown parameters in magnetometer error model are calculated using the
%  lm optimization algorithm.
close all
clc
clear
load MagRaw.mat
%magnetometer calibration parameters in the Pixhawk autopilot which can
 %be obtained from QGC
CAL_MAG_SCALE = [1, 1, 1]';
CAL_MAG_OFF = [0.064, 0.014, -0.053]';
MagRaw = (mag + CAL_MAG_OFF)./CAL_MAG_SCALE;  % raw data

m = length(MagRaw);
MagSum = 0;
for k = 1 : m
    MagSum = MagSum + norm(MagRaw(:, k));
end
MagAver = MagSum/m;  % estimated magnetic field strength
Vdata = MagRaw/MagAver;  % normalization

y_dat = ones(m, 1);
p0 = [1 1 1 0 0 0]';
p_init = [1 1 1 0.01 0.01 0.01]';  % initial value of unknown parameters

y_raw = calFunc(Vdata, p0);  %magnetic field strength measured by magnetometer
y_raw = y_raw(:);
r_raw = y_dat - y_raw;
p_fit = lm('calFunc', p_init, Vdata, y_dat, 0.001);
y_lm = calFunc(Vdata, p_fit);  % magnetic field strength measured by calibrated magnetometer
y_lm = y_lm(:);
r_lm = y_dat - y_lm;
y_px4 = calFunc(mag/MagAver, p0);  % magnetic field strength measured by calibrated magnetometer of PX4
y_px4 = y_px4(:);
r_px4 = y_dat - y_px4;

kx = p_fit(1);
ky = p_fit(2);
kz = p_fit(3);
bx = p_fit(4);
by = p_fit(5);
bz = p_fit(6);

Km = [kx 0 0;0 ky 0;0 0 kz]
bm = [bx by bz]'

figure
bar([r_raw'*r_raw, r_lm'*r_lm, r_px4'*r_px4])
grid on;
set(gca, 'XTickLabel', {'raw','lm','px4'});
ylabel('Index');

t=1:m;
figure
title('Magnetometer Calibration')
plot(t, r_raw, '-.', t, r_lm, '-')
legend('Uncalibrated','Calibrated-LM')
xlabel('Measurement sampling number')
ylabel('Residual')


classdef DOC4 < PROBLEM
% <multi> <real> <constrained>
% Benchmark MOP with constraints in both decision and objective spaces

%------------------------------- Reference --------------------------------
% Z. Liu and Y. Wang, Handling constrained multiobjective optimization
% problems with constraints in both the decision and objective spaces. IEEE
% Transactions on Evolutionary Computation, 2019, 23(5): 870-884.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 8;
            obj.lower    = [0 -10 -10 -10 -10 -10 -10 -10];
            obj.upper    = [ 1 10 10 10 10 10 10 10];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g_temp = (X(:, 2) - 10).^2 + 5 * (X(:, 3) - 12).^2 + X(:, 4).^4 + 3 * (X(:, 5) - 11).^2 + 10 * X(:, 6).^6 + ...
            7 * X(:, 7).^2 + X(:, 8).^4 - 4 * X(:, 7).* X(:, 8) - 10 * X(:, 7) - 8 * X(:, 8);
            g = g_temp-680.6300573745 +1;
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1))./g);
            % Constraints in objective space
            c(:,1) = max( -(PopObj(:,1) + PopObj(:,2)-1), 0);
            c(:,2) = max(- ( PopObj(:,1)+ PopObj(:,2) - 1 - abs(sin(10*pi*(PopObj(:,1) - PopObj(:,2) + 1) ))), 0);
            % Constraints in decision space
            c(:,3) = -127 + 2 * X(:, 2).^2 + 3 * X(:, 3).^4 + X(:, 4) + 4 * X(:, 5).^2 + 5 * X(:, 6);
            c(:,4) = -282 + 7 * X(:, 2) + 3 * X(:, 3) + 10 * X(:, 4).^2 + X(:, 5) - X(:, 6);
            c(:,5) = -196 + 23 * X(:, 2) + X(:, 3).^2 + 6 * X(:, 7).^2 - 8 * X(:, 8);
            c(:,6) = 4 * X(:, 2).^2 + X(:, 3).^2 - 3 * X(:, 2).* X(:, 3) + 2 * X(:, 4).^2 + 5 * X(:, 7) - 11 * X(:, 8);
            Population = SOLUTION(X,PopObj,c,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = (0:20)/20;
            R(:,2) = 1 - R(:,1);
        end
    end
end


function La = YeePattanaikLuminanceAdaptation(img, maxLayers)
%
%
%       La = YeePattanaikLuminanceAdaptation(img, maxLayers)
%
%
%       Input:
%           -img: HDR image
%           -maxLayers: the number of layers in [16,96]
%
%       Output:
%           -La: luminance adaptation
%
%     Copyright (C) 2010-2020  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "Segmentation and Adaptive Assimilation for Detail-Preserving Display of High-Dynamic Range Images"
% 	  by Hector Yee, Sumanta N. Pattanaik
%     in Elsevier The Visual Computer 2003
%

%is it a three color channels image?
check13Color(img);

checkNegative(img);

%check parameters
if(~exist('maxLayers', 'var'))
    maxLayers = 32;
end

if((maxLayers < 16) || (maxLayers > 96))
    maxLayers = 32;
end

maxLayers = round(maxLayers);

%these could be parameters
bin_size1 = 0.5;
bin_size2 = 2.0;

%compute luminance channel
L = lum(img);

%compute the adaptation
L_log = log10(L + 1e-6);
minL_Log = min(L_log(:));

La = zeros(size(L));

for i=0:(maxLayers - 1)
    bin_size = bin_size1 + (bin_size2 - bin_size1) * i / (maxLayers - 1);
    category = round((L_log - minL_Log) / bin_size) + 1;

    %compute layers
    [imgLabel, ~, ~] = computeConnectedComponents(category, 8);
    labels = unique(imgLabel);

    for j=1:length(labels) %adaptation group
        indx = find(imgLabel == labels(j));
        La(indx) = La(indx) + mean(L_log(indx));
    end
end

La = 10.^(La / maxLayers);
La(La < 0.0) = 0.0;

end


function [ R ] = vl_nnr2R( r,dCdR )
%r2R Axis angle to rotation matrix layer
% Forwards mode:
% R = vl_nnr2R(r);
%   r is of size 1 x 1 x 3 x nbatch containing axis angle vectors
%   R is of size 1 x 3 x 3 x nbatch containing rotation matrices
% Backwards mode:
% dCdr = vl_nnr2R(r,dCdR);
%   dCdR is derivative of the cost function with respect to R and is of
%   size 1 x 3 x 3 x nbatch
%   dCdr is derivative of the cost function with respect to r and is of
%   size 1 x 1 x 3 x nbatch
%
% Useful reference for formula:
% [1] Gallego, Guillermo, and Anthony Yezzi. "A compact formula for the
%     derivative of a 3-D rotation in exponential coordinates." Journal
%     of Mathematical Imaging and Vision 51.3 (2015): 378-384.

nbatch = size(r,4);
if nargin<2
    % Forwards

    % Vectors with zero length are a special case - they just have an
    % identity rotation matrix
    idmask = squeeze(all(r==0,3));

    R = zeros(1,3,3,nbatch,'single');

    % Fill in the identity cases
    R(1,1,1,idmask)=1;
    R(1,2,2,idmask)=1;
    R(1,3,3,idmask)=1;

    theta = sqrt(sum(r(:,:,:,~idmask).^2,3)); % 1 x 1 x 1 x non-zero nbatch rotation angles
    k1 = r(:,:,1,~idmask)./theta; % rotation axis unit vectors
    k2 = r(:,:,2,~idmask)./theta; % rotation axis unit vectors
    k3 = r(:,:,3,~idmask)./theta; % rotation axis unit vectors

    % Fill in the non-identity cases using the following formula (see [1]):
    % K = [0 -k(3) k(2); k(3) 0 -k(1); -k(2) k(1) 0];
    % R = eye(3) + sin(theta).*K + (1-cos(theta)).*K*K;

    R(1,1,1,~idmask) = (cos(theta) - 1).*k2.^2 + (cos(theta) - 1).*k3.^2 + 1;
    R(1,1,2,~idmask) =  -k3.*sin(theta) - k1.*k2.*(cos(theta) - 1);
    R(1,1,3,~idmask) = k2.*sin(theta) - k1.*k3.*(cos(theta) - 1);
    R(1,2,1,~idmask) = k3.*sin(theta) - k1.*k2.*(cos(theta) - 1);
    R(1,2,2,~idmask) = (cos(theta) - 1).*k1.^2 + (cos(theta) - 1).*k3.^2 + 1;
    R(1,2,3,~idmask) = -k1.*sin(theta) - k2.*k3.*(cos(theta) - 1);
    R(1,3,1,~idmask) = -k2.*sin(theta) - k1.*k3.*(cos(theta) - 1);
    R(1,3,2,~idmask) = k1.*sin(theta) - k2.*k3.*(cos(theta) - 1);
    R(1,3,3,~idmask) = (cos(theta) - 1).*k1.^2 + (cos(theta) - 1).*k2.^2 + 1;
else
    % Backwards

	idmask = squeeze(all(r==0,3));

    % Handle identity case
    dRdr1 = zeros(size(dCdR));
    dRdr1(1,2,3,idmask)=-1;
    dRdr1(1,3,2,idmask)=1;
    dRdr2 = zeros(size(dCdR));
    dRdr2(1,1,3,idmask)=1;
    dRdr2(1,3,1,idmask)=-1;
    dRdr3 = zeros(size(dCdR));
    dRdr3(1,1,2,idmask)=-1;
    dRdr3(1,2,1,idmask)=1;

    % Call forwards version to get R
    [ R ] = vl_nnr2R( r );

    % Subselect and precompute some values
    R11 = R(1,1,1,~idmask); R12 = R(1,1,2,~idmask); R13 = R(1,1,3,~idmask);
    R21 = R(1,2,1,~idmask); R22 = R(1,2,2,~idmask); R23 = R(1,2,3,~idmask);
    R31 = R(1,3,1,~idmask); R32 = R(1,3,2,~idmask); R33 = R(1,3,3,~idmask);
    r1 = r(1,1,1,~idmask); r2 = r(1,1,2,~idmask); r3 = r(1,1,3,~idmask);
    sqnorm = (r1.^2 + r2.^2 + r3.^2);

    % For all other cases, find 3x3 derivatives of R with respect to each
    % element of r
    dRdr1(1,1,1,~idmask) = (R31.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R21.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,1,2,~idmask) = (R32.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R22.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,1,3,~idmask) = (R33.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R23.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,2,1,~idmask) = (R11.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R31.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,2,2,~idmask) = (R12.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R32.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,2,3,~idmask) = (R13.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R33.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,3,1,~idmask) = (R21.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R11.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;
    dRdr1(1,3,2,~idmask) = (R22.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R12.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;
    dRdr1(1,3,3,~idmask) = (R23.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R13.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;

    dRdr2(1,1,1,~idmask) = (R31.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R21.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,1,2,~idmask) = (R32.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R22.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,1,3,~idmask) = (R33.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R23.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,1,~idmask) = (R11.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R31.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,2,~idmask) = (R12.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R32.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,3,~idmask) = (R13.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R33.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,3,1,~idmask) = (R21.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R11.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;
    dRdr2(1,3,2,~idmask) = (R22.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R12.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;
    dRdr2(1,3,3,~idmask) = (R23.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R13.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;

    dRdr3(1,1,1,~idmask) = (R31.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R21.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
    dRdr3(1,1,2,~idmask) = (R32.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R22.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
    dRdr3(1,1,3,~idmask) = (R33.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R23.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
	dRdr3(1,2,1,~idmask) = (R11.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R31.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,2,2,~idmask) = (R12.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R32.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,2,3,~idmask) = (R13.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R33.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,1,~idmask) = (R21.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R11.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,2,~idmask) = (R22.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R12.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,3,~idmask) = (R23.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R13.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;

    dCdr(1,1,1,:) = sum(sum(dCdR.*dRdr1,2),3);
    dCdr(1,1,2,:) = sum(sum(dCdR.*dRdr2,2),3);
    dCdr(1,1,3,:) = sum(sum(dCdR.*dRdr3,2),3);

    R = dCdr;
end

end


function blend_test13 ( )

%*****************************************************************************80
%
%% BLEND_TEST13 tests BLEND_123.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2008
%
%  Author:
%
%    John Burkardt
%
  n1 = 3;
  n2 = 5;
  n3 = 4;

  d(1:n1,1:n2,1:n3) = 0.0;

  for i = 1 : n1
    for j = 1 : n2
      for k = 1 : n3
        if ( i == 1 | i == n1 | j == 1 | j == n2 | k == 1 | k == n3 )
          d(i,j,k) = ( i + j + k );
        end
      end
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLEND_TEST13\n' );
  fprintf ( 1, '  BLEND_123 blends face values into a table.\n' );
  fprintf ( 1, '\n' );

  r8block_print ( n1, n2, n3, d, '  Initial data array' );

  for i = 1 : n1

    r = ( i - 1 ) / ( n1 - 1 );

    for j = 1 : n2

      s = ( j - 1 ) / ( n2 - 1 );

      for k = 1 : n3

        t = ( k - 1 ) / ( n3 - 1 );

        if ( i == 1 | i == n1 | j == 1 | j == n2 | k == 1 | k == n3 )
          continue
        end

        d(i,j,k) = blend_123 ( r, s, t, ...
          d(1,1,1), d(1,1,n3), d(1,n2,1), d(1,n2,n3), ...
          d(n1,1,1), d(n1,1,n3), d(n1,n2,1), d(n1,n2,n3), ...
          d(i,1,1), d(i,1,n3), d(i,n2,1), d(i,n2,n3), ...
          d(1,j,1), d(1,j,n3), d(n1,j,1), d(n1,j,n3), ...
          d(1,1,k), d(1,n2,k), d(n1,1,k), d(n1,n2,k), ...
          d(1,j,k), d(n1,j,k), d(i,1,k), d(i,n2,k), d(i,j,1), d(i,j,n3) )

      end

    end

  end

  r8block_print ( n1, n2, n3, d, '  Interpolated data array' );

  return
end


function geometry_test0236 ( )

%*****************************************************************************80
%
%% TEST0236 tests DODEC_SIZE_3D, DODEC_SHAPE_3D, SHAPE_PRINT_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0236\n' );
  fprintf ( 1, '  For the dodecahedron,\n' );
  fprintf ( 1, '  DODEC_SIZE_3D returns dimension information;\n' );
  fprintf ( 1, '  DODEC_SHAPE_3D returns face and order information.\n' );
  fprintf ( 1, '  SHAPE_PRINT_3D prints this information.\n' );

  [ point_num, edge_num, face_num, face_order_max ] = dodec_size_3d ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of points =   %d\n', point_num );
  fprintf ( 1, '  Number of edges =    %d\n', edge_num );
  fprintf ( 1, '  Number of faces =    %d\n', face_num );
  fprintf ( 1, '  Maximum face order = %d\n', face_order_max );

  [ point_coord, face_order, face_point ] = dodec_shape_3d ( point_num, ...
    face_num, face_order_max );

  shape_print_3d ( point_num, face_num, face_order_max, ...
    point_coord, face_order, face_point );

  return
end


function iszscored = BF_iszscored(x)
% BF_iszscored  Crude check for whether a data vector is z-scored.
%
% (~eps-close to being) z-scored.
% Used for displaying warning messages for functions that require z-scored inputs.
%
%---INPUT:
% x, the input time series (or any vector)
%
%---OUTPUT:
% iszscored, a logical with the verdict.

% ------------------------------------------------------------------------------
% 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/>.
% ------------------------------------------------------------------------------

% Give it a bit of numerical lee-way... Down in the 2e-14 region:
numericThreshold = 100*eps;

iszscored = ((abs(mean(x)) < numericThreshold) && (abs(std(x)-1) < numericThreshold));

end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcMesh(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

figure;
mesh(double(handles.BMatrix));
colormap(handles.V.Color_map);

end


function [ n_data, alpha, beta, x, fx ] = extreme_values_cdf_values ( n_data )

%*****************************************************************************80
%
%% EXTREME_VALUES_CDF_VALUES returns some values of the Extreme Values CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = ExtremeValuesDistribution [ alpha, beta ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real ALPHA, the first parameter of the distribution.
%
%    Output, real BETA, the second parameter of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  alpha_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01 ];

  beta_vec = [ ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01 ];

  fx_vec = [ ...
     0.3678794411714423E+00, ...
     0.8734230184931166E+00, ...
     0.9818510730616665E+00, ...
     0.9975243173927525E+00, ...
     0.5452392118926051E+00, ...
     0.4884435800065159E+00, ...
     0.4589560693076638E+00, ...
     0.4409910259429826E+00, ...
     0.5452392118926051E+00, ...
     0.3678794411714423E+00, ...
     0.1922956455479649E+00, ...
     0.6598803584531254E-01 ];

  x_vec = [ ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    alpha = 0.0;
    beta = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    alpha = alpha_vec(n_data);
    beta = beta_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function chebcoeffs = legcoeffs2chebcoeffs(legcoeffs)
%CHEB2LEG  Convert Legendre coefficients to Chebyshev coefficients.
%   C_CHEB = LEG2CHEB(C_LEG) converts the vector C_LEG of Legendre coefficients
%   to a vector C_CHEB of Chebyshev coefficients such that
%       C_CHEB(1)*T0 + ... + C_CHEB(N)*T{N-1} = ...
%           C_LEG(N)*P0 + ... + C_LEG(1)*P{N-1},
%   where P{k} is the degree k Legendre polynomial normalized so that max(|P{k}|
%   = 1.

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

% This command is a wrapper for leg2cheb.
chebcoeffs = leg2cheb(legcoeffs);

end


function [C,P]=knn(d, Cp, K)

%KNN K-Nearest Neighbor classifier using an arbitrary distance matrix
%
%  [C,P]=knn(d, Cp, [K])
%
%  Input and output arguments ([]'s are optional):
%   d     (matrix) of size NxP: This is a precalculated dissimilarity (distance matrix).
%           P is the number of prototype vectors and N is the number of data vectors
%           That is, d(i,j) is the distance between data item i and prototype j.
%   Cp    (vector) of size Px1 that contains integer class labels. Cp(j) is the class of
%            jth prototype.
%   [K]   (scalar) the maximum K in K-NN classifier, default is 1
%   C     (matrix) of size NxK: integers indicating the class
%           decision for data items according to the K-NN rule for each K.
%           C(i,K) is the classification for data item i using the K-NN rule
%   P     (matrix) of size NxkxK: the relative amount of prototypes of
%           each class among the K closest prototypes for each classifiee.
%           That is, P(i,j,K) is the relative amount of prototypes of class j
%           among K nearest prototypes for data item i.
%
% If there is a tie between representatives of two or more classes
% among the K closest neighbors to the classifiee, the class i selected randomly
% among these candidates.
%
% IMPORTANT  If K>1 this function uses 'sort' which is considerably slower than
%            'max' which is used for K=1. If K>1 the knn always calculates
%            results for all K-NN models from 1-NN up to K-NN.
%
% EXAMPLE 1
%
% sP;                           % a SOM Toolbox data struct containing labeled prototype vectors
% [Cp,label]=som_label2num(sP); % get integer class labels for prototype vectors
% sD;                           % a SOM Toolbox data struct containing vectors to be classified
% d=som_eucdist2(sD,sP);        % calculate euclidean distance matrix
% class=knn(d,Cp,10);           % classify using 1,2,...,10-rules
% class(:,5);                   % includes results for 5NN
% label(class(:,5))             % original class labels for 5NN
%
% EXAMPLE 2 (leave-one-out-crossvalidate KNN for selection of proper K)
%
% P;                          % a data matrix of prototype vectors (rows)
% Cp;                         % column vector of integer class labels for vectors in P
% d=som_eucdist2(P,P);        % calculate euclidean distance matrix PxP
% d(eye(size(d))==1)=NaN;     % set self-dissimilarity to NaN:
%                             % this drops the prototype itself away from its neighborhood
%                             % leave-one-out-crossvalidation (LOOCV)
% class=knn(d,Cp,size(P,1));  % classify using all possible K
%                             % calculate and plot LOOC-validated errors for all K
% failratep = ...
%  100*sum((class~=repmat(Cp,1,size(P,1))))./size(P,1); plot(1:size(P,1),failratep)

% See also SOM_LABEL2NUM, SOM_EUCDIST2, PDIST.
%
% Contributed to SOM Toolbox 2.0, October 29th, 2000 by Johan Himberg
% Copyright (c) by Johan Himberg
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta Johan 291000

%% Init %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Check K
if nargin<3 || isempty(K),
  K=1;
end

if ~vis_valuetype(K,{'1x1'})
  error('Value for K must be a scalar');
end

% Check that dist is a matrix
if ~vis_valuetype(d,{'nxm'}),
  error('Distance matrix not valid.')
end

[N_data N_proto]=size(d);

% Check class label vector: must be numerical and of integers
if ~vis_valuetype(Cp,{[N_proto 1]});
  error(['Class vector is invalid: has to be a N-of-data_rows x 1' ...
	 ' vector of integers']);
elseif sum(fix(Cp)-Cp)~=0
  error('Class labels in vector ''Cp'' must be integers.');
end

if size(d,2) ~= length(Cp),
  error('Distance matrix and prototype class vector dimensions do not match.');
end

% Check if the classes are given as labels (no class input arg.)
% if they are take them from prototype struct

% Find all class labels
ClassIndex=unique(Cp);
N_class=length(ClassIndex); % number of different classes

%%%% Classification %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if K==1,   % sort distances only if K>1

  % 1NN
  % Select the closest prototype
  [~,proto_index]=min(d,[],2);
  C=Cp(proto_index);

else

  % Sort the prototypes for each classifiee according to distance
  [~, proto_index]=sort(d);

  %% Select up to K closest prototypes
  proto_index=proto_index(1:K,:);
  knn_class=Cp(proto_index);
  for i=1:N_class,
    classcounter(:,:,i)=cumsum(knn_class==ClassIndex(i));
  end

  %% Vote between classes of K neighbors
  [winner,vote_index]=max(classcounter,[],3);

  %%% Handle ties

  % Set index to classes that got as much votes as winner

  equal_to_winner=(repmat(winner,[1 1 N_class])==classcounter);

  % set index to ties
  [tie_indexi,tie_indexj]=find(sum(equal_to_winner,3)>1); % drop the winner from counter

  % Go through tie cases and reset vote_index randomly to one
  % of them

  for i=1:length(tie_indexi),
    tie_class_index=find(squeeze(equal_to_winner(tie_indexi(i),tie_indexj(i),:)));
    fortuna=randperm(length(tie_class_index));
    vote_index(tie_indexi(i),tie_indexj(i))=tie_class_index(fortuna(1));
  end

  C=ClassIndex(vote_index)';
end

%% Build output %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Relative amount of classes in K neighbors for each classifiee

if K==1,
  P=zeros(N_data,N_class);
  if nargout>1,
    for i=1:N_data,
      P(i,ClassIndex==C(i))=1;
    end
  end
else
  P=shiftdim(classcounter,1)./repmat(shiftdim(1:K,-1), [N_data N_class 1]);
end


function fx2 = p08_fx2 ( x )

%*****************************************************************************80
%
%% P08_FX2 evaluates the second derivative of the function for problem 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX2, the second derivative of the function at X.
%
  fx2 = - cos ( x );

  return
end


function f = sqrt(f, pref)
%SQRT   Square root of a CHEBFUN.
%   SQRT(F) returns the square root of a CHEBFUN F.
%
% See also POWER.

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

% Trivial case: (f is empty)
if ( isempty(f) )
    return
end

if ( nargin < 2 )
    pref = chebfunpref();
end

% Simply call POWER()
f = power(f, 0.5, pref);

end


function a = r8mat_geinverse ( a, n, pivot )

%*****************************************************************************80
%
%% R8MAT_GEINVERSE computes the inverse of a matrix factored by R8MAT_GEFA.
%
%  Discussion:
%
%    R8MAT_GEINVERSE is a modified version of the LINPACK routine DGEDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete 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/output, real A(N,N).
%    On input, the factor information computed by R8MAT_GEFA.
%    On output, the inverse matrix.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer PIVOT(N), the pivot vector from R8MAT_GEFA.
%

%
%  Compute Inverse(U).
%
  for k = 1 : n

    a(k,k) = 1.0 / a(k,k);

    a(1:k-1,k) = - a(1:k-1,k) * a(k,k);

    for j = k + 1 : n

      temp = a(k,j);
      a(k,j) = 0.0;

      a(1:k,j) = a(1:k,j) + temp * a(1:k,k);

    end

  end
%
%  Form Inverse(U) * Inverse(L).
%
  for k = n - 1 : -1 : 1

    work(k+1:n) = a(k+1:n,k);
    a(k+1:n,k) = 0.0;

    for j = k + 1 : n
      a(1:n,k) = a(1:n,k) + work(j) * a(1:n,j);
    end

    if ( pivot(k) ~= k )

      for i = 1 : n
        temp = a(i,k);
        a(i,k) = a(i,pivot(k));
        a(i,pivot(k)) = temp;
      end

    end

  end

  return
end


%% Clean the slate
% The demo here in the main.html document can be run directly from main.m
% In this section the memory is cleared, some parameters are set, and the
% original images are loaded and displayed.
close all
clear
clc

% Setting up parameters and filenames
params.VERBOSE          = 1;                    %Get more plots out (True or False)
params.PLANE            = 3;                    %Which plane of the RGB is of interest to us for opperations on a single plane
params.BLACK_BACKGROUND = 1;                    %Images have a black background (True or False)
params.map                      = copper(256);  %Set colormap to something that is similar to wood
params.number_angular_divisions = 2^7;          %Powers of two are faster FFT
params.number_radial_divisions  = 40;           %Arbitrary constant
base_filename = 'dowel01.jpg';                  %Filename of non-moving image
move_filename = 'dowel02.jpg';                  %Filename of image that will move to the base image

base_image = imread(base_filename);    %reads in the image
move_image = imread(move_filename);    %reads in the image

% Data visualization.
subplot(1,2,1)
subimage(base_image);
title('Base image')
subplot(1,2,2)
subimage(move_image);
title('Image to move')
set (gcf, 'color', 'w')

%% Correct the X and Y displacements
% The two images are displaced from each other in the X and Y direcions.
% First the X and Y direction will be corrected by lining up the centroids
% of the two images.
%
% Method: First mask the image from background then find
% centroid of the image. Finally center the image based
% on its centroid.

base_plane_of_interest              = base_image(:,:,params.PLANE);
%Must use a single layer grayscale for most operations.  Blue plane is best
%contrast for these images
base_bw_plane_of_interest           = im2bw(base_plane_of_interest, graythresh(base_plane_of_interest));
%Turn to binary based on threshold gotten from graythresh
base_plane_of_interest_segmented    = bwmorph(base_bw_plane_of_interest, 'open');
%morphology
base_binary_mask                    = imfill (base_plane_of_interest_segmented, 'holes');
%fill it in

% Get some data about the new region
base_properties                     = regionprops(real(base_binary_mask),'all');
base_centroid_row                   = round(base_properties.Centroid(2));
base_centroid_col                   = round(base_properties.Centroid(1));

% Place a dot at the centroid, one for each layer
base_image(base_centroid_row, base_centroid_col, 1) = 255;
base_image(base_centroid_row, base_centroid_col, 2) = 255;
base_image(base_centroid_row, base_centroid_col, 3) = 255;

% Nice to have image be 0dd number of pixels in each diension
base_image       = make_odd_by_odd(base_image);
base_binary_mask = make_odd_by_odd(base_binary_mask);
% Grab the new size
[base_num_rows, base_num_cols, base_num_layers] = size(base_image);

% Where is the center of the image?
base_goal_row   = (base_num_rows - 1) / 2 + 1;
base_goal_col   = (base_num_cols - 1) / 2 + 1;

% how much do I need to move to center this?
base_delta_rows = base_goal_row - base_centroid_row;
base_delta_cols = base_goal_col - base_centroid_col;

%shift the images to be centered
base_image       = circshift(base_image      , [base_delta_rows, base_delta_cols]);
base_binary_mask = circshift(base_binary_mask, [base_delta_rows, base_delta_cols]);

% Same thing for the second image.  In production code this would be in
% a function, but this script was made for seminar presentation where all
% the code should be visible.
% Begin repeated code -------------------------------------
move_plane_of_interest              = move_image(:,:,params.PLANE);
move_bw_plane_of_interest           = im2bw(move_plane_of_interest, graythresh(move_plane_of_interest));
move_plane_of_interest_segmented    = bwmorph(move_bw_plane_of_interest, 'open');
move_binary_mask                    = imfill (move_plane_of_interest_segmented, 'holes');
move_properties                     = regionprops(real(move_binary_mask),'all');
move_centroid_row                   = round(move_properties.Centroid(2));
move_centroid_col                   = round(move_properties.Centroid(1));
move_image(move_centroid_row, move_centroid_col, 1) = 255;
move_image(move_centroid_row, move_centroid_col, 2) = 255;
move_image(move_centroid_row, move_centroid_col, 3) = 255;
move_image                          = make_odd_by_odd(move_image);
move_binary_mask                    = make_odd_by_odd(move_binary_mask);
[move_num_rows, move_num_cols, move_num_layers]     = size(move_image);
move_goal_row                       = (move_num_rows - 1) / 2 + 1;
move_goal_col                       = (move_num_cols - 1) / 2 + 1;
move_delta_rows                     = move_goal_row - move_centroid_row;
move_delta_cols                     = move_goal_col - move_centroid_col;
move_image                          = circshift(move_image      , [move_delta_rows, move_delta_cols]);
move_binary_mask                    = circshift(move_binary_mask, [move_delta_rows, move_delta_cols]);
% End of repeated code -------------------------------------

% Data visualization
subplot(1,2,1)
subimage(base_image);
title('Centered base image')
subplot(1,2,2)
subimage(move_image);
title('Centered moveable image')
set (gcf, 'color', 'w')

%%

% Data visualization
subplot(1,2,1)
subimage(base_binary_mask);
title('Centered base binary mask')
subplot(1,2,2)
subimage(move_binary_mask);
title('Centered movable binary mask')
set (gcf, 'color', 'w')

%% Convert the data from X-Y coordinates to Theta-Radius coordinates
% This section transforms the image such that each set of pixels that radiate
% out at a given angle from the centroid become a vertical line in a new image.
% These images can then be 'slid' left and right across each other to
% find the highest correlation between the images.

[base_radii, base_theta, base_intensity] = image2angularintesity(base_binary_mask, rgb2gray(base_image), params);
set (gcf, 'color', 'w')

%%
[move_radii, move_theta, move_intensity] = image2angularintesity(move_binary_mask, rgb2gray(move_image), params);
set (gcf, 'color', 'w')

%% Find the angular displacement by using frequency domain mathematics
% The Fast Fourier Tranform has some nice properties that allow the
% angle of highest correlation be found quickly and cleanly.
% The three dimensional plot of correlation as a function of
% angle and radius can be used to figure out which angular shift
% yields the highest correlation between the two images.
%
% Looking at the graph, it can be seen that most of the radius
% have reached a consenus as to which angular shift should be used, however
% some of the angles of smaller radius tend to disagree.  Because the
% smaller radii have less pixels, their votes are less informed, so must be
% given less weight in the final decision as to the proper angular shift.

BI = fft(base_intensity);  %Base Intensity FFT
MI = fft(move_intensity);  %Move Intensity FFT
CI = conj(BI) .* (MI);     %Correlation Intensity FFT
ci = real(ifft(CI));       %Correlation out of FFT space
nci = ci;                  %normalized correlation being built
nci = nci - repmat(mean(nci), size(nci,1), 1); % normalized correlation removes the mean value

%%
h = surf(move_radii(:,2:end), move_theta(:,2:end), nci(:,2:end));
set (h, 'linestyle', 'none')
title('Correlation of the two images')
xlabel('radius')
ylabel('angle')
zlabel('correlation')
colormap(jet)
shading interp
set (gcf, 'color', 'w')

%%
% Find the peak correlation
[value_of_correlation, indice_of_peak_correlation_at_given_radius] = max(ci);
% Do voting to find out how much to shift by
index_of_peak_correlation = best_correlation_index(indice_of_peak_correlation_at_given_radius, params);

%% Correct the theta displacement
% The angle decided upon in the previous section is now used as the input
% argument to an IMROTATE command which will rotate the image as needed.

% mathmatical housekeeping
degrees_per_step = 360 / params.number_angular_divisions;
angle_to_shift = index_of_peak_correlation * degrees_per_step;

% Finally rotate the images (mask and the image itself)
rotated_image       = imrotate(move_image      , angle_to_shift,'bicubic','crop');
rotated_binary_mask = imrotate(move_binary_mask, angle_to_shift,'bicubic','crop');

% Data visualization
subplot(1,3,1)
subimage(base_image)
title('Base Image')

subplot(1,3,2)
subimage(rotated_image)
title(['Image rotated by ' num2str(angle_to_shift) ' degrees'])

subplot(1,3,3)
subimage(move_image)
title('Image to rotate')
set (gcf, 'color', 'w')

%%
%    Doug Hull <hull@mathworks.com>     5/20/2002
%    Copyright 1984-2001 The MathWorks, Inc.
%    This function is not supported by The MathWorks, Inc.
%    It is provided 'as is' without any guarantee of
%    accuracy or functionality.
% Copyright 2002 - 2009 The MathWorks, Inc.


function [f,g] = funObj_mfvi_exact(v, y, X, lambda)
% Primal objective function (Omega = inv(Sigma)) for mean-field
% f = 0.5*(logdet(V) - logdet(Sigma) - tr(V*SigmaInv) - (m-mu)'*SigmaInv*(m-mu)
%     + L) - sum_d fb(mbar_d, vbar_d)
% where mbar = X*m, vbar = diag(X*V*X')
%
% Written by Emtiyaz,
% Modified by Wu Lin
  [D L] = size(X);

  Omega = diag(lambda);
  %Extract mean, Cholesky and bias
  m = v(1:L);
  sigma = v(L+1:end);
  U = diag(sigma);

  % compute V
  V = diag(sigma.^2);

  % compute kl and its gradient
  kl = 0.5*(2*sum(log(diag(U))) + sum(log(lambda(2:end))) - trace(V*Omega) -m'*(Omega*m) + L);

  % contribution from the bound
  mbar = X*m;
  vbar = sum(X.*(V*X')',2); % diag(X*V*X') efficient
  [fb, gmb, gvb] = E_log_p('bernoulli_logit', y, mbar, vbar, []);
  fb = -fb;
  gm_lvb = X'*(-gmb);
  gV_lvb = X'*bsxfun(@times, -gvb, X); %efficient X'*diag(gllp/2)*X;

  % final
  f = kl - sum(fb);
  gm = - Omega*m - gm_lvb;
  gU = diag(1./diag(U)) - diag(U*Omega) - diag(2*U*gV_lvb);

  g=[gm(:); diag(gU)];

  % return
  g=-g;
  f=-f;


function [ C , coord , MN ] = ccv ( A , N , F )
% Generates a crystal consists of lattice sites and bases, Calculates
% connecting vectors from point to point, Finds the number of nearest
% neighbors of each atomic site.
%
% function [ C , coord , MN ] = ccv ( A , N , F )
%
% arguments: ( input )
%
%  A - ( class - double ) a ( 3 * 3 ) matrix that each row is a vector that
%  shows primary generator vectors of a lattice in three dimensions.
%
%  N - ( class - double ) a matrix with three possitive integers that shows
%  the number of atoms in each dimensions.
%
%  F - ( class - double ) a ( m * 3 ) matrix that show the bases
%  coordinates. ( m is the number of bases )
%
% arguments: ( output )
%
%  C - ( class - double ) A ( N ( 1 ) * N ( 2 ) * N ( 3 ) ) * ( N ( 1 ) *
%  N ( 2 ) * N ( 3 ) ) * 3 matrix connecting vector between each two atomic
%  sites
%
%  coord - ( class - double ) Coordination number ( Number of nearest
%  neighbors ) of each atomic site
%
%  MN - ( class - double ) Mean distance between the nearest neighbor of
%  each atomic site
%
% Example:
%  A = [ 2 3 3 ; 6 5 3 ; 2 5 3 ] ;
%  N = [ 4 3 5 ] ;
%  F = [ 0 0 0 ; 0 0 1 ; 0 -2 0 ] ;
%  [ CV , COORD , MN ] = ccv ( A , N , F )
%
% See also rcv.
%
% Copyright 2009
%
% Release Date: 2009-10-12

% check for simple errors

if nargin < 3
    F = [ 0 0 0 ] ;
end % end of if loop

if nargin < 2
    N = [ 3 3 3 ] ;
end % end of if loop

if nargin < 1
    A = [ 1 0 0 ; 0 1 0 ; 0 0 1 ] ;
end % end of if loop

if ( N ( 1 ) <= 0 ) || ( N ( 2 ) <= 0 ) || ( N ( 3 ) <= 0 ) % condition of negative numbers
    error ' Enterd demensions must be positive integers. ' % error message
end % end of if loop

V = dot ( A ( 1 , : ) , cross ( A ( 2 , : ) , A ( 3 , : ) ) ) ; % Primitive Cell Volume

if V == 0 % condition of same plane vectors
    error ' Vectors must not be in same plane. ' % error message
end % end of if loop

% end of error checking

n = ( N ( 1 ) * N ( 2 ) ) * N ( 3 ) ; % calculates the total number of atomic sites
xy = zeros ( n , 3 ) ; % Preallocating

for i = 0 : N ( 1 ) - 1 % i is the numerator of for loop
    for j = 0 : N ( 2 ) - 1 % j is the numerator of for loop
        for k = 0 : N ( 3 ) - 1 % k is the numerator of for loop
            xy ( ( ( N ( 1 ) * j ) + ( i + 1 ) ) + ( N ( 1 ) * N ( 2 ) ) * k , : ) = A ( 1 , : ) * i + A ( 2 , : ) * j + A( 3 , : ) * k ;
        end % end of for loop
    end % end of for loop
end % end of for loop

xyold = xy ;
f = size ( F ) ;
xyo = [ ] ;

for q = 1 : f ( 1 ) % q is the numerator of for loop
    for p = 1 : 3 % p is the numerator of for loop
        xy ( : , p ) = xyold ( : , p ) + F ( q , p ) ;
    end % end of for loop
    xyo = [ xyo ; xy ] ;
end % end of for loop

n = length ( xyo ) ;
xy = xyo ;

C = zeros ( n , n , 3 ) ; % Preallocating
D = zeros ( n , n , 3 ) ; % Preallocating
E = zeros ( n , 3 ) ; % Preallocating
coord = zeros ( 1 , n ) ; % Preallocating
MIN = zeros ( n-1 , n ); % Preallocating
k = 1 ; % Preallocating

for P = 1 : n % P is the numerator of for loop
    for j = 1 : 3 % j is the numerator of for loop
        C ( P , 1 : n , j ) = xy ( 1 : n , j ) - xy ( P , j ) ; % Connecting vector elements between to lattice atomic sites
        D ( P , 1 : n , j ) = ( xy ( 1 : n , j ) - xy ( P , j ) ) .^ 2 ;
    end % end of for loop
    E ( P , 1 : n ) = sqrt ( D ( P , 1 : n , 1 ) + D ( P , 1 : n , 2 ) + D ( P , 1 : n , 3 ) ) ;
end % end of for loop

for P = 1 : n % P is the numerator of for loop
    for i = 1 : n % i is the numerator of for loop
        if E ( P , i ) ~= 0
            MIN ( k ) = E ( P , i ) ; % Eliminates zero value elements
            k = k + 1 ;
        end % end of if loop
    end % end of for loop
end % end of for loop

MN = min ( MIN ) ; % Nearest neighbors distance

for P = 1 : n % P is the numerator of for loop
    t = 0 ; % Preallocating
    for i = 1 : n % i is the numerator of for loop
        if E ( P , i ) == MN ( P )
            t = t + 1 ; % Counts number of nearest neighbors
        end % end of if loop
    end % end of for loop
    coord ( P ) = t ;
end % end of for loop

% With special thanks to John D'Errico
% By Ali Mohammad Razeghi
% My Email ( am_razeghi@yahoo.com ) is also ready to get full detailed
% commentation.


function fcstr = directCollocation(obj, name, x, dx)
    % Return the SymFunction object of the direct collocation constraint
    % given the state (x) and derivatives (dx).
    %
    % Parameters:
    % name: the name suffix of the function @type char
    % x: the state SymVariable @type SymVariable
    % dx: the derivative of states @type SymVariable

    T  = [SymVariable('t0');SymVariable('tf')];
    Ts = T(2) - T(1);
    N = SymVariable('nNode');
    numState = length(x);
    nNode = obj.NumNode;
    switch obj.Options.CollocationScheme
        case 'HermiteSimpson'

            xn = SymVariable('xn',[numState,1]);
            dxn = SymVariable('dxn',[numState,1]);
            xm = SymVariable('xm',[numState,1]);
            dxm = SymVariable('dxm',[numState,1]);

            int_x = [xn - x - (2.*Ts./(N-1)).*(dxn + 4.*dxm + dx)./6;
                xm - (x+xn)./2 - 2.*Ts.*(dx-dxn)./(8*(N-1))];

            if isnan(obj.Options.ConstantTimeHorizon)
                fcstr = SymFunction(['hs_int_' name],int_x,{T,x,dx,xm,dxm,xn,dxn},{N});
            else
                fcstr = SymFunction(['hs_int_' name],int_x,{x,dx,xm,dxm,xn,dxn},{T,N});
            end

        case 'Trapezoidal'
            xn = SymVariable('xn',[numState,1]);
            dxn = SymVariable('dxn',[numState,1]);

            int_x = xn - x - (Ts./(N-1)).*(dxn + dx)./2;

            if isnan(obj.Options.ConstantTimeHorizon)
                fcstr = SymFunction(['tr_int_' name],int_x,{T,x,dx,xn,dxn},{N});
            else
                fcstr = SymFunction(['tr_int_' name],int_x,{x,dx,xn,dxn},{T,N});
            end

        case 'PseudoSpectral'
            t = sym('t');
            p = legendreP(nNode-1,t);
            dp = jacobian(p,t);

            roots = vpasolve(dp*(1-t)*(1+t)==0);

            D_LGL = zeros(nNode);
            for i=1:nNode
                for j=1:nNode
                    if i==j
                        if j== 1
                            D_LGL(i,j) = - (nNode-1)*(nNode)/4;
                        elseif j==nNode
                            D_LGL(i,j) = (nNode-1)*(nNode)/4;
                        else
                            D_LGL(i,j) = 0;
                        end
                    else
                        D_LGL(i,j) = subs(p,t,roots(i))/(subs(p,t,roots(j))*(roots(i) - roots(j)));
                    end
                end
            end

            KD_LGL = kron(D_LGL, eye(numState));

            xn = cell(1,nNode);
            dxn = cell(1,nNode);

            for i=1:nNode
                xn{i} = SymVariable(['x',num2str(i)],[numState,1]);
                dxn{i} = SymVariable(['dx',num2str(i)],[numState,1]);
            end

            X = transpose(flatten([xn{:}]));
            dX = transpose(flatten([dxn{:}]));
            int_x = dX.*(Ts./2) - KD_LGL*X;

            dep = [xn;dxn];
            if isnan(obj.Options.ConstantTimeHorizon)
                fcstr = SymFunction(['ps_int_' name '_' obj.Name],int_x,[{T},dep(:)']);
            else
                fcstr = SymFunction(['ps_int_' name '_' obj.Name],int_x,dep(:)',{T});
            end
    end

end


%% Tracking of rotating point using Kalman filter
%
% Tracking of rotating point.
% Rotation speed is constant.
% Both state and measurements vectors are 1D (a point angle),
% Measurement is the real point angle + gaussian noise.
% The real and the estimated points are connected with yellow line segment,
% the real and the measured points are connected with red line segment.
% (if Kalman filter works correctly,
% the yellow segment should be shorter than the red one).
%
% Pressing any key will reset the tracking with a different speed.
% Close the window to stop the program.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/kalman.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/kalman.py>
%

%%
% set up display window
img = zeros(500, 500, 3, 'uint8');
hFig = figure('KeyPressFcn',@(o,e)setappdata(o, 'flag',true), ...
    'Menubar','none', 'Name','Kalman Filter demo');
setappdata(hFig, 'flag',false);
hImg = imshow(img);

%%
% helper anonymous functions
calcPoint = @(center,R,angle) center + [cos(angle), -sin(angle)]*R;
drawCross = @(img,center,clr,d) cv.line(...
    cv.line(img, center-d, center+d, 'Color',clr, ...
        'Thickness',1, 'LineType','AA'), ...
    center+[d -d], center+[-d d], 'Color',clr, ...
        'Thickness',1, 'LineType','AA');

%%
% create and initialize Kalman filter
KF = cv.KalmanFilter(2, 1);
state = zeros(2,1);  % [phi; delta_phi]
processNoise = zeros(2,1);
measurement = zeros(1,1);

%%
% keep repeating until figure is closed
while ishghandle(hFig)
    setappdata(hFig, 'flag',false);

    % initialize KF
    state = randn(size(state))*0.1;
    KF.transitionMatrix = [1 1; 0 1];
    KF.measurementMatrix = eye(size(KF.measurementMatrix));
    KF.processNoiseCov = eye(size(KF.processNoiseCov))*1e-5;
    KF.measurementNoiseCov = eye(size(KF.measurementNoiseCov))*1e-1;
    KF.errorCovPost = eye(size(KF.errorCovPost));
    KF.statePost = randn(size(KF.statePost))*0.1;

    % main loop
    while ishghandle(hFig)
        center = [size(img,2) size(img,1)]/2;
        R = size(img,2)/3;
        stateAngle = state(1);
        statePt = calcPoint(center, R, stateAngle);

        prediction = KF.predict();
        predictAngle = prediction(1);
        predictPt = calcPoint(center, R, predictAngle);

        measurement = randn(size(measurement))*KF.measurementNoiseCov(1);

        % generate measurement
        measurement = measurement + KF.measurementMatrix*state;

        measAngle = measurement(1);
        measPt = calcPoint(center, R, measAngle);

        % plot points
        img(:) = 0;
        img = drawCross(img, statePt, [255 255 255], 3);
        img = drawCross(img, measPt, [255 0 0 0], 3);
        img = drawCross(img, predictPt, [0 255 0], 3);
        img = cv.line(img, statePt, measPt, 'Color',[255 0 0], ...
            'Thickness',3, 'LineType','AA');
        img = cv.line(img, predictPt, measPt, 'Color',[255 255 0], ...
            'Thickness',3, 'LineType','AA');

        if rand > 0.75
            KF.correct(measurement);
        end

        processNoise = randn(size(processNoise))*sqrt(KF.processNoiseCov(1,1));
        state = KF.transitionMatrix*state + processNoise;

        % update display
        set(hImg, 'CData',img);

        % break of inner loop on any key press
        flag = getappdata(hFig, 'flag');
        if isempty(flag)||flag, break; end
        pause(0.1)
    end
end


function logpdf = logmvtpdf(t,mu,f,Sigma);
% function logpdf = logmvtpdf(t,mu,f,Sigma);
% log pdf of multivariate t-student dist.
% t : D by N

[d,n] = size(t);

c = gammaln((d+f)*0.5) - (d*0.5)*log(f*pi) - gammaln(f*0.5) - 0.5*detln(Sigma);
diff = t - repmat(mu,1,n); % d by n
logpdf = c - (f+d)*0.5 * log(1 + sum(diff.*(inv(f*Sigma)*diff),1));

% Local Variables: ***
% mode: matlab ***
% End: ***


function [xResampleV,yResampleV,zResampleV] = ...
    getResampledGrid(resampResolutionV,xValsV,yValsV,zValsV,...
    originV,gridAlignMethod,varargin)
%getResampledGrid_v2
% ------------------------------------------------------------------------
% INPUTS
% resampResolutionV  : Output voxel spacing in cm [dx dy dz]
% xValsV             : x coordinates of voxel centers in original scan
% yValsV             : y coordinates of voxel centers in original scan
% zValsV             : z coordinates of voxel centers in original scan
%------------------------------------------------------------------------
%Ref: https://arxiv.org/pdf/1612.07003.pdf
%AI 12/01/22

if ~exist('method','var')
    gridAlignMethod = 'center';
end

% Set default perturbation offsets:
if nargin<7
    perturbX = 0;
    perturbY = 0;
    perturbZ = 0;
else
    perturbV = varargin{1};
    perturbX = perturbV(1);
    perturbY = perturbV(2);
    perturbZ = perturbV(3);
end

%% Define voxel spacing
dx = abs(median(diff(xValsV)));
dy = abs(median(diff(yValsV)));
dz = abs(median(diff(zValsV)));
origResolutionV = [dx dy dz];

%Correct to match DICOM convention
origResolutionV(3) = -origResolutionV(3);
resampResolutionV(3) = -resampResolutionV(3);

if length(resampResolutionV)==3 && ~isnan(resampResolutionV(3))
    resamp3DFlag = 1;
else
    %Resample in-plane
    resamp3DFlag = 0;
end

%% No. voxels
origSizeV = [length(xValsV) length(yValsV) length(zValsV)];
resampSizeV = ceil( origSizeV.*origResolutionV ./ resampResolutionV);

switch(gridAlignMethod)

    case 'center'

        %Get output grid origin
        % In world coordinates:
        resampOriginV = originV + (origResolutionV.*(origSizeV-1) - ...
            resampResolutionV.*(resampSizeV-1))/2;

        %In grid co-ords:
        %resampOriginV = 0.5* (origSizeV- 1 -
        %resampResolutionV.*(resampSizeV-1)/origResolutionV);

        %Generate output grid
        xResampleV = resampOriginV(1):resampResolutionV(1):...
            resampOriginV(1)+(resampSizeV(1)-1)*resampResolutionV(1);

        yResampleV = resampOriginV(2):resampResolutionV(2):...
            (resampOriginV(2)+(resampSizeV(2)-1)*resampResolutionV(2));

        if resamp3DFlag
            zResampleV = resampOriginV(3):resampResolutionV(3):...
                (resampOriginV(3)+(resampSizeV(3)-1)*resampResolutionV(3));
            zResampleV = flip(zResampleV);
        else
            zResampleV = zValsV;
        end

    otherwise
        error('Unsupported grid alignment method %s',gridAlignMethod)
end

end