function [ imat, mat, ifault ] = invmod ( mat, rmod, cmod, nrow, ifault )

%*****************************************************************************80
%
%% INVMOD inverts a matrix using modulo arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    Original FORTRAN77 version by Roger Payne.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Payne,
%    Inversion of matrices with contents subject to modulo arithmetic,
%    Applied Statistics,
%    Volume 46, Number 2, 1997, pages 295-298.
%
%  Parameters:
%
%    Input/output, integer MAT(NROW*NROW).
%    On input, the matrix to be inverted.
%    On output, the product of the input matrix and IMAT.
%
%    Output, integer IMAT(NROW*NROW), the inverse matrix.
%    If IFAULT = -1 on output, then IMAT is only a left inverse.
%
%    Input, integer RMOD(NROW), the modulus for values in each row.
%
%    Input, integer CMOD(NROW), the modulus for values
%    in each column.
%
%    Input, integer NROW, the order of the matrix.
%
%    Output, integer IFAULT, an error flag.
%    0, no error was detected.
%    -1, only a left inverse could be formed.
%    1, the matrix contains elements that are negative, or too large.
%    2, the matrix contains nonzero elements in mixed modulus positions.
%    3, the matrix cannot be inverted.
%
  imat = [];
%
%  Check that elements in 'mixed-moduli' positions are all zero.
%
  n = 0;
  for i = 1 : nrow
    for j = 1 : nrow

      n = n + 1;

      if ( ( rmod(i) ~= cmod(j) ) && ( 0 < mat(n) ) )
        ifault = 2;
        return
      end

      if ( ( rmod(i) < mat(n) ) || ( mat(n) < 0 ) )
        ifault = 1;
        return
      end

    end
  end

  n = 0;
  for i = 1 : nrow
    for j = 1 : nrow
      n = n + 1;
      imat(n) = 0;
    end
  end
%
%  Sort rows and columns into ascending order of moduli
%
  [ mat, rmod, cmod, rsort, csort ] = msort ( mat, imat, rmod, cmod, nrow );
%
%  Complete initialization of inverse matrix
%
  for n = 1 : nrow + 1 : nrow * nrow
    imat(n) = 1;
  end
%
%  Invert the matrix.
%
  for ir = 1 : nrow

    kir = ( ir - 1 ) * nrow;

    if ( mat(kir+ir) == 0 )
%
%  Find a row JR below IR such that K(JR,IR)>0
%
      all_zero = 1;

      for kjr = kir + nrow + ir : nrow : nrow * nrow
        if ( 0 < mat(kjr) )
          all_zero = 0;
          break;
        end
      end
%
%  Column IR contains all zeros in rows IR or below:
%  look for a row above with zeros to left of column IR
%  and K(JR,IR)>0
%
      if ( all_zero )
        for kjr = ir : nrow : kir
          if ( 0 < mat(kjr) )
            for i = kjr - ir + 1 : kjr - 1
              if ( 0 < mat(i) )
                ifault = 3;
                return
              end
            end
            all_zero = 0;
            break
          end
        end
      end
%
%  Column IR contains all zeros
%
      if ( all_zero )
        continue;
      end
%
%  Switch row JR with row IR
%
      kjr = kjr - ir;

      for i = 1 : nrow

        k = mat(kir+i);
        mat(kir+i) = mat(kjr+i);
        mat(kjr+i) = k;

        k = imat(kir+i);
        imat(kir+i) = imat(kjr+i);
        imat(kjr+i) = k;

      end

    end
%
%  Find a multiplier N such that N*MAT(IR,IR)=1 mod(P{IR})
%
    k = mat(kir+ir);
    for n = 1 : rmod(ir) - 1
      if ( mod ( n * k, rmod(ir) ) == 1 )
        break;
      end
    end
%
%  Multiply row IR by N.
%
    if ( 1 < n )
      for i = kir + 1 : ir * nrow
        mat(i) = mat(i) * n;
        imat(i) = imat(i) * n;
      end
    end
%
%  Subtract MAT(JR,IR) * row IR from each row JR
%
    for kjr = 0 : nrow : nrow * nrow - 1
      n = rmod(ir) - mat(kjr+ir);
      if ( ( kjr ~= kir ) && ( n ~= 0 ) )
        for i = 1 : nrow
          mat(kjr+i)  = mod (  mat(kjr+i) + n *  mat(kir+i), cmod(i) );
          imat(kjr+i) = mod ( imat(kjr+i) + n * imat(kir+i), cmod(i) );
        end
      end
    end

  end
%
%  Check inversion was possible - that result has
%  non-zero elements only on diagonal.
%
  ifault = 0;
%
%  If we encounter a zero diagonal element, then only a left inverse
%  will be formed.
%
  for n = 1 : nrow + 1 : nrow * nrow
    if ( mat(n) == 0 )
      ifault = -1;
    end
    mat(n) = - mat(n);
  end

  for n = 1 : nrow * nrow
    if ( 0 < mat(n) )
      ifault = 3;
      return
    end
  end

  for n = 1 : nrow + 1 : nrow * nrow
    mat(n) = - mat(n);
  end
%
%  Unsort the rows and columns back into their original order.
%
  [ mat, imat, rmod, cmod, rsort, csort ] = musort ( mat, imat, rmod, ...
    cmod, rsort, csort, nrow );

  return
end


function [dynpcm2] = mbar2dynpcm2(mbar)
% Convert pressure from millibars to dyne per centimeter squared.
% Chad Greene 2012
dynpcm2 = mbar*1000.00;


function rd = jed_to_rd ( jed )

%*****************************************************************************80
%
%% JED_TO_RD converts a JED to an RD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Reingold, Nachum Dershowitz,
%    Calendrical Calculations, the Millennium Edition,
%    Cambridge, 2002.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, real RD, the RD date.
%
  rd_epoch = epoch_to_jed_rd ( );

  rd = jed - rd_epoch;

  return
end


function movingcircle
% MOVINGCIRCLE will track the interface defined by x^2+y^2 = (0.5-t)^2
%--------------------------------------------------------------------------
% Copyright (C) 2008 Long Chen. See COPYRIGHT.txt for details.
%--------------------------------------------------------------------------

close all; clear all;
%---------------------- Parameters ----------------------------------------
figure(1); set(gcf,'Units','normal'); set(gcf,'Position',[0,0,0.8,0.4]);
t = 0; dt = 0.075; maxIt = 30;
%---------------------- Initial Grid --------------------------------------
node = [-1 -1; 1 -1; 1 1; -1 1];
elem = [2 3 1; 4 1 3];
N0 = size(elem,1);
for i = 1:5
	[node,elem] = uniformbisect(node,elem);
end
u = -sign(f(node,t));
subplot(1,2,1); showmesh(node,elem); pause(0.1)
subplot(1,2,2);  showsolution(node,elem,u,[0,90]); colorbar;
for k = 1:maxIt
	if (mod(k,4) == 0), t = t + dt; end
	%---------- detect element cross interface or away from interface -----
	eta = abs(sign(f(node(elem(:,1),:),t)) + sign(f(node(elem(:,2),:),t))...
            + sign(f(node(elem(:,3),:),t)));
	refineElem = find(eta < 3);
    coarsenElem = find(eta == 3);
    %---------- refine elements cross the interface -----------------------
    [node,elem] = bisect(node,elem,refineElem);
    u = -sign(f(node,t));
    subplot(1,2,1); showmesh(node,elem); pause(0.025)
    subplot(1,2,2);  showsolution(node,elem,u); view(2); colorbar;
    %---------- coarsen elements away from the interface ------------------
    [node,elem] = coarsen(node,elem,coarsenElem);
    u = -sign(f(node,t));
    subplot(1,2,1);  showmesh(node,elem); pause(0.025)
    subplot(1,2,2);  showsolution(node,elem,u); view(2); colorbar;
end
end
%-------------------- End of MOVINGCIRCLE --------------------------------

%-------------------- Sub functions called by MOVINGCIRCLE ---------------
function z = f(p,t)
z = sum(p.^2,2) - (0.5-t)^2;
end


% SUMMARY:  Log Forward Backward, to solve overflow problem
% AUTHOR:   QIUQIANG KONG
% Created:  18-11-2015
% Modified: 19-11-2015 modify max to sum (exact solution)
%           20-11-2015 add a ceiling for logbeta
%           21-11-2015 use log instead of all to avoid overfit
%           25-11-2015 Add annotation
% -----------------------------------------------------------
% input:
%   p_xn_given_zn  p(xn|zn), size: N*Q
%   p_start        p(z1), size: Q
%   A              p(zn|zn-1), size: Q*Q
% output:
%   loggamma       ln p(zn|X), size: N*Q
%   logksi         ln p(zn,zn-1|X), size: N*Q*Q
%   loglik         ln p(X), to monitor convergence
% ===========================================================
function [loggamma, logksi, loglik] = LogForwardBackward(logp_xn_given_zn, p_start, A)
    % reserve space
    [N,Q] = size(logp_xn_given_zn);
    logalpha = zeros(N,Q);
    logbeta = zeros(N,Q);
    logc = zeros(N,1);
    loggamma = zeros(N,Q);
    logksi = zeros(N,Q,Q);

    % init log alpha(z1), log beta(zN), c(1)
    tmp = logp_xn_given_zn(1,:) + log(p_start);
    logc(1) = log( sum( exp( tmp - max(tmp) ) ) ) + max(tmp);
    logalpha(1,:) = -logc(1) + logp_xn_given_zn(1,:) + log(p_start);
    logbeta(N,:) = 0;

    % calculate logalpha, c
    for n = 2:N
        tmp = bsxfun(@plus, bsxfun(@plus, log(A), logalpha(n-1,:)'), logp_xn_given_zn(n,:));
        logc(n) = log ( sum( sum ( exp ( tmp - max(tmp(:)) ) ) ) ) + max(tmp(:));
        for q = 1:Q
            tmp2 = logalpha(n-1,:) + log(A(:,q)');
            if (isinf(max(tmp2)))
                logalpha(n,q) = -inf;
            else
                logalpha(n,q) = -logc(n) + logp_xn_given_zn(n,q) + log( sum( exp( tmp2 - max(tmp2) ) ) ) + max(tmp2);
            end
        end
    end

    % calculate logbeta
    for n = N-1:-1:1
        for q = 1:Q
            tmp = logbeta(n+1,:) + logp_xn_given_zn(n+1,:) + log(A(q,:));
            logbeta(n,q) = -logc(n+1) + log( sum( exp( tmp - max(tmp) ) ) ) + max(tmp);
        end
    end

    % calculate loggamma
    loggamma = logalpha + logbeta;

    % calculate logksi
    for n = 2:N
        logksi(n,:,:) = -logc(n) + bsxfun(@plus, bsxfun(@plus, log(A), logalpha(n-1,:)'), logp_xn_given_zn(n,:) + logbeta(n,:));
    end
    logksi(1,:,:) = [];

    % calculate likelihood
    loglik = sum(logc);
end


function [Vseq,Dseq] = eigenshuffle(Asequence)
% eigenshuffle: Consistent sorting for an eigenvalue/vector sequence
% [Vseq,Dseq] = eigenshuffle(Asequence)
%
% Includes munkres.m (by gracious permission from Yi Cao)
% to choose the appropriate permutation. This greatly
% enhances the speed of eigenshuffle over my previous
% release.
%
% http://www.mathworks.com/matlabcentral/fileexchange/20652
%
% Arguments: (input)
%  Asequence - an array of eigenvalue problems. If
%      Asequence is a 3-d numeric array, then each
%      plane of Asequence must contain a square
%      matrix that will be used to call eig.
%
%      Eig will be called on each of these matrices
%      to produce a series of eigenvalues/vectors,
%      one such set for each eigenvalue problem.
%
% Arguments: (Output)
%  Vseq - a 3-d array (pxpxn) of eigenvectors. Each
%      plane of the array will be sorted into a
%      consistent order with the other eigenvalue
%      problems. The ordering chosen will be one
%      that maximizes the energy of the consecutive
%      eigensystems relative to each other.
%
%  Dseq - pxn array of eigen values, sorted in order
%      to be consistent with each other and with the
%      eigenvectors in Vseq.
%
% Example:
%  Efun = @(t) [1 2*t+1 t^2 t^3;2*t+1 2-t t^2 1-t^3; ...
%               t^2 t^2 3-2*t t^2;t^3 1-t^3 t^2 4-3*t];
%
%  Aseq = zeros(4,4,21);
%  for i = 1:21
%    Aseq(:,:,i) = Efun((i-11)/10);
%  end
%  [Vseq,Dseq] = eigenshuffle(Aseq);
%
% To see that eigenshuffle has done its work correctly,
% look at the eigenvalues in sequence, after the shuffle.
%
% t = (-1:.1:1)';
% [t,Dseq']
% ans =
%        -1     8.4535           5      2.3447     0.20181
%      -0.9     7.8121      4.7687      2.3728     0.44644
%      -0.8     7.2481        4.56      2.3413     0.65054
%      -0.7     6.7524      4.3648      2.2709      0.8118
%      -0.6     6.3156      4.1751      2.1857     0.92364
%      -0.5     5.9283      3.9855      2.1118     0.97445
%      -0.4     5.5816      3.7931      2.0727     0.95254
%      -0.3     5.2676      3.5976      2.0768       0.858
%      -0.2     4.9791      3.3995      2.1156     0.70581
%      -0.1     4.7109         3.2      2.1742     0.51494
%         0     4.4605           3      2.2391     0.30037
%       0.1     4.2302         2.8      2.2971    0.072689
%       0.2     4.0303      2.5997      2.3303    -0.16034
%       0.3     3.8817      2.4047      2.3064    -0.39272
%       0.4     3.8108      2.1464      2.2628    -0.62001
%       0.5     3.8302      1.8986      2.1111    -0.83992
%       0.6     3.9301      1.5937      1.9298     -1.0537
%       0.7     4.0927      1.2308       1.745     -1.2685
%       0.8     4.3042     0.82515      1.5729     -1.5023
%       0.9     4.5572     0.40389      1.4272     -1.7883
%         1     4.8482  -8.0012e-16     1.3273     -2.1755
%
% Here, the columns are the shuffled eigenvalues.
% See that the second eigenvalue goes to zero, but
% the third eigenvalue remains positive. We can plot
% eigenvalues and see that they have crossed, near
% t = 0.35 in Efun.
%
% plot(-1:.1:1,Dseq')
%
% For a better appreciation of what eigenshuffle did,
% compare the result of eig directly on Efun(.3) and
% Efun(.4). Thus:
%
% [V3,D3] = eig(Efun(.3))
% V3 =
%     -0.74139      0.53464     -0.23551       0.3302
%      0.64781       0.4706     -0.16256      0.57659
%    0.0086542     -0.44236     -0.89119      0.10006
%     -0.17496     -0.54498      0.35197      0.74061
%
% D3 =
%     -0.39272            0            0            0
%            0       2.3064            0            0
%            0            0       2.4047            0
%            0            0            0       3.8817
%
% [V4,D4] = eig(Efun(.4))
% V4 =
%     -0.73026      0.19752      0.49743      0.42459
%      0.66202      0.21373      0.35297      0.62567
%     0.013412     -0.95225      0.25513      0.16717
%     -0.16815    -0.092308     -0.75026      0.63271
%
% D4 =
%     -0.62001            0            0            0
%            0       2.1464            0            0
%            0            0       2.2628            0
%            0            0            0       3.8108
%
% With no sort or shuffle applied, look at V3(:,3). See
% that it is really closest to V4(:,2), but with a sign
% flip. Since the signs on the eigenvectors are arbitrary,
% the sign is changed, and the most consistent sequence
% will be chosen. By way of comparison, see how the
% eigenvectors in Vseq have been shuffled, the signs
% swapped appropriately.
%
% Vseq(:,:,14)
% ans =
%       0.3302      0.23551     -0.53464      0.74139
%      0.57659      0.16256      -0.4706     -0.64781
%      0.10006      0.89119      0.44236   -0.0086542
%      0.74061     -0.35197      0.54498      0.17496
%
% Vseq(:,:,15)
% ans =
%      0.42459     -0.19752     -0.49743      0.73026
%      0.62567     -0.21373     -0.35297     -0.66202
%      0.16717      0.95225     -0.25513    -0.013412
%      0.63271     0.092308      0.75026      0.16815
%
% See also: eig
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 3.0
% Release date: 2/18/09

% Is Asequence a 3-d array?
Asize = size(Asequence);
if (Asize(1)~=Asize(2))
  error('Asequence must be a (pxpxn) array of eigen-problems, each of size pxp')
end
p = Asize(1);
if length(Asize)<3
  n = 1;
else
  n = Asize(3);
end

% the initial eigenvalues/vectors in nominal order
Vseq = zeros(p,p,n);
Dseq = zeros(p,n);
for i = 1:n
  [V,D] = eig(Asequence(:,:,i));
  D = diag(D);
  % initial ordering is purely in decreasing order.
  % If any are complex, the sort is in terms of the
  % real part.
  [junk,tags] = sort(real(D),1,'descend');

  Dseq(:,i) = D(tags);
  Vseq(:,:,i) = V(:,tags);
end

% was there only one eigenvalue problem?
if n < 2
  % we can quit now, having sorted the eigenvalues
  % as best as we could.
  return
end

% now, treat each eigenproblem in sequence (after
% the first one.)
for i = 2:n
  % compute distance between systems
  V1 = Vseq(:,:,i-1);
  V2 = Vseq(:,:,i);
  D1 = Dseq(:,i-1);
  D2 = Dseq(:,i);
  dist = (1-abs(V1'*V2)).*sqrt( ...
    distancematrix(real(D1),real(D2)).^2+ ...
    distancematrix(imag(D1),imag(D2)).^2);

  % Is there a best permutation? use munkres.
  % much faster than my own mintrace, munkres
  % is used by gracious permission from Yi Cao.
  reorder = munkres(dist);

  Vseq(:,:,i) = Vseq(:,reorder,i);
  Dseq(:,i) = Dseq(reorder,i);

  % also ensure the signs of each eigenvector pair
  % were consistent if possible
  S = squeeze(real(sum(Vseq(:,:,i-1).*Vseq(:,:,i),1))) < 0;
  Vseq(:,S,i) = -Vseq(:,S,i);
end

% =================
% end mainline
% =================
% begin subfunctions
% =================

function d = distancematrix(vec1,vec2)
% simple interpoint distance matrix
[vec1,vec2] = ndgrid(vec1,vec2);
d = abs(vec1 - vec2);

function [assignment,cost] = munkres(costMat)
% MUNKRES   Munkres (Hungarian) Algorithm for Linear Assignment Problem.
%
% [ASSIGN,COST] = munkres(COSTMAT) returns the optimal column indices,
% ASSIGN assigned to each row and the minimum COST based on the assignment
% problem represented by the COSTMAT, where the (i,j)th element represents the cost to assign the jth
% job to the ith worker.
%

% This is vectorized implementation of the algorithm. It is the fastest
% among all Matlab implementations of the algorithm.

% Examples
% Example 1: a 5 x 5 example
%{
[assignment,cost] = munkres(magic(5));
disp(assignment); % 3 2 1 5 4
disp(cost); %15
%}
% Example 2: 400 x 400 random data
%{
n=400;
A=rand(n);
tic
[a,b]=munkres(A);
toc                 % about 2 seconds
%}
% Example 3: rectangular assignment with inf costs
%{
A=rand(10,7);
A(A>0.7)=Inf;
[a,b]=munkres(A);
%}
% Reference:
% "Munkres' Assignment Algorithm, Modified for Rectangular Matrices",
% http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html

% version 2.0 by Yi Cao at Cranfield University on 10th July 2008

assignment = zeros(1,size(costMat,1));
cost = 0;

costMat(costMat~=costMat)=Inf;
validMat = costMat<Inf;
validCol = any(validMat,1);
validRow = any(validMat,2);

nRows = sum(validRow);
nCols = sum(validCol);
n = max(nRows,nCols);
if ~n
    return
end

maxv=10*max(costMat(validMat));

dMat = zeros(n) + maxv;
dMat(1:nRows,1:nCols) = costMat(validRow,validCol);

%*************************************************
% Munkres' Assignment Algorithm starts here
%*************************************************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   STEP 1: Subtract the row minimum from each row.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
minR = min(dMat,[],2);
minC = min(bsxfun(@minus, dMat, minR));

%**************************************************************************
%   STEP 2: Find a zero of dMat. If there are no starred zeros in its
%           column or row start the zero. Repeat for each zero
%**************************************************************************
zP = dMat == bsxfun(@plus, minC, minR);

starZ = zeros(n,1);
while any(zP(:))
    [r,c]=find(zP,1);
    starZ(r)=c;
    zP(r,:)=false;
    zP(:,c)=false;
end

while 1
%**************************************************************************
%   STEP 3: Cover each column with a starred zero. If all the columns are
%           covered then the matching is maximum
%**************************************************************************
    if all(starZ>0)
        break
    end
    coverColumn = false(1,n);
    coverColumn(starZ(starZ>0))=true;
    coverRow = false(n,1);
    primeZ = zeros(n,1);
    [rIdx, cIdx] = find(dMat(~coverRow,~coverColumn)==bsxfun(@plus,minR(~coverRow),minC(~coverColumn)));
    while 1
        %**************************************************************************
        %   STEP 4: Find a noncovered zero and prime it.  If there is no starred
        %           zero in the row containing this primed zero, Go to Step 5.
        %           Otherwise, cover this row and uncover the column containing
        %           the starred zero. Continue in this manner until there are no
        %           uncovered zeros left. Save the smallest uncovered value and
        %           Go to Step 6.
        %**************************************************************************
        cR = find(~coverRow);
        cC = find(~coverColumn);
        rIdx = cR(rIdx);
        cIdx = cC(cIdx);
        Step = 6;
        while ~isempty(cIdx)
            uZr = rIdx(1);
            uZc = cIdx(1);
            primeZ(uZr) = uZc;
            stz = starZ(uZr);
            if ~stz
                Step = 5;
                break;
            end
            coverRow(uZr) = true;
            coverColumn(stz) = false;
            z = rIdx==uZr;
            rIdx(z) = [];
            cIdx(z) = [];
            cR = find(~coverRow);
            z = dMat(~coverRow,stz) == minR(~coverRow) + minC(stz);
            rIdx = [rIdx(:);cR(z)];
            cIdx = [cIdx(:);stz(ones(sum(z),1))];
        end
        if Step == 6
            % *************************************************************************
            % STEP 6: Add the minimum uncovered value to every element of each covered
            %         row, and subtract it from every element of each uncovered column.
            %         Return to Step 4 without altering any stars, primes, or covered lines.
            %**************************************************************************
            [minval,rIdx,cIdx]=outerplus(dMat(~coverRow,~coverColumn),minR(~coverRow),minC(~coverColumn));
            minC(~coverColumn) = minC(~coverColumn) + minval;
            minR(coverRow) = minR(coverRow) - minval;
        else
            break
        end
    end
    %**************************************************************************
    % STEP 5:
    %  Construct a series of alternating primed and starred zeros as
    %  follows:
    %  Let Z0 represent the uncovered primed zero found in Step 4.
    %  Let Z1 denote the starred zero in the column of Z0 (if any).
    %  Let Z2 denote the primed zero in the row of Z1 (there will always
    %  be one).  Continue until the series terminates at a primed zero
    %  that has no starred zero in its column.  Unstar each starred
    %  zero of the series, star each primed zero of the series, erase
    %  all primes and uncover every line in the matrix.  Return to Step 3.
    %**************************************************************************
    rowZ1 = find(starZ==uZc);
    starZ(uZr)=uZc;
    while rowZ1>0
        starZ(rowZ1)=0;
        uZc = primeZ(rowZ1);
        uZr = rowZ1;
        rowZ1 = find(starZ==uZc);
        starZ(uZr)=uZc;
    end
end

% Cost of assignment
rowIdx = find(validRow);
colIdx = find(validCol);
starZ = starZ(1:nRows);
vIdx = starZ <= nCols;
assignment(rowIdx(vIdx)) = colIdx(starZ(vIdx));
cost = trace(costMat(assignment>0,assignment(assignment>0)));

function [minval,rIdx,cIdx]=outerplus(M,x,y)
[nx,ny]=size(M);
minval=inf;
for r=1:nx
    x1=x(r);
    for c=1:ny
        M(r,c)=M(r,c)-(x1+y(c));
        if minval>M(r,c)
            minval=M(r,c);
        end
    end
end
[rIdx,cIdx]=find(M==minval);


function lambda = lambda_search(x, L, delta, xi)
% lambda = phi^{-1}(delta),
% where phi(lambda)=|min{xi/lambda*E, (L/L+lambda)x}|_F and E=ones(m,n)

    treshold = 1e-12;
    norm_x = norm(x);
    if norm_x <= delta
        lambda = 0;
    else
        n = size(x,1);
        x = abs(x);
        x = sort(x, 'descend');

        if norm_x*(1-xi/(x(1)*L))<delta %This also includes the case: x(k)-xi/L<0
            lambda = L*(norm_x/delta - 1);
        else
            partial_sum = norm_x^2 - x(1)^2;
            flag = 0;
            for k=2:n
                xi_lambda_ratio = x(k)-xi/L;
                if xi_lambda_ratio > 0
                    tail_sum = (k-1)*(xi_lambda_ratio)^2;
                    test_sum = sqrt(tail_sum + (1-xi/(x(k)*L))^2*partial_sum);
                    if test_sum<delta
                        %lambda = L*(sqrt(partial_sum/(delta^2-tail_sum))-1);
                        coef = [delta^2, 2*L*delta^2, (delta^2-partial_sum)*L^2-(k-1)*xi^2, -2*L*(k-1)*xi^2, -(k-1)*xi^2*L^2];
                        lambda = roots(coef);
                        ind=(abs(imag(lambda))<treshold).*(lambda>0);
                        lambda = lambda'*ind;
                        flag = 1;
                        break
                    end
                    partial_sum = partial_sum - x(k)^2;
                else
                    coef = [delta^2, 2*L*delta^2, (delta^2-partial_sum)*L^2-(k-1)*xi^2, -2*L*(k-1)*xi^2, -(k-1)*xi^2*L^2];
                    lambda = roots(coef);
                    ind=(abs(imag(lambda))<treshold).*(lambda>0);
                    lambda = lambda'*ind;
                    flag = 1;
                    break
                end
            end
            if flag == 0
                lambda = sqrt(n)*xi/delta;
            end
        end
    end


function [ n_data, h, a, t ] = owen_values ( n_data )

%*****************************************************************************80
%
%% OWEN_VALUES returns some values of Owen's T function.
%
%  Discussion:
%
%    Owen's T function is useful for computation of the bivariate normal
%    distribution and the distribution of a skewed normal distribution.
%
%    Although it was originally formulated in terms of the bivariate
%    normal function, the function can be defined more directly as
%
%      T(H,A) = 1 / ( 2 * pi ) *
%        Integral ( 0 <= X <= A ) e^(H^2*(1+X^2)/2) / (1+X^2) dX
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = 1/(2*Pi) * Integrate [ E^(-h^2*(1+x^2)/2)/(1+x^2), {x,0,a} ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Mike Patefield, David Tandy,
%    Fast and Accurate Calculation of Owen's T Function,
%    Journal of Statistical Software,
%    Volume 5, Number 5, 2000, pages 1-25.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real H, a parameter.
%
%    Output, real A, the upper limit of the integral.
%
%    Output, real T, the value of the function.
%
  n_max = 28;

  a_vec = [ ...
    0.2500000000000000E+00, ...
    0.4375000000000000E+00, ...
    0.9687500000000000E+00, ...
    0.0625000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.9999975000000000E+00, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.1000000000000000E+02, ...
    0.1000000000000000E+03 ];

  h_vec = [ ...
    0.0625000000000000E+00, ...
    6.5000000000000000E+00, ...
    7.0000000000000000E+00, ...
    4.7812500000000000E+00, ...
    2.0000000000000000E+00, ...
    1.0000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02 ];

  t_vec = [ ...
    3.8911930234701366E-02, ...
    2.0005773048508315E-11, ...
    6.3990627193898685E-13, ...
    1.0632974804687463E-07, ...
    8.6250779855215071E-03, ...
    6.6741808978228592E-02, ...
    0.4306469112078537E-01, ...
    0.6674188216570097E-01, ...
    0.7846818699308410E-01, ...
    0.7929950474887259E-01, ...
    0.6448860284750376E-01, ...
    0.1066710629614485E+00, ...
    0.1415806036539784E+00, ...
    0.1510840430760184E+00, ...
    0.7134663382271778E-01, ...
    0.1201285306350883E+00, ...
    0.1666128410939293E+00, ...
    0.1847501847929859E+00, ...
    0.7317273327500385E-01, ...
    0.1237630544953746E+00, ...
    0.1737438887583106E+00, ...
    0.1951190307092811E+00, ...
    0.7378938035365546E-01, ...
    0.1249951430754052E+00, ...
    0.1761984774738108E+00, ...
    0.1987772386442824E+00, ...
    0.2340886964802671E+00, ...
    0.2479460829231492E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    h = 0.0;
    a = 0.0;
    t = 0.0;
  else
    h = h_vec(n_data);
    a = a_vec(n_data);
    t = t_vec(n_data);
  end

  return
end


function [day_weight]=rmr_day_weight(data_close,data,t1,day_weight, w, epsilon)
%to calculate the day_weight at the t+1 day with robust mean reversion method
%Input:
%    data
%    t1         ---the new day
%    day_weight ---the weight before the new day
%    w          ---the length of window
%    epsilon      ---the parameter to control the reversion threshold
%Output:
%    day_weight ---the weight at the new day
%w=min(w,t1-1);
%x_t1 = l1median_VaZh_z(data((t1-w):(t1-1),:));
if t1<w+2
    x_t1=data(t1-1,:);
else
   x_t1 = l1median_VaZh_z(data_close((t1-w):(t1-1),:))./data_close(t1-1,:);
end
%   if sum(x_t1)==size(data,2)
%       x_t1 = median(data_close((t1-w):(t1-1),:))./data_close(t1-1,:);
%   end

if (norm(x_t1-mean(x_t1)))^2==0
    tao = 0;
else
    tao = min(0,(x_t1*day_weight-epsilon)/(norm(x_t1-mean(x_t1)))^2);
end
day_weight = day_weight - tao*(x_t1-mean(x_t1)*ones(size(x_t1)))';
day_weight = simplex_projection(day_weight,1);

end


function [out] = evap_21(p1,p2,S,Ep,dt)
%evap_21

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Threshold-based evaporation with constant minimum rate
% Constraints:  f <= S/dt
% @(Inputs):    p1   - wilting point (1st threshold) [mm]
%               p2   - 2nd threshold as fraction of wilting point [-]
%               S    - current storage [mm]
%               Ep   - potential evapotranspiration rate [mm/d]
%               dt   - time step size [d]

out = min(max(p2,min(S./p1,1)).*Ep,S./dt);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_IRB1200(robot, T)
%   Solves the inverse kinematic problem for the ABB IRB 1200 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_IRB1200 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   abb=load_robot('abb', 'IRB1200');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(abb, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(abb, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(abb, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%
%   Author: Arturo Gil Aparicio
%           Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60� to -219�, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi- eta;
q3(2) = pi - phi + eta;


classdef SMD10 < PROBLEM
% <multi> <real> <constrained> <bilevel>
% Bilevel optimization problems proposed by Sinha, Malo, and Deb
% maxFElower --- 500 --- Maximum number of lower level function evaluations for each solution

%------------------------------- Reference --------------------------------
% A. Sinha, P. Malo, K. Deb, Test problem construction for single-objective
% bilevel optimization, Evolutionary Computation, 2014, 22(3): 439-477.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(SetAccess = private)
        maxFElower; % Maximum number of lower level function evaluations for each solution
        DU;         % Number of decision variables of the upper level
        DL;         % Number of decision variables of the lower level
        C;       	% Number of upper constraints
        p;          % The length of xu1
        q;          % The length of xl1
        r;          % The length of xu2 and xl2
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.maxFElower = obj.ParameterSet(500);
            obj.M = 2;
            if isempty(obj.D); obj.D = 5; end
            obj.DU = floor(obj.D/2);
            obj.DL = obj.D - obj.DU;
            obj.C  = obj.DU;
            obj.r  = floor(obj.DU/2);
            obj.p  = obj.DU - obj.r;
            obj.q  = obj.DL - obj.r;
            obj.lower    = [-5*ones(1,obj.DU), -5*ones(1,obj.q), -pi/2*ones(1,obj.r)+1e-6];
            obj.upper    = [10*ones(1,obj.DU), 10*ones(1,obj.q),  pi/2*ones(1,obj.r)-1e-6];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate upper level and lower level objective values
        function PopObj = CalObj(obj,PopDec)
            xu1 = PopDec(:,1:obj.p);
            xu2 = PopDec(:,obj.p+1:obj.p+obj.r);
            xl1 = PopDec(:,obj.p+obj.r+1:obj.p+obj.r+obj.q);
            xl2 = PopDec(:,obj.p+obj.r+obj.q+1:end);
            % Calculate  UL objective values
            PopObj(:,1) = sum((xu1-2).^2,2) + sum(xl1.^2,2) + sum((xu2-2).^2,2) - sum((xu2-tan(xl2)).^2,2);
            % Calculate  LL objective values
            PopObj(:,2) = sum(xu1.^2,2) + sum((xl1-2).^2,2) + sum((xu2-tan(xl2)).^2,2);
        end
        %% Calculate upper level and lower level constraint violations
        function PopCon = CalCon(obj,PopDec)
            xu1 = PopDec(:,1:obj.p);
            xu2 = PopDec(:,obj.p+1:obj.p+obj.r);
            xl1 = PopDec(:,obj.p+obj.r+1:obj.p+obj.r+obj.q);
            xl2 = PopDec(:,obj.p+obj.r+obj.q+1:end);
            % Upper level constraint violation
            PopCon(:,1:obj.p) = -xu1 - xu1.^3 + sum(xu1.^3,2) + sum(xu2.^3,2);
            PopCon(:,obj.p+1:obj.p+obj.r) = -xu2 - xu2.^3 + sum(xu2.^3,2) + sum(xu1.^3,2);
            % Lower level constraint violation
            PopCon(:,obj.p+obj.r+1:obj.p+obj.r+obj.q) = -xl1 - xl1.^3 + sum(xl1.^3,2);
        end
        %% Calculate lower level objective values
        function llPopulation = EvaluationLower(obj,varargin)
            PopDec            = obj.CalDec(varargin{1});
            PopObj            = obj.CalObj(PopDec);
            PopObj(:,1)       = nan;
            PopCon            = obj.CalCon(PopDec);
            PopCon(:,1:obj.C) = nan;
            llPopulation      = SOLUTION(PopDec,PopObj,PopCon,varargin{2:end});
        end
    end
end


function y = obliq(t)

% function to compute mean obliquity of the ecliptic in arcseconds
% capitaine et al. (2003), astronomy and astrophysics 412, 567-586,
% expression from eq. (39) with obliquity at j2000.0 taken from
% eq. (37) or table 8

% input

%  t = tdb julian centuries

% output

%  y = mean obliquity of the ecliptic in arcseconds

% ported from NOVAS 3.0

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

y = (((( - 0.0000000434d0 * t ...
    - 0.000000576d0) * t ...
    + 0.00200340d0) * t ...
    - 0.0001831d0) * t ...
    - 46.836769d0) * t + 84381.406d0;


function [Pa] = atm2Pa(atm)
% Convert pressure from atmospheres to pascals.
% Chad A. Greene 2012, it's www.chadagreene.com if you're painfully bored.
Pa = atm*101325;


function kronrod_test02 ( )

%*****************************************************************************80
%
%% KRONROD_TEST02 tests the code for the even case N = 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 August 2010
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1,  'KRONROD_TEST02\n' );
  fprintf ( 1,  '  Request KRONROD to compute the Gauss rule\n' );
  fprintf ( 1,  '  of order 4, and the Kronrod extension of\n' );
  fprintf ( 1,  '  order 4+5=9.\n' );

  tol = 0.000001;

  [ x, w1, w2 ] = kronrod ( n, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  KRONROD returns 3 vectors of length %d\n', n + 1 );
  fprintf ( 1, '\n' );
  fprintf ( 1,  '     I      X               WK              WG\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n + 1
    fprintf ( 1, '  %4d  %14f  %14f  %14f\n', i, x(i), w1(i), w2(i) );
  end

  return
end


function q=v_qrdivide(q1,q2)
%V_QRDIVIDE divdes two real quaternions q=[q1,q2]
%
% Inputs:
%
%     q1(4,1), q2(4,1)  Two real quaternions in the form [r, i, j, k]' where i^2=j^2=k^2=ijk=-1
%
% Outputs:
%
%     q(4,1)   Quotient of q1/q2 such that q1=q*q2.
%              Note that q*q2 ~= q2*q since quaternion multiplication does not commute.

%      Copyright (C) Mike Brookes 2000-2008
%      Version: $Id: v_qrdivide.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=[5 8 9 10 15 13];
    b=[6 7 11 12 14 16];
    c=[1 2 3 4 6 7 11 12 16 14];
    d=[1 2 3 4 5 8 9 10 13 15];
end
if nargin<2
    %    just take the inverse of the only input argument
    q=q1/(q1'*q1);
    q(2:4)=-q(2:4);
else
    %    invert q2 and do a multiply
    q=q2/(q2'*q2);
    q(2:4)=-q(2:4);
    t=q1*q.';
    s=zeros(4,4);
    s(a)=-t(b);
    s(c)=t(d);
    q=sum(s,2);
end


function [scimat, sigma] = scimat_estimate_bias_field(scimat, x, a, sigma, FASTINTERP)
% SCIMAT_ESTIMATE_BIAS_FIELD  Estimate MRI bias field.
%
%   This function provides an estimate of the bias field from a magnetic
%   resonance image (MRI).
%
%   The function samples a SCIMAT image in locations provided by the
%   user (typically, corresponding to background voxels), and then creates
%   another image that interpolates the sampled intensity values.
%
%   The locations can be selected e.g. using our Spline Tool extension to
%   the Seg3D platform (click the points, and then export the control
%   points).
%
%   Note that the interpolant is an interpolating thin-plate spline (TPS),
%   but instead of just sampling the selected voxels, a neighbourhood is
%   sampled, and used to locally low-pass filter the image with a Gaussian
%   filter. Thus, this function is somehow robust in the presence of noise.
%   Alternatively, this function could use an approximating TPS instead of
%   an interpolating one.
%
% SCIMAT2 = scimat_estimate_bias_field(SCIMAT, X)
%
%   SCIMAT is an image (see "help scimat" for details).
%
%   X is a 3-colum matrix with the real world coordinates of the sampling
%   points. Note that each point is rounded to the closest voxel centre.
%
%   SCIMAT2 is the estimated bias field in SCIMAT format.
%
% SCIMAT2 = scimat_estimate_bias_field(SCIMAT, X, A, SIGMA, FASTINTERP)
%
%   A is a scaling factor. Because using the TPS to interpolate all voxels
%   can be rather slow, and the bias field is anyway a slow varying field,
%   it's convenient to first quickly reduce the image size by A (using
%   bilinear interpolation), interpolate the bias field in the smaller
%   image with the TPS, and then expand to the original size. By default, A
%   = 1.0 and no rescaling is used. If A has 1 value, then the same scaling
%   is applied to all dimensions. If A has 3 values, each value is used to
%   scale one dimension.
%
%   Note: Small values of A imply a large SIGMA to avoid aliasing.
%   Internally, this attempts to create a large Gaussian filter, and the
%   computer may run out of memory. Larger values of A require a lot of
%   memory from the thin-plate spline, and this may give an out of memory
%   error too.
%
%   SIGMA is the standard deviation of the Gaussian filter. By default,
%   sigma = 2*sqrt(-2 ln(alpha)) / a, alpha=1/sqrt(2)*ones(1,3), so that
%   the Gaussian filter has a 3dB drop at the cut-off frequency in each
%   dimension (see Note 1 for details).
%
%   FASTINTERP is a boolean to decide whether fast (more memory consuming)
%   thin-plate spline interpolation should be used. By default,
%   FASTINTERP=true. If you run out of memory, try setting it to false.
%
% [SCIMAT2, SIGMA] = ...
%
%   SIGMA as an output is the sigma value (in voxel units) used for the
%   Gaussian filter.
%
% ======
% Note 1
% ======
%
% The Gaussian filter in the frequency domain is
%
%   F(u) = 1/sqrt(2*pi) * exp(-u^2 *sigma^2 / 2)
%
% So if we want our filter to have a drop alpha at cut-off frequency B'
%
%   exp(-B'^2 * sigma^2 / 2) = alpha
%
% then
%
%   sigma = sqrt(-2 ln(alpha)) / B'
%
% Because of the Nyquist theorem, in order to prevent aliasing, we require
% the cut-off frequency to be
%
%   B' < fs' / 2 = a / (2*dx)
%
% then the standard deviation for the Gaussian must be
%
%   sigma > 2*sqrt(-2 ln(alpha)) / fs
%
%   sigma > 2*sqrt(-2 ln(alpha)) * dx / a
%
% There are two free parameters:
%
%   * alpha: filter drop at cut-off frequency (alpha=1/sqrt(2) makes a 3 dB
%            cut-off point)
%
%   * a: scaling factor for the image (a=.5, image becomes half size)
%
%  and a parameter that depends on the image
%
%   * dx: voxel size, image resolution; because sigma is given in pixel
%     units, dx = 1 for any image.
%
% ======
% Note 2
% ======
%
% The Gaussian filter in the space domain is
%
%   f(x) = 1/(sqrt(2*pi)*sigma) * exp(-x^2 / (2*sigma^2))
%
% The Gaussian tails tend asymptotically to 0, so we need to truncate the
% filter at some point. For sigma = 1.0, the filter tail at
% f(4)=1.3383e-04, which is a good compromise between having a small
% numerical error and a small filter.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2011-2014 University of Oxford
% Version: 0.2.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 5);
nargoutchk(0, 2);

% defaults
alpha = 1/sqrt(2)*ones(1,3);
if (nargin < 3 || isempty(a))
    a = 1.0;
end
if (length(a) == 1)
    a = a([1 1 1]);
elseif (length(a) ~= 3)
    error('A must have 1 or 3 values')
end
if (nargin < 4 || isempty(sigma))
    sigma = 2*sqrt(-2*log(alpha)) ./ a;
end
if (nargin < 5 || isempty(FASTINTERP))
    FASTINTERP = true;
end

% squeeze volume
scimat = scimat_squeeze(scimat);

% size of input volume
nin = [scimat.axis.size];

% recompute the scaling factor so that we obtain an integer number of
% voxels in the output volume
nout = round(nin .* a);
a = nout ./ nin;

% Seg3D provides continuous point coordinates over the whole image volume.
% But in fact, intensity values correspond to the voxel centre, so we have
% to convert the coordinates to indices and round them
x = scimat_world2index(x, scimat);
x = round(x);

% compute low-pass anti-aliasing filter
% sigma = 2 * sqrt(-2 * log(alpha)) ./ a .* dx; % dx not needed, sigma
% given in pixel units
halfsz = round(4 * sigma);
h = fspecial3('gaussian', halfsz*2+1, sigma);

% loop each sampling point. For each sampling point we want to sample a
% whole cube around it, so that we can apply a low-pass filter without
% having to filter the whole image volume
v = zeros(size(x, 1), 1);
for I = 1:size(x, 1)

    % get indices of the area around the sampling point to sample. If the
    % sampling point is too close to the edge, we have to be careful to not
    % overflow
    idxr = max(1, x(I, 1)-halfsz(1)):min(nin(1), x(I, 1)+halfsz(1));
    idxc = max(1, x(I, 2)-halfsz(2)):min(nin(2), x(I, 2)+halfsz(2));
    idxs = max(1, x(I, 3)-halfsz(3)):min(nin(3), x(I, 3)+halfsz(3));

    % if necessary, crop low-pass filter so that it has the same size as
    % the sampling area
    hbox = h(...
        max(1, idxr - x(I, 1) + halfsz(1) + 1), ...
        max(1, idxc - x(I, 2) + halfsz(2) + 1), ...
        max(1, idxs - x(I, 3) + halfsz(3) + 1));

    % extract image area around the sampling point
    im = scimat.data(idxr, idxc, idxs);

    % compute low-pass filtered intensity of current voxel
    v(I) = sum(hbox(:) .* im(:)) / sum(hbox(:));

%     v(I) = sum(hbox(:) .* im(:)); % this is how Matlab's imfilter() does
%     it, which is incorrect near the edges because it reduces the value of
%     the pixels

end

% clear memory
clear h hbox im

% convert back to coordinates
x = scimat_index2world(x, scimat);

% compute coordinates of the two extreme voxels that define the image
% volume
cmin = scimat_index2world([1 1 1], scimat);
cmax = scimat_index2world(nin, scimat);

% if we have point coordinates with values like 100, 400, matrix L for the
% thin-plate spline weight computation is badly scaled. Thus, we make use
% of the invariability of thin-plate splines to scaling and make all the
% coordinate values <= 5.0 (if we make them <= 1.0, interpn gives an error
% saying that they grid is not monotonic)
K = max(cmax)/5;

% the warp will be defined as from the xyz-points to the corresponding
% intensity values

% compute what would be the size of the image if we downsample it
nmid = round(nin .* a);

% compute weights for thin-plate spline interpolation
w = pts_tps_weights(x/K, v);

% generate grid
[gx, gy, gz] = meshgrid(...
    linspace(cmin(1)/K, cmax(1)/K, nmid(2)), ...
    linspace(cmin(2)/K, cmax(2)/K, nmid(1)), ...
    linspace(cmin(3)/K, cmax(3)/K, nmid(3)));

% clear memory
scimat.data = [];

% interpolate intensity values for each point in the grid
scimat.data = pts_tps_map(x/K, v, [gx(:) gy(:) gz(:)], w, FASTINTERP, false);

% clear memory
clear gx gy gz

% reshape the interpolated values to go from a vector to an image volume
scimat.data = reshape(single(scimat.data), nmid);

% recover original size
scimat.data = tformarray(scimat.data, ...
    maketform('affine', [1/a(1) 0 0 0; 0 1/a(2) 0 0; 0 0 1/a(3) 0; 0 0 0 1]), ...
    makeresampler('linear', 'replicate'), ...
    1:length(scimat.axis), 1:length(scimat.axis), ...
    nin, [], []);


function loglik = compute_log_lik(P, S_bar, V, E_z, E_zz, RO, c, Tr, sigma_sq)

[K, T] = size(E_z);
J = size(S_bar, 2);

M_t = zeros(2*J, K);

loglik = - 0.5*T * (2*J*log(sigma_sq));
for t = 1:T,
   %%%%%%%%%%%%%%%%%%%%%%%%% Changed code here %%%%%%%%%%%%%%%%%%%%%%
   %R_t = RO{t};
   R_t  = c(t,1)*RO{t};
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   Sdef = S_bar;
   for kk = 1:K,
      Sdef = Sdef + E_z(kk,t)*V((kk-1)*3+[1:3],:);

      M_t(1:J, kk) = (R_t(1,:)*V((kk-1)*3+[1:3],:))';
      M_t(J+1:end, kk) = (R_t(2,:)*V((kk-1)*3+[1:3], :))';
   end;

   invSigmaSq_p = eye(2*J)./sigma_sq;

   f_bar_t = R_t(1:2,:)*S_bar;
   f_bar_t = [f_bar_t(1,:) f_bar_t(2,:)]';

   f_t = [P(t, :) P(t+T, :)]';
   t_vect_t = [Tr(t,1)*ones(J,1); Tr(t,2)*ones(J,1)];

   covZ_t = E_zz((t-1)*K+1:t*K,:) - E_z(:,t)*E_z(:,t)';
   loglik = loglik - 0.5*(((f_t-f_bar_t-t_vect_t)./sigma_sq)'*(f_t-f_bar_t-t_vect_t)) + (((f_t-f_bar_t-t_vect_t)'./sigma_sq)*M_t*E_z(:,t)) ...
      - 0.5*trace(((M_t./sigma_sq)'*M_t) * E_zz((t-1)*K+1:t*K,:)) - 0.5*log(det(covZ_t));
end


function [condMu,condCov] = prtConditionalMvnMuCov(x,indices,globalMu,globalCov)
%[condMu,condCov] = conditionalMuCov(x,indices,globalMu,globalCov)

indices2 = indices;
indices1 = setdiff(1:length(globalMu),indices);

mu2 = globalMu(indices2);
mu1 = globalMu(indices1);

cov22 = globalCov(indices2,indices2);
cov12 = globalCov(indices1,indices2);
cov21 = globalCov(indices2,indices1);
cov11 = globalCov(indices1,indices1);

condMu = mu1 + (cov12*cov22^-1*(x - mu2)')';
condCov = cov11 - cov12*cov22^-1*cov21;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomEdge.m                                  |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Triangular quadrature and integration         |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
Nvtx = 1e3;
rho  = 1;
tol  = 1e-3;

% Square mesh
mesh = mshSegment(Nvtx,rho);

% Domain
omega = dom(mesh,3);

% Finite element space
u = fem(mesh,'P1');
v = fem(mesh,'P1');

% Graphical representation
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(omega)
plot(u,'or')
axis equal
alpha(0.1)
view(0,90)

% Numerical functions
Fx   = @(X) ones(size(X,1),1);
Fx3  = {Fx,Fx,Fx};
Fxy  = @(X,Y) (X(:,1)==Y(:,1));
Fxy3 = {Fxy,Fxy,Fxy};
Gxy  = @(X,Y) 1./(4*pi) * femGreenKernel(X,Y,'[exp(ikr)/r]',5);
Gxy3 = {Gxy,Gxy,Gxy};

%%%%%%%%%%%%%%% 2 ARGUMENTS %%%%%%%%%%%%%%%
disp('=========== 2 ARGUMENTS  ============')

% \int_{mesh(x)} f(x) dx
ref = integral(omega,Fx);
abs(ref-1)

% \int_{mesh(x)} f3(x) dx
sol = integral(omega,Fx3);
norm(sol{1}-ref,'inf')

%%%%%%%%%%%%%%% 3 ARGUMENTS %%%%%%%%%%%%%%%
disp('=========== 3 ARGUMENTS  ============')

% \int_{mesh(y)} f(x,y) dy
ref = integral(omega.qud,omega,Fxy);
abs(sum(ref,1)-1)

% \int_{mesh(y)} f3(x,y) dy
sol = integral(omega.qud,omega,Fxy3);
norm(sol{1}-ref,'inf')

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

% \int_{mesh(x)} f(x,y) dx
ref = integral(omega,omega.qud,Fxy);
abs(sum(ref,2)-1)

% \int_{mesh(y)} f3(x,y) dx
sol = integral(omega,omega.qud,Fxy3);
norm(sol{1}-ref,'inf')

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

% \int_{mesh(x)} f(x) psi(x) dx
ref = integral(omega,Fx,v);
abs(sum(ref,2) - 1)

% \int_{mesh(x)} f3(x) psi(x) dx
sol = integral(omega,Fx3,v);
norm(sol{1}-ref,'inf')

% \int_{mesh(x)} f(x) ntimes(psi(x)) dx
sol = integral(omega,Fx,ntimes(v));
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} f3(x) ntimes(psi(x)) dx
sol = integral(omega,Fx3,ntimes(v));
norm(sol-ref,'inf')

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

% \int_{mesh(x)} psi(x)' f(x)  dx
ref = integral(omega,u,Fx);
abs(sum(ref,1) - 1)

% \int_{mesh(x)} psi(x)' f3(x)  dx
sol = integral(omega,u,Fx3);
norm(sol{1}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f(x)  dx
sol = integral(omega,ntimes(u),Fx);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f3(x)  dx
sol = integral(omega,ntimes(u),Fx3);
norm(sol-ref,'inf')

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

% \int_{mesh(x)} psi(x)' psi(x) dx
ref = integral(omega,u,v);
abs(sum(sum(ref,1),2) - 1)

% \int_{mesh(x)} ntimes(psi(x))' psi(x) dx
sol = integral(omega,ntimes(u),v);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} psi(x)' ntimes(psi(x)) dx
sol = integral(omega,u,ntimes(v));
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' ntimes(psi(x)) dx
sol = integral(omega,ntimes(u),ntimes(v));
norm(sol-ref,'inf')

%%%%%%%%%%%%%%% 4 ARGUMENTS %%%%%%%%%%%%%%%
disp('=========== 4 ARGUMENTS  ============')

% \int_{mesh(x)} psi(x)' f(x) psi(x) dx
ref = integral(omega,u,Fx,v);
abs(sum(sum(ref,1),2) - 1)

% \int_{mesh(x)} ntimes(psi(x))' f(x) psi(x) dx
sol = integral(omega,ntimes(u),Fx,v);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} psi(x)' f3(x) psi(x) dx
sol = integral(omega,u,Fx3,v);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} psi(x)' f(x) ntimes(psi(x)) dx
sol = integral(omega,u,Fx,ntimes(v));
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f3(x) psi(x) dx
sol = integral(omega,ntimes(u),Fx3,v);
norm(sol-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f(x) ntimes(psi(x)) dx
sol = integral(omega,ntimes(u),Fx,ntimes(v));
norm(sol-ref,'inf')

% \int_{mesh(x)} (psi(x))' f3(x) ntimes(psi(x)) dx
sol = integral(omega,u,Fx3,ntimes(v));
norm(sol-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' (f3(x)) ntimes(psi(x)) dx
sol = integral(omega,ntimes(u),Fx3,ntimes(v));
norm(sol,'inf')

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

% \int_{mesh(y)} f(x,y) psi(y) dy
ref = integral(omega.qud,omega,Fxy,v);
abs(sum(sum(ref,1),2) - 1)

% \int_{mesh(y)} f(x,y) ntimes(psi(y)) dy
sol = integral(omega.qud,omega,Fxy,ntimes(v));
norm(sol{2}-ref,'inf')

% \int_{mesh(y)} f(3x,y) psi(y) dy
sol = integral(omega.qud,omega,Fxy3,v);
norm(sol{2}-ref,'inf')

% \int_{mesh(y)} f3(x,y) ntimes(psi(y)) dy
sol = integral(omega.qud,omega,Fxy3,ntimes(v));
norm(sol-ref,'inf')

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

% \int_{mesh(x)} psi(x)' f(x,y) dx
ref = integral(omega,omega.qud,u,Fxy);
abs(sum(sum(ref,1),2) - 1)

% \int_{mesh(x)} psi(x)' f3(x,y) dx
sol = integral(omega,omega.qud,u,Fxy3);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f(x,y) dx
sol = integral(omega,omega.qud,ntimes(u),Fxy);
norm(sol{2}-ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' f3(x,y) dx
sol = integral(omega,omega.qud,ntimes(u),Fxy3);
norm(sol-ref,'inf')

%%%%%%%%%%%%%% 5 ARGUMENTS %%%%%%%%%%%%%%%
disp('=========== 5 ARGUMENTS  ============')

% \int_{mesh(y)} G(x,y) psi(y) dy
ref = integral(mesh.ctr,omega,Gxy,v);
sol = integral(mesh.ctr,omega,Gxy,v,tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% % \int_{mesh(y)} G3(x,y) psi(y) dy
sol = integral(mesh.ctr,omega,Gxy3,v,tol);
norm(full(sol{1})-ref,'inf')./norm(ref,'inf')

% \int_{mesh(y)} G(x,y) ntimes(psi(y)) dy
sol = integral(mesh.ctr,omega,Gxy,ntimes(v),tol);
norm(full(sol{2})-ref,'inf')./norm(ref,'inf')

% \int_{mesh(y)} G3(x,y) ntimes(psi(y)) dy
sol = integral(mesh.ctr,omega,Gxy3,ntimes(v),tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

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

% \int_{mesh(x)} psi(x)' G(x,y) dx
ref = integral(omega,mesh.ctr,u,Gxy);
sol = integral(omega,mesh.ctr,u,Gxy,tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} psi(x)' G3(x,y) dx
sol = integral(omega,mesh.ctr,u,Gxy3,tol);
norm(full(sol{1})-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' G(x,y) dx
sol = integral(omega,mesh.ctr,ntimes(u),Gxy,tol);
norm(full(sol{2})-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} ntimes(psi(x))' G3(x,y) dx
sol = integral(omega,mesh.ctr,ntimes(u),Gxy3,tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

%%%%%%%%%%%%%%% 6 ARGUMENTS %%%%%%%%%%%%%%%
disp('=========== 6 ARGUMENTS  ============')

% \int_{mesh(x)} \int_{mesh(y)} psi(x)' G(x,y) psi(y) dx dy
ref = integral(omega,omega,u,Gxy,v);
sol = integral(omega,omega,u,Gxy,v,tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} \int_{mesh(y)} ntimes(psi(x))' G(x,y) psi(y) dx dy
ref = integral(omega,omega,ntimes(u),Gxy,v);
sol = integral(omega,omega,ntimes(u),Gxy,v,tol);
norm(full(sol{2})-ref{2},'inf')./norm(ref{2},'inf')

% \int_{mesh(x)} \int_{mesh(y)} psi(x)' G3(x,y) psi(y) dx dy
ref = integral(omega,omega,u,Gxy3,v);
sol = integral(omega,omega,u,Gxy3,v,tol);
norm(full(sol{2})-ref{2},'inf')./norm(ref{2},'inf')

% \int_{mesh(x)} \int_{mesh(y)} psi(x)' G(x,y) ntimes(psi(y)) dx dy
ref = integral(omega,omega,u,Gxy,ntimes(v));
sol = integral(omega,omega,u,Gxy,ntimes(v),tol);
norm(full(sol{2})-ref{2},'inf')./norm(ref{2},'inf')

% \int_{mesh(x)} \int_{mesh(y)} ntimes(psi(x))' G3(x,y) psi(y) dx dy
ref = integral(omega,omega,ntimes(u),Gxy3,v);
sol = integral(omega,omega,ntimes(u),Gxy3,v,tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} \int_{mesh(y)} psi(x)' G3(x,y) ntimes(psi(y)) dx dy
ref = integral(omega,omega,u,Gxy3,ntimes(v));
sol = integral(omega,omega,u,Gxy3,ntimes(v),tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} \int_{mesh(y)} ntimes(psi(x))' G(x,y) ntimes(psi(y)) dx dy
ref = integral(omega,omega,ntimes(u),Gxy,ntimes(v));
sol = integral(omega,omega,ntimes(u),Gxy,ntimes(v),tol);
norm(full(sol)-ref,'inf')./norm(ref,'inf')

% \int_{mesh(x)} \int_{mesh(y)} ntimes(psi(x))' G3(x,y) ntimes(psi(y)) dx dy
ref = integral(omega,omega,ntimes(u),Gxy3,ntimes(v));
sol = integral(omega,omega,ntimes(u),Gxy3,ntimes(v),tol);
norm(full(sol)-ref,'inf')

disp('~~> Michto gypsilab !')


function [apexPoints,signVals,exitCode]=trace2EarthMagApex(zCart,modSelParam,a,f,RelTol,AbsTol,maxSteps)
%TRACE2EARTHMAGAPEX Given a point in the in the International Terrestial
%           Reference System (ITRS), a type of Earth-Centered Earth-Fixed
%           (ECEF) system trace along the magnetic field line of the Earth
%           until the apex is found. The apex is the highest point of the
%           magnetic field line above the reference ellipsoid. Such tracing
%           is used for determining apex coordinates and quasi-dipole
%           coordinates, both of which play a role in ionospheric analyses.
%
%INPUTS: zCart One or more points given in Cartesian coordinates in the
%              ITRS with units of meters. zCart is a 3XN matrix with each
%              column having the format [x;y;z]. One would generally
%              assume that points are not underground.
%  modSelParam An optional parameter selecting or providing the magnetic
%              field model to use. If omitted or an empty matrix is
%              passed, the International Geomagnetic Reference Field
%              (IGRF) at the latest epoch of the model is used via the
%              function getIGRFCoeffs. Possible other values are
%              1) year The parameters of the IGRF for the specified epoch
%                 year are used. The year is in the Gregorian calendar and
%                 is specified as noted in the comments to the
%                 getIGRFCoeffs  function.
%              2) modSelParam is a structure where modSelParam.algorithm
%                 selects the algorithm to use Possible values for
%                 modSelParam.algorithm are 'IGRF', 'WMM' and 'preloaded'.
%                 If modSelParam.algorithm is 'IGRF or 'WMM', then the IGRF
%                 or the World Magnetic Model (WMM) is used at the
%                 fractional year specified by modSelParam.year (using the
%                 function getIGRFCoeffs or getWMMCoeffs). If
%                 modSelParam.algorithm is preloaded, then the parameters
%                 for the model are given by the modSelParam.C,
%                 modSelParam.S, modSelParam.a, and modSelParam.c, where
%                 the parameters have the same format as the fully
%                 normalized outputs of getIGRFCoeffs or getWMMCoeffs. See
%                 comments below for the use of custom coeffients.
%            a The semi-major axis of the reference ellipsoid. The apex is
%              defined in terms of a maximum magnetic field line height
%              above the reference ellipsoid. If this argument is omitted
%              or an empty matrix is passed, the value in
%              Constants.WGS84SemiMajorAxis is used.
%            f The flattening factor of the reference ellipsoid. If this
%              argument is omitted or an empty matrix is passed, the value
%              in Constants.WGS84Flattening is used.
%       RelTol The maximum relative error tolerance allowed for the
%              Runge-Kutta algorithm to trace the path towards the apex.
%              If omitted or an empty matrix is passed, the default value
%              of 1e-6 is used.
%       AbsTol The absolute error tolerance allowed, a positive scalar.
%              If omitted or an empty matrix is passed, the default value
%              of 1e-9 is used. This is used in the Runge-Kutta algorithm
%              to trace along the path towards the apex, and it is used
%              for the fminbnd function to find the apex location once it
%              has been bounded.
%     maxSteps The maximum allowable number of steps to perform the
%              adaptive Runge-Kutta integration along a magnetic field
%              line to find the apex. If omitted or an empty matrix is
%              passed, the default of 1024 is used.
%
%OUTPUS: apexPoints A 3XN matrix of the apex points corresponding to the
%               points in zCart. The apex points are obtained by tracing the
%               magnetic field line starting from zCart until reaching the
%               point farthest from the reference ellipsoid. Near the
%               geomagnetic poles, these points will be very far from the
%               Earth. Thus, if the point ends up being 1e37 m or more away
%               from the Earth, then integration is stopped and the
%               components of apexPoints are just set to Inf (rather than
%               returning an error). If  exitCode~=0, for any point, an
%               empty matrix is returned.
%      signVals An NX1 vector of values (+1, or -1) indicating whether the
%               direction to the apex point along the field line was
%               obtained by tracing in the direction of the magnetic field
%               or opposite to it. This can be used to determine which
%               magnetic hemisphere the points in zCart are in, because
%               +1 means that one starts closer to the magnetic North pole
%               (geographic South pole), and -1 means that one starts
%               closer to the magnetic South pole (geographic North pole).
%               If an error occurs, an empty matrix is returned.
%      exitCode A NX1 vector of codes indicating whether an error occurred.
%               If an error occurs, then the algorithm is halted at the
%               point causing the error.
%               0: Integration was successful.
%               1: Unable to get a small enough step size.
%               2: Apex not found within the maximum number of iterations.
%               3: Non-finite number encountered.
%               4: Problems performing a line search to the peak point.
%
%Basically, starting at the given point, one traces along a magnetic field
%line until reaching the highest point above the reference ellipsoid as
%defined in [2]. The tracing is done using an order 5(4) adaptive
%Runge-Kutta method until the direction of the field line is descending, in
%which case the peak is bounded between the last two points on the field
%line. An interpolation routine is then used with fminbnd to find the peak.
%This assumes that there is only one peak, which is safe to assume with the
%IMM and WMM. On the other hand, if using a custom magnetic field model,
%there might be more peaks and this function will not return the correct
%solution.
%
%Note that in [1], the definition of the apex is given in terms of a
%maximum height or a magnetic field line above the geoid, not above the
%reference ellipsoid (which is an approximation of the geoid). The
%definition with respect to the geoid seems to not be used in the
%literature. Moreover, the fast undulations of the geoid would make a
%magnetic field line have more than one peak, requiring that one find every
%peak to determine where the apex is.
%
%REFERENCES:
%[1] T. E. VanZandt, W. L. Clark, and J. M. Warnock, "Magnetic apex
%    coordinates: A magnetic coordinate system for the ionospheric f2
%    layer," Journal of Geophysical Research, vol. 77, no. 13, pp. 2406-
%    2411, 1 May 1972.
%[2] A. D. Richmond, "Ionospheric electrodynamics using magnetic apex
%    coordinates," Journal of Geomagnetism and Geoelectricity, vol. 47,
%    no. 2, pp. 191-212, 1995.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(modSelParam))
    %If no magnetic model is specified, use the IGRF at the reference
    %epoch.
    [C,S,aMagMod,cMagMod]=getIGRFCoeffs([],true);
elseif(isa(modSelParam,'double'))
    %If just a scalar is passed, assume that it is the epoch year for the
    %IGRF.
    [C,S,aMagMod,cMagMod]=getIGRFCoeffs(modSelParam,true);
else
    %Otherwise, a structrue should be passed indicating the model and the
    %year.
    switch(modSelParam.algorithm)
        case 'WMM'%World Magnetic Model
            [C,S,aMagMod,cMagMod]=getWMMCoeffs(modSelParam.year,true);
        case 'IGRF'%Intergnational Geomagnetic Reference Field
            [C,S,aMagMod,cMagMod]=getIGRFCoeffs(modSelParam.year,true);
        case 'preloaded'%Custom coefficients.
            C=modSelParam.C;
            S=modSelParam.S;
            aMagMod=modSelParam.a;
            cMagMod=modSelParam.c;
        otherwise
            error('Unknown model specified')
    end
end

if(nargin<3||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<4||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<5||isempty(RelTol))
    RelTol=1e-6;
end

if(nargin<6||isempty(AbsTol))
    AbsTol=1e-9;
end

if(nargin<7||isempty(maxSteps))
    maxSteps=1024;
end

%The parameters selecting the Runge-Kutta algorithm to use for the
%numerical integration.
order=5;
solutionChoice=0;

numPoints=size(zCart,2);
apexPoints=zeros(3,numPoints);
signVals=zeros(numPoints,1);
exitCode=zeros(numPoints,1);
for curPoint=1:numPoints
    zITRS=zCart(:,curPoint);

    %The initial stepsize is arbitrarily set to 10km.
    deltaS=10e3;
    %The maximum stepsize is arbitrarily set very high, so that it can be
    %quickly detected when the apex is extremely far away, because the
    %chosen point is very near the magnetic pole.
    deltaSMaxMag=1e36;

    xCur=zITRS;%The initial Cartesian location.
    sCur=0;%Distance traveled from the initial point is zero.

    %First, we have to decide which direction to go. We want to go in the
    %direction of increasing ellipsoidal height.
    dxdsCur=dxds(xCur,1,C,S,aMagMod,cMagMod);%The positive direction.

    uVert=getEllipsVert(zITRS,a,f);

    %If the angle between the direction and the local vertical is >90
    %degrees, then it is a decreasing direction. If it is greater, then it
    %is an increasing direction. This is the same as checking whether the
    %dot product is negative (the dot product is proportional to the cosine
    %of the angle).
    if(dot(uVert,dxdsCur)<0)
        signVal=-1;%Go the other way.
    else
        signVal=1;
    end
    signVals(curPoint)=signVal;

    %The path direction function.
    derivFun=@(x,s)dxds(x,signVal,C,S,aMagMod,cMagMod);

    %Now, we integrate along the path until the next point takes us to a
    %location where the direction along the magnetic field line is no longer
    %going in a direction of increasing ellipsoidal height. That means that we
    %have passed the apex. Thus, the apex height can be interpolated beteen
    %those final two points.
    apexPoint=[];
    for curStep=1:maxSteps
        xPrev=xCur;
        sPrev=sCur;

        %Arbitrary minimum step size.
        deltaSMinMag=2^4*eps(sCur);

        [deltaS,xCur,sCur,k,dxdsCur,exitCode(curPoint)]=performOneAdaptiveRKStep(xCur,sCur,derivFun,deltaS,deltaSMinMag,deltaSMaxMag,dxdsCur,order,solutionChoice,AbsTol,RelTol);

        if(exitCode(curPoint)~=0)
            signVals=[];
            apexPoints=[];
            return;
        end

        %Determine whether the new point is still a point of increasing
        %ellipsoidal height.
        uVert=getEllipsVert(xCur,a,f);

        %If the peak has been reached.
        if(dot(uVert,dxdsCur)<0)
            %Interpolate to find the peak.

            %Get a Hermite interpolating polynomial over the step. Note that the
            %interpolating polynomial takes a parameter from 0->1 indicating the
            %fraction of the distance traveled between sPrev and sCur.
            [interpPolyA,interpPolyC]=RKInterpPolys(xPrev,sPrev,xCur,sCur,derivFun,order,solutionChoice,k);

            %We will now do a simple line search to find where the direction of
            %the magnetic field vector is 90 degrees offset.
            costFun=@(sFracEst)levelCostFun(sFracEst,interpPolyA,interpPolyC,a,f);
            try
                options=optimset('TolX',AbsTol);
                minFrac=fminbnd(costFun,0,1,options);
            catch%If some error in fminbnd occurred.
                apexPoints=[];
                exitCode(curPoint)=4;
                return
            end

            apexPoint=polyValNewton(minFrac,interpPolyA,interpPolyC);

            break;
        elseif(Cart2Ellipse(xCur,[],a,f)>=1e37)
            %If the ellipsoidal height exceeds 1e37m, then just declare it
            %to be at infinity. This helps near the magnetic poles.
            apexPoint=[Inf;Inf;Inf];
        end
    end

    if(isempty(apexPoint))
       %If the maximum number of iterations was achieved without hitting the
       %apex point, then return with an error.
        apexPoints=[];
        signVals=[];
        exitCode(curPoint)=2;
        return;
    end

    %We have the apexPoint. We can now compute the apex coordinates.
    apexPoints(:,curPoint)=apexPoint;
    exitCode(curPoint)=0;
end

end

function uVert=getEllipsVert(xCart,a,f)
%Get a vector pointing in the direction of the ellipsoidal vertical. This
%requires the position to be converted into ellipsoidal coordinates.

    ellipsCur=Cart2Ellipse(xCart,[],a,f);
    justVertical=true;
    uVert=getENUAxes(ellipsCur,justVertical,a,f);
end

function costVal=levelCostFun(sFracEst,interpPolyA,interpPolyC,a,f)
%The cost function to determine when the angle between the local vertical
%and the magnetic field is 90 degrees. We are trying to drive the dot
%product to zero.

    xValCart=polyValNewton(sFracEst,interpPolyA,interpPolyC);
    dxdsFracCart=polyDerValNewton(sFracEst,interpPolyA,interpPolyC);

    uVert=getEllipsVert(xValCart,a,f);

    costVal=abs(dot(uVert,dxdsFracCart));
end

function derivVal=dxds(xCart,signVal,C,S,aMagMod,cMagMod)
%The function providing the derivative of position with respect to
%arclength for tracing out the field line.

    pointSpher=Cart2Sphere(xCart);
    [~,gradV]=spherHarmonicEval(C,S,pointSpher,aMagMod,cMagMod);
    B=-gradV;%The magnetic flux

    %Integration is performed in space, so the normalized B vector is the
    %position derivative for the differential equation. The problem of running
    %into a point where B=0 should not arise.
    derivVal=signVal*B/norm(B);

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 [ x, y, z, w ] = ld0302 ( )

%*****************************************************************************80
%
%% LD0302 computes the 302 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(302,1);
  y = zeros(302,1);
  z = zeros(302,1);
  w = zeros(302,1);
  a = 0.0;
  b = 0.0;
  v = 0.8545911725128148E-03;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.3599119285025571E-02;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.3515640345570105;
  v = 0.3449788424305883E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6566329410219612;
  v = 0.3604822601419882E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4729054132581005;
  v = 0.3576729661743367E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.9618308522614784E-01;
  v = 0.2352101413689164E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.2219645236294178;
  v = 0.3108953122413675E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.7011766416089545;
  v = 0.3650045807677255E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.2644152887060663;
  v = 0.2982344963171804E-02;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.5718955891878961;
  v = 0.3600820932216460E-02;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.2510034751770465;
  b = 0.8000727494073952;
  v = 0.3571540554273387E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.1233548532583327;
  b = 0.4127724083168531;
  v = 0.3392312205006170E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );

  return
end