function Population = subNSGAIII(Population,N,Z,Zmin)
% The environmental selection of NSGA-III

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    if isempty(Zmin)
        Zmin = ones(1,size(Z,2));
    end
    CC = min([length(Population),N]);
    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,CC);
    Next = FrontNo < MaxFNo;

    %% Select the solutions in the last front
    Last   = find(FrontNo==MaxFNo);
    Choose = LastSelection(Population(Next).objs,Population(Last).objs,CC-sum(Next),Z,Zmin);
    Next(Last(Choose)) = true;
    % Population for next generation
    Population = Population(Next);
end

function Choose = LastSelection(PopObj1,PopObj2,K,Z,Zmin)
% Select part of the solutions in the last front

    PopObj = [PopObj1;PopObj2] - repmat(Zmin,size(PopObj1,1)+size(PopObj2,1),1);
    [N,M]  = size(PopObj);
    N1     = size(PopObj1,1);
    N2     = size(PopObj2,1);
    NZ     = size(Z,1);

    %% Normalization
    % Detect the extreme points
    Extreme = zeros(1,M);
    w       = zeros(M)+1e-6+eye(M);
    for i = 1 : M
        [~,Extreme(i)] = min(max(PopObj./repmat(w(i,:),N,1),[],2));
    end
    % Calculate the intercepts of the hyperplane constructed by the extreme
    % points and the axes
    Hyperplane = PopObj(Extreme,:)\ones(M,1);
    a = 1./Hyperplane;
    if any(isnan(a))
        a = max(PopObj,[],1)';
    end
    % Normalization
    PopObj = PopObj./repmat(a',N,1);

    %% Associate each solution with one reference point
    % Calculate the distance of each solution to each reference vector
    Cosine   = 1 - pdist2(PopObj,Z,'cosine');
    Distance = repmat(sqrt(sum(PopObj.^2,2)),1,NZ).*sqrt(1-Cosine.^2);
    % Associate each solution with its nearest reference point
    [d,pi] = min(Distance',[],1);

    %% Calculate the number of associated solutions except for the last front of each reference point
    rho = hist(pi(1:N1),1:NZ);

    %% Environmental selection
    Choose  = false(1,N2);
    Zchoose = true(1,NZ);
    % Select K solutions one by one
    while sum(Choose) < K
        % Select the least crowded reference point
        Temp = find(Zchoose);
        Jmin = find(rho(Temp)==min(rho(Temp)));
        j    = Temp(Jmin(randi(length(Jmin))));
        I    = find(Choose==0 & pi(N1+1:end)==j);
        % Then select one solution associated with this reference point
        if ~isempty(I)
            if rho(j) == 0
                [~,s] = min(d(N1+I));
            else
                s = randi(length(I));
            end
            Choose(I(s)) = true;
            rho(j) = rho(j) + 1;
        else
            Zchoose(j) = false;
        end
    end
end


% GEODOME   Generates geodesic sphere
%
% Usage:  [xyz, A, F] = geodome(frequency, radius)
%
% The sphere is generated from subdivisions of the faces of an icosahedron.
%
% Arguments:
%        frequency - The number of subdivisions of each edge of an
%                    icosahedron.  A frequency of 1 will give you an
%                    icosahedron. A high number will give you a more
%                    spherical shape. Defaults to 2.
%           radius - Radius of the sphere. Defaults to 1.
% Returns:
%              xyz - A nx3 matrix defining the vertex list, each row is the
%                    x,y,z coordinates of a vertex.
%                A - Adjacency matrix defining the connectivity of the
%                    vertices
%                F - A mx3 matrix defining the face list.  Each row of F
%                    specifies the 3 vertices that make up the face.
%
% Example:
%    [xyz, A, F] = geodome(3, 5);  % Generate a 3-frequency sphere with
%                                  % radius 5
%    gplot3d(A, xyz);              % Use adjacency matrix and vertex list to
%                                  % generate a wireframe plot.
%    drawfaces(xyz, F);            % Use face and vertex lists to generate
%                                  % surface patch plot.
%    axis vis3d, axis off, rotate3d on
%
% See also: ICOSAHEDRON, GPLOT3D, DRAWFACES

% Copyright (c) 2009 Peter Kovesi
% Centre for Exploration Targeting
% The University of Western Australia
% peter.kovesi at uwa edu au
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% May   2009
% April 2014  - Dome radius scaling fixed (thanks to Brad Keserich)

function [xyz, A, F] = geodome(frequency, radius)

    if ~exist('frequency','var')
        frequency = 2;
    end

    if ~exist('radius','var')
        radius = 1;
    end

    if frequency < 1
        error('Frequency must be an integer >= 1');
    end

    % Generate vertices of base icosahedron
    [icosXYZ, icosA, icosF] = icosahedron(1);

    % Compute number of vertices in geodesic sphere.  This is the 12 vertices
    % of the icosahedron + the extra vertices introduced on each of the 30
    % edges + the extra vertices in the interior of the of each of the 20
    % faces.  This latter is a sum of an arithmetic series [1 .. (frequency-2)]
    fm1 = frequency - 1;
    fm2 = frequency - 2;
    nVert = 12 + 30*fm1 + 20*fm2*fm1/2;

    xyz = zeros(nVert,3);

    if frequency == 1             % Just return the icosahedron
        xyz = icosXYZ;
        A = icosA;
        F = icosF;
    else                          % For all frequencies > 1
        xyz(1:12,:) = icosXYZ;    % Grab the vertices of the icosahedron
        offset = 13;

        % Find the nodes that connect edges.  Note we use the upper triangular
        % part of the adjacency matrix so that we only get each edge once.
        [i,j] = find(triu(icosA));

        % Generate extra vertices on every edge of the icoshedron.
        for n = 1:length(i)
            xyz(offset:(offset+fm2) ,:) = ...
                   divideEdge(icosXYZ(i(n),:), icosXYZ(j(n),:), frequency);
            offset = offset+fm1;
        end

        % Generate the extra vertices within each face of the icoshedron.
        for f = 1:length(icosF)

            % Re subdivide two of the edges of the face and get the vertices
            % (Yes, this is wasteful but it makes code logic easier)
            V1 = divideEdge(icosXYZ(icosF(f,1),:), icosXYZ(icosF(f,2),:), frequency);
            V2 = divideEdge(icosXYZ(icosF(f,1),:), icosXYZ(icosF(f,3),:), frequency);

            % Now divide the edges that join the new vertices along the
            % subdivided edges.
            for v = 2:fm1
               VF = divideEdge(V1(v,:), V2(v,:), v);
               xyz(offset:(offset+v-2),:) = VF;
               offset = offset+v-1;
            end
        end

    end

    xyz = xyz*radius;       % Scale vertices to required radius
    A = adjacency(xyz);
    F = faces(A);

%---------------------------------------------------------------------
% Function to divide an edge defined between vertices V1 and V2 into nSeg
% segments and return the coordinates of the vertices.
% This function simplistically divides the distance between V1 and V2

function vert = divideEdgeOld(V1, V2, nSeg)

    edge = V2 - V1;  % Vector along edge

    % Now add appropriate fractions of the edge length to the first node
    vert = zeros(nSeg-1, 3);
    for f = 1:(nSeg-1)
        vert(f,:) = V1 + edge * f/nSeg;
        vert(f,:) = vert(f,:)/norm(vert(f,:));   % Normalize to unit length
    end

%---------------------------------------------------------------------
% Function to divide an edge defined between vertices V1 and V2 into nSeg
% segments and return the coordinates of the vertices.
% This function divides the *angle* between V1 and V2
% rather than the distance.
function vert = divideEdge(V1, V2, nSeg)

    axis = cross(V1,V2);
    angle = atan(norm(axis)/dot(V1,V2));
    axis = axis/norm(axis);

    % Now add appropriate fractions of the edge length to the first node
    vert = zeros(nSeg-1, 3);
    for f = 1:(nSeg-1)
        Q = newquaternion(f*angle/nSeg, axis);
        vert(f,:) =  quaternionrotate(Q,V1);
        vert(f,:) = vert(f,:)/norm(vert(f,:));   % Normalize to unit length
    end

%-------------------------------------------------------------------------
% Function to build adjacency matrix for geodesic sphere by brute force

function A = adjacency(xyz)

    nVert = length(xyz);
    A = zeros(nVert);

    % Find distances between all pairs of vertices
    for n1 = 1:nVert-1
        A(n1,n1) = Inf;
        for n2 = n1+1:nVert
            A(n1,n2) = norm(xyz(n2,:) - xyz(n1,:));
        end
    end
    A(nVert,nVert) = Inf;

    A = A+A';   % Make A symmetric

    % Find min distance in first row.
    minD = min(A(1,:)');

    % Assume that no edge can be more than 1.5 times this minimum distance, use
    % this to decide connectivity of nodes.
    A = A < minD*1.5;

%-----------------------------------------------------------------------------
% Function to find the triplets of vertices that define the faces of the
% geodesic sphere

function  F = faces(A)

    % Strategy:  We are only after cycles of length 3 in graph defined by A.
    % For every node N0 (except the last two, which we will not need to visit)
    % - Get list of neighbours, N1
    % - For each neighbour N1i in N1 find its list of neighbours, N2.
    % - Any neighbour in N2 that is in N1 must form a cycle of length 3 with N0

    nVert = length(A);
    F = [];

    for N0 = 1:nVert-2
        N1 = find(A(N0,:));            % Neighbours of N0
        for N1i = N1
            N2 = find(A(N1i,:));
            cycle = intersect(N2,N1);  % Find the 2 nodes of N2 that are in N1
            F = [F; [N0 N1i cycle(1)]; [N0 N1i cycle(2)]];
        end
    end

    % Each face will be found multiple times, eliminate duplicates.
    F = sort(F,2);            % Sort rows of face list
    F = unique(F,'rows');     % ...then extract the unique rows.


function varargout = cumsum2(varargin)
%CUMSUM2   Double indefinite integral of a CHEBFUN2.
%   F = CUMSUM2(F) returns the double indefinite integral of a CHEBFUN2. That is
%                   y  x
%                  /  /
%   CUMSUM2(F) =  |  |   f(x,y) dx dy   for  (x,y) in [a,b] x [c,d],
%                 /  /
%                c  a
%
%   where [a,b] x [c,d] is the domain of f.
%
% See also CUMSUM, SUM, SUM2.

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

[varargout{1:nargout}] = cumsum2@separableApprox(varargin{:});

end


function [km3] = oz2km3(oz)
% Convert volume from US liquid ounces to cubic kilometers.
% Chad Greene 2012
km3 = oz*2.9573529563e-14;


function r = randomc(varargin)
%RANDOMC      Complex random numbers in M+i*M with M=[min,max], default [-1,+1]
%
% Calling conventions as function  random
%

% written  03/18/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

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

  if length(varargin)==0
    r = random(NaN) + sqrt(-1)*random(NaN);
  else
    r = random(NaN, varargin) + sqrt(-1)*random(NaN,varargin);
  end

  if rndold
    setround(rndold)
  end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%	SurfBox-MATLAB (c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	Yue Lu and Minh N. Do
%%
%%	Department of Electrical and Computer Engineering
%%	Coordinated Science Laboratory
%%	University of Illinois at Urbana-Champaign
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	ccsym.m
%%
%%	First created: 08-14-05
%%	Last modified: 04-13-06
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function y = ccsym(x, k, type)

%% Exploit the complex conjugate symmetry in the fft of real signals.

%% type: 'c' compact 'e' expand
%% k: along which dimension

%% Dimensions of the problem
N = ndims(x);
szX = size(x);

if type == 'c'
    %% initialize the subscript array
    sub_array = repmat({':'}, [N, 1]);
    sub_array{k} = 1 : szX(k) / 2 + 1;
    y = x(sub_array{:});
else
    %% subscript mapping for complex conjugate symmetric signal
    %% recovery

    szX(k) = (szX(k)-1) * 2;
    sub_conj = cell(N, 1);

    for m = 1 : N
        sub_conj{m} = [1 szX(m):-1:2];
    end

    sub_conj{k} = [szX(k)/2 : -1 : 2];
    %% recover the full signal in the spatial domain complex
    %% conjugate symmetry.
    y = cat(k, x, conj(x(sub_conj{:})));

end

%%	This software is provided "as-is", without any express or implied
%%	warranty. In no event will the authors be held liable for any
%%	damages arising from the use of this software.


function [ outputTexture ] = Remap( inputTexture, mapX, mapY )
%REMAP Summary of this function goes here
%   Detailed explanation goes here

    outputTexture = zeros(size(mapX));

    [X,Y] = meshgrid(0:size(inputTexture,2)-1,0:size(inputTexture,1)-1);

    inds = find(mapX ~= -1);

    xSources = mapX(inds);
    ySources = mapY(inds);

    Z = interp2(X, Y, double(inputTexture), xSources, ySources);
    outputTexture(inds) = Z;
end


function [xStateUpdate,PUpdate,innov,Pzz,W]=BLUEPolarMeasUpdateApprox(xStatePred,PPred,z,R)
%%BLUEPOLARMEASUPDATEAPPROX Perform the measurement update step in the
%                approximate best linear unbiased estimator (BLUE) for a
%                Cartesian state consisting of 2D position and velocity
%                when given a measurement in monostatic polar coordinates.
%
%INPUTS: xStatePred The 4X1 target state consisting of position components
%                   followed by velocity components:
%                   xStatePred=[x;y;xDot;yDot].
%             PPred The 3X3 covariance matrix associated with the predicted
%                   target state estimate.
%                 z A 2X1 one-way (monostatic) polar measurement (no
%                   refraction) with components ordered [range;azimuth]
%                   with azimuth in radians measured in radians,
%                   counterclockwise from the x-axis.
%                 R The 2X2 diagonal covariance matrix associated with the
%                   measurement z. Any cross terms present in R will be
%                   ignored.
%
%OUTPUTS: xStateUpdate The 4 X 1 updated state vector.
%              PUpdate The updated 4 X 4 state covariance matrix.
%           innov, Pzz The 2X1 innovation and the 2X2 innovation covariance
%                      matrix are returned in case one wishes to analyze
%                      the consistency of the estimator or use those values
%                      in gating or likelihood evaluation.
%                    W The gain used in the update. This can be useful
%                      when gating and using the function
%                      calcMissedGateCov.
%
%The algorithm is taken from Section IV of [1]. One equation that is not
%directly written in the above journal article is taken from the conference
%version [2].
%
%REFERENCES:
%[1] Z. Zhao, X. R. Li, and V. P. Jilkov, "Best linear unbiased filtering
%    with nonlinear measurements for target tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 40, no. 4, pp. 1324-1336,
%    Oct. 2004.
%[2] Z. Zhao, X. R. Li, and V. P. Jilkov, "Optimal linear unbiased
%    filtering with polar measurements for target tracking," in Proceedings
%    of the 5th International Conference on Information Fusion, Annapolis,
%    MD, 8-11 Jul. 2002, pp. 1527-1534.
%
%February 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Change the ordering of the elements in the state to match that used in the
%paper.
permuteIdx=[1;3;2;4];
undoPermuteIdx=[1;3;2;4];%The inverse permutation
xStateBar=xStatePred(permuteIdx);
PBar=PPred(permuteIdx,permuteIdx);

xBar=xStateBar(1);
yBar=xStateBar(3);

covXTilde=PBar(1,1);
covYTilde=PBar(3,3);
covXTileYTilde=PBar(1,3);

sigmaR2=R(1,1);
sigmaTheta2=R(2,2);

%The Taylor series approximation to the expected value of
%(y^2-x^2)/(x^2+y^2) from Section VI of the journal article.
denom=(xBar^2+yBar^2)^3;
ExyRat1=(yBar^2-xBar^2)/(xBar^2+yBar^2)+...
        2*yBar^2*(yBar^2-3*xBar^2)*covXTilde/denom+...
        4*xBar*yBar*(xBar^2-yBar^2)*covXTileYTilde/denom-...
        2*xBar^2*(xBar^2-3*yBar^2)*covYTilde/denom;

%The Taylor series approximation to the expected value of x*y/(x^2+y^2)
%from Section VI is taken from the form written out explicitly in Equation
%18 of the conference paper.
ExyRat2=xBar*yBar/(xBar^2+yBar^2)+(1/2)*(...
        2*xBar*yBar*(xBar^2-3*yBar^2)*covXTilde/denom+...
        (6*xBar^2*yBar^2-xBar^4-yBar^4)*covXTileYTilde/denom+...
        2*xBar*yBar*(yBar^2-3*xBar^2)*covYTilde/denom);

lambda1=exp(-sigmaTheta2/2);
lambda2=(1/2)*(1+exp(-2*sigmaTheta2));
lambda3=(1/2)*(1-exp(-2*sigmaTheta2));

%The value of S11 is from Section IV of the journal article.
Pzz(1,1)=lambda2*covXTilde+lambda3*covYTilde+(1/2)*sigmaR2+lambda3*yBar^2+...
    (lambda2-lambda1^2)*xBar^2+(1/2)*sigmaR2*exp(-2*sigmaTheta2)*(-ExyRat1);
Pzz(2,2)=lambda2*covYTilde+lambda3*covXTilde+(1/2)*sigmaR2+lambda3*xBar^2+...
    (lambda2-lambda1^2)*yBar^2+(1/2)*sigmaR2*exp(-2*sigmaTheta2)*(ExyRat1);
Pzz(1,2)=exp(-2*sigmaTheta2)*covXTileYTilde+(exp(-2*sigmaTheta2)-...
    lambda1^2)*xBar*yBar-sigmaR2*exp(-2*sigmaTheta2)*ExyRat2;
Pzz(2,1)=Pzz(1,2);

%Equation 17
covXTildeZTilde=lambda1*[PBar(:,1),PBar(:,3)];
W=covXTildeZTilde/Pzz;
zCartPred=lambda1*[xBar;yBar];

zCart=pol2Cart(z);
innov=zCart-zCartPred;
xStateUpdate=xStateBar+W*(zCart-zCartPred);
PUpdate=PBar-W*Pzz*W';

%Make the ordering go back to the way it was.
xStateUpdate=xStateUpdate(undoPermuteIdx);
PUpdate=PUpdate(undoPermuteIdx,undoPermuteIdx);

end

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


function [params, s_new] = denoise_fica_gauss(params, s, state)
% FastICA gauss nonlinearity as DSS denoising function
%   [params, s_new] = denoise_fica_gauss(params, s, state)
%     params  Function specific modifiable parameters
%     params.a  Scale constant (default: 1)
%     state   DSS algorithm state
%     s       Source signal estimate, matrix of row vector signals
%     s_new   Denoised signal estimate

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<3 | ~isstruct(state)
    params.name = 'FastICA gaussian nonlinearity';
    params.description = '';
    params.param = {'a'};
    params.param_value ={1};
    params.param_type = {'scalar'};
    params.param_desc = {'Scaling constant'};
    params.approach = {'defl','symm'};
    params.alpha = {};
    params.beta = {};
    return;
end

if ~isfield(params, 'a')
  params.a = 1;
end
a = params.a;

s2=s.^2;
ex=exp(-a * s2/2);
gauss =  s.*ex;
beta = mean((1 - a * s2) .*ex, 2);
s_new = gauss - diag(beta) * s;


%-fanDTasia ToolBox------------------------------------------------------------------
% This Matlab script is part of the fanDTasia ToolBox: a Matlab library for Diffusion
% Weighted MRI (DW-MRI) Processing, Diffusion Tensor (DTI) Estimation, High-order
% Diffusion Tensor Analysis, Diffusion Kurtosis Imaging (DKI) Estimation,
% Tensor ODF estimation, Visualization and more.
%
% A Matlab Tutorial on DW-MRI can be found in:
% http://www.cise.ufl.edu/~abarmpou/lab/fanDTasia/tutorial.php
%
%-CITATION---------------------------------------------------------------------------
% If you use this software please cite the following papers on 4th-order tensors:
% 1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI
%    using Homogeneous Polynomials", In the Proceedings of ISBI, 2011, pp. 262-265.
% 2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion
%    Tensors of any order with Symmetric Positive-Definite Constraints",
%    In the Proceedings of ISBI, 2010, pp. 1385-1388.
%
%-DESCRIPTION------------------------------------------------------------------------
% This demo script shows how to compute the Diffusion Kurtosis Coefficients from a given
% Diffusion-Weighted MRI dataset. The method guarantees that the estimated diffusivity is
% positive semi-definite, and computes the diffusion and kurtosis tensors using the method
% in Sec. 4.1 of the ISBI'11 paper. Here the given demo dataset consists of 5 voxels,
% 30 gradient directions x 2 b-values from the real brain dataset used in the ISBI'11 paper.
%
%-USE--------------------------------------------------------------------------------
% [D,W]=DEMO_DKI_Estimation_Method1_v2;
%
% D: is a vector with the computed Unique Coefficients of the 2nd-order Diffusion Tensor
% W: is a vector with the computed Unique Coefficients of the 4th-order Kurtosis Tensor
%
%-DISCLAIMER-------------------------------------------------------------------------
% You can use this source code for non commercial research and educational purposes
% only without licensing fees and is provided without guarantee or warrantee expressed
% or implied. You cannot repost this file without prior written permission from the
% authors. If you use this software please cite the following papers:
% 1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI
%    using Homogeneous Polynomials", In the Proceedings of ISBI, 2011.
% 2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion
%    Tensors of any order with Symmetric Positive-Definite Constraints",
%    In the Proceedings of ISBI, 2010, pp. 1385-1388.
%
%-AUTHOR-----------------------------------------------------------------------------
% Angelos Barmpoutis, PhD
% Digital Worlds Institute
% University of Florida, Gainesville, FL 32611, USA
% angelbar at ufl dot edu
%------------------------------------------------------------------------------------
function [DKI_D,DKI_W]=DEMO_DKI_Estimation_Method1_v2

%%% DATA OPENING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%open data files
[S,B]=openFDT('real_data_5voxels.fdt');

%S0 signal, no diffusion weighting
S0=S(:,:,:,1);

%acquisition shell 1, 30 orientations
S_1real=S(:,:,:,[2:31]);
GradientOrientations_1=B([2:31],[1:3]);
BValue_1=B(2,4);

%acquisition shell 2, 30 orientations
S_2real=S(:,:,:,[32:61]);
GradientOrientations_2=B([32:61],[1:3]);
BValue_2=B(32,4);

%%% OPTIONAL: ADD RICIAN NOISE TO THE DATA FOR QUANTITATIVE EVALUATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%

stdv=input('Do you want to add Rician noise to the data for validation? \n If yes, then give the Std. Dev. (e.g: 0.1), otherwise type 0.\n STD.DEV.:');
S_1=S_1real;
for i=1:size(S_1real,4)
    S_1(:,:,:,i)=sqrt((S_1real(:,:,:,i)+stdv*S0.*randn(size(S_1real,1),size(S_1real,2),size(S_1real,3))).^2+(stdv*S0.*randn(size(S_1real,1),size(S_1real,2),size(S_1real,3))).^2);
end
S_2=S_2real;
for i=1:size(S_2real,4)
    S_2(:,:,:,i)=sqrt((S_2real(:,:,:,i)+stdv*S0.*randn(size(S_2real,1),size(S_2real,2),size(S_2real,3))).^2+(stdv*S0.*randn(size(S_2real,1),size(S_2real,2),size(S_2real,3))).^2);
end
%your data can have as many shells and bvalues you want

%%% INITIALIZATION - COMPUTING AUXILIARY DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Construct a constant set of polynomial coefficients C
C_order4=constructSetOf321Polynomials(4)'; %computes C from section 5.1 (ISBI'10)
A_1=D4toDKImatrix(BValue_1); %Computes the matrix A from Table 1 (ISBI'11)
A_2=D4toDKImatrix(BValue_2); %Computes the matrix A from Table 1 (ISBI'11)

%shell 1
G_1_order2=constructMatrixOfMonomials(GradientOrientations_1, 2); %computes G from section 5.1 (ISBI'10)
G_1_order4=constructMatrixOfMonomials(GradientOrientations_1, 4); %computes G from section 5.1 (ISBI'10)
Gbig_1=[-BValue_1*G_1_order2 BValue_1*BValue_1/6*G_1_order4];  %all the monomials for orders 2 and 4.

%shell 2
G_2_order2=constructMatrixOfMonomials(GradientOrientations_2, 2); %computes G from section 5.1 (ISBI'10)
G_2_order4=constructMatrixOfMonomials(GradientOrientations_2, 4); %computes G from section 5.1 (ISBI'10)
Gbig_2=[-BValue_2*G_2_order2 BValue_2*BValue_2/6*G_2_order4];  %all the monomials for orders 2 and 4.

%your data can have as many shells and bvalues you want
Gbig=[Gbig_1; Gbig_2]; %all the monomials for orders 2 and 4 and bvalues b1 and b2.

%%%%%% MAIN LOOP - METHOD: LINEAR FITTING - NO CONSTRAINTS %%%%%%%%%%%%%%%%%%%%%%%%%
for x=1:size(S,1)
    for y=1:size(S,2)
        for z=1:size(S,3)

            logS_1=log(squeeze(S_1(x,y,z,:))/S0(x,y,z));
            logS_2=log(squeeze(S_2(x,y,z,:))/S0(x,y,z));

            %The following 2 steps implement the method in Sec. 4.1 of the ISBI'11 paper.
            %Step 1: Compute a positive-definite 4th-order tensor for each b-value
            D4_1=C_order4*lsqnonneg(-G_1_order4*C_order4, logS_1);%computes a positive-definite tensor for b1
            D4_2=C_order4*lsqnonneg(-G_2_order4*C_order4, logS_2);%computes a positive-definite tensor for b2

            %Step 2: Compute DKI from the positive definite 4th-order tensors.
            dki=pinv([A_1;A_2])*[D4_1;D4_2];

            %Optional Validation if user adds noise to the data
            if stdv>0
                logS_1=log(squeeze(S_1real(x,y,z,:))/S0(x,y,z));
                logS_2=log(squeeze(S_2real(x,y,z,:))/S0(x,y,z));
                err(:,x,y,z)=abs(Gbig*dki-[logS_1; logS_2]);
            end

            %Store the data
            DKI_D(:,x,y,z)=dki([1:6]); %The 6 unique coefficients of the diffusion tensor D
            %If you want you can put the result in the form of a 3x3 matrix
            %D=[dki(6) dki(5)/2 dki(4)/2
            %   dki(5)/2 dki(3) dki(2)/2
            %   dki(4)/2 dki(2)/2 dki(1)];

            DKI_W(:,x,y,z)=dki([7:21]); %The 15 unique coefficients of the kurtosis tensor W
            %You can see which coefficient is which you can use the function: printTensor(DKI_W(:,x,y,z),4)
            % or if you want to plot a tensor or a tensor field as spherical functions
            % you have to download the plotTensors.m function developed by Angelos Barmpoutis, Ph.D.
            % and then uncomment the following lines.
            %
            % plotTensors(DKI_D(:,1,1,1),1,[321 1]); %3D ellipsoidal plot of D
            % plotTensors(DKI_W(:,1,1,1),1,[321 1]); %3D ellipsoidal plot of W

            %Optional Calculation of Dapp and Kapp
            Dapp(:,x,y,z) = G_1_order2*dki(1:6);
            Kapp(:,x,y,z) = (G_1_order4*dki(7:21))./((G_1_order2*dki(1:6)).^2);

        end
    end
end

%%%%%% OPTIONAL: PRINT RESULTS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
all_err=[];
all_Dapp=[];
all_Kapp=[];
counter=0;
meanD=zeros(6,1);
meanW=zeros(15,1);
for x=1:size(S,1)
    for y=1:size(S,2)
        for z=1:size(S,3)
            if stdv>0
               all_err=[all_err; err(:,x,y,z)];
            end
            all_Dapp=[all_Dapp; Dapp(:,x,y,z)];
            all_Kapp=[all_Kapp; Kapp(:,x,y,z)];
            meanD=meanD+DKI_D(:,x,y,z);
            meanW=meanW+DKI_W(:,x,y,z);
            counter=counter+1;
        end
    end
end
meanD=meanD/counter;
meanW=meanW/counter;

fprintf(1,'\n-----RESULTS:-----\n\n');
fprintf('Number of fitted voxels: %d\n',counter);
if stdv>0
    fprintf(1,'Fitting Error: %.4f (std. dev. %.4f)\n',mean(all_err),std(all_err));
end
fprintf(1,'Mean Dapp: %.4f (std. dev. %.4f)\n',mean(all_Dapp),std(all_Dapp));
fprintf(1,'Mean Kapp: %.4f (std. dev. %.4f)\n',mean(all_Kapp),std(all_Kapp));
fprintf(1,'\nMean Diffusion Tensor D:\n');
printTensor(meanD,2);
fprintf(1,'\nMean Kurtosis Tensor W:\n');
printTensor(meanW,4);

% If you want to plot a Diffusion or Kurtosis tensor as spherical functions
% you have to download the plotTensors.m function developed by Angelos Barmpoutis, Ph.D.
% and then uncomment the following lines.
%
% subplot(1,2,1)
% plotTensors(meanD,1,[321 1]);
% title('Mean Diffusion Tensor');
% subplot(1,2,2);
% plotTensors(meanW,1,[321 1]);
% title('Mean Kurtosis Tensor');

fprintf(1,'\nIf you use this software please cite the following papers on DKI and DTI estimation:\n');
fprintf(1,'1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI\n');
fprintf(1,'   using Homogeneous Polynomials", In the Proceedings of ISBI, 2011, pp. 262-265.\n');
fprintf(1,'2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors\n');
fprintf(1,'   of any order with Symmetric Positive-Definite Constraints",\n');
fprintf(1,'   In the Proceedings of ISBI, 2010, pp. 1385-1388.\n');


function [GAmp,GTime]=GxFSE(p)

global VCtl;
global VObj;

t1Start=p.t1Start;
t2Middle=p.t2Middle;
Gx1Sign=p.Gx1Sign;
Gx2Sign=p.Gx2Sign;

GAmp=[];
GTime=[];
GxAmp=(1/VCtl.FOVFreq)/((VObj.Gyro/(2*pi))*(1/VCtl.BandWidth));
tHalf=1/(2*(VObj.Gyro/(2*pi))*GxAmp*VCtl.RFreq);
tRamp=GxAmp/VCtl.MaxSlewRate;

% prephasing
[GAmp1,GTime1]=StdTrap(t1Start-tRamp,            ...
                       t1Start+tHalf+tRamp,      ...
                       t1Start,                          ...
                       t1Start+tHalf,                    ...
                       GxAmp*Gx1Sign,2,2,2);

GAmp=[GAmp GAmp1];
GTime=[GTime GTime1];

% frequency encoding
TimeOffset = (t2Middle+VCtl.TEAnchorTime)-floor(VCtl.FSE_ETL/2)*VCtl.FSE_ESP;
for i = 1: VCtl.FSE_ETL
    [GAmpt,GTimet]=StdTrap(TimeOffset + (i-1)*VCtl.FSE_ESP -tHalf -tRamp, ...
                           TimeOffset + (i-1)*VCtl.FSE_ESP +tHalf +tRamp, ...
                           TimeOffset + (i-1)*VCtl.FSE_ESP -tHalf, ...
                           TimeOffset + (i-1)*VCtl.FSE_ESP +tHalf, ...
                           GxAmp*Gx2Sign,2,2,2);
     GAmp=[GAmp GAmpt];
     GTime=[GTime GTimet];

end

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

end


function pass = test_coeffs_vals( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e3*pref.techPrefs.chebfuneps;
j = 1;
%grab some coeffs
u = randnfunsphere(.3);
v = randnfunsphere(.4);
w = randnfunsphere(.3);
f = [u;v;w];

% test coeffs2:
[x, y, z] = coeffs2(f);
pass(j) = norm(spherefun.coeffs2spherefun(x)-u, inf) < tol;
pass(j+1) = norm(spherefun.coeffs2spherefun(y)-v, inf) < tol;
pass(j+2) = norm(spherefun.coeffs2spherefun(z)-w, inf) < 10*tol;
j = j+3;

%parameters
[x, y, z] = coeffs2(f, 50, 60);
pass(j) = norm(x-coeffs2(u, 50, 60), inf) < tol;
pass(j+1) = norm(y-coeffs2(v, 50, 60), inf) < tol;
pass(j+2) = norm(z-coeffs2(w, 50, 60), inf) < tol;
j = j+3;

% test coeffs2diskfunv:
f2 = spherefunv.coeffs2spherefunv(coeffs2(u), coeffs2(v),...
    coeffs2(w));
pass(j) = norm(f - f2) < tol;
j = j+1;

%test coeffs2vals:
[x, y, z] = coeffs2(f2);
[u, v, w] = spherefunv.coeffs2vals(x,y,z);
pass(j) = norm(spherefun.coeffs2vals(x)-u, 'inf') < tol;
pass(j+1) = norm(spherefun.coeffs2vals(y)-v, 'inf')< tol;
pass(j+2) = norm(spherefun.coeffs2vals(z)-w, 'inf')< tol;
j= j+2;

%testvals2coeffs:
[a,b,c] = spherefunv.vals2coeffs(u,v, w);
pass(j) = norm(x - a, 'inf');
pass(j+1) = norm(y - b, 'inf');
pass(j+2) = norm(z - c, 'inf');

if (nargout > 0)
    pass = all(pass(:));
end
end


function linplus_test2655 ( )

%*****************************************************************************80
%
%% TEST2655 tests R8GB_TO_R8S3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 8;
  ml = 2;
  mu = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST2655\n' );
  fprintf ( 1, '  R8GB_TO_R8S3 copies a R8GB matrix to a R8S3 matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =      %d\n', m );
  fprintf ( 1, '  Matrix columns N =   %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );

  a = r8gb_indicator ( m, n, ml, mu );

  r8gb_print ( m, n, ml, mu, a, '  The R8GB matrix:' );

  nz_num = r8gb_nz_num ( m, n, ml, mu, a );

  fprintf ( 1, '  Nonzeros NZ_NUM =    %d\n', nz_num );

  [ isym, row, col, b ] = r8gb_to_r8s3 ( m, n, ml, mu, a, nz_num );

  r8s3_print ( m, n, nz_num, isym, row, col, b, '  The R8S3 matrix:' );

  return
end


% test for image warping
M = load_image('barb');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% extract eyes %%%
clf;
imagesc([0,1],[0,1],M);
title('Click on the each eye');
colormap gray(256);
axis image; axis off;
[x,y,b] = ginput(2);

% position of the eye
u = [x(1) y(1)];
v = [x(2) y(2)];

u1 = [0.3,0.3];
v1 = [0.7,0.3];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% computations %%%
M1 = perform_image_similitude(M,u,u1,v,v1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% display %%%
clf;
subplot(1,2,1);
hold on;
imagesc([0,1],[0,1],M);
plot([u(1) v(1)], [u(2) v(2)], '*');
hold off;
title('Original');
colormap gray(256);
axis image; axis off; axis ij

subplot(1,2,2);
hold on;
imagesc([0,1],[0,1],M1);
plot([u1(1) v1(1)], [u1(2) v1(2)], '*');
hold on;
title('Warped');
colormap gray(256);
axis image; axis off; axis ij