function g = graf(matriks)

n=length(matriks);
theta = 2*pi./n;
angle = theta*(0:(n-1));
radius = n* ones(1,length(angle));
textRadius =  n*( ones(1,length(angle)) + 0.2);
x = radius .* cos(angle);
y = radius .* sin(angle);
textX = textRadius .* cos(angle);
textY =  textRadius .* sin(angle);
fHandle = figure;
figure(fHandle);
dummy = 1:1:n;
edges = combntns(dummy,2);

mat1 = zeros(n,n);
mat2 = zeros(n,n);
for i=1:1:n
    for j=1:1:n
        if matriks(i,j)==1
            mat1(i,j)=1;             % edge yang positif
        end
        if matriks(i,j)==-1
            mat2(i,j)=1;              % edge yang negatif
        end
    end
end

for m = 1:n
    h = rectangle('Position',[x(m)-0.5, y(m)-0.5, 1, 1], 'Curvature', [1,1], 'FaceColor','k');
end

grid off;
axis off;

for j=1:n
    text(textX(j), textY(j), int2str(j),'Color','black');
    nodeUpdated(j).output = [];
    indices = find(mat1(j,:));
    for k=1:length(indices)
        multiplicity(k) = matriks(j,indices(k));
    end

    for m=1:length(indices)
        nodeUpdated(j).output = [nodeUpdated(j).output repmat(indices(m),1,multiplicity(m))];
        line([x(j) x(indices(m))] , [y(j) y(indices(m))],'LineWidth',3);
    end
end

for j=1:n
    nodeUpdated(j).output = [];
    indices = find(mat2(j,:));
    for k=1:length(indices)
        multiplicity(k) = matriks(j,indices(k));
    end

    for m=1:length(indices)
        nodeUpdated(j).output = [nodeUpdated(j).output repmat(indices(m),1,multiplicity(m))];
        line([x(j) x(indices(m))] , [y(j) y(indices(m))],'Color','k','LineStyle',':','LineWidth',3);
    end
end


function [transformationMatrix] = ...
    optical_flow_linear_registration2d(moving, fixed, varargin)
% OPTICAL_FLOW_LINEAR_REGISTRATION2D Estimates a transformation matrix using optical flow
%
% INPUT ARGUMENTS
% moving                - Moving iamge
% fixed                 - Fixed image
%
% OPTIONAL INPUT ARGUMENTS
% 'transformationModel'     - Transformation model for estimating the
%                             displacement field
%                             'translation', 'affine' (default)
%
% 'multiModal'              - Set whether to perform multi-modal or
%                             uni-modal image registration
%                             false (default), true
%
% 'numberOfChannels'        - Number of channels to use in when computing
%                             the entropy (based on channel coding). This
%                             is only relevant if multiModal is set to
%                             true.
%                             Default value is 8
%
% OUTPUT ARGUMENTS
% transformationMatrix  - Estimated transformation matrix

% Copyright (c) 2012
% danne.forsberg@outlook.com
%
% 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/>.

%% Setup default parameters
% translation, affine
transformationModel = 'affine';

% multi-modal
multiModal = false;

% number of channels, only valid for multi-modal registration
numberOfChannels = 8;

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

% Initialize transformation matrix
transformationMatrix = eye(3);

[b_moving(:,:,1) b_moving(:,:,2)] = gradient(moving);
[b_fixed(:,:,1) b_fixed(:,:,2)] = gradient(fixed);

if multiModal
    b = b_fixed(:,:,2);
    b(:,:,2) = b_fixed(:,:,1);

    [delta_c mask] = estimate_delta_c(fixed,moving,numberOfChannels);
    delta_c(mask ~= 1) = 0;
    mask = repmat(mask,[1 1 2]);
    b(mask ~= 1) = 0;
else
    b = (b_moving(:,:,2) + b_fixed(:,:,2))/2;
    b(:,:,2) = (b_moving(:,:,1) + b_fixed(:,:,1))/2;

    delta_c = fixed - moving;
end

[G, h] = build_G_h_linear2d(b, delta_c, transformationModel);

% Solve the equation system
switch transformationModel
    case 'translation'
        d = G \ h;
        transformationMatrix(1:2,3) = d;
    case {'rigid','affine'}
        p = G \ h;
        transformationMatrix(1:2,1:3) = [1+p(1) p(2) p(5);...
                                         p(3) 1+p(4) p(6)];
end


function [mssim, ssim_map, mcs, cs_map] = ssim_index_new(img1, img2, K, win, max_val)

if (nargin < 2 || nargin > 5)
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

if (size(img1) ~= size(img2))
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

[M N] = size(img1);

if (nargin == 2)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   win = fspecial('gaussian', 11, 1.5);	%
   K(1) = 0.01;										% default settings
   K(2) = 0.03;										%
end

if (nargin == 3)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   win = fspecial('gaussian', 11, 1.5);
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 4 || nargin == 5)
   [H W] = size(win);
   if ((H*W) < 4 || (H > M) || (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

C1 = (K(1)*max_val)^2;
C2 = (K(2)*max_val)^2;
win = win/sum(sum(win));

mu1   = filter2(win, img1, 'valid');
mu2   = filter2(win, img2, 'valid');
mu1_sq = mu1.*mu1;
mu2_sq = mu2.*mu2;
mu1_mu2 = mu1.*mu2;
sigma1_sq = filter2(win, img1.*img1, 'valid') - mu1_sq;
sigma2_sq = filter2(win, img2.*img2, 'valid') - mu2_sq;
sigma12 = filter2(win, img1.*img2, 'valid') - mu1_mu2;

if (C1 > 0 & C2 > 0)
   ssim_map = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2));
   cs_map = (2*sigma12 + C2)./(sigma1_sq + sigma2_sq + C2);
else
   numerator1 = 2*mu1_mu2 + C1;
   numerator2 = 2*sigma12 + C2;
	denominator1 = mu1_sq + mu2_sq + C1;
   denominator2 = sigma1_sq + sigma2_sq + C2;

   ssim_map = ones(size(mu1));
   index = (denominator1.*denominator2 > 0);
   ssim_map(index) = (numerator1(index).*numerator2(index))./(denominator1(index).*denominator2(index));
   index = (denominator1 ~= 0) & (denominator2 == 0);
   ssim_map(index) = numerator1(index)./denominator1(index);

   cs_map = ones(size(mu1));
   index = denominator2 > 0;
   cs_map(index) = numerator2(index)./denominator2(index);
end

mssim = mean2(ssim_map);
mcs = mean2(cs_map);

return


function [Y, problem, S] = elliptope_SDP_complex(A, p, Y0)
% Solver for complex semidefinite programs (SDP's) with unit diagonal.
%
% function [Y, problem, S] = elliptope_SDP_complex(A)
% function [Y, problem, S] = elliptope_SDP_complex(A, p)
% function [Y, problem, S] = elliptope_SDP_complex(A, p, Y0)
%
% A is a Hermitian matrix of size n.
%
% This function uses a local optimization method in Manopt to solve the SDP
%
%   min_X trace(A*X) s.t. diag(X) = 1, X is complex, positive semidefinite.
%
% In practice, the Hermitian matrix X of size n is parameterized as
% X = Y*Y', where Y has size n x p. By default, p is taken large enough
% (that is, sqrt(n)) to ensure that there exists an optimal X whose rank is
% smaller than p. This ensures that the SDP is equivalent to the new
% problem in Y:
%
%   min_Y  trace(Y'*A*Y)  s.t.  diag(Y*Y') = 1, Y complex
%
% The constraints on Y require each row of Y to have unit norm, which is
% why Manopt is appropriate software to solve this problem. An optional
% initial guess can be specified via the input Y0.
%
% See the paper below for theory, specifically, for a proof that, for
% almost all A, second-order critical points of the problem in Y are
% globally optimal. In other words: there are no local traps in Y, despite
% non-convexity.
%
% Outputs:
%
%       Y: is the best point found (an nxp matrix with unit norm rows.)
%          To find X, form Y*Y' (or, more efficiently, study X through Y.)
%
%       problem: is the Manopt problem structure used to produce Y.
%
%       S: is a dual optimality certificate (a Hermitian matrix of size n,
%          sparse if A is sparse). The optimality gap (in the cost
%          function) is at most n*min(eig(S)), for both Y and X = Y*Y'.
%          Hence, if min(eig(S)) is close to zero, Y is close to globally
%          optimal. This can be computed via eigs(S, 1, 'SR').
%
% Paper: https://arxiv.org/abs/1606.04970
%
% @inproceedings{boumal2016bmapproach,
%   author  = {Boumal, N. and Voroninski, V. and Bandeira, A.S.},
%   title   = {The non-convex {B}urer-{M}onteiro approach works on smooth semidefinite programs},
%   booktitle={Neural Information Processing Systems (NIPS 2016)},
%   year    = {2016}
% }
%
% See also: maxcut elliptope_SDP

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Oct. 21, 2016
% Contributors:
% Change log:
%
%    Xiaowen Jiang Aug. 20, 2021
%       Added AD to compute the egrad and the ehess

    % If no inputs are provided, since this is an example file, generate
    % a random complex matrix. This is for illustration purposes only.
    if ~exist('A', 'var') || isempty(A)
        n = 100;
        A = randn(n) + 1i*randn(n);
        A = (A+A')/sqrt(2*n);
    end

    n = size(A, 1);
    assert(n >= 2, 'A must be at least 2x2.');
    assert(size(A, 2) == n, 'A must be square.');

    % Force A to be Hermitian
    A = (A+A')/2;

    % By default, pick a sufficiently large p (number of columns of Y).
    if ~exist('p', 'var') || isempty(p)
        p = floor(sqrt(n)+1);
    end

    assert(p >= 1 && p == round(p), 'p must be an integer >= 1.');

    % Pick the manifold of complex n-by-p matrices with unit norm rows.
    manifold = obliquecomplexfactory(p, n, true);

    problem.M = manifold;

    % These three, quick commented lines of code are sufficient to define
    % the cost function and its derivatives. This is good code to write
    % when prototyping. Below, a more advanced use of Manopt is shown,
    % where the redundant computation A*Y is avoided between the gradient
    % and the cost evaluation.
    % % problem.cost  = @(Y) .5*sum(sum(real((A*Y).*conj(Y))));
    % % problem.egrad = @(Y) A*Y;
    % % problem.ehess = @(Y, Ydot) A*Ydot;

    % Products with A dominate the cost, hence we store the result.
    % This allows to share the results among cost, grad and hess.
    % This is completely optional.
    function store = prepare(Y, store)
        if ~isfield(store, 'AY')
            AY = A*Y;
            store.AY = AY;
            store.diagAYYt = sum(real(AY .* conj(Y)), 2);
        end
    end

    % Define the cost function to be /minimized/.
    problem.cost = @cost;
    function [f, store] = cost(Y, store)
        store = prepare(Y, store);
        f = .5*sum(store.diagAYYt);
    end

    % Define the Riemannian gradient.
    problem.grad = @grad;
    function [G, store] = grad(Y, store)
        store = prepare(Y, store);
        G = store.AY - bsxfun(@times, Y, store.diagAYYt);
    end

    % If you want to, you can specify the Riemannian Hessian as well.
    problem.hess = @hess;
    function [H, store] = hess(Y, Ydot, store)
        store = prepare(Y, store);
        SYdot = A*Ydot - bsxfun(@times, Ydot, store.diagAYYt);
        H = manifold.proj(Y, SYdot);
    end

    % An alternative way to compute the egrad and the ehess is to use
    % automatic differentiation provided in the deep learning toolbox
    % (slower). AD does not support complex numbers if the Matlab version
    % is R2021a or earlier. The cost function should be defined differently
    % In this case. See complex_example_AD.m and manoptADhelp.m for more
    % information.
    % problem.cost = @cost_AD;
    %    function f = cost_AD(Y)
    %        AY = cprod(A, Y);
    %        diagAYYt = csum(creal(cdottimes(AY, cconj(Y))), 2);
    %        f = .5*csum(diagAYYt);
    %    end
    % Call manoptAD to automatically obtain egrad and ehess:
    % problem = manoptAD(problem);

    % If the version of Matlab installed is R2021b or later, specify the
    % cost function in the normal way and call manoptAD.
    % problem.cost = @cost_AD;
    %    function f = cost_AD(Y)
    %        AY = A*Y;
    %        diagAYYt = sum(real(AY .* conj(Y)), 2);
    %        f = .5*sum(diagAYYt);
    %    end
    % problem = manoptAD(problem);

    % If no initial guess is available, tell Manopt to use a random one.
    if ~exist('Y0', 'var') || isempty(Y0)
        Y0 = [];
    end

    % Call your favorite solver.
    opts = struct();
    opts.verbosity = 0;      % Set to 0 for no output, 2 for normal output
    opts.maxinner = 500;     % maximum Hessian calls per iteration
    opts.tolgradnorm = 1e-6; % tolerance on gradient norm
    Y = trustregions(problem, Y0, opts);

    % If required, produce an optimality certificate.
    if nargout >= 3
        S = A - spdiags(sum(real((A*Y).*conj(Y)), 2), 0, n, n);
    end

end


function idct = myidct1(vector)
%Determines the inverse discrete cosine tranform of a vector using an fft.
%
%Given a row vector, returns the inverse discrete cosine transformation of
%the vector. Since the DCT-I is its own inverse, this simply entails
%renormalizing.

    idct = (2/(numel(vector)-1))*mydct1(vector);

end


function val = peval(x,coef,deg)
% function val = peval(x,coef,deg)
%
% DESCRIPTION
%   Evaluate a polynomial at x.
%
%   Note: The SUBS function should be used for polynomial evaluations.
%   The PEVAL function has no error checking for speed and it assumes
%   that the polynomial is a row or column vector.  It also assumes that
%   the rows of x are ordered to correspond to the columns in the degree
%   matrix deg.
%
% INPUTS
%   x: nvars-by-npts matrix each column of which specifies a point
%        at which to evaluate the polynomial.
%   coef: nterms-by-lp coefficient matrix where lp is the length
%        of the polynomial
%   deg: nterms-by-nvars degree matrix
%
% OUTPUTS
%   val: lp-by-npts matrix where each column specifies the value of
%        the polynomial evaluated at the corresponding column of x.
%
% SYNTAX
%   val = peval(x,coef,deg)

% 6/15/06  PJS  Initial Coding

% Compute dimensions
[nterms,nvars]=size(deg);
npts = size(x,2);
lp = size(coef,2);
coef = full(coef);
deg = full(deg);

% Compute monomials
xs = shiftdim(x,-1);            % xs is 1-by-nvars-by-npts
xrep = repmat(xs,[nterms 1 1]); % xrep is nterms-by-nvars-by-npts
drep = repmat(deg,[1 1 npts]);  % drep is nterms-by-nvars-by-npts

% npow = xrep;
% idx = find(drep~=1);
% npow(idx) = xrep(idx).^drep(idx);

npow = xrep.^drep;
nmonom = prod(npow,2);          % nmonom is nterms-by-1-by-npts

% Evalute polynomial--mulitply by coefs and sum monomials
nmonomrep = repmat(nmonom,[1 lp 1]); % nmonomrep is nterms-by-lp-by-npts
coefrep = repmat(coef,[1 1 npts]);   % coefrep is nterms-by-lp-by-npts
val = sum(coefrep.*nmonomrep,1);     % val is 1-by-lp-by-npts

% Reshape val from 1-by-lp-by-npts to lp-by-npts
if npts==1
    val = val(:);
elseif lp==1
    val = val(:)';
else
    val = squeeze(val);
end


function [ n_data, n, x, fx ] = p_polynomial_values ( n_data )

%*****************************************************************************80
%
%% P_POLYNOMIAL_VALUES returns values of the Legendre polynomials P(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  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, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 22;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.2500000000000000E+00, ...
     -0.4062500000000000E+00, ...
     -0.3359375000000000E+00, ...
      0.1577148437500000E+00, ...
      0.3397216796875000E+00, ...
      0.2427673339843750E-01, ...
     -0.2799186706542969E+00, ...
     -0.1524540185928345E+00, ...
      0.1768244206905365E+00, ...
      0.2212002165615559E+00, ...
      0.0000000000000000E+00, ...
     -0.1475000000000000E+00, ...
     -0.2800000000000000E+00, ...
     -0.3825000000000000E+00, ...
     -0.4400000000000000E+00, ...
     -0.4375000000000000E+00, ...
     -0.3600000000000000E+00, ...
     -0.1925000000000000E+00, ...
      0.8000000000000000E-01, ...
      0.4725000000000000E+00, ...
      0.1000000000000000E+01 ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3 ];

  x_vec = [ ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.00E+00, ...
     0.10E+00, ...
     0.20E+00, ...
     0.30E+00, ...
     0.40E+00, ...
     0.50E+00, ...
     0.60E+00, ...
     0.70E+00, ...
     0.80E+00, ...
     0.90E+00, ...
     1.00E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    x = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [resid] = residual(Xw,Xd,aXw,aXd,sXw,sXd,Y,D)
%
%  'w' refers to time series on wet days
%  'd' refers to time series on dry days
%   X refers to observed time series
%   aX refers to average values of periodic time series (calculated by Fourier analysis)
%   sX refers to standard deviation of periodic time series (Fourier analysis)
%
residw=[];
residd=[];
%
for i = 1:Y
   resw=zeros(1,D);
   resd=resw;
   jw=find(Xw(i,:)~=-999 & Xw(i,:)~=0);
   jd=find(Xd(i,:)~=-999 & Xd(i,:)~=0);
   %
   resw(jw)=(Xw(i,jw)-aXw(jw))./sXw(jw);
   resd(jd)=(Xd(i,jd)-aXd(jd))./sXd(jd);
   %
   residw=[residw resw];
   residd=[residd resd];
   %
   clear resw;
   clear resd;
   %
end
resid=residw+residd;


function [ha] = km22ha(km2)
% Convert area from square kilometers to hectares.
% Chad A. Greene 2012
ha = km2*100;


function xform = find_xform(c, d)
%Finds the xform that maps from c to d
%in this case the xform is a simple translation and scaling
%xform is a [3 x 3] matrix which maps coordinates from c's frame to
%d's frame.  the transformation is applied to homogeneous
%coordinaes

%convert bounding box to cornners
xs(:,1) = c([1 2])';
xs(:,2) = c([3 2])';
xs(:,3) = c([1 4])';
xs(:,4) = c([3 4])';

ys(:,1) = d([1 2])';
ys(:,2) = d([3 2])';
ys(:,3) = d([1 4])';
ys(:,4) = d([3 4])';

xs(3,:) = 1;
ys(3,:) = 1;

A=ys*pinv(xs);
A(abs(A)<.000001) = 0;

xform = A;


function [in2] = km22in2(km2)
% Convert area from square kilometers to square inches.
% Chad A. Greene 2012
in2 = km2*1550003100.006;


function classifier = lapsvm(options,data)
% {lapsvm} trains a Laplacian SVM classifier (dual solved with Libsvm).
%
%      classifier = lapsvm(options,data)
%
%      options: a structure with the following fields
%               options.gamma_A: regularization parameter (ambient space)
%               options.gamma_I: regularization parameter (intrinsic norm)
%
%               [optional fields]
%               options.UseBias: {0,1} i.e. use or not a bias (fx=Kalpha+b)
%      data: a structure with the following fields
%            data.X: a M-by-D matrix of M D-dimensional training examples
%            data.K: a M-by-M kernel Gram matrix of M training examples
%            data.Y: a M-by-1 label vector in {-1,0,+1}, where 0=unlabeled
%            data.L: a M-by-M matrix of the graph Laplacian
%
%      classifier: structure of the trained classifier (see the
%                  'saveclassfier' function)
%
% Author: Stefano Melacci (2009)
%         mela@dii.unisi.it
%         * based on the code of Vikas Sindhwani, vikas.sindhwani@gmail.com

tic
if ~isfield(options,'UseBias'),           options.UseBias=1; end

C=1;
parameters =    [4 ... % kernel_type (4=Gram Matrix)
                1 ... % deg
                1 ... % gamma
                0 ... % coef0
                C ... % C
                40.00 ... % cache
                0.001 ... % eps
                0 ... % svm_type
                0.5 ... % nu
                0.1 ... % p
                1 ... % shrinking
                ];

I=eye(size(data.K,1));
lab=(data.Y~=0);
l=nnz(lab);

if isempty(data.L) || options.gamma_I==0
    G=I/(2*options.gamma_A);
else
    G=(2*options.gamma_A*I + 2*options.gamma_I*data.L*data.K)\I;
end

Gram=data.K(lab,:)*G(:,lab);
Ylab=data.Y(lab);

[beta, svs, b, nsv, nlab] = mexGramSVMTrain(Gram', Ylab', parameters);

if nlab(1)==-1
    beta=-beta;
else
    b=-b;
end

betaz=zeros(l,1);
betaz(svs)=beta';
alpha=G(:,lab)*betaz;
sec=toc;

classifier = saveclassifier('lapsvm',1:length(data.Y),alpha, ...
                            data.X,b*options.UseBias,options,sec);


function psi = conv(psi1,psi2,varargin)
% convolution of an SO3Kernel function with a function or a kernel on SO(3)
%
% We convolute an SO3Kernel $f$ with another SO3Kernel or an SO3Fun $g$
% by the convolution
%
% $$ (f *_L g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(q^{-1}\,R) \, dq $$
%
% which in this case is similar to the so caled right sided convolution,
% see SO3FunHarmonic/conv.
%
% The convolution of an SO3Kernel with an S2Kernel or an S2Fun $h$ is
% defined by
%
% $$ (f * h)(\xi) =  \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot h(q^{-1}\,\xi) \, dq $$.
%
%
% Syntax
%   psi = conv(psi1,psi2)
%   SO3F2 = conv(psi1,SO3F1)
%   sF2 = conv(psi1,sF1)
%   phi2 = conv(psi1,phi1)
%   psi = conv(psi1)
%
% Input
%  psi1, psi2 - @SO3Kernel
%  phi1       - @S2Kernel
%  SO3F1      - @SO3Fun
%  sF1        - @S2Fun
%
% Output
%  psi   - @SO3Kernel
%  SO3F2 - @SO3Fun
%  sF2   - @S2Fun
%  phi2  - @S2Kernel
%
% See also
% SO3FunHarmonic/conv SO3FunRBF/conv S2FunHarmonic/conv S2Kernel/conv

if nargin == 1, psi2 = psi1; end

% ------------------- convolution with a SO3Fun -------------------
% In case psi2 is a SO3Fun, convolute the SO3Fun with the kernel
% conv is commutative if kernel is included
if isa(psi2,'SO3Fun')
  psi = conv(psi2,psi1,varargin{:});
  return
end

% ------------------- convolution with a S2Fun -------------------
if isa(psi2,'S2Fun')
  sF = S2FunHarmonic(psi2);
  L = min(psi1.bandwidth,sF.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = psi1.A(l+1) * sF.fhat(l^2+1:(l+1)^2) ./ (2*l+1);
  end

  if isa(psi2,'S2FunHarmonicSym')
    warning(['There is no symmetry given for the SO3Kernel function. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the symmetry of the S2Fun.'])
  end
  psi = S2FunHarmonic(fhat);
  return
end

% ------------------- convolution with a S2Kernel -------------------
if isa(psi2,'S2Kernel')
  L = min(psi1.bandwidth,psi2.bandwidth);
  l = (0:L);
  psi = S2Kernel(psi1.A(1:L+1) .* psi2.A(1:L+1) ./ (2*l+1));
  return
end

% ------------------- convolution of SO3Kernels -------------------
if isnumeric(psi1)
  psi = conv(psi2,psi1,varargin{:});
  return
end

% extract Legendre coefficients of psi1
A1 = psi1.A(:);

% extract Legendre coefficients of psi2
if isnumeric(psi2)
  A2 = psi2(:);
else
  A2 = psi2.A(:);
  A2 = A2 ./ (2*(0:length(A2)-1)+1).';
end

% multiplication in harmonic domain
L = min(psi1.bandwidth,psi2.bandwidth);
psi = SO3Kernel(A1(1:L+1) .* A2(1:L+1));

end


%--------------------------------------------------------------------------
% C = lrsc(A,tau)
% Low Rank Subspace Clustering algorithm for clean data lying in a
% union of subspaces
%
% C = argmin |C|_* + tau/2 * |A - AC|_F^2 s.t. C = C'
%
% A: clean data matrix whose columns are points in a union of subspaces
% tau: scalar parameter
%--------------------------------------------------------------------------
% Adapted from LRSC by @ Rene Vidal, November 2012
%--------------------------------------------------------------------------

function C = clean_relaxed(A,tau)
% Make an estimate of tau if necessary
    if nargin < 2
        tau = 100/norm(A)^2;
    end
    threshold = 1/sqrt(tau)
    options = struct;
    options.lambda = threshold;
    options.tolerance =  16*eps;
    M = size(A, 1);
    options.p0 = ones(M, 1);
    [~, S, V, details] = spx.fast.lansvd(A, options);
    r = numel(S);
    C = V * (eye(r) - diag(1./(S.^2)/tau)) * V';
end


function [x, infos] = sep_symm_nmtf(V, rank, in_options)
% Separable symmetric nonnegative matrix tri-factorization (Sep-Symm-NMTF)
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       Arora, Ge, Halpern, Mimno, Moitra, Sontag, Wu, Zhu,
%       "A practical algorithm for topic modeling with provable guarantees,"
%       International Conference on Machine Learning (ICML), pp. 280-288,
%       2013.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%       septrisymNMF.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%       by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Change log:
%
%       June 14, 2022 (Hiroyuki Kasai): Ported initial version
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.delta = 1e-6;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    method_name = 'Sep-Symm-NMTF';
    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % idendity K such that V(K,K) = W(K,:) S W(K,:)^T
    spa_options.normalize = 1;
    spa_sol = spa(V, rank, spa_options);

    % solve V(K,K) z = q = V(K,:)
    q = V(:, spa_sol.K)' * ones(m,1);
    % y = A(K,K)\q; % This works in noiseless conditions
    options.alg_name = method_name;
    [y, WtW, WtV, infos] = nnls_solver(q, V(spa_sol.K, spa_sol.K), options);
    fprintf('\n');

    % recover S and W
    S = diag(y) * V(spa_sol.K, spa_sol.K) * diag(y);
    % W = V(:,K)/( diag(z)*V(K,K) ); % This works in noiseless conditions
    [W, WtW, WtV, infos] = nnls_solver(V(:,spa_sol.K)', (diag(y) * V(spa_sol.K, spa_sol.K))', options);
    fprintf('\n');

    if options.verbose > 0
        fprintf('# %s: finished.\n', options.alg_name);
    end

    %% store results
    x.S = S;
    x.W = W';

end


%function ind = MS_nearest(x,tau,v);
%
% returns the row vector containing the indicies of the nearest
% neighbours to each of the columns of x. Each point and its tau
% temporal neighbours are excluded from the search.
% v is an array (not necessarily logical) indicating which columns of x to
% use or, the relative importance of these columns, in the computation
% (i.e. use v(i)*x(i,:), not x(i,:)).
%
% default : tau=0
%           v=ones(1,length(x(:,1)))
%
% this is a mex version of nearneigh, and provides a speed-up of
% atleast 1000%.
%
% Michael Small
% michael.small@uwa.edu.au, http://school.maths.uwa.edu.au/~small/
% 15/7/04
% For further details, please see M. Small. Applied Nonlinear Time Series
% Analysis: Applications in Physics, Physiology and Finance. Nonlinear Science
% Series A, vol. 52. World Scientific, 2005. (ISBN 981-256-117-X) and the
% references therein.


function y = prtRvUtilNormPdf(X,mu,sigma)
%Y = prtRvUtilNormPdf(X,mu,sigma)
%Y = prtRvUtilNormPdf(X)
%Y = prtRvUtilNormPdf(X,mu)

% Test Drop in replacement of normpdf from stats toolbox
% X = randn([2 3 3]); mu = 1; sigma = 2; prtUtilApproxEqual(prtRvUtilNormPdf(X,mu,sigma),normpdf(X,mu,sigma))

if nargin < 2 %|| isempty(mu) % normpdf returns [] if mu is []
    mu = 0;
end

if nargin < 3 %|| isempty(sigma) % normpdf returns [] if sigma is []
    sigma = 1;
end

xSize = size(X);

y = reshape(exp(prtRvUtilMvnLogPdf(X(:), mu, sigma.^2)),xSize);


function pass = test_norm( )
% Test spherefun norm() command

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

f = spherefun( @(x,y,z) 1+0*x );
pass(1) = abs( sqrt(sum2( f )) - norm( f ) ) < tol;

f = spherefun( @(x,y,z) cos(x.*y.*z) );
s = svd( f );
pass(2) = abs( sum(s.^2) - norm(f).^2 ) < tol;

f = spherefun( @(x,y,z) x + y + z );
pass(3) = abs( norm(f,inf) - sqrt(3) ) < tol;

end


function t = bartlett_sample ( m, df, sigma )

%*****************************************************************************80
%
%% BARTLETT_SAMPLE samples the Bartlett distribution.
%
%  Discussion:
%
%    If the matrix T is sampled from the Bartlett distribution, then
%    the matrix W = T' * T is a sample from the Wishart distribution.
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Input, real SIGMA(M,M), the covariance matrix, which should be
%    a symmetric positive definite matrix.
%
%    Output, real T(M,M), the sample matrix from the Bartlett distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BARTLETT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'BARTLETT_SAMPLE - Fatal error!\n' );
  end
%
%  Get the upper triangular Cholesky factor of SIGMA.
%
  r = chol ( sigma );
%
%  Sample the unit Bartlett distribution.
%
  tu = bartlett_unit_sample ( m, df );
%
%  Construct the matrix.
%
  t = tu * r;

  return
end


function[]=makefigs_psi2fields
%MAKEFIGS_PSI2FIELDS  Makes a sample figure for PSI2FIELDS.

load qgsnapshot,use qgsnapshot
psi=qgsnapshot.psi;

[cv,zeta,N,S,P]=psi2fields(x(2)-x(1),psi);
x=x/1000;
y=y/1000;
figure
subplot(2,2,1),jpcolor(x,y,psi),title('Streamfunction')
subplot(2,2,2),jpcolor(x,y,zeta),title('Vorticity')
subplot(2,2,3),jpcolor(x,y,P),title('Okubo-Weiss')
subplot(2,2,4),ii=1:10:length(x);
quiver(x(ii),y(ii),real(cv(ii,ii)),imag(cv(ii,ii))),title('Velocity')

for i=1:4
    subplot(2,2,i)
    axis equal, axis tight,xtick([-3:3]),ytick([-3:1:3])
    fontsize 16 14 14 14
end


function nearest = find_closest1 ( m, nr, r, ns, s )

%*****************************************************************************80
%
%% FIND_CLOSEST1 finds the nearest R point to each S point.
%
%  Discussion:
%
%    We are given R, a set of NR points in M dimensions.
%
%    We are given S, a set of NS points in M dimensions.
%
%    For each S(I) in S, we seek the index J of the point R(J)
%    which is nearest to S(I) over all points in R.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer NR, the number of cell generators.
%
%    Input, real R(M,NR), the cell generators.
%
%    Input, integer NS, the number of sample points.
%
%    Input, real S(M,NS), the points to be checked.
%
%    Output, integer NEAREST(NS), the index of the nearest cell generators.
%
  nearest = zeros ( ns, 1 );

  for js = 1 : ns

    distance = Inf;
    nearest(js) = -1;

    for jr = 1 : nr

      dist_sq = 0.0;
      for i = 1 : m
        dist_sq = dist_sq + ( r(i,jr) - s(i,js) )^2;
      end

      if ( dist_sq < distance )
        distance = dist_sq;
        nearest(js) = jr;
      end

    end

  end

  return
end


function class_calc = knnclass2(neighbors_class,neighbors_distance,num_class,K)

% find class on the basis of K neighbors
for g = 1:num_class
    freq(g) = length(find(neighbors_class == g));
end

unique_class = find(freq == max(freq));

if neighbors_distance(1)<1e-5
    class_calc=neighbors_class(1);
elseif length(unique_class) == 1
    [M,class_calc] = max(freq);
else
    mean_dist = ones(num_class,1).*max(neighbors_distance);
    for g = 1:length(unique_class)
        in = find(neighbors_class == unique_class(g));
        mean_dist(unique_class(g)) = mean(neighbors_distance(in));
    end
    [m,class_calc] = min(mean_dist);
end


function R = fit_rotation(S)
  % FIT_ROTATION find rotation for a given covariance matrix
  %
  % R = fit_rotation(S)
  %
  % Inputs:
  %   S  n by n covariance matrix
  % Outputs:
  %   R  n by n rotation matrix closest to S
  %

  % svd
  [su,ss,sv]=svd(S);
  R = sv*su';
  % if reflection then flip last column
  if( det(R) < 0 )
    su(:,end) = -su(:,end);
    R = sv*su';
  end
  % should definitely be rotation now
  %assert( det(R) >= 0 );
end


function mbr = mbrot()
    i_list = 1:1024;
    mbr = nan(1024,1024,3);
    for i = i_list(:)'
        for j = i_list(:)'

            mbr(i,j,1) = red(i,j);
            mbr(i,j,2) = green(i,j);
            mbr(i,j,3) = blue(i,j);
        end
    end

    figure; image(mbr./max(mbr(:)));

    function out = red(i, j)
        a = 0;
        b = 0;
        d = 0;
        n = 0;
        while (a*a+(b*b)<4 && n<1024)
            b=2*a*b+(j-1)/5e4+.06;
            a=a*a-d+(i-1)/5e4+.34;
            d = b*b;
            n = n+1;
        end
        out = n/4;
    end

    function out = green(i, j)
        out = 4 * red(i, j);
    end

    function out = blue(i, j)
        out = 6 * red(i, j);
    end
end


function [disp_row, disp_col, scale_ind] = optimize_scores(scores_fs, iterations)

% Maximizes the continuous convolution response (classification scores).
% Find the size of the output.
[sz1, sz2, num_scales] = size(scores_fs);
output_sz = [sz1 sz2];

% Do the grid search step by finding the maximum in the sampled response
% for each scale.
sampled_scores = sample_fs(scores_fs);
[max_resp_row, max_row] = max(sampled_scores, [], 1);
[init_max_score, max_col] = max(max_resp_row, [], 2);
max_row_perm = permute(max_row, [2 3 1]);
col = max_col(:)';
row = max_row_perm(sub2ind(size(max_row_perm), col, 1:size(sampled_scores,3)));

% Shift and rescale the coordinate system to [-pi, pi]
trans_row = mod(row - 1 + floor((output_sz(1)-1)/2), output_sz(1)) - floor((output_sz(1)-1)/2);
trans_col = mod(col - 1 + floor((output_sz(2)-1)/2), output_sz(2)) - floor((output_sz(2)-1)/2);
init_pos_y = permute(2*pi * trans_row / output_sz(1), [1 3 2]);
init_pos_x = permute(2*pi * trans_col / output_sz(2), [1 3 2]);

% Set the current maximum to the sampled one
max_pos_y = init_pos_y;
max_pos_x = init_pos_x;

% construct grid
ky = -ceil((output_sz(1) - 1)/2) : floor((output_sz(1) - 1)/2);
kx = (-ceil((output_sz(2) - 1)/2) : floor((output_sz(2) - 1)/2))';

% pre-compute complex exponential
exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

ky2 = ky.*ky;
kx2 = kx.*kx;

iter = 1;
while iter <= iterations
    % Compute gradient
    ky_exp_ky = bsxfun(@times, ky, exp_iky);
    kx_exp_kx = bsxfun(@times, kx, exp_ikx);
    y_resp = mtimesx(exp_iky, scores_fs, 'speed');
    resp_x = mtimesx(scores_fs, exp_ikx, 'speed');
    grad_y = -imag(mtimesx(ky_exp_ky, resp_x, 'speed'));
    grad_x = -imag(mtimesx(y_resp, kx_exp_kx, 'speed'));

    % Compute Hessian
    ival = 1i * mtimesx(exp_iky, resp_x, 'speed');
    H_yy = real(-mtimesx(bsxfun(@times, ky2, exp_iky), resp_x, 'speed') + ival);
    H_xx = real(-mtimesx(y_resp, bsxfun(@times, kx2, exp_ikx), 'speed') + ival);
    H_xy = real(-mtimesx(ky_exp_ky, mtimesx(scores_fs, kx_exp_kx, 'speed'), 'speed'));
    det_H = H_yy .* H_xx - H_xy .* H_xy;

    % Compute new position using newtons method
    max_pos_y = max_pos_y - (H_xx .* grad_y - H_xy .* grad_x) ./ det_H;
    max_pos_x = max_pos_x - (H_yy .* grad_x - H_xy .* grad_y) ./ det_H;

    % Evaluate maximum
    exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
    exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

    iter = iter + 1;
end

% Evaluate the Fourier series at the estimated locations to find the
% corresponding scores.
max_score = real(mtimesx(mtimesx(exp_iky, scores_fs, 'speed'), exp_ikx, 'speed'));

% check for scales that have not increased in score
ind = max_score < init_max_score;
max_score(ind) = init_max_score(ind);
max_pos_y(ind) = init_pos_y(ind);
max_pos_x(ind) = init_pos_x(ind);

% Find the scale with the maximum response
[max_scale_response, scale_ind] = max(max_score(:));

% Scale the coordinate system to output_sz
disp_row = (mod(max_pos_y(1,1,scale_ind) + pi, 2*pi) - pi) / (2*pi) * output_sz(1);
disp_col = (mod(max_pos_x(1,1,scale_ind) + pi, 2*pi) - pi) / (2*pi) * output_sz(2);
end


%compute ang between vector tailsminfl and vector inflheadsm

function [data,units]=compute_angihsmvsangtsmi(trx,n)

larvae = trx.exp2flies{n};
numlarvae = numel(larvae);
angihsmvsangtsmi=cell(1,numlarvae);
absangihsmvsangtsmi=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    absangihsmvsangtsmi{1,i}=real(acos(cos(trx(larva).tailsminflang).*cos(trx(larva).inflheadsmang)+sin(trx(larva).tailsminflang).*sin(trx(larva).inflheadsmang)));
    temp=trx(larva).tailsminflang-pi/2;
    cosperp=sign(cos(temp).*cos(trx(larva).inflheadsmang)+sin(temp).*sin(trx(larva).inflheadsmang));
    angihsmvsangtsmi{1,i}=bsxfun(@times,absangihsmvsangtsmi{1,i},cosperp);
end

units=parseunits('rad');
data=angihsmvsangtsmi;


% Frequency response 3rd Order Bessel HPF
%  and G array.
% File:  c:\M_files\short_updates\UsingGarray2.m
% 02/10/07
% See Word file UsingGarray2.doc for schemtic and equations.
clear;clc;
% unit suffixes
u=1e-6;K=1e3;m=1e-3;u=1e-6;n=1e-9;p=1e-12;
% component values
R1=52*K;R2=75*K;R3=287*K;
C1=1.55*n;C2=C1;C3=C1;
Ein=1; % Unity input for (normalized) transfer function
N=3; % Number of capacitors = order of circuit
%
% Form W, Q, S, and P arrays:
%
W=[R2 -R2 0;0 R1 R3-R1;0 0 R3];
Q=-[1 0 0;1 1 0;1 1 1];S=Ein*[1;1;1];P=diag([C1 C2 C3]);
%
% Get A, B, D, & E arrays:
%
C=inv(W*P);A=C*Q;B=C*S;
% D & E
D=[0 0 0];E=0;
%
F=[0 0 R3]; % R3 is the coefficient of both iC1 and iC2 in Vo2
% which are derivative terms (iC=dVc/dt, etc.)
G=F*P;
%
% * * * * * * * * * * * * Frequency response * * * * * * * * * * * *
%
I=eye(N); % identity matrix
%
% Log frequency sweep from BF to BF+ND
%
BF=2;ND=2;PD=50;NP=ND*PD+1;Fr=logspace(BF,BF+ND,NP);
for i=1:NP
   s=2*pi*Fr(i)*j; % j = sqrt(-1)
   stm=(s*I-A)\B;
   v2=abs((D+G*A)*stm+E+G*B);Vo(i)=20*log10(v2);
   asym(i)=60*log10(Fr(i)/1000); % +60 dB/decade slope
end
%
h=plot(log10(Fr),Vo,'r',log10(Fr),asym,'k--');
set(h,'LineWidth',2);
grid on;
axis([2 4 -60 20]);
ylabel('dBV');title('Bessel HPF');
xlabel('Log Freq(Hz)');
legend('Vo','Asymptote');
figure(1);
%


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Celestial Computing with MATLAB

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function [sigma_draw, Ft_draw] = VARdrawpost(VAR)
% =======================================================================
% Draw from the posterior distribution of a VAR model
% =======================================================================
% [sigma_draw, Ft_draw] = VARdrawpost(VAR)
% -----------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel function
% -----------------------------------------------------------------------
% OUPUT
%   - sigma_draw: draw from the posterior of VCV matrix of residuals of VAR
%   - Ft_draw: draw from the posterior of Ft of VAR
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

%% Get relevant parameters from VAR structure
%===============================================
nobs = VAR.nobs;
k    = [];
nvar = VAR.nvar;
X    = VAR.X;

%% OLS estimates
%============================================
Ft_hat = VAR.Ft;
sigma_hat = VAR.sigma;
inv_sigma_hat = inv(sigma_hat);

%% Draw the VCV matrix (sigma)
%============================================
inv_sigma_draw = wishrnd(inv_sigma_hat/nobs,nobs);
sigma_draw     = inv(inv_sigma_draw);

%% Draw the coeffiecient matrix (Ft)
%============================================
aux1 = inv(X'*X);
aux2 = kron(sigma_draw, aux1);
Fthat_vec = Ft_hat(:);
Ftdraw = mvnrnd(Fthat_vec,aux2);
Ft_draw = reshape(Ftdraw,k,nvar);


function msg_out = CreateAppend16BitCRC(msg_no_zeros)

valueCRC = 65535;
genPoly = 4129;

msg_in = [msg_no_zeros 0 0];
for i1 = 1:length(msg_in)
    for i2 = 1:8
        b = mod(floor(msg_in(i1)/(2^(8-i2))),2);
        valueCRCsh1 = bitsll(valueCRC,1);
        valueCRCadd1 = bitor(valueCRCsh1,b);
        if floor(valueCRCadd1/2^16) == 1
            valueCRC = bitxor(valueCRCadd1,genPoly);
        else
            valueCRC = valueCRCadd1;
        end
        valueCRC = mod(valueCRC,2^16);
        2;
    end
end

%msg_out = [msg_in mod(valueCRC,2^8) mod(floor(valueCRC/2^8),2^8)];
msg_out = [msg_no_zeros mod(floor(valueCRC/2^8),2^8) mod(valueCRC,2^8)];


classdef DCF < dagnn.ElementWise
%DCF  layer
%   Discriminant Correlation Filters(DCF)
%
%   QiangWang, 2016
% -------------------------------------------------------------------------------------------------------------------------
    properties
        win_size = [3,3];
        sigma = 1;
    end
    properties (Transient)
        yf = [];
        lambda = 1e-4;
    end
    methods
        function outputs = forward(obj, inputs, params)

            xf = fft2(inputs{1});% target region
            zf = fft2(inputs{2});% search region
            xf_conj = conj(xf);
            [h,w,c,~] = size(xf);
            hwc = h*w*c;

            useGPU = isa(xf, 'gpuArray');
            if isempty(obj.yf)
                obj.initYF(useGPU);
            end

            kxxf = sum(xf .* xf_conj, 3) ./ hwc;
            alphaf = bsxfun(@rdivide, obj.yf, (kxxf + obj.lambda));
            kzxf = sum(zf .* xf_conj, 3) ./ hwc;
            outputs{1} = real(ifft2(alphaf .* kzxf));
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)

            dldrf = fft2(derOutputs{1});
            xf = fft2(inputs{1});% target region
            zf = fft2(inputs{2});% search region
            xf_conj = conj(xf);

            [h,w,c,~] = size(xf);
            hwc = h*w*c;

            kxxf = sum(xf .* xf_conj, 3) ./ hwc +obj.lambda;

            alphaf = bsxfun(@rdivide,obj.yf, kxxf);
            dldz = real(ifft2(bsxfun(@times,dldrf.*conj(alphaf),xf)))/hwc;
            kzxf = sum(zf .* xf_conj, 3) ./ hwc;
            dldx = real(ifft2(bsxfun(@times,conj(dldrf).*alphaf,zf)-...
            2*bsxfun(@times,xf,real(dldrf.*conj(alphaf.*kzxf)./kxxf))))/hwc;

            derInputs{1} = dldx;
            derInputs{2} = dldz;
            derParams = {};
        end

        function initYF(obj, useGPU)
            yf_ = single(fft2(gaussian_shaped_labels(obj.sigma, obj.win_size)));
            lambda_ = gather(obj.lambda);
            if useGPU
                obj.yf = gpuArray(yf_);
                obj.lambda = gpuArray(lambda_);
            else
                obj.yf = yf_;
                obj.lambda = lambda_;
            end
        end

        function obj = reset(obj)
            obj.yf = [] ;
            obj.lambda = 1e-4;
        end

        function obj = DCF(varargin)
            obj.load(varargin);
            obj.win_size = obj.win_size;
            obj.sigma = obj.sigma ;
        end
    end
end

function labels = gaussian_shaped_labels(sigma, sz)%kcf
[rs, cs] = ndgrid((1:sz(1)) - floor(sz(1)/2), (1:sz(2)) - floor(sz(2)/2));
labels = exp(-0.5 / sigma^2 * (rs.^2 + cs.^2));
labels = circshift(labels, -floor(sz(1:2) / 2) + 1);
assert(labels(1,1) == 1)
end


function [X1, Y1] = Direction2CubeMap(D, r, c)
%
%        [X1, Y1] = Direction2CubeMap(D, r, c)
%
%
%        Input:
%           -D: 3D directions of the img format
%        Output:
%           -X1: X coordinates in the CubeMap format
%           -Y1: Y coordinates in the CubeMap format
%
%     Copyright (C) 2011  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

[rD, cD, ~] = size(D);

X1 = zeros(rD, cD);
Y1 = zeros(rD, cD);
totD = rD * cD;

face_cm = round((r / 4 + c / 3) / 2);

%Calculating faces' directions
Mul = [1, 1, 1, -1, -1, -1];

T = [2, 1, 3, 2, 1, 3];
A = [1, 3, 1, 1, 3, 1];
B = [3, 2, 2, 3, 2, 2];

X1_1 = [1.5,2.5, 1.5, 1.5, 0.5, 1.5];
X1_2 = [0.5,0.5, 0.5,-0.5, 0.5,-0.5];
Y1_1 = [2.5,1.5, 3.5, 0.5, 1.5, 1.5];
Y1_2 = [0.5,0.5,-0.5, 0.5,-0.5,-0.5];

for i=1:6
    indx = find(    (Mul(i) * D(:,:,T(i)) > 0)&...
                    (Mul(i) * D(:,:,T(i)) >= abs(D(:,:,A(i))))&...
                    (Mul(i) * D(:,:,T(i)) >= abs(D(:,:,B(i)))));

    indx_shifted = indx + (T(i) - 1) * totD;

    X1(indx) = X1_1(i) + X1_2(i) * D(indx + (A(i) - 1) * totD) ./ D(indx_shifted);
    Y1(indx) = Y1_1(i) + Y1_2(i) * D(indx + (B(i) - 1) * totD) ./ D(indx_shifted);
end

X1 = RemoveSpecials(X1) * face_cm;
Y1 = flipud(RemoveSpecials(Y1) * face_cm);

end


function reinit_params(net, varargin)
% REINIT_PARAMS re-initializes the parameters of a DAG.

opts.scale = 1;
opts.weightInitMethod = 'xavierimproved';
opts.initBias = 0.1;
opts = vl_argparse(opts, varargin);

for l = 1:numel(net.layers)
    if isempty(net.layers(l).params)
        continue;
    end
    if isa(net.layers(l).block, 'dagnn.Conv')
        sz = net.layers(l).block.size;
        filters_param = net.layers(l).params{1};
        net.params(net.getParamIndex(filters_param)).value = ...
            init_weight(opts, sz(1), sz(2), sz(3), sz(4), 'single');
        % Optional bias.
        if numel(net.layers(l).params) > 1
            bias_param = net.layers(l).params{2};
            net.params(net.getParamIndex(bias_param)).value = ...
                ones(sz(4), 1, 'single') * opts.initBias;
        end
    elseif isa(net.layers(l).block, 'dagnn.BatchNorm')
        gain_param    = net.layers(l).params{1};
        bias_param    = net.layers(l).params{2};
        moments_param = net.layers(l).params{3};
        out = net.layers(l).block.numChannels;
        if isempty(out) || out < 1
            out = numel(net.params(net.getParamIndex(gain_param)).value);
            if out == 0
                error('output size unknown');
            end
        end
        net.params(net.getParamIndex(gain_param)).value = ...
            ones(out, 1, 'single');
        net.params(net.getParamIndex(bias_param)).value = ...
            zeros(out, 1, 'single');
        net.params(net.getParamIndex(moments_param)).value = ...
            zeros(out, 2, 'single');
    else
        % Every layer with params should be reinitialized.
        error(['unknown layer with params: ' class(net.layers(l).block)]);
    end
end

end

function weights = init_weight(opts, h, w, in, out, type)
% From examples/imagenet/cnn_imagenet_init.m

    % See K. He, X. Zhang, S. Ren, and J. Sun. Delving deep into
    % rectifiers: Surpassing human-level performance on imagenet
    % classification. CoRR, (arXiv:1502.01852v1), 2015.
    switch lower(opts.weightInitMethod)
      case 'gaussian'
        sc = 0.01/opts.scale ;
        weights = randn(h, w, in, out, type)*sc;
      case 'xavier'
        sc = sqrt(3/(h*w*in)) ;
        weights = (rand(h, w, in, out, type)*2 - 1)*sc ;
      case 'xavierimproved'
        sc = sqrt(2/(h*w*out)) ;
        weights = randn(h, w, in, out, type)*sc ;
      otherwise
        error('Unknown weight initialization method''%s''', opts.weightInitMethod) ;
    end
end


function value = mean(SO3F, varargin)
% Calculates the mean value of a SO3Fun or calculates the mean
% along a specified dimension of a vector-valued SO3Fun
%
% Syntax
%   value = mean(SO3F)
%   SO3F = mean(SO3F, d)
%
% Input
%  SO3F - @SO3Fun
%  d - dimension to take the mean value over
%
% Output
%  SO3F  - @SO3Fun
%  value - double
%
% Description
%
% If SO3F is a 3x3 SO3Fun then
% |mean(SO3F)| returns a 3x3 matrix with the mean values of each function
% |mean(SO3F, 1)| returns a 1x3 SO3Fun which contains the pointwise mean values along the first dimension
%
% Example
%   %generate SO3Funs
%   SO3F1 = SO3Fun.dubna
%   SO3F2 = SO3FunHandle(@(rot) SO3F1.eval(rot))
%   A = ones(2,3);
%   SO3F3 = SO3F2.*A
%
%   %calculate mean values of SO3Funs
%   mean(SO3F2)
%   mean(SO3F3)
%   mean(SO3F3,2)
%   mean(SO3F3,1)
%

% mean along specific dimension
if nargin>1 && isnumeric(varargin{1})
  value = SO3FunHandle(@(rot) mean(SO3F.eval(rot),varargin{1}+1),SO3F.SRight,SO3F.SLeft);
  return
end

% mean value of a SO3Fun
res = get_option(varargin,'resolution',2.5*degree);
nodes = equispacedSO3Grid(SO3F.SRight,SO3F.SLeft,'resolution',res);
value = mean(SO3F.eval(nodes(:)));
value = reshape(value,size(SO3F));
if isalmostreal(value,'componentwise')
  value = real(value);
end

end


function e1_test ( )

%*****************************************************************************80
%
%% E1_TEST tests R4_E1 and R8_E1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'E1_TEST:\n' );
  fprintf ( 1, '  Test E1_VALUES, R4_E1, R8_E1.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X           E1(X)\n' );
  fprintf ( 1, '                      R4_E1(X)        Diff\n' );
  fprintf ( 1, '                      R8_E1(X)        Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = e1_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_e1 ( single ( x ) );
    fx3 = r8_e1 ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


% Test file for chebtech2/chebpts.m

function pass = test_chebpts(varargin)

% Set a tolerance (pref.chebfuneps doesn't matter)
tol = 10*eps;

% Test that n = 0 returns empty results:
[x, w, v] = chebtech2.chebpts(0);
pass(1) = ( isempty(x) && isempty(w) && isempty(v) );

% Test n = 1:
[x, w, v] = chebtech2.chebpts(1);
pass(2) = ( x == 0 && w == 2 && v == 1);

% Test that n = 2 returns [-1 ; 1]:
x = chebtech2.chebpts(2);
pass(3) = ( all(size(x) == [2, 1]) && all(x == [-1 ; 1]) );
[x, w, v] = chebtech2.chebpts(2);
pass(4) = ( all(size(w) == [1, 2]) && all(w == [1, 1]) );
pass(5) = ( all(size(v) == [2, 1]) && all(v == .5*[-1 ; 1]) );

% Test that n = 3 returns [-1 ; 0 ; 1]:
x = chebtech2.chebpts(3);
pass(6) = ( all(size(x) == [3, 1]) && all(x == [-1 ; 0 ; 1]) );
[x, w, v] = chebtech2.chebpts(3);
pass(7) = ( all(size(w) == [1, 3]) && norm( w - ([0, 1, 0] + 1/3), inf) < tol );
pass(8) = ( all(size(v) == [3, 1]) && norm( v - ([.5 ; -1 ; .5]) , inf) < tol );

% Test that n = 129 returns vectors of the correct size:
n = 129;
[x, w, v] = chebtech2.chebpts(n);
pass(9) = ( all([size(x) == [n, 1], size(w) == [1, n], size(v) == [n, 1]]) );
% and that the nodes are symmetric:
pass(10) = ( norm(x(1:(n-1)/2) + x(n:-1:(n+3)/2), inf) == 0 );
pass(11) =  ( x((n+1)/2) == 0 );

end


function points3d = mytriangualation(matchedPoints1,matchedPoints2,cam1, cam2)
points2d(:,:,1)=matchedPoints1;
points2d(:,:,2)=matchedPoints2;
cam(:,:,1)=cam1;
cam(:,:,2)=cam2;
nPoints = size(points2d, 1);
points3d = zeros(nPoints, 3, 'like', points2d);

for i = 1:nPoints
    pairs=squeeze(points2d(i, :, :))';
    A = zeros(4, 4);
    for j = 1:2
        P = cam(:,:,j)';
        A(2*j-1,:)=pairs(j, 1)*P(3,:)-P(1,:);
        A(2*j,:)=pairs(j, 2)*P(3,:)-P(2,:);
    end
    [~,~,V] = svd(A);
    X = V(:, end);
    X = X/X(end);
    points3d(i, :) = X(1:3)';
end


function DeltaR=solidTideShift(rStation,rSun,rMoon,Jul1,Jul2,addPermTide)
%%SOLIDTIDESHIFT Compute the vector offset of a point on the surface of the
%             Earth due to solid Earth tidal effects. These effects can be
%             over 31cm. If a Julian date is given, third-order solid Earth
%             tidal effects will be taken into account. Note that site
%             displacement due to ocean loading, which is not included in
%             the solid Earth tide offset, can also be quite significant,
%             with the IERS Conventions 2010 listing it up to 10cm.
%
%INPUTS: rStation A vector from the geocenter to the (non-offset) location
%                 of a station on the surface of the Earth in WGS-84 Earth-
%                 centered Earth-fixed (ECEF) coordinates in meters. This
%                 value is converted into spherical coordinates by this
%                 function, and only the angles in spherical coordinates,
%                 not the radius, are important for this algorithm.
%                 However, since locations on the ground are generally
%                 given in WGS-84 ellipsoidal coordinates, the ellipsoidal
%                 height of the ground plays a role in the conversion to
%                 spherical coordinates. However, variations in the
%                 ellipsoidal height provided on the order of 2km result in
%                 differences in the output DeltaR on the order of microns,
%                 so in practice, one could just set rStation to the
%                 Cartesian position corresponding to a particular
%                 ellipsoidal latitude and longitude with zero ellipsoidal
%                 height. Ideally, however, rStation is the Cartesian
%                 location of the land in a mean-tide model (if
%                 addPermTide=false), or in a tide-free model if
%                 addPermTide=true.
%            rSun A vector from the geocenter to the sun in ITRS
%                 coordinates in meters. This can be obtained using the
%                 readJPLEphem and GCRS2ITRS functions.
%           rMoon A vector from the geocenter to the moon in ITRS
%                 coordinates in meters. This can be obtained using the
%                 readJPLEphem and GCRS2ITRS functions.
%      Jul1, Jul2 Two parts of a Julian date given in terrestrial time
%                 (TT). The units of the date are days. The full date is
%                 the sum of both terms. The date is broken into two
%                 parts to provide more bits of precision. It does not
%                 matter how the date is split. These parameters must be
%                 given for the third-order tidal components to be taken
%                 into account.
%     addPermTide The deltaR values are for a conventional tide-free model.
%                 If addPermTide is true, then a constant (depending on
%                 latitude) permanent tide offset will be added to put the
%                 point into a mean-tide model. If this parameter is
%                 omitted, the default value is "false".
%
%OUTPUTS: DeltaR The offset of a point on the surface of the Earth due to
%                solid Earth tides. rStation+DeltaR is the location of the
%                station (point on the ground) at the terrestial time given
%                by Jul1 and Jul2 taking into account solid Earth tides.
%                Note that the addPermTide term must be consistent with the
%                coordinate system of rStation. If rStation already
%                includes the permanent tides (is in a mean-tide model),
%                then addPermTide should be false. Otherwise, addPermTide
%                should be true.
%
%The Sun and Moon cause the crust of the Earth to warp and points on the
%Earth in WGS-84 coordinates to move over time. The formulae for computing
%tidal shits of the crust are given in Section 7.1 of [1].
%
%Section 7.1.1 discusses solid Earth tides. While the second-order
%corrections are well documneted, the third order corrections can not be
%implemented directly from the standard. However, the standard specifies
%FORTRAN routines, whose algorithms do not resemble the work given in the
%standard. These Fortran routines, have been converted to Matlab and are
%called for the third-order corrections if Julian dates are given. The
%converted routines are separate functions at the bottom of this file.
%
%Note that third-order model has an implied set of ephemerides built into
%it that might not be perfectly consistent with whatever model was used to
%obtain rSun and rMoon.
%
%One time component is supposed to be in Julian centuries since J2000.0. As
%J2000.0 is defined in TDB and the time is given in TT, the TDB2TT function
%is used. The extra parameters needed for the TDB2TT function are not
%requested as inputs to this function, since it is assumed that the
%difference between the tides is too small to matter for the model.
%
%To test this function, one can use the same values that are given in the
%IERS's implementation, DEHANTTIDEINEL.F Those are
% rStation = [4075578.385;931852.890;4801570.154];
% rSun=[137859926952.015;54228127881.4350;23509422341.6960];
% rMoon=[-179996231.920342;-312468450.131567;-169288918.592160];
% addPermTide=false;
% [Jul1,Jul2]=Cal2TT(2009,4,13,0,0,0);
% DeltaR=solidTideShift(rStation,rSun,rMoon,addPermTide,Jul1,Jul2);
%
%The value of DeltaR truncated to 9 places should be about
%DeltaR = 0.0770042035
%         0.0630405632
%         0.0551656815
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The convention for indexing arrays is that 1 refers to a value with
%respect to lunar parameters and 2 refers to a value with respect to solar
%parameters.

if(nargin<6||isempty(addPermTide))
    addPermTide=false;
end

%%%%%%General constants that are used%%%%%%
Re=Constants.EarthEqRadius;%meters
GMGMe(1)=Constants.MoonEarthMassRatio;
GMGMe(2)=Constants.SunEarthMassRatio;

%%%%%%Convert the input parameters into different coordinate systems%%%%%%

%Obtain the spherical coordinates of the station.
pointSpherStation=Cart2Sphere(rStation);
lambda=pointSpherStation(2);%Geocentric longitude
phi=pointSpherStation(3);%Geocentric latitude.
r=pointSpherStation(1);
rHat=rStation/r;%A unit vector in the direction of the station.

%Get East-North-Up Axes using a local SPHERICAL Earth model. A flattening
%factor of zero makes the Earth flat.
uENU=getENUAxes([phi;lambda;0],false,r,0);
uE=uENU(:,1);%Spherical East unit vector
uN=uENU(:,2);%Spherical North unit vector.
uU=uENU(:,3);%Spherical radial *up) unit vector

%Extract the x,y, and z coordinates of the moon.
Xj(1)=rMoon(1);
Yj(1)=rMoon(2);
Zj(1)=rMoon(3);
%Obtain the spherical coordinates of the moon.
pointSpherMoon=Cart2Sphere(rMoon);
Lambdaj(1)=pointSpherMoon(2);
Phij(1)=pointSpherMoon(3);
Rj(1)=pointSpherMoon(1);
RjHat(:,1)=rMoon/Rj(1);%A unit vector in the direction of the moon.

%Extract the x,y, and z coordinates of the sun.
Xj(2)=rSun(1);
Yj(2)=rSun(2);
Zj(2)=rSun(3);
%Obtain the spherical coordinates of the sun.
pointSpherSun=Cart2Sphere(rSun);
Lambdaj(2)=pointSpherSun(2);
Phij(2)=pointSpherSun(3);
Rj(2)=pointSpherSun(1);
RjHat(:,2)=rSun/Rj(2);%A unit vector in the direction of the sun.

%%%%%%Determine the solid Earth tidal displacement.%%%%%%
%This is a three step process. The first two steps are described in the
%table on page 103 of the 2010 IERS conventions.

%%%Step 1: time-domain contributions
%%First, the in-phase contribution.

%Determine the second degree nomial Love (h) and Shida (l) numbers using
%the latitude correction in the second paragraph on page 105.
h0=0.6078;
h2=-0.0006;
l0=0.0847;
l2=0.0002;

h2=h0+h2*(3*sin(phi)^2-1)/2;
l2=l0+l2*(3*sin(phi)^2-1)/2;

%Equation 7.5 for the degree 2 tidal offset
DeltaR=0;
for n=1:2
    RjHatDotrHat=dot(RjHat(:,n),rHat);
    DeltaR=DeltaR+GMGMe(n)*(Re^4/Rj(n)^3)*(h2*rHat*(3*RjHatDotrHat^2-1)/2+3*l2*RjHatDotrHat*(RjHat(:,n)-RjHatDotrHat*rHat));
end

%Equation 7.6 for the degree 3 tidal offset
h3=0.292;
l3=0.015;
for n=1:2
    RjHatDotrHat=dot(RjHat(:,n),rHat);
    DeltaR=DeltaR+GMGMe(n)*(Re^5/Rj(n)^4)*(h3*rHat*((5/2)*RjHatDotrHat^3-(3/2)*RjHatDotrHat)+l3*((15/2)*RjHatDotrHat^2-3/2)*(RjHat(:,n)-RjHatDotrHat*rHat));
end

%Equation 7.8 for the diurnal latitude dependence of the Love numbers.
l1Diurn=0.0012;
deltaT=0;
for n=1:2
    P12Cos=3*Xj(n)*Zj(n)/Rj(n)^2;%Equation 7.7b
    P12Sin=3*Yj(n)*Zj(n)/Rj(n)^2;%Equation 7.7b

    %This is a term in the sum of Equation 7.8. However, it does not look
    %like Equation 7.8, since angle difference formulae had to be used to
    %get terms suitable for the values in Equation 7.7b to be used.
    %Specifically,
    %sin(lambda-Lambdaj(n))=sin(lambda)*cos(Lambdaj(n))-cos(lambda)*sin(Lambdaj(n));
    %cos(lambda-Lambdaj(n))=cos(lambda)*cos(Lambdaj(n))+sin(lambda)*sin(Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*(uN*sin(phi)*(cos(lambda)*P12Cos+sin(lambda)*P12Sin)-uE*cos(2*phi)*(sin(lambda)*P12Cos-cos(lambda)*P12Sin));
end
deltaT=-l1Diurn*sin(phi)*deltaT;

DeltaR=DeltaR+deltaT;

%Equation 7.9 for the semi-diurnal latitude dependence of the Love numbers.
l1SemiDiurn=0.0024;
deltaT=0;
for n=1:2
    P22Cos=(3/Rj(n)^2)*(Xj(n)^2-Yj(n)^2);%Equation 7.7c
    P22Sin=(6/Rj(n)^2)*Xj(n)*Yj(n);%Equation 7.7c

    %This is a term in the sum of Equation 7.9. However, it does not look
    %like Equation 7.9, since angle difference formulae had to be used to
    %get terms suitable for the values in Equation 7.7c to be used.
    %Specifically,
    %sin(2*(lambda-Lambdaj(n)))=sin(2*lambda)*cos(2*Lambdaj(n))-cos(2*lambda)*sin(2*Lambdaj(n));
    %cos(2*(lambda-Lambdaj(n)))=cos(2*lambda)*cos(2*Lambdaj(n))+sin(2*lambda)*sin(2*Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*(uN*(cos(2*lambda)*P22Cos+sin(2*lambda)*P22Sin)+uE*sin(phi)*(sin(2*lambda)*P22Cos-cos(2*lambda)*P22Sin));
end
deltaT=-(1/2)*l1SemiDiurn*sin(phi)*cos(phi)*deltaT;

DeltaR=DeltaR+deltaT;

%%Next, the out-of-phase contributions for the second degree terms

%First, the diurnal tides in Equations 7.10a and 7.10b
hIDiurn=-0.0025;
lIDirun=-0.0007;

deltaR=0;
deltaT=0;
for n=1:2
    deltaR=deltaR+GMGMe(n)*(Re^4/Rj(n)^3)*sin(2*Phij(n))*sin(2*phi)*sin(lambda-Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*sin(2*Phij(n))*(cos(2*phi)*sin(lambda-Lambdaj(n))*uN+sin(phi)*cos(lambda-Lambdaj(n))*uE);
end
deltaR=-(3/4)*hIDiurn*deltaR;
deltaT=-(3/2)*lIDirun*deltaT;

DeltaR=DeltaR+deltaR*uU+deltaT;

%Next, the semidiurnal tides in Equations 7.11a and 7.11b.
hISemiDiurn=-0.0022;
lISemiDiurn=-0.0007;

deltaR=0;
deltaT=0;
for n=1:2
    deltaR=deltaR+GMGMe(n)*(Re^4/Rj(n)^3)*cos(Phij(n))^2*cos(phi)^2*sin(2*(lambda-Lambdaj(n)));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*cos(Phij(n))^2*(sin(2*phi)*sin(2*(lambda-Lambdaj(n)))*uN-2*cos(phi)*cos(2*(lambda-Lambdaj(n)))*uE);
end
deltaR=-(3/4)*hISemiDiurn*deltaR;
deltaT=(3/4)*lISemiDiurn*deltaT;

DeltaR=DeltaR+deltaR*uU+deltaT;

%%%Step 2: frequency-domain contributions

%Only apply the corrections if a Julian date has been provided.
if(nargin>4)
    %%First, the contributions for the diurnal band
      %Convert Terrestrial Time to Julian centuries since J2000.0. A Julian
      %century is defined to have 36525 days in it. The offset of 2451545.0
      %days is the Julian date at J2000.0 in TDB. In TT, a difference of a
      %few milliseconds exists.
      [TDB1,TDB2]=TT2TDB(Jul1,Jul2);
      T=((TDB1-2451545.0)+TDB2)/36525;

      %Convert the Julian date to UTC and determine the fractional number
      %of hours passed in the day assuming precisely 24 hours in a say.
      [Jul1,Jul2]=TT2UTC(Jul1,Jul2);
      [~,~,~,dayFrac]=UTC2Cal(Jul1,Jul2,true);
      FHR=24*dayFrac;

    DeltaR=DeltaR+diurBandCorr(rStation,FHR,T);

    %%Next, the contributions from the long-period band
    DeltaR=DeltaR+longBandCorr(rStation,T);
end

if(addPermTide~=false)
    P2=(3*sin(phi)^2-1)/2;
    %Equation 7.14a in the IERS conventions
    deltaR=(-0.1206+0.0001*P2)*P2;
    %Equation 7.14b in the IERS conventions
    deltaT=(-0.0252-0.0001*P2)*sin(2*phi);
    DeltaR=DeltaR+deltaR*uU+deltaT*uN;
end
end

function XCORSTA=diurBandCorr(XSTA,FHR,T)
%%DIURBANDCORR  This subroutine is a Matlab translation of the subroutine
%               STEP2DIU that is provided by the IERS at
%               ftp://tai.bipm.org/iers/convupdt/chapter7/
%               The algorithm is not fully documented by the IERS 2010
%               conventions. For example, most of the constants are not
%               listed anywhere in the documentation.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%
%SUBROUTINE STEP2DIU (XSTA,FHR,T,XCORSTA)
%
%  - - - - - - - - - - -
%   S T E P 2 D I U
%  - - - - - - - - - - -
%
%  This routine is part of the International Earth Rotation and
%  Reference Systems Service (IERS) Conventions software collection.
%
%  This subroutine gives the in-phase and out-of-phase corrections
%  induced by mantle anelasticity in the diurnal band.
%
%  In general, Class 1, 2, and 3 models represent physical effects that
%  act on geodetic parameters while canonical models provide lower-level
%  representations or basic computations that are used by Class 1, 2, or
%  3 models.
%
%  Status: Class 1
%
%     Class 1 models are those recommended to be used a priori in the
%     reduction of raw space geodetic data in order to determine
%     geodetic parameter estimates.
%     Class 2 models are those that eliminate an observational
%     singularity and are purely conventional in nature.
%     Class 3 models are those that are not required as either Class
%     1 or 2.
%     Canonical models are accepted as is and cannot be classified as a
%     Class 1, 2, or 3 model.
%
%  Given:
%     XSTA          d(3)   Geocentric position of the IGS station (Note 1)
%     FHR           d      Fractional hours in the day (Note 2)
%     T             d      Centuries since J2000
%
%  Returned:
%     XCORSTA       d(3)   In phase and out of phase station corrections
%                          for diurnal band (Note 4)
%
%  Notes:
%
%  1) The IGS station is in ITRF co-rotating frame.  All coordinates are
%     expressed in meters.
%
%  2) The fractional hours in the day is computed as the hour + minutes/60.0
%     + sec/3600.0.  The unit is expressed in Universal Time (UT).
%
%  4) All coordinates are expressed in meters.
%
%  Test case:
%     given input: XSTA(1) = 4075578.385D0 meters
%                  XSTA(2) =  931852.890D0 meters
%                  XSTA(3) = 4801570.154D0 meters
%                  FHR     = 0.00D0 hours
%                  T       = 0.1059411362080767D0 Julian centuries
%
%     expected output:  XCORSTA(1) = 0.4193085327321284701D-02 meters
%                       XCORSTA(2) = 0.1456681241014607395D-02 meters
%                       XCORSTA(3) = 0.5123366597450316508D-02 meters
%
%  References:
%
%     Mathews, P. M., Dehant, V., and Gipson, J. M., 1997, ''Tidal station
%     displacements," J. Geophys. Res., 102(B9), pp. 20,469-20,477
%
%     Petit, G. and Luzum, B. (eds.), IERS Conventions (2010),
%     IERS Technical Note No. 36, BKG (2010)
%
%  Revisions:
%  1996 March    23 V. Dehant      Original code
%  2009 July     31 B.E. Stetzler  Initial standardization of code
%  2009 August   06 B.E. Stetzler  Provided a test case
%  2009 August   06 B.E. Stetzler  Capitalized all variables for
%                                  Fortran 77 compatibility
%  2010 October  20 B.E. Stetzler  Input T corrected to be number of
%                                  centuries since J2000
%-----------------------------------------------------------------------

      D2PI = 6.283185307179586476925287;

      DATDI=[-3,    0,  2,   0,  0, -0.01,   0,      0,      0;
             -3,    2,  0,   0,  0, -0.01,   0,      0,      0;
             -2,    0,  1,  -1,  0, -0.02,   0,      0,      0;
             -2,    0,  1,   0,  0, -0.08,   0,     -0.01,   0.01;
             -2,    2, -1,   0,  0, -0.02,   0,      0,      0;
             -1,    0,  0,  -1,  0, -0.10,   0,      0,      0;
             -1,    0,  0,   0,  0, -0.51,   0,     -0.02,   0.03;
             -1,    2,  0,   0,  0,  0.01,   0,      0,      0;
              0,   -2,  1,   0,  0,  0.01,   0,      0,      0;
              0,    0, -1,   0,  0,  0.02,   0,      0,      0;
              0,    0,  1,   0,  0,  0.06,   0,      0,      0;
              0,    0,  1,   1,  0,  0.01,   0,      0,      0;
              0,    2, -1,   0,  0,  0.01,   0,      0,      0;
              1,   -3,  0,   0,  1, -0.06,   0,      0,      0;
              1,   -2,  0,  -1,  0,  0.01,   0,      0,      0;
              1,   -2,  0,   0,  0, -1.23,  -0.07,   0.06,   0.01;
              1,   -1,  0,   0, -1,  0.02,   0,      0,      0;
              1,   -1,  0,   0,  1,  0.04,   0,      0,      0;
              1,    0,  0,  -1,  0, -0.22,   0.01,   0.01,   0;
              1,    0,  0,   0,  0, 12.00,  -0.80,  -0.67,  -0.03;
              1,    0,  0,   1,  0,  1.73,  -0.12,  -0.10,   0;
              1,    0,  0,   2,  0, -0.04,   0,      0,      0;
              1,    1,  0,   0, -1, -0.50,  -0.01,   0.03,   0;
              1,    1,  0,   0,  1,  0.01,   0,      0,      0;
              0,    1,  0,   1, -1, -0.01,   0,      0,      0;
              1,    2, -2,   0,  0, -0.01,   0,      0,      0;
              1,    2,  0,   0,  0, -0.11,   0.01,   0.01,   0;
              2,   -2,  1,   0,  0, -0.01,   0,      0,      0;
              2,    0, -1,   0,  0, -0.02,   0,      0,      0;
              3,    0,  0,   0,  0,  0,      0,      0,      0;
              3,    0,  0,   1,  0,  0,      0,      0,      0];

    DEG2RAD = D2PI/360;

%  Compute the phase angles in degrees.
    S = 218.31664563+(481267.88194+(-0.0014663889+(0.00000185139)*T)*T)*T;

    TAU = FHR*15+280.4606184+(36000.7700536+(0.00038793+(-0.0000000258)*T)*T)*T+(-S);

    PR = (1.396971278+(0.000308889+(0.000000021+(0.000000007)*T)*T)*T)*T;

    S = S + PR;

    H = 280.46645+(36000.7697489+(0.00030322222+(0.000000020+(-0.00000000654)*T)*T)*T)*T;

    P = 83.35324312+(4069.01363525+(-0.01032172222+(-0.0000124991+(0.00000005263)*T)*T)*T)*T;

    ZNS = 234.95544499+(1934.13626197+(-0.00207561111+(-0.00000213944+(0.00000001650)*T)*T)*T)*T;

    PS = 282.93734098+(1.71945766667+(0.00045688889+(-0.00000001778+(-0.00000000334)*T)*T)*T)*T;

% Reduce angles to between the range 0 and 360.
    S =  mod(S,360);
    TAU = mod(TAU,360);
    H =  mod(H,360);
    P =  mod(P,360);
    ZNS = mod(ZNS,360);
    PS = mod(PS,360);

    RSTA = norm(XSTA);
    SINPHI = XSTA(3)/RSTA;
    COSPHI = norm(XSTA(1:2))/RSTA;

    COSLA = XSTA(1)/COSPHI/RSTA;
    SINLA = XSTA(2)/COSPHI/RSTA;
    ZLA = atan2(XSTA(2),XSTA(1));

% Initialize.
    XCORSTA=zeros(3,1);

    for J=1:31
        % Convert from degrees to radians.
        THETAF=(TAU+DATDI(J,1)*S+DATDI(J,2)*H+DATDI(J,3)*P+DATDI(J,4)*ZNS+DATDI(J,5)*PS)*DEG2RAD;

        DR=DATDI(J,6)*2*SINPHI*COSPHI*sin(THETAF+ZLA)+DATDI(J,7)*2*SINPHI*COSPHI*cos(THETAF+ZLA);

        DN=DATDI(J,8)*(COSPHI^2-SINPHI^2)*sin(THETAF+ZLA)+DATDI(J,9)*(COSPHI^2-SINPHI^2)*cos(THETAF+ZLA);
        %      DE=DATDI(8,J)*SINPHI*COS(THETAF+ZLA)+
        %     Modified 20 June 2007

        DE=DATDI(J,8)*SINPHI*cos(THETAF+ZLA)-DATDI(J,9)*SINPHI*sin(THETAF+ZLA);

        XCORSTA(1)=XCORSTA(1)+DR*COSLA*COSPHI-DE*SINLA-DN*SINPHI*COSLA;
        XCORSTA(2)=XCORSTA(2)+DR*SINLA*COSPHI+DE*COSLA-DN*SINPHI*SINLA;
        XCORSTA(3)=XCORSTA(3)+DR*SINPHI+DN*COSPHI;
    end

    XCORSTA=XCORSTA/1000;

%  Finished.

%+----------------------------------------------------------------------
%
%  Copyright (C) 2008
%  IERS Conventions Center
%
%  ==================================
%  IERS Conventions Software License
%  ==================================
%
%  NOTICE TO USER:
%
%  BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING TERMS AND CONDITIONS
%  WHICH APPLY TO ITS USE.
%
%  1. The Software is provided by the IERS Conventions Center ("the
%     Center").
%
%  2. Permission is granted to anyone to use the Software for any
%     purpose, including commercial applications, free of charge,
%     subject to the conditions and restrictions listed below.
%
%  3. You (the user) may adapt the Software and its algorithms for your
%     own purposes and you may distribute the resulting "derived work"
%     to others, provided that the derived work complies with the
%     following requirements:
%
%     a) Your work shall be clearly identified so that it cannot be
%        mistaken for IERS Conventions software and that it has been
%        neither distributed by nor endorsed by the Center.
%
%     b) Your work (including source code) must contain descriptions of
%        how the derived work is based upon and/or differs from the
%        original Software.
%
%     c) The name(s) of all modified routine(s) that you distribute
%        shall be changed.
%
%     d) The origin of the IERS Conventions components of your derived
%        work must not be misrepresented; you must not claim that you
%        wrote the original Software.
%
%     e) The source code must be included for all routine(s) that you
%        distribute.  This notice must be reproduced intact in any
%        source distribution.
%
%  4. In any published work produced by the user and which includes
%     results achieved by using the Software, you shall acknowledge
%     that the Software was used in obtaining those results.
%
%  5. The Software is provided to the user "as is" and the Center makes
%     no warranty as to its use or performance.   The Center does not
%     and cannot warrant the performance or results which the user may
%     obtain by using the Software.  The Center makes no warranties,
%     express or implied, as to non-infringement of third party rights,
%     merchantability, or fitness for any particular purpose.  In no
%     event will the Center be liable to the user for any consequential,
%     incidental, or special damages, including any lost profits or lost
%     savings, even if a Center representative has been advised of such
%     damages, or for any claim by any third party.
%
%  Correspondence concerning IERS Conventions software should be
%  addressed as follows:
%
%                     Gerard Petit
%     Internet email: gpetit[at]bipm.org
%     Postal address: IERS Conventions Center
%                     Time, frequency and gravimetry section, BIPM
%                     Pavillon de Breteuil
%                     92312 Sevres  FRANCE
%
%     or
%
%                     Brian Luzum
%     Internet email: brian.luzum[at]usno.navy.mil
%     Postal address: IERS Conventions Center
%                     Earth Orientation Department
%                     3450 Massachusetts Ave, NW
%                     Washington, DC 20392
%
%
%-----------------------------------------------------------------------
end

function XCORSTA=longBandCorr(XSTA,T)
%%DIURBANDCORR  This subroutine is a Matlab translation of the subroutine
%               STEP2LON that is provided by the IERS at
%               ftp://tai.bipm.org/iers/convupdt/chapter7/
%               The algorithm is not fully documented by the IERS 2010
%               conventions. For example, most of the constants are not
%               listed anywhere in the documentation.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%
%SUBROUTINE STEP2LON (XSTA,T,XCORSTA)
%
%  - - - - - - - - - - -
%   S T E P 2 L O N
%  - - - - - - - - - - -
%
%  This routine is part of the International Earth Rotation and
%  Reference Systems Service (IERS) Conventions software collection.
%
%  This subroutine gives the in-phase and out-of-phase corrections
%  induced by mantle anelasticity in the long period band.
%
%  In general, Class 1, 2, and 3 models represent physical effects that
%  act on geodetic parameters while canonical models provide lower-level
%  representations or basic computations that are used by Class 1, 2, or
%  3 models.
%
%  Status: Class 1
%
%     Class 1 models are those recommended to be used a priori in the
%     reduction of raw space geodetic data in order to determine
%     geodetic parameter estimates.
%     Class 2 models are those that eliminate an observational
%     singularity and are purely conventional in nature.
%     Class 3 models are those that are not required as either Class
%     1 or 2.
%     Canonical models are accepted as is and cannot be classified as a
%     Class 1, 2, or 3 model.
%
%  Given:
%     XSTA          d(3)   Geocentric position of the IGS station (Note 1)
%     T             d      Centuries since J2000
%
%  Returned:
%     XCORSTA       d(3)   In phase and out of phase station corrections
%                          for diurnal band (Note 2)
%
%  Notes:
%
%  1) The IGS station is in ITRF co-rotating frame.  All coordinates are
%     expressed in meters.
%
%  2) All coordinates are expressed in meters.
%
%  Test case:
%     given input: XSTA(1) = 4075578.385D0 meters
%                  XSTA(2) =  931852.890D0 meters
%                  XSTA(3) = 4801570.154D0 meters
%                  T       = 0.1059411362080767D0 Julian centuries
%
%     expected output:  XCORSTA(1) = -0.9780962849562107762D-04 meters
%                       XCORSTA(2) = -0.2236349699932734273D-04 meters
%                       XCORSTA(3) =  0.3561945821351565926D-03 meters
%
%  References:
%
%     Mathews, P. M., Dehant, V., and Gipson, J. M., 1997, ''Tidal station
%     displacements," J. Geophys. Res., 102(B9), pp. 20,469-20,477
%
%     Petit, G. and Luzum, B. (eds.), IERS Conventions (2010),
%     IERS Technical Note No. 36, BKG (2010)
%
%  Revisions:
%  1996 March    23 V. Dehant      Original code
%  2009 August   07 B.E. Stetzler  Initial standardization of code
%                                  and found unnecessary variables tau
%                                  and fhr
%  2009 August   07 B.E. Stetzler  Provided a test case
%  2009 August   07 B.E. Stetzler  Capitalized all variables for
%                                  Fortran 77 compatibility
%  2010 October  20 B.E. Stetzler  Input T corrected to be number of
%                                  centuries since J2000
%-----------------------------------------------------------------------

    D2PI = 6.283185307179586476925287;

    DATDI=[0, 0, 0, 1, 0,   0.47, 0.23, 0.16, 0.07;
         0, 2, 0, 0, 0,  -0.20,-0.12,-0.11,-0.05;
         1, 0,-1, 0, 0,  -0.11,-0.08,-0.09,-0.04;
         2, 0, 0, 0, 0,  -0.13,-0.11,-0.15,-0.07;
         2, 0, 0, 1, 0,  -0.05,-0.05,-0.06,-0.03];

    DEG2RAD = D2PI/360;

%  Compute the phase angles in degrees.
    S = 218.31664563+(481267.88194+(-0.0014663889+(0.00000185139)*T)*T)*T;

    PR = (1.396971278+(0.000308889+(0.000000021+(0.000000007)*T)*T)*T)*T;

    S = S + PR;

    H = 280.46645+(36000.7697489+(0.00030322222+(0.000000020+(-0.00000000654)*T)*T)*T)*T;

    P = 83.35324312+(4069.01363525+(-0.01032172222+(-0.0000124991+(0.00000005263)*T)*T)*T)*T;

    ZNS = 234.95544499+(1934.13626197+(-0.00207561111+(-0.00000213944+(0.00000001650)*T)*T)*T)*T;

    PS = 282.93734098+(1.71945766667+(0.00045688889+(-0.00000001778+(-0.00000000334)*T)*T)*T)*T;

    RSTA=norm(XSTA);
    SINPHI=XSTA(3)/RSTA;
    COSPHI=norm(XSTA(1:2))/RSTA;

    COSLA=XSTA(1)/COSPHI/RSTA;
    SINLA=XSTA(2)/COSPHI/RSTA;

% Reduce angles to between the range 0 and 360.
    S =  mod(S,360);
    %      TAU = DMOD(TAU,360D0)
    H =  mod(H,360);
    P =  mod(P,360);
    ZNS = mod(ZNS,360);
    PS = mod(PS,360);

    DR_TOT = 0;
    DN_TOT = 0;

    XCORSTA=zeros(3,1);

    for J=1:5
        THETAF=(DATDI(J,1)*S+DATDI(J,2)*H+DATDI(J,3)*P+DATDI(J,4)*ZNS+DATDI(J,5)*PS)*DEG2RAD;

        DR=DATDI(J,6)*(3D0*SINPHI^2-1)/2*cos(THETAF)+DATDI(J,8)*(3D0*SINPHI^2-1)/2*sin(THETAF);

        DN=DATDI(J,7)*(COSPHI*SINPHI*2)*cos(THETAF)+DATDI(J,9)*(COSPHI*SINPHI*2)*sin(THETAF);

        DE = 0;
        DR_TOT = DR_TOT+DR;
        DN_TOT = DN_TOT+DN;

        XCORSTA(1)=XCORSTA(1)+DR*COSLA*COSPHI-DE*SINLA-DN*SINPHI*COSLA;
        XCORSTA(2)=XCORSTA(2)+DR*SINLA*COSPHI+DE*COSLA-DN*SINPHI*SINLA;
        XCORSTA(3)=XCORSTA(3)+DR*SINPHI+DN*COSPHI;
    end

    XCORSTA=XCORSTA/1000;

%  Finished.

%+----------------------------------------------------------------------
%
%  Copyright (C) 2008
%  IERS Conventions Center
%
%  ==================================
%  IERS Conventions Software License
%  ==================================
%
%  NOTICE TO USER:
%
%  BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING TERMS AND CONDITIONS
%  WHICH APPLY TO ITS USE.
%
%  1. The Software is provided by the IERS Conventions Center ("the
%     Center").
%
%  2. Permission is granted to anyone to use the Software for any
%     purpose, including commercial applications, free of charge,
%     subject to the conditions and restrictions listed below.
%
%  3. You (the user) may adapt the Software and its algorithms for your
%     own purposes and you may distribute the resulting "derived work"
%     to others, provided that the derived work complies with the
%     following requirements:
%
%     a) Your work shall be clearly identified so that it cannot be
%        mistaken for IERS Conventions software and that it has been
%        neither distributed by nor endorsed by the Center.
%
%     b) Your work (including source code) must contain descriptions of
%        how the derived work is based upon and/or differs from the
%        original Software.
%
%     c) The name(s) of all modified routine(s) that you distribute
%        shall be changed.
%
%     d) The origin of the IERS Conventions components of your derived
%        work must not be misrepresented; you must not claim that you
%        wrote the original Software.
%
%     e) The source code must be included for all routine(s) that you
%        distribute.  This notice must be reproduced intact in any
%        source distribution.
%
%  4. In any published work produced by the user and which includes
%     results achieved by using the Software, you shall acknowledge
%     that the Software was used in obtaining those results.
%
%  5. The Software is provided to the user "as is" and the Center makes
%     no warranty as to its use or performance.   The Center does not
%     and cannot warrant the performance or results which the user may
%     obtain by using the Software.  The Center makes no warranties,
%     express or implied, as to non-infringement of third party rights,
%     merchantability, or fitness for any particular purpose.  In no
%     event will the Center be liable to the user for any consequential,
%     incidental, or special damages, including any lost profits or lost
%     savings, even if a Center representative has been advised of such
%     damages, or for any claim by any third party.
%
%  Correspondence concerning IERS Conventions software should be
%  addressed as follows:
%
%                     Gerard Petit
%     Internet email: gpetit[at]bipm.org
%     Postal address: IERS Conventions Center
%                     Time, frequency and gravimetry section, BIPM
%                     Pavillon de Breteuil
%                     92312 Sevres  FRANCE
%
%     or
%
%                     Brian Luzum
%     Internet email: brian.luzum[at]usno.navy.mil
%     Postal address: IERS Conventions Center
%                     Earth Orientation Department
%                     3450 Massachusetts Ave, NW
%                     Washington, DC 20392
%
%
%-----------------------------------------------------------------------
end

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


function [bvals, bvec, f_bvec, f_bvals] = scd_scheme2bvecsbvals(scheme, acq_basename)
% scd_scheme2bvecs(scheme, acq_basename)
% EXAMPLE:  scheme = scd_schemefile_read(ls('*.scheme'));
%           [bvals, bvec] = scd_scheme2bvecsbvals(scheme);

bvec = scheme(:,1:3);
gyro = 42.57; % kHz/mT
bvals = (2*pi*gyro*scheme(:,4).*scheme(:,6).*10^(3)).^2.*(scheme(:,5)-scheme(:,6)/3)*10^(-3);

if nargin>1
f_bvec=[acq_basename '.bvec.txt'];
f_bvals=[acq_basename '.bvals.txt'];
fid_bvec_tot = fopen(f_bvec,'w');
fid_bvals_tot = fopen(f_bvals,'w');

for i=1:size(scheme,1)
        % write bvecs
        fprintf(fid_bvec_tot, '%f %f %f\n',bvec(i,:));
        % write bvals
        fprintf(fid_bvals_tot, '%f\n',bvals(i));

end

fclose all;

end


% edgeNL: Edge detection function based on nonlinear derivatives
%   (elementary demo):
%   This function differientiates the image to estimate the gradient.
%   The principle is based on polarized derivatives to automatically select
%   the best edge localization.
%   The main benefits are
%     - univocal edge localization for synthetic and real images
%     - noise reduction with no regularization: the noise level is weaker than
%       the noise level in the original image
%     - better direction estimation of the gradient
%     - can product a confident edge reference map for synthetic images
%     - extremely efficient on salt noise OR pepper noise (this last case needs
%       a change in the nonlinear derivatives)
%     - still noise reduction with regularized schemes (Canny, Demigny, ...,
%       can also be adapted to the asymetrical filters (Prewitt, Sobel, ...)
%   Drawback
%     - no detection of vertical and horizontal "white" thin (1 pixel) lines
%   Rk: this demo only performs edge detection and does not include edge
%       extraction (local maxima) and other steps to obtain a binary
%       edge map.
%   Written by O. Laligant - 2009, University of Burgundy
%   Ref: A Nonlinear Derivative Scheme Applied to Edge Detection,
%       Olivier Laligant, Frederic Truchetet, IEEE Transactions on Pattern
%       Analysis and Machine Intelligence - PAMI , vol. 32, no. 2,
%       pp. 242-257, 2010
function edgeNL()

%  ------------- Edge detection on a simple synthetic image  -------------
Im = example();
figure(1), imagesc(Im);
title ('Original image');

% edge detection (nonlinear gradient estimate)
% better localization and direction estimation than the linear scheme
gI = algoNL(Im);

% For a synthetic object, the edges are localized inside the object shape
figure(2), imagesc(gI);
title('Edge detection');

% ------------- Additive gaussian white noise ------------------------

Imn = imnoise(Im, 'gaussian', 0, 0.01);
figure(3), imagesc(Imn);
title ('Noisy image');

gIn = algoNL(Imn);

figure(4), imagesc(gIn);
title('Edge detection on the noisy image');

end
% ----------------------------------------------------------------------------

%
%  --------------------------- functions  -------------------------------------
%

% ------------- computation of the nonlinear derivatives  -------------
% polarized derivatives
% lead to an univocal localization of edges
function [gm, gh, gv] = algoNL(I);
% for classical regularization schemes, I can be replaced by a regularized
%   version
% for asymetrical schemes (Prewitt, Sobel, etc), I must be replaced by two
%   regularized versions (the regularization is different on columns and rows)
dph = thresh0(conv2(I, [0 1 -1], 'same'));
dnh = -thresh0(-conv2(I, [1 -1 0], 'same'));
gh = dph+dnh;
dpv = thresh0(conv2(I, [0; 1; -1], 'same'));
dnv = -thresh0(-conv2(I, [1;  -1; 0], 'same'));
gv = dpv + dnv;
gm = sqrt(gh.*gh + gv.*gv);

end

% ------------- threshold -------------
function st = thresh0(s)
	st = s.*(sign(s)+1)/2;
end%function

% ------------- test image -------------
% an example of image to illustrate the localization of the method
function Im = example()
Im =[
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0   255   255   255     0     0     0     0     0     0     0   255     0     0
     0     0   255   255   255     0     0     0     0     0     0   255   255     0     0
     0     0   255   255   255     0     0     0     0     0   255   255   255     0     0
     0     0     0     0     0     0     0     0     0   255   255   255   255     0     0
     0     0     0     0     0     0     0     0   255   255   255   255   255     0     0
     0     0     0     0     0     0     0   255   255   255   255     0     0     0     0
     0     0     0     0     0     0   255   255   255   255     0     0     0     0     0
     0     0     0     0     0   255   255   255   255   255     0     0     0     0     0
     0     0     0     0   255   255   255     0   255   255     0     0     0     0     0
     0     0     0   255   255   255     0     0     0   255   255     0     0     0     0
     0     0   255   255   255   255   255     0   255   255   255   255   255     0     0
     0     0     0   255   255   255   255   255   255   255   255   255     0     0     0
     0     0     0     0   255   255   255   255   255   255   255   255   255     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
];
end


function W=twoMatDiag(C1,C2,algorithm)
%%TWOMATDIAG Given two real or complex Hermitian matrices, the first of
%            which must be positive definite, find a matrix W that
%            diagonalizes both of them. Specifically, W*C1*W'=I and
%            W*C2*W'=a diagonal matrix.
%
%INPUTS: C1 An NXN real or complex positive definite Hermitian matrix.
%        C2 An NXN real or complex Hermitian matrix. This matrix does not
%           have to be positive definite.
% algorithm An optional parameter specifying the algorithm to use. Possible
%           values are:
%           0 (The default if omitted or an empty matrix is passed) Use the
%             singular value decomposition (SVD)-based algorithm given in
%             [1].
%           1 Use the eigenvalue-based decomposition given in [2]. There is
%             generally not a need to use the algorithmic variant. It is
%             not implemented in the C++/mex version of this function.
%
%OUTPUTS: W An NXN matrix such that W*C1*W'=identity matrix and W*C2*W'=a
%           diagonal matrix.
%
%Joint diagonalization of a pair of matrices arises in the fusion problem
%discussed in [1] and [2], among other applications. If both matrices are not
%positive definite, then the jointMatDiagFrob function can produce the
%desired W diagonalization matrix.
%
%EXAMPLE 1:
%Here, we diagonalize two real, positive definite matrices:
% C1=[87, 25, 18, 31;
%     25, 63, 20, 17;
%     18, 20, 65, 29;
%     31, 17, 29, 65];
% C2=[57,  7, 12, 17;
%      7, 47, 17, 22;
%     12, 17, 37, 27;
%     17, 22, 27, 27];
% %C1 is positive definite. C2 is positive semi-definite.
% W=twoMatDiag(C1,C2);
% offDiagErr1=W*C1*W'-diag(diag(W*C1*W'))
% offDiagErr2=W*C2*W'-diag(diag(W*C2*W'))
%One will see that the off-diagonal errors are on the order of 1e-16, which
%is around what one would expect with finite precision limitiations.
%
%EXAMPLE 2:
%This is the diagonalization of two complex Hermitian matrices, the first
%of which is positive definite and the second of which has some negative
%eigenvalues.
%C1 is positive definite.
% C1=[  9+  0*1i,   -65+  0*1i,  -11-153*1i,  -91-173*1i;
%     -65+  0*1i,    83+  0*1i,   54- 38*1i,   31+ 28*1i;
%     -11+153*1i,    54+ 38*1i,  130+  0*1i,   16- 47*1i;
%     -91+173*1i,    31- 28*1i,   16+ 47*1i,   22+  0*1i]+215*eye(4);
% %C2 has both positive and negative eigenvalues.
% C2=[-16+  0*1i,  -32- 56*1i,  -12-128*1i,   16+114*1i;
%     -32+ 56*1i,   79+  0*1i,  -87- 67*1i,  -48+ 51*1i;
%     -12+128*1i,  -87+ 67*1i,   76+  0*1i,   -7- 96*1i;
%      16-114*1i,  -48- 51*1i,   -7+ 96*1i, -147+  0*1i];
% W=twoMatDiag(C1,C2);
% offDiagErr=W*C1*W'-diag(diag(W*C1*W'))
% offDiagErr=W*C2*W'-diag(diag(W*C2*W'))
%One will see that the errors are on the order of 1e-14 or less, which is
%around what one would expect with finite precision errors.
%
%REFERENCES:
%[1] J. Nygårds, V. Deleskog, and G. Hendeby, "Safe fusion compared to
%    established distributed fusion methods," in IEEE International
%    Conference on Multisensor Fusion and Integration for Intelligent
%    Systems, Baden-Baden, Germany, 19-21 Sep. 2016, pp. 265-271.
%[2] M. Reinhardt, B. Noack, and U. D. Hanebeck, "Closed-form optimization
%    of covariance intersection for low-dimensional matrices," in
%    Proceedings of the 15th International Conference on Information
%    Fusion, Singapore, 9-12 Jun. 2012, pp. 1891-1896.
%
%February 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

switch(algorithm)
    case 0
        %Use the SVD-based algorithm.
        %Note that C1=U1*D1*U1' for a valid covariance matrix.
        [U1,D1,~]=svd(C1);%Equation 6
        d1=diag(D1);

        D1Root=diag(1./sqrt(d1));

        temp=U1*D1Root;
        [U2,~,~]=svd(temp'*C2*temp);%Equation 7 in [1].

        W=U2'*D1Root*U1';%Equation 8a in [1].
        %Note that W*C1*W'=eye(xDim,xDim);
    case 1
        %Use the eigenvalue-based algorithm.
        [V1,E1]=eig(C1);
        T1=inv(V1*diag(sqrt(diag(E1))));
        C2p=T1*C2*T1';
        [V2p,~]=eig(C2p);

        %The transformation matrix in Equation 2 of [2].
        W=V2p'*diag(1./sqrt(diag(E1)))*V1';
    otherwise
        error('Unknown algorithm specified.')
end
end

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


function [yd3] = oz2yd3(oz)
% Convert volume from US liquid ounces to cubic yards.
% Chad Greene 2012
yd3 = oz*0.000038680716307;