function [yi, ypi, yppi] = qhermite(x, y, yp, ypp, xi, v)

% QHERMITE 1-D piecewise quintic Hermite spline
%    QHERMITE(X,Y,YP,YPP,XI,M) interpolates to find YI, the values of
%    the underlying function Y at the points in the array XI, using
%    piecewise quintic Hermite splines.  X and Y must be vectors
%    of length N.
%
%    V specifies how tangents are calculated when derivatives are not
%    specified.  V can be:
%       0 : Finite difference (default)
%       1 : Catmull-Rom spline
%
%    [YI,YPI,YPPI] = QHERMITE() also returns the interpolated
%    quartic derivative and cubic second derivative of the underlying
%    function Y at points XI.

% Joe Henning - Fall 2011

if (nargin < 6)
   v = 0;
end

if (v == 0)
   % precompute finite difference derivatives
   if (isempty(yp) && isempty(ypp))
      m = 0;
      yp = lfindiff (x, y);
      ypp = lfindiff (x, yp);
   elseif (isempty(yp))
      m = 1;
      yp = lfindiff (x, y);
   elseif (isempty(ypp))
      m = 2;
      ypp = lfindiff (x, yp);
   else
      m = -1;
   end
else
   if (isempty(yp) && isempty(ypp))
      m = 0;
   elseif (isempty(yp))
      m = 1;
   elseif (isempty(ypp))
      m = 2;
   else
      m = -1;
   end
end

n = length(x);

for i = 1:length(xi)
   % Find the right place in the table by means of a bisection.
   klo = 1;
   khi = n;
   while (khi-klo > 1)
      k = fix((khi+klo)/2.0);
      if (x(k) > xi(i))
         khi = k;
      else
         klo = k;
      end
   end

   h = x(khi) - x(klo);
   if (h == 0.0)
      fprintf('??? Bad x input to qhermite ==> x values must be distinct\n');
      yi(i) = NaN;
      ypi(i) = NaN;
      yppi(i) = NaN;
      continue;
   end

   if (m == -1)
      a = yp(klo);
      b = yp(khi);
      c = ypp(klo);
      d = ypp(khi);
   elseif (v == 0)
      % Finite difference
      a = yp(klo);
      b = yp(khi);
      c = ypp(klo);
      d = ypp(khi);
   elseif (m == 0)
      if (klo == 1)
         a = (y(khi) - y(klo))/h;
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
         c = ((y(khi+1) - y(khi))/(x(khi+1) - x(khi)) - (y(khi) - y(klo))/h)/...
             (x(khi+1) - x(klo));
         d = 2*(y(khi+1) - 2*y(khi) + y(klo) - (x(khi+1) - 2*x(khi) + x(klo))*b)/...
             ((x(khi+1) - x(khi))*(x(khi+1) - x(khi)) + h*h);
      elseif (khi == n)
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi) - y(klo))/h;
         c = 2*(y(khi) - 2*y(klo) + y(klo-1) - (x(khi) - 2*x(klo) + x(klo-1))*a)/...
             (h*h + (x(klo) - x(klo-1))*(x(klo) - x(klo-1)));
         d = ((y(khi) - y(klo))/h - (y(klo) - y(klo-1))/(x(klo) - x(klo-1)))/...
             (x(khi) - x(klo-1));
      else
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
         c = 2*(y(khi) - 2*y(klo) + y(klo-1) - (x(khi) - 2*x(klo) + x(klo-1))*a)/...
             (h*h + (x(klo) - x(klo-1))*(x(klo) - x(klo-1)));
         d = 2*(y(khi+1) - 2*y(khi) + y(klo) - (x(khi+1) - 2*x(khi) + x(klo))*b)/...
             ((x(khi+1) - x(khi))*(x(khi+1) - x(khi)) + h*h);
      end
   elseif (m == 1)
      c = ypp(klo);
      d = ypp(khi);
      if (klo == 1)
         a = (y(khi) - y(klo))/h;
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
      elseif (khi == n)
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi) - y(klo))/h;
      else
         a = (y(khi) - y(klo-1))/(x(khi) - x(klo-1));
         b = (y(khi+1) - y(klo))/(x(khi+1) - x(klo));
      end
   else
      a = yp(klo);
      b = yp(khi);
      if (klo == 1)
         c = ((y(khi+1) - y(khi))/(x(khi+1) - x(khi)) - (y(khi) - y(klo))/h)/...
             (x(khi+1) - x(klo));
         d = 2*(y(khi+1) - 2*y(khi) + y(klo) - (x(khi+1) - 2*x(khi) + x(klo))*b)/...
             ((x(khi+1) - x(khi))*(x(khi+1) - x(khi)) + h*h);
      elseif (khi == n)
         c = 2*(y(khi) - 2*y(klo) + y(klo-1) - (x(khi) - 2*x(klo) + x(klo-1))*a)/...
             (h*h + (x(klo) - x(klo-1))*(x(klo) - x(klo-1)));
         d = ((y(khi) - y(klo))/h - (y(klo) - y(klo-1))/(x(klo) - x(klo-1)))/...
             (x(khi) - x(klo-1));
      else
         c = 2*(y(khi) - 2*y(klo) + y(klo-1) - (x(khi) - 2*x(klo) + x(klo-1))*a)/...
             (h*h + (x(klo) - x(klo-1))*(x(klo) - x(klo-1)));
         d = 2*(y(khi+1) - 2*y(khi) + y(klo) - (x(khi+1) - 2*x(khi) + x(klo))*b)/...
             ((x(khi+1) - x(khi))*(x(khi+1) - x(khi)) + h*h);
      end
   end

   % Evaluate quintic Hermite polynomial
   t = (xi(i) - x(klo))/h;
   t2 = t*t;
   t3 = t2*t;
   t4 = t3*t;
   t5 = t4*t;
   h2 = h*h;
   z0 = -6*t5 + 15*t4 - 10*t3 + 1;
   z1 = -3*t5 + 8*t4 - 6*t3 + t;
   z2 = 0.5*(-t5 + 3*t4 - 3*t3 + t2);
   z3 = 1 - z0;
   z4 = -3*t5 + 7*t4 - 4*t3;
   z5 = 0.5*(t5 - 2*t4 + t3);

   yi(i) = z0*y(klo) + z1*h*a + z2*h2*c + z3*y(khi) + z4*h*b + z5*h2*d;

   % Differentiate to find the fourth-order interpolant
   z0 = -30*t4 + 60*t3 - 30*t2;
   z1 = -15*t4 + 32*t3 - 18*t2 + 1;
   z2 = 0.5*(-5*t4 + 12*t3 - 9*t2 + 2*t);
   z3 = -z0;
   z4 = -15*t4 + 28*t3 - 12*t2;
   z5 = 0.5*(5*t4 - 8*t3 + 3*t2);

   ypi(i) = z0*y(klo)/h + z1*a + z2*h*c + z3*y(khi)/h + z4*b + z5*h*d;

   % Differentiate to find the third-order interpolant
   z0 = -120*t3 + 180*t2 - 60*t;
   z1 = -60*t3 + 96*t2 - 36*t;
   z2 = 0.5*(-20*t3 + 36*t2 - 18*t + 2);
   z3 = -z0;
   z4 = -60*t3 + 84*t2 - 24*t;
   z5 = 0.5*(20*t3 - 24*t2 + 6*t);

   yppi(i) = z0*y(klo)/h2 + z1*a/h + z2*c + z3*y(khi)/h2 + z4*b/h + z5*d;
end

% 3-pt finite difference
function [yp] = lfindiff(x, y)
% Finite Difference Formulae for Unequal Sub-Intervals Using Lagrange's Interpolation Formula
% Singh, Ashok K. and B. S. Bhadauria
% Int. Journal of Math. Analysis, Vol. 3, 2009, no. 17, 815-827

n = length(x);

if (n ~= length(y))
   fprintf('Error, len(x) != len(y), returning\n');
   yp = NaN;
   return;
end

for k = 1:n
   if (k == 1)
      h1 = x(k+1) - x(k);
      h2 = x(k+2) - x(k+1);
      yp(k) = -(2*h1+h2)/(h1*(h1+h2))*y(k) + (h1+h2)/(h1*h2)*y(k+1) - h1/(h2*(h1+h2))*y(k+2);
   elseif (k == n)
      h1 = x(k-1) - x(k-2);
      h2 = x(k) - x(k-1);
      yp(k) = h2/(h1*(h1+h2))*y(k-2) - (h1+h2)/(h1*h2)*y(k-1) + (h1+2*h2)/(h2*(h1+h2))*y(k);
   else
      h1 = x(k) - x(k-1);
      h2 = x(k+1) - x(k);
      yp(k) = -h2/(h1*(h1+h2))*y(k-1) - (h1-h2)/(h1*h2)*y(k) + h1/(h2*(h1+h2))*y(k+1);
   end
end


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.2 by Yi Cao at Cranfield University on 1st March 2010

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)
ny=size(M,2);
minval=inf;
for c=1:ny
    M(:,c)=M(:,c)-(x+y(c));
    minval = min(minval,min(M(:,c)));
end
[rIdx,cIdx]=find(M==minval);


function [ x, seed ] = cube01_sample ( n, seed )

%*****************************************************************************80
%
%% CUBE01_SAMPLE samples points in the unit cube in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(3,N), the points.
%
  m = 3;

  [ x, seed ] = r8mat_uniform_01 ( m, n, seed );

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: MI versus rotation
%
%   - data                 PETCT, Omega=(0,140)x(0,151), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             MI
%   - transformation       rotation2D
%==============================================================================

clear, close all, help(mfilename);

% setup data, interpolation, transformation, regularization
setup2DPETCTData; level = 6; omega = ML{level}.omega; m = ML{level}.m;

viewImage('reset',viewPara{:},'axis','off');
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e0);
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);
distance('reset','distance','MI');
center = (omega(2:2:end)-omega(1:2:end))'/2;
trafo('reset','trafo','rotation2D','c',center);
fprintf('%20s : %s\n','viewImage',viewImage);
fprintf('%20s : %s\n','imgModel',imgModel);
fprintf('%20s : %s\n','distance',distance);
fprintf('%20s : %s\n','trafo',trafo);

xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);

% diffImage = @(Tc) viewImage(128+(Tc-Rc)/2,omega,m);
wc = linspace(-pi/2,pi/2,51);
Dc = zeros(size(wc));

% run the loop over all rotations
for j = 1:length(wc),
  yc = trafo(wc(j),xc);                 % compute transformed grid
  Tc = imgModel(T,omega,yc);               % compute transformed image
  [Dc(j),rc] = distance(Tc,Rc,omega,m); % compute distance

  % visualize
  if j == 1, th = [];
    FAIRfigure(1,'figname',mfilename); clf;
    subplot(1,3,1);  viewImage(Rc,omega,m);            th(1) = title('R');
    subplot(1,3,2);  vh = viewImage(Tc,omega,m);       th(2) = title('T(yc)');
    subplot(1,3,3);  ph = plot(wc(1),Dc(1),'r.','markersize',20);
    th(3) = title(sprintf('%s versus rotation',distance));
    axis([wc(1),wc(end),-inf,inf]); hold on;
    axis('auto y')
    set(th,'fontsize',30);
    FAIRpause;
  else
    set(vh,'cdata',reshape(Tc,m)')
    subplot(1,3,3); set(ph,'visible','off');
    plot(wc(1:j),Dc(1:j),'k-','linewidth',2);
    ph = plot(wc(j),Dc(j),'r.','markersize',20);
    drawnow,
    FAIRpause(1/100)
  end;
  fprintf('.'); if ~rem(j,50) || j == length(wc), fprintf('\n'); end;
end;

%==============================================================================


function [m]=qtt_mg_interp(d)
%The simplest multigrid operator in QTT as a TT-matrix
%   [M]=QTT_MG_INTERP(D) Computes the simplest MG-operator in the
%   QTT-format on 2^D grid. Its stencil is
% 1,	0
% 0.5,	0.5
% 0,	1
% 0,	0.5
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

prol0 = [1 0; 0.5 0.5; 0 1; 0 0.5];
h0 = [0 0; 0 0; 0 0; 0.5 0];
I = eye(2);
I12 = [0 1; 0 0];
I21 = [0 0; 1 0];

m = cell(d,1);

m{1}=zeros(4,2,2);
m{1}(:,:,1)=prol0; m{1}(:,:,2)=h0;
for i=2:d-1
    m{i}=zeros(2,2,2,2);
    m{i}(:,:,1,1)=I;
    m{i}(:,:,2,1)=I12; m{i}(:,:,2,2)=I21;
end;
m{d} = zeros(2,2,2);
m{d}(:,:,1)=I; m{d}(:,:,2)=I12;

m = tt_matrix(m);

end


function rgb = dna_to_rgb_average ( dna )

%*****************************************************************************80
%
%% DNA_TO_RGB_AVERAGE creates an RGB image from DNA.
%
%  Discussion:
%
%    The DNA is subdivided into 32 separate "chromosomes", each of which
%    represents on rectangle.
%
%    For each rectangle, we have 7 "genes", which represent the (x,y) of
%    corner 1, (x,y) of corner 2, and the R, G, and B values for the rectangle.
%
%    Each gene, in turn is represented by 8 bits, because each gene is
%    an integer between 0 and 255.
%
%    This function computes the RGB image by averaging the R, G, and B
%    values from each block.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nick Berry,
%    A "Practical" Use for Genetic Programming,
%    http://www.datagenetics.com/blog.html
%
%  Parameters:
%
%    Input, integer DNA(56,32), the 8 bits of the 7 "genes" of the
%    32 "chromosomes" of the DNA.
%
%    Output, uint8 RGB(256,256,3), the RGB information.
%
  c = zeros ( 256, 256 );
  rgb = zeros ( 256, 256, 3 );

  for j = 1 : 32

    x1 = b8_to_i ( dna(1:8,j) );
    y1 = b8_to_i ( dna(9:16,j) );
    x2 = b8_to_i ( dna(17:24,j) );
    y2 = b8_to_i ( dna(25:32,j) );
    r = b8_to_i ( dna(33:40,j) );
    g = b8_to_i ( dna(41:48,j) );
    b = b8_to_i ( dna(49:56,j) );

    xlo = min ( x1, x2 ) + 1;
    xhi = max ( x1, x2 ) + 1;
    ylo = min ( y1, y2 ) + 1;
    yhi = max ( y1, y2 ) + 1;

    c(xlo:xhi,ylo:yhi) = c(xlo:xhi,ylo:yhi) + 1;

    rgb(xlo:xhi,ylo:yhi,1) = rgb(xlo:xhi,ylo:yhi,1) + r;
    rgb(xlo:xhi,ylo:yhi,2) = rgb(xlo:xhi,ylo:yhi,2) + g;
    rgb(xlo:xhi,ylo:yhi,3) = rgb(xlo:xhi,ylo:yhi,3) + b;

  end

  rgb(1:256,1:256,1) = rgb(1:256,1:256,1) ./ c(1:256,1:256);
  rgb(1:256,1:256,2) = rgb(1:256,1:256,2) ./ c(1:256,1:256);
  rgb(1:256,1:256,3) = rgb(1:256,1:256,3) ./ c(1:256,1:256);
  rgb = uint8 ( rgb );

  return
end


% fdr() - compute false detection rate mask
%
% Usage:
%   >> [p_fdr, p_masked] = fdr( pvals, alpha);
%
% Inputs:
%   pvals   - vector or array of p-values
%   alpha   - threshold value (non-corrected). If no alpha is given
%             each p-value is used as its own alpha and FDR corrected
%             array is returned.
%   fdrtype - ['parametric'|'nonParametric'] FDR type. Default is
%             'parametric'.
%
% Outputs:
%   p_fdr    - pvalue used for threshold (based on independence
%              or positive dependence of measurements)
%   p_masked - p-value thresholded. Same size as pvals.
%
% Author: Arnaud Delorme, SCCN, 2008-
%         Based on a function by Tom Nichols
%
% Reference: Bejamini & Yekutieli (2001) The Annals of Statistics

% Copyright (C) 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [pID, p_masked] = fdr(pvals, q, fdrType);

if nargin < 3, fdrType = 'parametric'; end;
if isempty(pvals), pID = []; return; end;
p = sort(pvals(:));
V = length(p);
I = (1:V)';

cVID = 1;
cVN = sum(1./(1:V));

if nargin < 2
    pID = ones(size(pvals));
    thresholds = exp(linspace(log(0.1),log(0.000001), 100));
    for index = 1:length(thresholds)
        [tmp p_masked] = fdr(pvals, thresholds(index));
        pID(p_masked) = thresholds(index);
    end;
else
    if strcmpi(fdrType, 'parametric')
        pID = p(max(find(p<=I/V*q/cVID))); % standard FDR
    else
        pID = p(max(find(p<=I/V*q/cVN)));  % non-parametric FDR
    end;
end;
if isempty(pID), pID = 0; end;

if nargout > 1
    p_masked = pvals<=pID;
end;


%% BRENTMIN: Brent's minimization method in one dimension
function [xmin,fmin,funccount,varargout] = ...
                                  brentmin(xlow,xupp,Nitmax,tol,f,nout,varargin)
% code taken from
%    § 10.2 Parabolic Interpolation and Brent's Method in One Dimension
%    Press, Teukolsky, Vetterling & Flannery
%    Numerical Recipes in C, Cambridge University Press, 2002
%
% [xmin,fmin,funccout,varargout] = BRENTMIN(xlow,xupp,Nit,tol,f,nout,varargin)
%    Given a function f, and given a search interval this routine isolates
%    the minimum of fractional precision of about tol using Brent's method.
%
% INPUT
% -----
% xlow,xupp:  search interval such that xlow<=xmin<=xupp
% Nitmax:     maximum number of function evaluations made by the routine
% tol:        fractional precision
% f:          [y,varargout{:}] = f(x,varargin{:}) is the function
% nout:       no. of outputs of f (in varargout) in addition to the y value
%
% OUTPUT
% ------
% fmin:      minimal function value
% xmin:      corresponding abscissa-value
% funccount: number of function evaluations made
% varargout: additional outputs of f at optimum
%
% Copyright (c) by Hannes Nickisch 2010-01-10.

if nargin<6, nout = 0; end
varargout = cell(nout,1);

% tolerance is no smaller than machine's floating point precision
tol = max(tol,eps);

% Evaluate endpoints
fa = f(xlow,varargin{:});
fb = f(xupp,varargin{:});
funccount = 2; % number of function evaluations
% Compute the start point
seps = sqrt(eps);
c = 0.5*(3.0 - sqrt(5.0));% golden ratio
a = xlow; b = xupp;
v = a + c*(b-a);
w = v; xf = v;
d = 0.0; e = 0.0;
x = xf; [fx,varargout{:}] = f(x,varargin{:});
funccount = funccount + 1;

fv = fx; fw = fx;
xm = 0.5*(a+b);
tol1 = seps*abs(xf) + tol/3.0;
tol2 = 2.0*tol1;

% Main loop
while ( abs(xf-xm) > (tol2 - 0.5*(b-a)) )
    gs = 1;
    % Is a parabolic fit possible
    if abs(e) > tol1
        % Yes, so fit parabola
        gs = 0;
        r = (xf-w)*(fx-fv);
        q = (xf-v)*(fx-fw);
        p = (xf-v)*q-(xf-w)*r;
        q = 2.0*(q-r);
        if q > 0.0,  p = -p; end
        q = abs(q);
        r = e;  e = d;

        % Is the parabola acceptable
        if ( (abs(p)<abs(0.5*q*r)) && (p>q*(a-xf)) && (p<q*(b-xf)) )

            % Yes, parabolic interpolation step
            d = p/q;
            x = xf+d;

            % f must not be evaluated too close to ax or bx
            if ((x-a) < tol2) || ((b-x) < tol2)
                si = sign(xm-xf) + ((xm-xf) == 0);
                d = tol1*si;
            end
        else
            % Not acceptable, must do a golden section step
            gs=1;
        end
    end
    if gs
        % A golden-section step is required
        if xf >= xm, e = a-xf;    else e = b-xf;  end
        d = c*e;
    end

    % The function must not be evaluated too close to xf
    si = sign(d) + (d == 0);
    x = xf + si * max( abs(d), tol1 );
    [fu,varargout{:}] = f(x,varargin{:});
    funccount = funccount + 1;

    % Update a, b, v, w, x, xm, tol1, tol2
    if fu <= fx
        if x >= xf, a = xf; else b = xf; end
        v = w; fv = fw;
        w = xf; fw = fx;
        xf = x; fx = fu;
    else % fu > fx
        if x < xf, a = x; else b = x; end
        if ( (fu <= fw) || (w == xf) )
            v = w; fv = fw;
            w = x; fw = fu;
        elseif ( (fu <= fv) || (v == xf) || (v == w) )
            v = x; fv = fu;
        end
    end
    xm = 0.5*(a+b);
    tol1 = seps*abs(xf) + tol/3.0; tol2 = 2.0*tol1;

    if funccount >= Nitmax
        % typically we should not get here
        % warning(sprintf(['Maximum number of iterations (%d) exceeded:', ...
        %                  'precision is not guaranteed'],Nitmax))
        % fprintf('[%1.3f,%1.3f,%1.3f]\n',xlow,xf,xupp)
        break
    end
end % while

% check that endpoints are less than the minimum found
if ( (fa < fx) && (fa <= fb) )
    xf = xlow; fx = fa;
elseif fb < fx
    xf = xupp; fx = fb;
end
fmin = fx;
xmin = xf;


%% 1 Param, 1 State
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [No Sensitivity]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','none');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [With Sensitivity]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, Reversed Timestamps
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm = flipud(zm); tm = fliplr(tm);

zm(3) = zm(3)*1.5;
weights = ones(size(zm)); weights(3) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [No Sensitivity] as NLP
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','none');
opts = optiset('solver','ipopt','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [With Sensitivity] as NLP
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','ipopt','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(4,1) = zm(4,1)*1.5;

weights = ones(size(zm)); weights(4,1) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [only fitting first state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)]; %note z1 in 2nd ode allows us to estimate p1
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 1;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [only fitting second state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)]; %note z1 in 2nd ode allows us to estimate p1
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 2;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [different measurement times]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:1;              %measurement times
tm2 = 0:0.15:1;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,tm2,z0); zm2 = zm(:,2); %measurements

zm1(4) = zm1(4)*1.5;

tm = {tm1;tm2};
zm = {zm1;zm2};

wts1 = ones(size(zm1)); wts1(4) = 0;
wts2 = ones(size(zm2));
weights = {wts1;wts2};

%Build OPTI Object
theta0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

tm = fliplr(tm);
zm = flipud(zm);
zm(3,1) = zm(3,1)*1.5;

weights = ones(size(zm));
weights(3,1) = 0;

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State [Different Measurement Times]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:1;              %measurement times
tm2 = 0:0.15:1;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,tm2,z0); zm2 = zm(:,2); %measurements

zm2(4) = zm2(4)*0.1;
tm = {tm1;tm2};
zm = {zm1;zm2};

wts1 = ones(6,1);
wts2 = ones(size(zm2)); wts2(4) = 0;
weights = {wts1;wts2};

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State [Only fit 2nd state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 2;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','lmder','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% Lorenz System [Analytical Derivatives + Different Measurement Times + Non-Zero Initial Time + Estimate IC]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*(z(2) - z(1));
                z(1)*(p(2) - z(3)) - z(2);
                z(1)*z(2) - p(3)*z(3)];
z0 = [5.7;10.50;30.58]; %ic
%Analytical Derivatives
dfdz = @(t,z,p) [-p(1), p(1), 0;
                 p(2) - z(3), -1, -z(1);
                 z(2), z(1), -p(3)];
dfdp = @(t,z,p) [z(2) - z(1), 0, 0;
                 0, z(1), 0;
                 0, 0, -z(3)];

%Generate measurement data
p = [10,46,8/3];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:2;
tm2 = 0.2:0.1:4;            %measurement times
tm3 = 0.5:0.5:3;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,[0 tm2],z0); zm2 = zm(2:end,2); %measurements
[~,zm] = ode45(oi,[0 tm3],z0); zm3 = zm(2:end,3); %measurements

zm3(3) = zm3(3)*1.2;

tm = {tm1;tm2;tm3};
zm = {zm1;zm2;zm3};

wts1 = ones(size(zm1));
wts2 = ones(size(zm2));
wts3 = ones(size(zm3)); wts3(3) = 0;
weights = {wts1;wts2;wts3};

%Given states 2 + 3 start from non-zero time, we should estimate
z0(2:3) = NaN;

%Build OPTI Object
p0 = [10+0.1,46+0.2,8/3,10.4,30.55]; %inital parameter guess
dopts = optidynset('dfdz',dfdz,'dfdp',dfdp);
opts = optiset('solver','nl2sol','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State [Repeated Measurements + z0]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.95);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Estimate initial condition
z0 = NaN;

%Build OPTI Object
theta0 = [1;0.5]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','nl2sol','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 1 State [Repeated Measurements + z0]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1) + p(2);
z0 = 1; %ic

%Generate measurement data
p = [2.5; 5.5];             %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.5);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0 = NaN;

%Build OPTI Object
theta0 = [1;1;0.5]; %inital parameter guess + initial state guess
dopts = optidynset('integrator','ode45','sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State, Repeated Measurements + Both z0
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%Build OPTI Object
theta0 = [1;0.5;2.5]; %inital parameter guess + initial state guess
dopts = optidynset('integrator','ode45','sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State + Solve for both z0 + Repeated Measurements [Only fit 1st state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%States to Measure
state = 1;
zm_m = zm_m(:,state);
weights = weights(:,state);

%Build OPTI Object
p0 = [1;0.1;0.1;0.1]; %inital parameter guess + initial state guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State + Solve for both z0 + Repeated Measurements [Only fit 2nd state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%States to Measure
state = 2;
zm_m = zm_m(:,state);
weights = weights(:,state);

%Build OPTI Object
p0 = [1;0.1;0.1;0.1]; %inital parameter guess + initial state guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% DIW MAPLE ODEs [Analytical Derivatives + IC Estimate + Different Measurement Times + Repeated Points + Non-Zero Initial Time]
clc
ode = @(t,z,p) [-p(1)*z(1) + 4;
                2*z(1) - p(1)*z(2) + 5;
                -4*z(1) - 2*z(3) - p(2)];
z0 = [-1.5;1.25;1]; %ic

%Analytical Derivatives
dfdz = @(t,z,p) [-p(1), 0, 0;
                 2, -p(1), 0;
                 -4, 0, -2];
dfdp = @(t,z,p) [-z(1), 0
                 -z(2), 0
                 0, -1];

% Measurement Times for each State
 tm1  = 0.5:0.1:2;              %state 1
 tm2  = 1:0.1:2;                %state 2
 tm3  = 0.2:0.2:2;              %state 3

 p = [2.345;1.1];            %true parameter value
% Solve ODEs and index Measurement Data
% Note 0 is required below just to match initial condition in this example
odeInt = @(t,z) ode(t,z,p);     %ode integrator function
[~,zm1] = ode45(odeInt,[0 tm1],z0); zm1 = zm1((2:end),1);
[~,zm1b] = ode45(odeInt,[0 tm1],z0*0.5); zm1b = zm1b((2:end),1);
[~,zm2] = ode45(odeInt,[0 tm2],z0); zm2 = zm2((2:end),2);
[~,zm3] = ode45(odeInt,[0 tm3],z0); zm3 = zm3((2:end),3);

% Group measurements and time stamps in cell arrays
 tm_multi = {[tm1 tm1];tm2;tm3};
 zm_multi = {[zm1;zm1b];zm2;zm3};

 wts1 = ones(size(zm1));
 wts1b = 10*ones(size(zm1));
 wts2 = ones(size(zm2));
 wts3 = ones(size(zm3));
 weights = {[wts1;wts1b];wts2;wts3};

% We will need to estimate all initial conditions in this problem
 z0 = [NaN;NaN;NaN];

% New Initial Guess Vector [p;z0];
 theta0 = [1;0.5;0.5;0.5;0.5];

%Build OPTI Object
dopts = optidynset('dfdz',dfdz,'dfdp',dfdp,'initialT',0,'sensitivity','user');
opts = optiset('solver','auto','display','iter','derivCheck','on','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_multi,zm_multi,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)


function x0 = bsv_crossing ( a, b, n, x, u )

%*****************************************************************************80
%
%% BSV_CROSSING estimates the location X0 where U(X0) = 0.
%
%  Discussion:
%
%    This function is intended for a special case, in which we are solving
%    the Burgers equation over an interval [A,B], with positive boundary
%    condition at one end and negative at the other.  In that case, the
%    solution must change be zero at some point X0 in the interval.
%
%    We assume the solution is discretized by a piecewise linear function.
%    We use binary search to locate consecutive indices I and I+1 so that
%    U(I) and U(I+1) are of opposite signs.  We use linear interpolation to
%    estimate the location of X0 between X(I) and X(I+1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, integer N, the number of nodes to use between A and B.
%
%    Input, real X(N), the node coordinates.
%
%    Input, real U(N), the computed discretized solution.
%    It must be the case that U(1) and U(N) are of opposite sign.
%
%    Output, real X0, a point where the piecewise linear approximation to
%    the solution is zero.
%
  if ( u(1) == 0.0 )
    x0 = x(1);
    return
  end

  if ( u(n) == 0.0 )
    x0 = x(n);
    return
  end

  if ( r8_sign ( u(1) ) == r8_sign ( u(n) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV_CROSSING - Fatal error!\n' );
    fprintf ( 1, '  We require opposite signs for U(1) and U(N).\n' );
    error ( 'BSV_CROSSING - Fatal error!\n' );
  end
%
%  Set the initial change of sign indices.
%
  i = 1;
  k = n;
%
%  Do a binary search for the smallest change of sign interval.
%
  while ( 1 < k - i )
%
%  Try halfway.
%
    j = floor ( ( i + k ) / 2 );

    if ( u(j) == 0.0 )
      x0 = x(j)
      return
    end

    if ( r8_sign ( u(j) ) == r8_sign ( u(i) ) )
      i = j;
    else
      k = j;
    end

  end
%
%  The change of sign interval indices are I and K=I+1.
%  Now interpolate to get X0.
%
  x0 = x(i) + u(i) * ( x(i) - x(k) ) / ( u(k) - u(i) );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% File: FRSPC
% Time: Sep. 7th, 2015.
%
% Smooth modeling for incomplete tensor by the PARAFAC decomposition.
% This algorithm is proposed in
%  "Yokota, Tatsuya, et al. "Smooth PARAFAC Decomposition for Tensor Completion." arXiv:1505.06611 (2015)."
%
% minimize || T.*Q - Z.*Q ||_F^2 + penalty_func(U),
%
%    s.t.  Z = [G; U{1}, U{2}, ..., U{N}]
%
% Inputs
% - T       : N-way incomplete tensor
% - Q       : binary tensor which represents elements are available or not (available:1, missing:0)
% - R       : Number of components
% - TV_QV   : it take 'tv' or 'qv' for selecting types of smoothing
% - rho     : N-dimensional vector which represents smoothness of individual modes
% - K       : accelerate parameter for fast optimization (small for fast <--> large for slow, typically 10)
% - SNR     : error threshold via signal-to-noise ratio
% - maxiter : maximum number of iteration
% - tol     : tolerance parameter for convergence evaluation
% - out     : 1 for image completion, 0 for the others
%
% Outputs
% - X       : Results of tensor completion X, where X(Q) = T and X(~Q) = Z
% - Z       : Results of smooth PARAFAC decomposition Z
% - G       : Results of core values G
% - U       : Results of factor matrices U
% - histo   : optimization behavior of error || T.*Q - Z.*Q ||_F^2
% - histo_R : optimization behavior of number of components R
%
% This code was implemented by Tatsuya Yokota
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X Z G U histo histo_R] = FRSPC(T,Q,R,TV_QV,rho,K,SNR,maxiter,tol,out)

  warning off;

  histo=[];
  histo_R=[];
  N  = ndims(T);
  II = size(T);
  NN = sum(Q(:));
  epsiron = 10^(-SNR/10) * sum_all(T(Q).^2);

  %% initialization
  E  = zeros(II);
  X  = zeros(II);
  Z  = zeros(II);
  ONE= zeros(II);

  X(Q) = T(Q);
  X(~Q) = sum(X(:))/NN;
  for n = 1:N
    Pp  = eye(II(n)-1,II(n));
    Pm  = eye(II(n)); Pm(1,:) = [];
    P{n} = Pp - Pm;
    PtP{n} = P{n}'*P{n};
    PtPI{n} = rho(n)*PtP{n} + eye(II(n));
    Pr{n} = inv(rho(n)*PtP{n} + eye(II(n)));
    U{n} = Pr{n}*randn(II(n),R);
    for r = 1:R
      U{n}(:,r) = U{n}(:,r)/norm(U{n}(:,r));
    end
  end

  G = 1e-16*ones(1,R);
  for r = 1:R
    Z = Z + G(r)*outerprod(U,r);
  end
  obj = sum_all((T(Q) - Z(Q)).^2);
  X(~Q) = Z(~Q);
  E = X - Z;

  %% output some figures (only for color image)
  h1 = figure();clf;
  subplot(1,2,1);cla;hold on;
  subplot(1,2,2);
  drawnow;
  if out == 1
    h2 = figure();clf;
    imagesc(uint8(X));drawnow;
    imwrite(uint8(X),['saved/' TV_QV '_iter_0.png']);
  end

  %% start main algorithm
  for iter = 1:maxiter

    [val ID] = sort(abs(G));
    for k = ID(1:min(end,K))

      ONE = G(k)*outerprod(U,k);
      Z = Z - ONE;
      E = E + ONE;

      div = 1;
      for n = 1:N

        if strcmp(TV_QV,'tv')

          u = innerprod_one_exc(E,U,k,n);
          u = u(:);
          %initilization
          a = u/norm(u);
          % main iteration for constrained version
          object = 0.5*G(k)^2*rho(n)*sum(abs(P{n}*a)) - G(k)*a'*u;
          for nn = 1:1000
            df = P{n}'*sign(P{n}*a);
            dL = (0.5*G(k)^2*rho(n)*df - G(k)*u + G(k)^2*a)/G(k)^2;
            al = [0 0.1 0.01 0.001 0.0001 0.00001];
            for ai = 1:length(al)
              a2 = a - al(ai)*dL;
              a2 = a2/norm(a2);
              score(ai) = 0.5*G(k)^2*rho(n)*sum(abs(P{n}*a2)) - G(k)*a2'*u;
            end
            [object2 ai] = min(score);
            a2 = a - al(ai)*dL;
            a2 = a2/norm(a2);
            if abs(object2 - object)/II(n) < 1e-3
              break;
            else
              a = a2;
              object = object2;
            end
          end
          lam = norm(a);
          u = a/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*sum(abs(P{n}*u));

        elseif strcmp(TV_QV,'qv')

          u = innerprod_one_exc(E,U,k,n);
          u = u(:);
          % initialization
          a = Pr{n}*u;
          a = a/norm(a);
          % main iteration for constrained version
          object = 0.5*G(k)^2*rho(n)*a'*PtP{n}*a - G(k)*a'*u;
          mu = 0.1;
          for nn = 1:1000
            dL = (G(k)^2*rho(n)*PtPI{n}*a - G(k)*u'*a)/G(k)^2;
            al = [0 0.1 0.01 0.001 0.0001 0.00001];
            for ai = 1:length(al)
              a2 = a - al(ai)*dL;
              a2 = a2/norm(a2);
              score(ai) = 0.5*G(k)^2*rho(n)*a2'*PtP{n}*a2 - G(k)*a2'*u;
            end
            [object2 ai] = min(score);
            a2 = a - al(ai)*dL;
            if abs(object2 - object)/II(n) < 1e-3
              break;
            else
              a = a2;
              object = object2;
            end
          end

          u = a2;
          lam = norm(u);
          u = u/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*u'*PtP{n}*u;

        else
          error('2rd input is ''tv'' or ''qv'' ');
        end

      end

      G(k) = tensor_allprod(E,v,1)/div;
      if G(k) < 0
        G(k) = -G(k);
        U{1}(:,k) = - U{1}(:,k);
      end

      ONE = G(k)*outerprod(U,k);
      E = E - ONE;
      E(~Q) = 0;
      Z = Z + ONE;
      X(~Q) = Z(~Q);

    end

    %% calculate MSE
    obj2 = sum_all(E.^2);

    %% convergence speed
    speed = abs(obj2 - obj)/abs(epsiron - obj2);

    %% checking convergence
    if obj2 < epsiron || abs(obj2-obj)/NN < tol
      break;
    else
      obj = obj2;
      if mod(iter,5)==0
        fprintf('%d:  %f :: %f :: %f :: Pid %d \n',iter,obj2/NN,epsiron/NN,speed,R);
      end
      histo(iter) = obj;
      histo_R(iter,:) = size(G);
    end

    %% output figures
    set(0,'CurrentFigure',h1);
    subplot(1,2,1);cla;hold on;
    plot((histo));
    plot((epsiron)*ones(1,length(histo)))
    grid on;
    set(gca,'YScale','log')
    title('MSE')
    subplot(1,2,2);
    plot(histo_R(:,2));
    title('number of components R')

    drawnow;
    if out == 1
      set(0,'CurrentFigure',h2);
      imagesc(uint8(X));
      title(['Number of R = ' num2str(R)]);
      drawnow;
    end

    if mod(iter,10) == 0 & out == 1
      imwrite(uint8(Z),['saved/' TV_QV '_iter_' num2str(iter) '.png']);
    end

    if mod(iter,100) == 0
      pack;
    end

  end


function machar_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests R8_MACHAR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  R8_MACHAR computes double\n' );
  fprintf ( 1, '  precision machine constants.\n' );

  [ ibeta, it, irnd, ngrd, machep, negep, iexp, ...
    minexp, maxexp, eps, epsneg, xmin, xmax ] = r8_machar ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  IBETA is the internal base for machine arithmetic.\n' );
  fprintf ( 1, '    IBETA =  %d\n', ibeta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IT is the number of digits, base IBETA, in the\n' );
  fprintf ( 1, '  floating point significand.\n' );
  fprintf ( 1, '    IT =     %d\n', it );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IRND reports on floating point addition rounding:\n' );
  fprintf ( 1, '  0, for chopping;\n' );
  fprintf ( 1, '  1, for non-IEEE rounding;\n' );
  fprintf ( 1, '  2, for IEEE rounding;\n' );
  fprintf ( 1, '  3, for chopping with partial underflow;\n' );
  fprintf ( 1, '  4, for non-IEEE rounding with partial underflow.\n' );
  fprintf ( 1, '  5, for IEEE rounding with partial underflow.\n' );
  fprintf ( 1, '    IRND =   %d\n', irnd );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NGRD is the number of guard digits for floating point\n' );
  fprintf ( 1, '  multiplication with truncating arithmetic.\n' );
  fprintf ( 1, '    NGRD =   %d\n', ngrd );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MACHEP is the largest negative integer such that\n' );
  fprintf ( 1, '  1.0 < 1.0 + BETA^MACHEP.\n' );
  fprintf ( 1, '    MACHEP = %d\n', machep );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NEGEPS is the largest negative integer such that\n' );
  fprintf ( 1, '  1.0 - BETA^NEGEPS < 1.0:\n' );
  fprintf ( 1, '    NEGEP =  %d\n', negep );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IEXP is the number of bits reserved for the exponent\n' );
  fprintf ( 1, '  of a floating point number:\n' );
  fprintf ( 1, '    IEXP =   %d\n', iexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MINEXP is the most negative power of BETA such that\n' );
  fprintf ( 1, '  BETA^MINEXP is positive and normalized.\n' );
  fprintf ( 1, '    MINEXP = %d\n', minexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MAXEXP is the smallest positive power of BETA that\n' );
  fprintf ( 1, '  overflows:\n' );
  fprintf ( 1, '    MAXEXP = %d\n', maxexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EPS is a small positive floating point number\n' );
  fprintf ( 1, '  such that 1.0 < 1.0 + EPS.\n' );
  fprintf ( 1, '    EPS    = %26.16e\n', eps );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EPSNEG is a small positive floating point number\n' );
  fprintf ( 1, '  such that 1.0 - EPSNEG < 1.0.\n' );
  fprintf ( 1, '    EPSNEG = %26.16e\n', epsneg );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  XMIN is the smallest positive normalized floating\n' );
  fprintf ( 1, '  point power of the radix:\n' );
  fprintf ( 1, '    XMIN =   %26.16e\n', xmin );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  XMAX is the largest finite floating point number:\n' );
  fprintf ( 1, '    XMAX   = %26.16e\n', xmax );

  return
end


function [S U sv tol] = hosvd_lpv(data, dep, gridsize, svtol, keep)
%HOSVD of a discretized lpv model
%	[S U]        = HOSVD_LPV(data, dep, gridsize)
%	[S U sv tol] = HOSVD_LPV(data, dep, gridsize, svtol, keep)
%
%	data     - sampled LPV model (cell array)
%	dep      - parameter dependency array
%	gridsize - sampling grid size for each parameter
%	svtol    - singular value tolerance (1e-8 by default)
%	keep     - number of kept singular values (if set then no interactive question)
%
%	S        - core tensor of the HOSVD based canonical representation
%	U        - basis functions of the canonical representation
%	sv       - singular values for each parameter
%	tol      - singular value tolerance for each parameter
%
%	See also SAMPLING_LPV

% TODO: matlab docs

%Dependencies in needed form
dep = dep2idx(dep);

%Number of parameters
P = size(dep, 3);

%Size of S matrix
[Sy, Sx] = size(data);

if nargin < 4
	svtol = 1e-8;
end
if nargin < 5 || isempty(keep)
	keep = zeros(1,P);
end

%Weighting function, singular value and tolerance allocation
U = cell(1,P);
sv = cell(1,P);
tol = cell(1,P);
gridprod = prod(gridsize);

%% Weighting functions
%TODO: i,j,k indexing..
for i = 1:P

	%Product of grid size of other dimensions
	Mprod = gridprod/gridsize(i);

	%Initial tmp and constant
	tmp = [];
	c = 0;

	%Every element of S matrix
	for j = 1:Sy
		for k = 1:Sx
			%Number of dependencies at current element
			n = sum(dep(j,k,:)>0);

			if n > 0
				% Create subtensor to reduce computation
				if dep(j,k,i) > 0
					%If dependent, layout, svd, keep column space weighting
					%functions
					lay = ndim_unfold(data{j,k}, dep(j,k,i));
					if n > 2
						%Reduce to column space
						[u, s] = svd(lay, 'econ');
						lay = u*s;
					end
					tmp = [tmp lay.*sqrt(prod(gridsize(dep(j,k,:)==0)))];
				else
					%Add constant
					c = c + sum(data{j,k}(:).^2) * prod(gridsize(dep(j,k,:)==0))/gridsize(i);
				end
			else
				% const element
				c = c + data{j,k}^2 * Mprod;
			end
		end
	end

	tmp = [tmp ones(gridsize(i),1).*sqrt(c)];

	% SVD based reduction of the current dimension (i)
	[Ui svi toli] = svdtrunc(tmp, svtol);
	if keep(i) <= 0
		disp('normalised singular values (and original ones):');
		for j = 1:length(svi)
			fprintf('%12.5f  (%g)\n',svi(j)/sqrt(gridprod), svi(j));
		end
		% TODO: rewrite with isstrprop
		ns = input('number of singular values to keep [all] = ');
	else
		ns = keep(i);
	end
	if ~isempty(ns) && ns < length(svi)
		Ui = Ui(:,1:ns);
		toli = svi(ns+1);
		svi = svi(1:ns);
	end
	U{i} = Ui;
	sv{i} = svi;
	tol{i} = toli;
end

%% Calculate coretensor
S = coretensor(U, data, dep);


function [ mainDirect, score, angle] = findMainDirectionEMA( lines )
%FINDMAINDIRECTION compute vp from set of lines
%   Detailed explanation goes here
fprintf('Computing vanishing point:\n');

% arcList = [];
% for i = 1:length(edge)
%     panoLst = edge(i).panoLst;
%     if size(panoLst,1) == 0
%         continue;
%     end
%     arcList = [arcList; panoLst];
% end

%% initial guess
segNormal = lines(:,1:3);
segLength = lines(:,7);
segScores = ones(size(lines,1),1);%lines(:,8);

shortSegValid = segLength < 5*pi/180;
segNormal = segNormal(~shortSegValid,:);
segLength = segLength(~shortSegValid);
segScores = segScores(~shortSegValid);

numLinesg = size(segNormal,1);
[candiSet, tri] = icosahedron2sphere(3);
ang = acos(dot(candiSet(tri(1,1),:), candiSet(tri(1,2),:), 2)) / pi * 180;
binRadius = ang/2;
[ initXYZ, score, angle] = sphereHoughVote( segNormal, segLength, segScores, 2*binRadius, 2, candiSet );

if isempty(initXYZ)
    fprintf('Initial Failed\n');
    mainDirect = [];
    return;
end

fprintf('Initial Computation: %d candidates, %d line segments\n', size(candiSet,1), numLinesg);
fprintf('direction 1: %f %f %f\ndirection 2: %f %f %f\ndirection 3: %f %f %f\n', ...
        initXYZ(1,1), initXYZ(1,2), initXYZ(1,3), ...
        initXYZ(2,1), initXYZ(2,2), initXYZ(2,3), ...
        initXYZ(3,1), initXYZ(3,2), initXYZ(3,3));
%% iterative refine
iter_max = 3;
[candiSet, tri] = icosahedron2sphere(5);
numCandi = size(candiSet,1);
angD = acos(dot(candiSet(tri(1,1),:), candiSet(tri(1,2),:), 2)) / pi * 180;
binRadiusD = angD/2;
curXYZ = initXYZ;
tol = linspace(4*binRadius, 4*binRadiusD, iter_max); % shrink down #ls and #candi
for iter = 1:iter_max
    dot1 = abs(dot( segNormal, repmat(curXYZ(1,:), [numLinesg 1]), 2));
    dot2 = abs(dot( segNormal, repmat(curXYZ(2,:), [numLinesg 1]), 2));
    dot3 = abs(dot( segNormal, repmat(curXYZ(3,:), [numLinesg 1]), 2));
    valid1 = dot1<cos((90-tol(iter))*pi/180);
    valid2 = dot2<cos((90-tol(iter))*pi/180);
    valid3 = dot3<cos((90-tol(iter))*pi/180);
    valid = valid1 | valid2 | valid3;

    if(sum(valid)==0)
        fprintf('ZERO line segment for voting\n');
        break;
    end

    subSegNormal = segNormal(valid,:);
    subSegLength = segLength(valid);
    subSegScores = segScores(valid);

    dot1 = abs(dot( candiSet, repmat(curXYZ(1,:), [numCandi 1]), 2));
    dot2 = abs(dot( candiSet, repmat(curXYZ(2,:), [numCandi 1]), 2));
    dot3 = abs(dot( candiSet, repmat(curXYZ(3,:), [numCandi 1]), 2));
    valid1 = dot1>cos(tol(iter)*pi/180);
    valid2 = dot2>cos(tol(iter)*pi/180);
    valid3 = dot3>cos(tol(iter)*pi/180);
    valid = valid1 | valid2 | valid3;

    if(sum(valid)==0)
        fprintf('ZERO candidate for voting\n');
        break;
    end

    subCandiSet = candiSet(valid,:);

    [ tcurXYZ ] = sphereHoughVote( subSegNormal, subSegLength, subSegScores, 2*binRadiusD, 2, subCandiSet );

    if(isempty(tcurXYZ))
        fprintf('NO answer found!\n');
        break;
    end
    curXYZ = tcurXYZ;

    fprintf('%d-th iteration: %d candidates, %d line segments\n', iter, size(subCandiSet,1), length(subSegScores));

end
fprintf('direction 1: %f %f %f\ndirection 2: %f %f %f\ndirection 3: %f %f %f\n', ...
        curXYZ(1,1), curXYZ(1,2), curXYZ(1,3), ...
        curXYZ(2,1), curXYZ(2,2), curXYZ(2,3), ...
        curXYZ(3,1), curXYZ(3,2), curXYZ(3,3));
mainDirect = curXYZ;

mainDirect(1,:) = mainDirect(1,:).*sign(mainDirect(1,3));
mainDirect(2,:) = mainDirect(2,:).*sign(mainDirect(2,3));
mainDirect(3,:) = mainDirect(3,:).*sign(mainDirect(3,3));

uv = xyz2uvN(mainDirect);
[~,I1] = max(uv(:,2));
J = setdiff(1:3, I1);
[~,I2] = min(abs(sin(uv(J,1))));
I2 = J(I2);
I3 = setdiff(1:3, [I1 I2]);
mainDirect = [mainDirect(I1,:); mainDirect(I2,:); mainDirect(I3,:)];

mainDirect(1,:) = mainDirect(1,:)*sign(mainDirect(1,3));
mainDirect(2,:) = mainDirect(2,:)*sign(mainDirect(2,2));
mainDirect(3,:) = mainDirect(3,:)*sign(mainDirect(3,1));

mainDirect = [mainDirect; -mainDirect];

% score = 0;

end


function [] = PUMA_InverseKinematics(Trans, theta, th, dstPoint)
%% Variable Initialization

% Variables to store error related data
Kp = 0.0045;
errorThreshold = 0.1;

% Matrix containing Joint angle Information and Desired Point Location
t1 = th(1); t2 = th(2); t3 = th(3);
Theta = [double(subs(theta(1))); double(subs(theta(2))); double(subs(theta(3)))];

% Make a column matrix representing the Position of the End-Effector
endPoint = [Trans(1,4,6); Trans(2,4,6); Trans(3,4,6)];
% Find the current numeric end-effector position
currPoint = double(subs(endPoint));
% Find the error vector
errorVec = (dstPoint - currPoint);
% Create a Jacobian Matrix using the given Theta values
Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1));

for i=1:3
    [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
end
for i=4:6
    X(i) = double(subs(Trans(1,4,i)));
    Y(i) = double(subs(Trans(2,4,i)));
    Z(i) = double(subs(Trans(3,4,i)));
end

PUMA_Draw(X, Y, Z);

%% Inverse Kinematics Initialization

% Apply the control equation to the error vector
deltaTheta = transpose(Jacobian) * errorVec * Kp;
% Apply the control changes to the Joint Angles
Theta(1,1) = Theta(1,1) + deltaTheta(1,1);
Theta(2,1) = Theta(2,1) + deltaTheta(2,1);
Theta(3,1) = Theta(3,1) + deltaTheta(3,1);
t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1);

% Find the euclidean distance between the desired and current position of
% the manipulator.
dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ...
    (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2);

%% Iterative Inverse Kinematics for correcting the manipulator position.

% Correct the manipulator position to reduce the distance.
while (dist > errorThreshold)
    Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1));
    currPoint = double(subs(endPoint));
    errorVec = (dstPoint - currPoint);
    deltaTheta = transpose(Jacobian) * errorVec * Kp;
    Theta(1,1) = Theta(1,1) + deltaTheta(1,1);
    Theta(2,1) = Theta(2,1) + deltaTheta(2,1);
    Theta(3,1) = Theta(3,1) + deltaTheta(3,1);
    t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1);
    dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ...
        (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2);

    for i=1:3
        [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
    end
    for i=4:6
        X(i) = double(subs(Trans(1,4,i)));
        Y(i) = double(subs(Trans(2,4,i))); Z(i) = double(subs(Trans(3,4,i)));
    end
    PUMA_Draw(X, Y, Z);

end

cla
for i=1:3
    [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
end
for i=4:6
    X(i) = double(subs(Trans(1,4,i)));
    Y(i) = double(subs(Trans(2,4,i)));
    Z(i) = double(subs(Trans(3,4,i)));
end
PUMA_Draw(X, Y, Z);
end


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

%*****************************************************************************80
%
%% HE_POLYNOMIAL_VALUES: tabulated values of He(i,x).
%
%  Discussion:
%
%    He(i,x) represents the probabilist's Hermite polynomial.
%
%    In Mathematica, the function can be evaluated by:
%
%      He(n,x) = HermiteH[n,x/Sqrt[2]] / Sqrt [ 2^n ]
%
%  First terms:
%
%   1
%   X
%   X^2  -  1
%   X^3  -  3 X
%   X^4  -  6 X^2 +   3
%   X^5  - 10 X^3 +  15 X
%   X^6  - 15 X^4 +  45 X^2 -   15
%   X^7  - 21 X^5 + 105 X^3 -  105 X
%   X^8  - 28 X^6 + 210 X^4 -  420 X^2 +  105
%   X^9  - 36 X^7 + 378 X^5 - 1260 X^3 +  945 X
%   X^10 - 45 X^8 + 630 X^6 - 3150 X^4 + 4725 X^2 - 945
%
%  Recursion:
%
%    He(0,X) = 1,
%    He(1,X) = X,
%    He(N,X) = X * He(N-1,X) - (N-1) * He(N-2,X)
%
%  Norm:
%
%    Integral ( -oo < X < +oo ) exp ( - 0.5 * X^2 ) * He(M,X) He(N,X) dX
%    = sqrt ( 2 * pi ) * N! * delta ( N, M )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  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 polynomial.
%
%    Output, real X, the point where the polynomial is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 18;

  fx_vec = [ ...
    1.000000000000000E+00, ...
    5.000000000000000E+00, ...
    24.00000000000000E+00, ...
    110.0000000000000E+00, ...
    478.0000000000000E+00, ...
    1950.000000000000E+00, ...
    7360.000000000000E+00, ...
    25100.00000000000E+00, ...
    73980.00000000000E+00, ...
    169100.0000000000E+00, ...
    179680.0000000000E+00, ...
   -792600.0000000000E+00, ...
   -5939480.000000000E+00, ...
    0.000000000000000E+00, ...
    6.281250000000000E+00, ...
    6.000000000000000E+00, ...
    18.00000000000000E+00, ...
    90150.00000000000E+00 ];

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

  x_vec = [ ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    0.0E+00, ...
    0.5E+00, ...
    1.0E+00, ...
    3.0E+00, ...
    1.0E+01 ];

  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 RPV01 = rpv01(Term,Settle,RPV_Dates,Basis,LIBOR,probfun,b)
%RPV01 Computes the "Risky PV01", ie, the expected present value of the
% premium leg at 1 bp

RPV_Time = yearfrac(Settle,RPV_Dates,Basis);
PaymentTimes = [RPV_Time(1);diff(RPV_Time)];

RPVQ = probfun(RPV_Time,b);
RPVDF = LIBOR.getDiscountFactors(RPV_Dates);

RPV01 = zeros(size(Term));
for spreadidx=1:length(Term)

    RPVtmpidx = RPV_Time < Term(spreadidx);

    PaymentTimestmp = PaymentTimes(RPVtmpidx);
    RPVDFtmp = RPVDF(RPVtmpidx);
    RPVQtmp = RPVQ(RPVtmpidx);

    RPV01(spreadidx) = sum(PaymentTimestmp.*RPVDFtmp.*(RPVQtmp + ...
        1/2*(([1;RPVQtmp(1:end-1)]) - RPVQtmp)));
end


function [V]=filletCurve(V,r,np,closedLoopOpt)

% [V]=filletCurve(V,r,np,closedLoopOpt)
% ------------------------------------------------------------------------
% This function fillets a curve based on the input radius r using np points
% per fillet arc. If closedLoopOpt==1 then closed end conditions are used
% such that the end and start regions are also filleted.
%
% %% EXAMPLE:
% Vt=[0 0 0; 10 0 0; 5 10 0; 10 0 10; 0 10 10; ];
% r=2; %Fillet radius
% np=25; %Number of points used to construct each fillet edge
% closedLoopOption=0; %Use 1 if curve represents a closed loop but containes unique points
% [VN]=filletCurve(Vt,r,np,closedLoopOption);
%
% figure; hold on;
% plotV(Vt,'k.-.','lineWidth',2,'MarkerSize',25);
% plotV(VN,'r.-','lineWidth',3);
% axis equal; view(3); axis tight;  grid on;
% drawnow;
% %%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 2014/03/19
% 2017/06/03 Fixed bug in relation to angles being 0 or 180 degrees
%------------------------------------------------------------------------

%%

angleTolerance=1e-3;
%%

%Cope with 2D input
if size(V,2)==2
    V(:,3)=0;
    nDim=2;
else
    nDim=3;
end

numPoints=size(V,1);
if numPoints>2
    numSteps=numPoints-2;
    indStart=1;
    for q=1:1:numSteps
        [Vr]=filletEdgeSet(V(indStart:indStart+2,:),r,np,angleTolerance);
        V=[V(1:indStart,:);Vr;V(indStart+2:end,:)];
        indStart=indStart+size(Vr,1);
    end
else
    error('Input curve V has too few points!');
end

if closedLoopOpt==1
    V_closed=[V(end-1:end,:); V(1,:);];
    [Vr]=filletEdgeSet(V_closed,r,np,angleTolerance);
    V=[V(1:end-1,:); Vr];

    V_closed=[V(end,:); V(1:2,:);];
    [Vr]=filletEdgeSet(V_closed,r,np,angleTolerance);
    V=[Vr; V(2:end,:); ];
end

if nDim==2
    V=V(:,1:2);
end
end

function [Vr]=filletEdgeSet(V,r,np,angleTolerance)
Vc=V-V(2*ones(size(V,1),1),:);

P1=Vc(1,:);
M1=norm(P1);
N1=P1./M1;

P2=Vc(3,:);
M2=norm(P2);
N2=P2./M2;

M_min=min([M1 M2]);

rMax=sqrt(sum((M_min.*N1-M_min.*N2).^2))/2;

if r>rMax
    error(['Radius is too big! Current max: ',num2str(rMax)]);
end

Nm=(N1+N2)/2;
Nm=Nm./norm(Nm);

a=real(acos(dot(P1,Nm)./norm(P1)));

if mod(abs(a),pi)<angleTolerance
    Vr=[];
else
    d=r./sin(a);
    d1=r./tan(a);
    d2=r./tan(a);

    Pc=Nm.*d;

    P1c=N1.*d1;
    P2c=N2.*d2;

    P1cc=P1c-Pc;
    P2cc=P2c-Pc;

    Vn=linspacen(P1cc,P2cc,np)';
    [theta_Vn,phi_Vn,~] = cart2sph(Vn(:,1),Vn(:,2),Vn(:,3));
    [Vn(:,1),Vn(:,2),Vn(:,3)]=sph2cart(theta_Vn,phi_Vn,r.*ones(size(phi_Vn)));
    Vn=Vn+Pc(ones(1,size(Vn,1)),:);

    Vr=Vn+V(2*ones(size(Vn,1),1),:);
end

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


function [V,F,bi,bo] = annulus(s,r,varargin)
  % ANNULUS Construct a triangle mesh of a unit annulus.
  %
  % [V,F] = annulus(s,r)
  % [V,F] = annulus(s,r,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %   s  number of samples on the inner ring
  %   r  radius of the inner ring
  %   Optional:
  %     'Flags'  followed by flags to pass to Triangle for meshing.
  %        {'-q30 -aX'} where X is squared boundary edge length
  %     'R'  followed by outer ring radius {1}
  % Outputs:
  %   V  #V by 2 list of mesh vertex positions
  %   F  #F by 3 list of triangle mesh indices
  %   bi  list of vertices on inner boundary
  %   bo  list of vertices on outer boundary
  %

  flags = nan;
  R = 1;
  params_to_variables = containers.Map( ...
    {'Flags','R'},{'flags','R'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isnan(flags)
    flags = sprintf('-q30 -a%0.17f',(2*pi*r/s)^2);
  end

  theta = linspace(0,2*pi,s+1)';theta = theta(1:end-1);
  Vr = r*[cos(theta) sin(theta)];
  Er = fliplr([1:size(Vr,1);2:size(Vr,1) 1]');
  theta = linspace(0,2*pi,ceil(R/r)*s+1)';theta = theta(1:end-1);
  VR = R*[cos(theta) sin(theta)];
  ER = [1:size(VR,1);2:size(VR,1) 1]';

  [V,F] = triangulate([Vr;VR],[Er;size(Vr,1)+ER],'Holes',[0 0],'Flags',flags);
  b = unique(outline(F));
  bi = intersect(find(normrow(V)< 0.5*(R+r)),b);
  bo = intersect(find(normrow(V)> 0.5*(R+r)),b);
end


function r = sqrt(a)
%SQRT         Hessian (elementwise) square root
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K = prod(size(a.x));
  if K==1                   % scalar hessian

    r.x = sqrt(a.x);
    rx1 = 0.5 / r.x;
    r.dx = a.dx * rx1;
    r.hx = ( a.hx - reshape( ((0.25/a.x)*a.dx) * a.dx.',size(a.hx)) ) * rx1;

  else                      % matrix hessian

    N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');
    N2 = N^2;

    r.x = sqrt(a.x);
    if issparse(a.hx)               % input sparse

      ax = 0.5./full(r.x(:));
      sizeax = length(ax);
      [ia,ja,sa] = find(a.dx);
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if isempty(ia)
        r.dx = sparse([],[],[],N,sizeax);
        r.hx = sparse([],[],[],N2,sizeax);
      else
        rdx = ax(ja).*sa(:);
        adx1 = (-0.25) ./ a.x(ja);
        if isa(a.x,'intval')          % sparse intval
          adx1 = times(adx1(:),rdx,0);
          if isreal(rdx)
            r.dx = intval( sparse(ia,ja,rdx.inf,N,sizeax) , sparse(ia,ja,rdx.sup,N,sizeax) , 'infsup' );
          else
            r.dx = intval( sparse(ia,ja,rdx.mid,N,sizeax) , sparse(ia,ja,rdx.rad,N,sizeax) , 'midrad' );
          end
          if adx1.complex
            adx1 = intval( sparse(ia,ja,adx1.mid,N,sizeax) , sparse(ia,ja,adx1.rad,N,sizeax) , 'midrad' );
          else
            adx1 = intval( sparse(ia,ja,adx1.inf,N,sizeax) , sparse(ia,ja,adx1.sup,N,sizeax) , 'infsup' );
          end
        else                          % sparse point
          r.dx = sparse(ia,ja,rdx,N,sizeax);
          adx1 = sparse(ia,ja,adx1(:).*rdx,N,sizeax);
        end
        r.hx = adx2rhx(N,sizeax,adx1,a.dx);
      end
      [ia,ja,sa] = find(a.hx);      % sparse point or intval
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if ~isempty(ia)
        if isa(a.x,'intval')
          rhx = times(ax(ja),sa(:),0);
          if rhx.complex
            r.hx = r.hx + intval( sparse(ia,ja,rhx.mid,N2,sizeax) , sparse(ia,ja,rhx.rad,N2,sizeax) , 'midrad' );
          else
            r.hx = r.hx + intval( sparse(ia,ja,rhx.inf,N2,sizeax) , sparse(ia,ja,rhx.sup,N2,sizeax) , 'infsup' );
          end
        else
          r.hx = r.hx + sparse(ia,ja,ax(ja).*sa(:),N2,sizeax);
        end
      end

    else                            % input full

      r.x = sqrt(a.x);
      rx1 = 0.5 ./ ( r.x(:).' );
      rx1 = rx1(ones(N*N,1),:);
      r.dx = a.dx .* rx1(1:N,:);
      adx = repmat(0.25./(a.x(:).'),N,1) .* a.dx;
      r.hx = ( a.hx - adx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:) ) .* rx1;

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


function [ x, more ] = sgmga_vcn_naive ( dim_num, level_weight, x_max, x, ...
  q_min, q_max, more )

%*****************************************************************************80
%
%% SGMGA_VCN_NAIVE returns the next constrained vector.
%
%  Discussion:
%
%    This function uses a naive algorithm, which quickly becomes unsuitable
%    for higher dimensions.  The function SGMGA_VCN is an attempt at a more
%    efficient calculation of the same quantities.
%
%    We consider vectors X of dimension DIM_NUM satisfying:
%
%      0 <= X(1:DIM_NUM) <= X_MAX(1:DIM_NUM).
%
%    and define
%
%      Q = sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * X(I)
%
%    and seek X satisfying the constraint:
%
%      Q_MIN < Q <= Q_MAX
%
%    For sparse grid applications, we compute
%
%      LEVEL_WEIGHT_MIN_POS = minimum positive entry in LEVEL_WEIGHT
%
%    and assume there is an underlying LEVEL used to index the sets of
%    constrained vectors, and that
%
%      Q_MAX = LEVEL * LEVEL_WEIGHT_MIN_POS
%      Q_MIN = LEVEL - LEVEL_WEIGHT_MIN_POS * sum ( LEVEL_WEIGHT(:) )
%      X_MAX(I) = LEVEL * LEVEL_WEIGHT_MIN_POS / LEVEL_WEIGHT(I)
%
%    This routine returns, one at a time exactly those X which satisfy
%    the constraint.  No attempt is made to return the X values in
%    any particular order as far as Q goes.
%
%  Example:
%
%    LEVEL_WEIGHT:          1.000000        1.000000
%
%    Q_MIN:        0.000000
%    Q_MAX:        2.000000
%    X_MAX:                         2         2
%
%         1        1.000000         1         0
%         2        2.000000         2         0
%         3        1.000000         0         1
%         4        2.000000         1         1
%         5        2.000000         0         2
%
%    LEVEL_WEIGHT:          1.000000        2.000000
%
%    Q_MIN:       -1.000000
%    Q_MAX:        2.000000
%    X_MAX:                         2         1
%
%         1        0.000000         0         0
%         2        1.000000         1         0
%         3        2.000000         2         0
%         4        2.000000         0         1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of components in the vector.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer X_MAX(DIM_NUM), the maximum
%    values allowed in each component of X.
%
%    Input, integer X(DIM_NUM).  On first call, with
%    MORE = FALSE, the input value of X is not important.  On subsequent calls,
%    the input value of X should be the output value from the previous call.
%
%    Input, real Q_MIN, Q_MAX, the lower and upper limits on the sum.
%
%    Input, logical MORE.  On input, if the user has set MORE
%    FALSE, the user is requesting the initiation of a new sequence
%    of values.  If MORE is TRUE, then the user is requesting "more"
%    values in the current sequence.
%
%    Output, integer X(DIM_NUM).
%    On output, (with MORE = TRUE), the value of X will be the "next"
%    vector in the reverse lexicographical list of vectors that satisfy
%    the condition.  However, on output with MORE = FALSE, the vector
%    X is meaningless, because there are no more vectors in the list.
%
%    Output, logical MORE.  If MORE is TRUE on output,
%    then another value was found and returned in X, but if MORE is
%    FALSE, then there are no more values in the sequence, and X is
%    NOT the next value.
%
  if ( ~more )

    more = 1;

    x = zeros ( dim_num, 1 );

    q = level_weight(1:dim_num)' * x(1:dim_num);

    if ( q_min < q && q <= q_max )
      return
    end

  end

  while ( 1 )

    i = 1;

    while ( 1 )

      if ( x(i) < x_max(i) )
        break
      end

      if ( dim_num <= i )
        more = 0;
        return
      end

      i = i + 1;

    end

    x(i) = x(i) + 1;
    x(1:i-1) = 0;

    q = level_weight(1:dim_num)' * x(1:dim_num);

    if ( q_min < q && q <= q_max )
      break
    end

  end

  return
end


function pde = HodgeLaplacian3Edata1
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde.f = @f;
pde.exactu = @exactu;
pde.exactsigma = @exactsigma;
pde.gu = @exactu;
pde.gsigma = @exactsigma;
pde.gun = @gun;
pde.curlu = [0 0 0];
pde.gcurlu = 0;

    function s = f(p)
    x = p(:,1); y = p(:,2);
    s = [cos(x), -sin(y), zeros(size(p,1),1)];
    end

    function s = exactu(p)
    x = p(:,1); y = p(:,2); z = p(:,3);
    s = [cos(x), -sin(y), 2*z+1];
    end

    function s = exactsigma(p)
    x = p(:,1); y = p(:,2);
    s = sin(x)+cos(y)-2;
    end

    function f = gun(p) % for unit cube [0,1]^3
    f = zeros(size(p,1),1);
    x = p(:,1); y = p(:,2); z = p(:,3);
    u = exactu(p);
    leftbd = (abs(x)<eps);    % n = (-1,0,0)
    f(leftbd) = - u(leftbd,1);
    rightbd = (abs(x-1)<eps); % n = (1,0,0)
    f(rightbd) = u(rightbd,1);
    frontbd = (abs(y)<eps);   % n = (0,-1,0)
    f(frontbd) = -u(frontbd,2);
    backbd = (abs(y-1)<eps);  % n = (0,1,0)
    f(backbd) = u(backbd,2);
    topbd = (abs(z-1)<eps);   % n = (0,0,1)
    f(topbd) = u(topbd,3);
    bottombd = (abs(z)<eps);  % n = (0,0,-1)
    f(bottombd) = - u(bottombd,3);
    end

end


%u2F    estimates fundamental matrix using ortogonal LS regression
%	F = u2F(u) estimates F from u using NORMU
%  F = u2F(u,'nonorm') disables normalization
%	see also NORMU, U2FA

function F = u2F (u, str)

if (nargin > 1) & strcmp(str, 'nonorm')
   donorm = 0;
else
   donorm = 1;
end

ptNum = size(u,2);

if donorm
   A1    = normu(u(1:3,:));
   A2    = normu(u(4:6,:));

   u1   = A1*u(1:3,:);
   u2   = A2*u(4:6,:);
end

for i = 1:ptNum
   Z(i,:)   = reshape(u1(:,i)*u2(:,i)',1,9);
end

M       = Z'*Z;
V       = seig(M);
F = reshape(V(:,1),3,3);

[uu,us,uv] = svd(F);
[y,i]      = min (abs(diag(us)));
us(i,i)    = 0;
F          = uu*us*uv';

if donorm
   F = A1'*F*A2;
end

F = F /norm(F,2);


function [c] = APGLASSOup(b,A,para)

%% Object: c = argmin (1/2)\|y-Dx\|_2^2+lambda\|x\|_1+\mu\|x_I\|_2^2
%%                s.t. x_T >0
%% input arguments:
%%         b -------  D'*y transformed object vector
%%         A -------  D'*D transformed dictionary
%%         para ------ Lambda: Sparsity Level
%%                     (lambda1: template set; lambda2:trivial template; lambda3:\mu)
%%                     Lip: Lipschitz Constant for F(x)
%%                     Maxit: Maximal Iteration number
%%                     nT: number of templates
%% output arguments:
%%         c ------  output Coefficient vetor

%  Initialization
ColDim = size(A,1);
xPrev = zeros(ColDim,1);
x = zeros(ColDim,1);
tPrev = 1;
t = 1;
lambda = para.Lambda;
Lip = para.Lip;
maxit = para.Maxit;
nT = para.nT;

temp_lambda = zeros(ColDim,1);
temp_lambda(1:nT) = lambda(1);

%% main loop
for iter =1:maxit
    tem_t = (tPrev-1)/t;
    tem_y = (1+tem_t)*x - tem_t*xPrev;
    temp_lambda(nT+1:end) = lambda(3)*tem_y(nT+1:end);
    tem_y = tem_y - (A*tem_y-b+temp_lambda)/Lip; % update residual
    xPrev = x;
    x(1:nT) = max(tem_y(1:nT),0);
    x(nT+1:end) = softthres(tem_y(nT+1:end),lambda(2)/Lip);
    tPrev = t;
    t = (1+sqrt(1+4*t^2))/2;
end
c = x;

%% soft thresholding operator
function y = softthres(x,lambda)
y = max(x-lambda,0)-max(-x-lambda,0);


function ival = r8mat_is_antipersymm ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_IS_ANTIPERSYMM checks for antipersymmetry.
%
%  Discussion:
%
%    A is antipersymmetric if A(I,J) = -A(N+1-J,N+1-I).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions of
%    the matrix.  M and N must be positive.
%
%    Input, real A(M,N), the matrix.
%
%    Output, integer IVAL:
%    -1, the matrix is not antipersymmetric.
%    1, the matrix is antipersymmetric.
%
  ival = 1

  for i = 1 : min ( m, n )
    for j = n : -1 : max ( 1, n - m + 1 )
      if ( a(i,j) ~= -a(n+1-j,n+1-i) )
        ival = -1;
        return
      end
    end
  end

  return
end


function pde = Kelloggdata
%%  KELLOGGDATA data of Kellogg Problem
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',0,'exactu',@exactu,'g_D',@exactu,'Du',@Du,'d',@d);

    function K =  d(p)  % Diffusion constant
    idx = (p(:,1).*p(:,2) >0);
    K  = ones(size(p,1),1);
    K(idx) = 161.4476387975881;
    end

    function u =  exactu(p) % exact solution
    gamma = 0.1;
    sigma = -14.9225565104455152;
    rho = pi/4;
    r = sqrt(sum(p.^2,2));
    theta = atan2(p(:,2),p(:,1));
    theta = (theta>= 0).*theta + (theta<0).*(theta+2*pi);
    mu = (theta>= 0 & theta<pi/2).*cos((pi/2-sigma)*gamma).*cos((theta-pi/2+rho)*gamma)...
        +(theta>= pi/2 & theta<pi).*cos(rho*gamma).*cos((theta-pi+sigma)*gamma)...
        +(theta>= pi & theta<1.5*pi).*cos(sigma*gamma).*cos((theta-pi-rho)*gamma)...
        +(theta>= 1.5*pi & theta<2*pi).*cos((pi/2-rho)*gamma).*cos((theta-1.5*pi-sigma)*gamma);
    u =  r.^gamma.*mu;
    end

    function uprime =  Du(p)
    % the gradient of exact solution
    gamma = 0.1;
    sigma = -14.92256510455152;
    rho = pi/4;
    theta = atan2(p(:,2),p(:,1));  % jiaodu
    theta = (theta>= 0).*theta +(theta<0).*(theta+2*pi);
    t = 1+(p(:,2).^2)./(p(:,1).^2);
    r = sqrt(sum(p.^2,2));
    rg = r.^gamma;

    ux1 = (p(:,1)>= 0.0 & p(:,2)>= 0.0).*(rg.*gamma./r.*cos((pi/2-sigma)*gamma)./r.*p(:,1)...
        .*cos((theta-pi/2+rho)*gamma)...
        +rg.*cos((pi/2-sigma)*gamma).*sin((theta-pi/2+rho)*gamma)...
        *gamma.*p(:,2)./(p(:,1).^2)./t);

    uy1 = (p(:,1)>= 0.0 & p(:,2)>= 0.0).*(rg*gamma./r.*cos((pi/2-sigma).*gamma)...
        .*cos((theta-pi/2+rho).*gamma)./r.*p(:,2)...
        -rg.*cos((pi/2-sigma).*gamma).*sin((theta-pi/2+rho)*gamma)...
        *gamma./p(:,1)./t);

    ux2 = (p(:,1)<= 0.0 & p(:,2)>= 0.0).*(r.^(-1.9).*p(:,1)*gamma.*cos(rho.*gamma)...
        .*cos((theta-pi+sigma).*gamma)...
        +rg.*cos(rho*gamma).*sin((theta-pi+sigma).*gamma)*gamma...
        .*p(:,2)./(p(:,1).^2)./t);
    uy2 = (p(:,1)<= 0.0 & p(:,2)>= 0.0).*( r.^(-1.9).*p(:,2)*gamma.*cos(rho.*gamma)...
        .*cos((theta-pi+sigma)*gamma)...
        -rg.*cos(rho*gamma).*sin((theta-pi+sigma).*gamma)*gamma./p(:,1)./t);

    ux3 = (p(:,1)<= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,1).*gamma.*cos(sigma.*gamma)...
        .*cos((theta-pi-rho).*gamma) ...
        +rg.*cos(sigma.*gamma).*sin((theta-pi-rho).*gamma).*gamma...
        .*p(:,2)./(p(:,1).^2)./t);
    uy3 = (p(:,1)<= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,2)*gamma.*cos(sigma*gamma)...
        .*cos((theta-pi-rho).*gamma) ...
        -rg.*cos(sigma*gamma).*sin((theta-pi-rho).*gamma)*gamma./p(:,1)./t);

    ux4 = (p(:,1)>= 0.0& p(:,2)<= 0.0).*(r.^(-1.9).*p(:,1).*gamma.*cos((pi/2-rho)*gamma)...
        .*cos((theta-3*pi/2-sigma)*gamma) ...
         +rg.*cos((pi/2-rho).*gamma).*sin((theta-3*pi/2-sigma)*gamma)...
         *gamma.*p(:,2)./(p(:,1).^2)./t);

    uy4 = (p(:,1)>= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,2)*gamma.*cos((pi/2-rho)*gamma)...
        .*cos((theta-3*pi/2-sigma)*gamma)...
        -rg.*cos((pi/2-rho)*gamma).*sin((theta-3*pi/2-sigma)*gamma)...
        *gamma./p(:,1)./t);

    uprime(:,1) =  ux1+ux2+ux3+ux4;
    uprime(:,2) =  uy1+uy2+uy3+uy4;
    end
end


function [ X ] = X_Solver_first(D,rho)
  [U,S,V] = svd(D);
  S0 = diag(S);
  r = length(S0);
  P = [ones(r,1), 1-S0, 1/2/rho-S0];
  rt = zeros(r,1);
  for t = 1:r
    p = P(t,:);
    Delta = p(2)^2-4*p(1)*p(3);
    if Delta <= 0
      rt(t) = 0;
    else
      rts = roots(p);
      rts = sort(rts);
      if rts(1)*rts(2)<=0
        rt(t) = rts(2);
      elseif rts(2)<0
        rt(t) = 0;
      else
        funval = log(1+rts(2))+rho*(rts(2)-S0(t)).^2;
        if funval>log(1+0)+rho*(0-S0(t)).^2;
          rt(t) = 0;
        end
      end
    end
  end

  SSS = diag(rt);
  [m,n] = size(D);
  sig = zeros(m,n);
  sig(1:min(m,n),1:min(m,n)) = SSS;

  X = U*sig*V';
end


function [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
% [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
%    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Backtracking linesearch to satisfy Armijo condition
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   fr: reference function value (usually funObj(x))
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   debug: display debugging information
%   LS_interp: type of interpolation
%   progTol: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested)
%
% recet change: LS changed to LS_interp and LS_multi

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 6
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
else
    [f_new,g_new] = funObj(x+t*d,varargin{:});
end
funEvals = 1;

while f_new > fr + c1*t*gtd || ~isLegal(f_new)
    temp = t;

    if LS_interp == 0 || ~isLegal(f_new)
        % Ignore value of new point
        if debug
            fprintf('Fixed BT\n');
        end
        t = 0.5*t;
    elseif LS_interp == 1 || ~isLegal(g_new)
        % Use function value at new point, but not its derivative
        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ quadratic interpolation based on two points
            if debug
                fprintf('Quad BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ cubic interpolation based on three points
            if debug
                fprintf('Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1); t_prev f_prev sqrt(-1)],doPlot,0,t);
        end
    else
        % Use function value and derivative at new point

        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ cubic interpolation w/ derivative
            if debug
                fprintf('Grad-Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d],doPlot,0,t);
        elseif ~isLegal(g_prev)
            % Backtracking w/ quartic interpolation 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quartic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ quintic interpolation of 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quintic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev g_prev'*d],doPlot,0,t);
         end
    end

    % Adjust if change in t is too small/large
    if t < temp*1e-3
        if debug
            fprintf('Interpolated Value Too Small, Adjusting\n');
        end
        t = temp*1e-3;
    elseif t > temp*0.6
        if debug
            fprintf('Interpolated Value Too Large, Adjusting\n');
        end
        t = temp*0.6;
    end

    % Store old point if doing three-point interpolation
    if LS_multi
        f_prev = f_new;
        t_prev = temp;
        if LS_interp == 2
            g_prev = g_new;
        end
    end

    if ~saveHessianComp && nargout == 6
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else
        [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals+1;

    % Check whether step size has become too small
    if max(abs(t*d)) <= progTol
        if debug
            fprintf('Backtracking Line Search Failed\n');
        end
        t = 0;
        f_new = f;
        g_new = g;
        break;
    end
end

% Evaluate Hessian at new point
if nargout == 6 && funEvals > 1 && saveHessianComp
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    funEvals = funEvals+1;
end

x_new = x + t*d;

end


function newp = eleminterpolate(p,tree,weight)
%% ELEMINTERPOLATE interpolate a piecewise constant function.
%
% newp = eleminterpolate(p,tree) interpolate a piecewise constant function
% p from a coarse grid to a fine grid or the other way around. tree(:,1:3)
% stores the binary tree of the coarsening. tree(:,1) is the index of
% parent element in the coarsened mesh and tree(:,2:3) are two children
% indices in the original mesh.
%
% For uniform refinement, the prolongation from the coarse grid to the fine
% grid is simply |newp = repmat(p,4,1)| and from the fine grid to the
% coarse one is |newp = p(1:NT/4)|.
%
% Example
%
%   [node,elem] = squaremesh([0 1 0 1],1/2);
%   p = 1:size(elem,1);
%   figure(1);
%   subplot(1,4,1); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = bisect(node,elem,[1 2]);
%   p = eleminterpolate(p,tree);
%   subplot(1,4,2); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = bisect(node,elem,[1 2]);
%   p = eleminterpolate(p,tree);
%   subplot(1,4,3); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = coarsen(node,elem,'all');
%   p = eleminterpolate(p,tree);
%   subplot(1,4,4); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%
% See also bisect, coarsen, nodeinterpolate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('weight','var'), weight = 1; end
%%
newp = p;
if (size(tree,1)==0), return; end % no change
NTin = length(p);
NTf = max(tree(:,3));
if NTin < NTf 		   % coarse grid to fine grid
    NTc = NTin;
    newp = zeros(NTf,1);
    isNew = tree(:,3); % Right child is a new element
    inCoarse = true(NTf,1);
    inCoarse(isNew) = false;
    % Case 0: still in coarse element
    newp(inCoarse) = p;
    % Case 1: The parent element is in the coarse mesh. A triangle t could
    % be bisected twice and a child could be a parent
    idx = (tree(:,1) <= NTc);
    if weight == 1
        newp(tree(idx,2)) = p(tree(idx,1));
        newp(tree(idx,3)) = p(tree(idx,1));
    else
        newp(tree(idx,2)) = weight*p(tree(idx,1));
        newp(tree(idx,3)) = weight*p(tree(idx,1));
    end
    % Case 2: The parent is in the intermediate mesh
    idx = (tree(:,1) > NTc);
    if weight == 1
        newp(tree(idx,2)) = newp(tree(idx,1));
        newp(tree(idx,3)) = newp(tree(idx,1));
    else
        newp(tree(idx,2)) = weight*newp(tree(idx,1));
        newp(tree(idx,3)) = weight*newp(tree(idx,1));
    end
else      % fine grid to coarse grid
    newp = p;
    newp(tree(:,3)) = [];
end


%% patchFeatureDetect
% Below is a demonstration of the features of the |patchFeatureDetect| function

%%
clear; close all; clc;

%% Syntax
% |[G]=patchFeatureDetect(F,V,a);|

%% Description
% This function detects surface features or groups for the input patch data
% defined by the faces F and the vertices V. Patch elements for which the
% dihedral angle is lower than a are grouped together.
% This function is useful for detecting sets of faces from for instance
% importance CAD geometry, e.g. top faces, side faces etc can be
% automatically detected provided that their boundaries have a significant
% enough angle change.

%%
% Plot settings
fontSize=15;

%%
% Create example data

testCase=4;
switch testCase
    case 1
        boxDim=[4 5 6]; %Width in each direction
        pointSpacing=1; %Desired point spacing
        [F,V]=triBox(boxDim,pointSpacing);
    case 2
        boxDim=[4 5 6]; %Width in each direction
        boxEl=[4 5 6]; %Desired number of elements
        [F,V]=quadBox(boxDim,boxEl);
    case 3
        defaultFolder = fileparts(fileparts(mfilename('fullpath')));
        pathName=fullfile(defaultFolder,'data','libSurf');
        dataStruct=load(fullfile(pathName,'enginePart_p1.mat'));
        F=dataStruct.F;
        V=dataStruct.V;
    case 4
        defaultFolder = fileparts(fileparts(mfilename('fullpath')));
        pathName=fullfile(defaultFolder,'data','libSurf');
        dataStruct=load(fullfile(pathName,'sprocket.mat'));
        F=dataStruct.F;
        V=dataStruct.V;
end

%%
% Use |patchFeatureDetect| to detect features in patch

%Angular threshold in radians
a=(45/180)*pi;

%Detect surface features
G=patchFeatureDetect(F,V,a);

%%

% Plotting model
cFigure;
subplot(1,2,1); hold on;
title('Input surface','FontSize',fontSize);
gpatch(F,V,'w','k',1);
axisGeom(gca,fontSize); camlight headlight;

subplot(1,2,2); hold on;
title('Surface feature labels','FontSize',fontSize);
gpatch(F,V,G,'k',1);
axisGeom(gca,fontSize); camlight headlight;
colormap gjet; icolorbar;
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.