% ######################################################################################################################################################################################
% We are here talking about spatio-temporal detections, i.e. a set of ground-truth bounding boxes that
%  I will denote by g_t, with t between t_g^b and t_g^e (beginning and end time of the ground-truth)
% versus a detection which is also a set of bounding boxes, denoted by d_t, with t between t_d^e et t_d^e.
%
% a) temporal iou =  T_i / T_u
%  this is the intersection over union between the timing of the the tubes,
% ie mathematically T_i / T_u with
% the intersection T_i = max(0,   max(t_g^b,t_d^b)-min(t_d^e,t_g^e) )
% and the union T_u = min(t_g^b,t_d^b)-max(t_d^e,t_g^e)
%
% b) for each t between max(tgb,tdb)-min(tde,tge), we compute the IoU between g_t and d_t, and average them
%
% Multiplying (a) and (b) is the same as computed the average of the spatial iou over all frames in T_u of the two tubes, with a spatial iou of 0 for frames where only one box exists.
% c) as this is standard in detection problem, if there are multiple detections for the same groundtruth detection, the first one is counted as positive and the other ones as negatives
% ######################################################################################################################################################################################
%{
gt_fnr = 1xn doube
gt_bb = nx4 doubld - [x y w h]
dt_fnr = 1xm double
dt_bb = mx4 double - [x y w h]
%}
% -------------------------------------------------------------------------
function st_iou = compute_spatio_temporal_iou(gt_fnr, gt_bb, dt_fnr, dt_bb)
% -------------------------------------------------------------------------

% time gt begin

tgb = gt_fnr(1);
% time gt end
tge = gt_fnr(end);
%time dt begin
tdb = dt_fnr(1);
tde = dt_fnr(end);
% temporal intersection
T_i = double(max(0, min(tge,tde)-max(tgb,tdb)));

if T_i>0
    T_i = T_i +1;
    % temporal union
    T_u = double(max(tge,tde) - min(tgb,tdb)+1);
    %temporal IoU
    T_iou = T_i/T_u;
    % intersect frame numbers
    int_fnr = max(tgb,tdb):min(tge,tde);

    % find the ind of the intersected frames in the detected frames
    [~,int_find_dt] = ismember(int_fnr, dt_fnr);
    [~,int_find_gt] = ismember(int_fnr, gt_fnr);

    assert(length(int_find_dt)==length(int_find_gt));

    iou = zeros(length(int_find_dt),1);
    for i=1:length(int_find_dt)
        if int_find_gt(i)<1
%             fprintf('error ')
            pf = pf;
        else
            pf = i;
        end

        gt_bound = gt_bb(int_find_gt(pf),:);
        dt_bound = dt_bb(int_find_dt(pf),:)+1;

        % gt_bound = [gt_bound(:,1:2) gt_bound(:,3:4)-gt_bound(:,1:2)];
        % dt_bound = [dt_bound(:,1:2) dt_bound(:,3:4)-dt_bound(:,1:2)];
        iou(i) = inters_union(double(gt_bound),double(dt_bound));
    end
    % finalspatio-temporal IoU threshold
    st_iou = T_iou*mean(iou);
else
    st_iou =0;
end
% % iou_thresh = 0.2,...,0.6 % 'Learing to track paper' takes 0.2 for UCF101 and 0.5 for JHMDB
% if delta >= iou_thresh
%     % consider this tube as valid detection
% end

end

% -------------------------------------------------------------------------
function iou = inters_union(bounds1,bounds2)
% -------------------------------------------------------------------------

inters = rectint(bounds1,bounds2);
ar1 = bounds1(:,3).*bounds1(:,4);
ar2 = bounds2(:,3).*bounds2(:,4);
union = bsxfun(@plus,ar1,ar2')-inters;

iou = inters./(union+eps);

end


% ######################################################################################################################################################################################
% We are here talking about spatio-temporal detections, i.e. a set of ground-truth bounding boxes that
%  I will denote by g_t, with t between t_g^b and t_g^e (beginning and end time of the ground-truth)
% versus a detection which is also a set of bounding boxes, denoted by d_t, with t between t_d^e et t_d^e.
%
% a) temporal iou =  T_i / T_u
%  this is the intersection over union between the timing of the the tubes,
% ie mathematically T_i / T_u with
% the intersection T_i = max(0,   max(t_g^b,t_d^b)-min(t_d^e,t_g^e) )
% and the union T_u = min(t_g^b,t_d^b)-max(t_d^e,t_g^e)
%
% b) for each t between max(tgb,tdb)-min(tde,tge), we compute the IoU between g_t and d_t, and average them
%
% Multiplying (a) and (b) is the same as computed the average of the spatial iou over all frames in T_u of the two tubes, with a spatial iou of 0 for frames where only one box exists.
% c) as this is standard in detection problem, if there are multiple detections for the same groundtruth detection, the first one is counted as positive and the other ones as negatives
% ######################################################################################################################################################################################
%{
gt_fnr = 1xn doube
gt_bb = nx4 doubld - [x y w h]
dt_fnr = 1xm double
dt_bb = mx4 double - [x y w h]
%}
% -------------------------------------------------------------------------
function st_iou = compute_spatio_temporal_iou(gt_fnr, gt_bb, dt_fnr, dt_bb)
% -------------------------------------------------------------------------

% time gt begin
tgb = gt_fnr(1);
% time gt end
tge = gt_fnr(end);
%time dt begin
tdb = dt_fnr(1);
tde = dt_fnr(end);
% temporal intersection
T_i = double(max(0, min(tge,tde)-max(tgb,tdb)));

if T_i>0
    T_i = T_i +1;
    % temporal union
    T_u = double(max(tge,tde) - min(tgb,tdb)+1);
    %temporal IoU
    T_iou = T_i/T_u;
    % intersect frame numbers
    int_fnr = max(tgb,tdb):min(tge,tde);

    % find the ind of the intersected frames in the detected frames
    [~,int_find_dt] = ismember(int_fnr, dt_fnr);
    [~,int_find_gt] = ismember(int_fnr, gt_fnr);

    assert(length(int_find_dt)==length(int_find_gt));

    iou = zeros(length(int_find_dt),1);
    for i=1:length(int_find_dt)
        if int_find_gt(i)<1
%             fprintf('error ')
            pf = pf;
        else
            pf = i;
        end

        gt_bound = gt_bb(int_find_gt(pf),:);
        dt_bound = dt_bb(int_find_dt(pf),:)+1;

        % gt_bound = [gt_bound(:,1:2) gt_bound(:,3:4)-gt_bound(:,1:2)];
        % dt_bound = [dt_bound(:,1:2) dt_bound(:,3:4)-dt_bound(:,1:2)];
        iou(i) = inters_union(double(gt_bound),double(dt_bound));
    end
    % finalspatio-temporal IoU threshold
    st_iou = T_iou*mean(iou);
else
    st_iou =0;
end
% % iou_thresh = 0.2,...,0.6 % 'Learing to track paper' takes 0.2 for UCF101 and 0.5 for JHMDB
% if delta >= iou_thresh
%     % consider this tube as valid detection
% end

end

% -------------------------------------------------------------------------
function iou = inters_union(bounds1,bounds2)
% -------------------------------------------------------------------------

inters = rectint(bounds1,bounds2);
ar1 = bounds1(:,3).*bounds1(:,4);
ar2 = bounds2(:,3).*bounds2(:,4);
union = bsxfun(@plus,ar1,ar2')-inters;

iou = inters./(union+eps);

end


function uI = faceinterpolate(u,node,elem,elemType)
%% FACEINTERPOLATE interpolate to face elements (RT0 or BDM1).
%
% uI = faceinterpolate(u,node,elem,elemType) interpolates a given function u
% into the lowesr order RT0 or BDM1 finite element spaces. The coefficient
% is given by the line integral int_e u*n ds. The input elemType can be 'RT0'
% or 'BDM1'.
%
% Example: RT0
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k =1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        [u,sigma,eqn] = PoissonRT0(node,elem,bdFlag,pde);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'RT0');
%        err(k,1) = getL2errorRT0(node,elem,pde.Du,sigmaI);
%        err(k,2) = sqrt((sigma-sigmaI)'*eqn.M*(sigma-sigmaI));
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
% Example: BDM1
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k =1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        [u,sigma,eqn] = PoissonBDM1(node,elem,bdFlag,pde);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'BDM1');
%        err(k,1) = getL2errorBDM1(node,elem,pde.Du,sigmaI);
%        err(k,2) = sqrt((sigma-sigmaI)'*eqn.M*(sigma-sigmaI));
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
%
% Example: RT1
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k = 1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'RT1');
%        err(k,1) = getL2errorRT1(node,elem,pde.Du,sigmaI);
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
%
% See also edgeinterpolate, edgeinterpolate1, edgeinterpolate2
%
% Created by Ming Wang at Mar 29, 2011, M-lint modified at May 14, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('elemType','var'), elemType = 'RT0'; end
%% edge
if size(elem,2) == 2 % the input elem is edge
    edge = elem;
else
    [elem2edge,edge] = dofedge(elem);
end
NE = size(edge,1);
NT = size(elem,1);
edgeVec = node(edge(:,2),:) - node(edge(:,1),:);
nVec = zeros(NE,2);
nVec(:,1) = edgeVec(:,2);
nVec(:,2) = -edgeVec(:,1);
clear edgeVec

%% dof for RT0
[lambda,weight] = quadpts1(4);
nQuad = size(lambda,1);
uI = zeros(NE,1);
for i = 1:nQuad
    pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
    flux = u(pxy);
    uI = uI + weight(i)*dot(flux,nVec,2);
end

%% dof for BDM1
if strcmp(elemType,'BDM1') || strcmp(elemType,'RT1')
   uI(NE+1:2*NE) = zeros(NE,1);
   for i = 1:nQuad
        pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
        uI(NE+1:2*NE) = uI(NE+1:2*NE)+ ...
                     weight(i)*3*(lambda(i,1)-lambda(i,2))*dot(u(pxy),nVec,2);
   end
end

%% dof for RT1
if strcmp(elemType,'RT1')
    mid = (node(edge(:,1),:) + node(edge(:,2),:))/2;
    umid = u(mid);
    uI(2*NE+2*NT) = 0;
    % Face dof coefficients
    uquadpts = umid(elem2edge(:,1),:)+umid(elem2edge(:,2),:)+umid(elem2edge(:,3),:);
    lf = [4*dot(nVec(elem2edge(:,2),:),uquadpts,2) ...
          4*dot(nVec(elem2edge(:,3),:),uquadpts,2)];
    elem2edgeDofValue = [uI(elem2edge(:,1:3)) uI(elem2edge(:,1:3)+NE)];
    localMatrix = [4 8 4 -4  0 4; ...
                   8 4 -4 0 -4 4]';
    lf = lf - elem2edgeDofValue*localMatrix;
    uI((2*NE+1):end) = [2*lf(:,1) - lf(:,2); ...
                        2*lf(:,2) - lf(:,1)]/3;
end


function varargout = rotation3dToEulerAngles(mat, varargin)
%ROTATION3DTOEULERANGLES Extract Euler angles from a rotation matrix.
%
%   [PHI, THETA, PSI] = rotation3dToEulerAngles(MAT)
%   Computes Euler angles PHI, THETA and PSI (in degrees) from a 3D 4-by-4
%   or 3-by-3 rotation matrix.
%
%   ANGLES = rotation3dToEulerAngles(MAT)
%   Concatenates results in a single 1-by-3 row vector. This format is used
%   for representing some 3D shapes like ellipsoids.
%
%   ... = rotation3dToEulerAngles(MAT, CONVENTION)
%   CONVENTION specifies the axis rotation sequence. Default is 'ZYX'.
%   Supported conventions are:
%       'ZYX','ZXY','YXZ','YZX','XYZ','XZY'
%       'ZYZ','ZXZ','YZY','YXY','XZX','XYX'
%
%   Example
%   rotation3dToEulerAngles
%
%   References
%   Code from '1994 - Shoemake - Graphics Gems IV: Euler Angle Conversion:
%   http://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
%   (see rotm2eul, that is part of MATLAB's Robotics System Toolbox)
%   Modified using explanations in:
%   http://www.gregslabaugh.net/publications/euler.pdf
%   https://www.geometrictools.com/Documentation/EulerAngles.pdf
%
%   See also
%   transforms3d, rotation3dAxisAndAngle, createRotation3dLineAngle,
%   eulerAnglesToRotation3d
%

% ------
% Authors: David Legland, oqilipo
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-08-11, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

p = inputParser;
validStrings = {...
    'ZYX','ZXY','YXZ','YZX','XYZ','XZY',...
    'ZYZ','ZXZ','YZY','YXY','XZX','XYX'};
addOptional(p,'convention','ZYX',@(x) any(validatestring(x,validStrings)));
logParValidFunc = @(x) (islogical(x) || isequal(x,1) || isequal(x,0));
addParameter(p,'IsRotation', 1, logParValidFunc);
valTol = @(x) validateattributes(x,{'numeric'},{'scalar', '>=',eps(class(mat)), '<=',1});
addParameter(p,'tolerance', 1e-8, valTol);
parse(p,varargin{:});
convention=p.Results.convention;
isRotation = p.Results.IsRotation;
tolerance = p.Results.tolerance;

if isRotation
    if ~isTransform3d(mat(1:3,1:3), 'rotation', 1, 'tolerance', tolerance)
        warning(['Rotation matrix contains reflection or scaling ' ...
            'tested with a tolerance of ' num2str(tolerance) '.' newline ...
            'Calculation of euler angles might be incorrect.'])
    end
end

switch convention
    case 'ZYX'
        % extract |cos(theta)|
        cy = hypot(mat(1,1), mat(2,1));
        % avoid dividing by 0
        if cy > 16*eps
            % normal case: theta <> 0
            phi   = atan2( mat(2,1), mat(1,1));
            theta = atan2(-mat(3,1), cy);
            psi   = atan2( mat(3,2), mat(3,3));
        else
            phi   = 0;
            theta = atan2(-mat(3,1), cy);
            psi   = atan2(-mat(2,3), mat(2,2));
        end
    case 'ZXY'
        cy = hypot(mat(2,2), mat(1,2));
        if cy > 16*eps
            phi   = -atan2( mat(1,2), mat(2,2));
            theta = -atan2(-mat(3,2), cy);
            psi   = -atan2( mat(3,1), mat(3,3));
        else
            phi   = 0;
            theta = -atan2(-mat(3,2), cy);
            psi   = -atan2(-mat(1,3), mat(1,1));
        end
    case 'YXZ'
        cy = hypot(mat(3,3), mat(1,3));
        if cy > 16*eps
            phi   = atan2( mat(1,3), mat(3,3));
            theta = atan2(-mat(2,3), cy);
            psi   = atan2( mat(2,1), mat(2,2));
        else
            phi   = 0;
            theta = atan2(-mat(2,3), cy);
            psi   = atan2(-mat(1,2), mat(1,1));
        end
    case 'YZX'
        cy = hypot(mat(1,1), mat(3,1));
        if cy > 16*eps
            phi   = -atan2( mat(3,1), mat(1,1));
            theta = -atan2(-mat(2,1), cy);
            psi   = -atan2( mat(2,3), mat(2,2));
        else
            phi   = 0;
            theta = -atan2(-mat(2,1), cy);
            psi   = -atan2(-mat(3,2), mat(3,3));
        end
    case 'XYZ'
        cy = hypot(mat(3,3), mat(2,3));
        if cy > 16*eps
            phi   = -atan2( mat(2,3), mat(3,3));
            theta = -atan2(-mat(1,3), cy);
            psi   = -atan2( mat(1,2), mat(1,1));
        else
            phi   = 0;
            theta = -atan2(-mat(1,3), cy);
            psi   = -atan2(-mat(2,1), mat(2,2));
        end
    case 'XZY'
        cy = hypot(mat(2,2), mat(3,2));
        if cy > 16*eps
            phi   = atan2( mat(3,2), mat(2,2));
            theta = atan2(-mat(1,2), cy);
            psi   = atan2( mat(1,3), mat(1,1));
        else
            phi   = 0;
            theta = atan2(-mat(1,2), cy);
            psi   = atan2(-mat(3,1), mat(3,3));
        end

    case 'ZYZ'
        cy = hypot(mat(3,2), mat(3,1));
        if cy > 16*eps
            phi   = -atan2(mat(2,3), -mat(1,3));
            theta = -atan2(cy, mat(3,3));
            psi   = -atan2(mat(3,2), mat(3,1));
        else
            phi   = 0;
            theta = -atan2(cy, mat(3,3));
            psi   = -atan2(-mat(2,1), mat(2,2));
        end
    case 'ZXZ'
        cy = hypot(mat(3,2), mat(3,1));
        if cy > 16*eps
            phi   = atan2(mat(1,3), -mat(2,3));
            theta = atan2(cy, mat(3,3));
            psi   = atan2(mat(3,1), mat(3,2));
        else
            phi   = 0;
            theta = atan2(cy, mat(3,3));
            psi   = atan2(-mat(1,2), mat(1,1));
        end
    case 'YZY'
        cy = hypot(mat(2,3), mat(2,1));
        if cy > 16*eps
            phi   = atan2(mat(3,2), -mat(1,2));
            theta = atan2(cy, mat(2,2));
            psi   = atan2(mat(2,3), mat(2,1));
        else
            phi   = 0;
            theta = atan2(cy, mat(2,2));
            psi   = atan2(-mat(3,1), mat(3,3));
        end
    case 'YXY'
        cy = hypot(mat(2,3), mat(2,1));
        if cy > 16*eps
            phi   = -atan2(mat(1,2), -mat(3,2));
            theta = -atan2(cy, mat(2,2));
            psi   = -atan2(mat(2,1), mat(2,3));
        else
            phi   = 0;
            theta = -atan2(cy, mat(2,2));
            psi   = -atan2(-mat(1,3), mat(1,1));
        end
    case 'XZX'
        cy = hypot(mat(1,3), mat(1,2));
        if cy > 16*eps
            phi   = -atan2(mat(3,1), -mat(2,1));
            theta = -atan2(cy, mat(1,1));
            psi   = -atan2(mat(1,3), mat(1,2));
        else
            phi   = 0;
            theta = -atan2(cy, mat(1,1));
            psi   = -atan2(-mat(3,2), mat(3,3));
        end
    case 'XYX'
        cy = hypot(mat(1,2), mat(1,3));
        if cy > 16*eps
            phi   = atan2(mat(2,1), -mat(3,1));
            theta = atan2(cy, mat(1,1));
            psi   = atan2(mat(1,2), mat(1,3));
        else
            phi   = 0;
            theta = atan2(cy, mat(1,1));
            psi   = atan2(-mat(2,3), mat(2,2));
        end
end

% format output arguments
if nargout <= 1
    % one array
    varargout{1} = rad2deg([phi theta psi]);
else
    % three separate arrays
    varargout = cellfun(@rad2deg, {phi theta psi},'uni',0);
end


classdef SPX_ConjugateDescent < handle
% Conjugate gradient algorithm implementation

    properties
        % Settable and gettable properties

        % Maximum number of iterations for which the algorithm can run
        MaxIterations
        % Threshold of norm (in terms of percentage)
        NormThreshold
    end

    properties(SetAccess=private)
        % Gettable properties

        % The sparse symmetric positive definite matrix operator
        A
        % The vectors to be solved
        B
        % Problem dimension
        N
        % Number of equations to be solved
        S
        % Number of iterations taken for solving the problem
        Iterations
        % Residual norms at the end
        ResidualNorms
        % The solution vectors
        X
        % Residual vectors
        Residuals
        % Indicates if the problems converged
        % This would be false only if the algorithm crossed max iterations
        Converged
    end

    methods
        % Public methods
        function self = SPX_ConjugateDescent(A, B)
            % Constructor
            if isa(A, 'spx.dict.Operator')
                self.A = A;
            elseif ismatrix(A)
                self.A = spx.dict.MatrixOperator(A);
            else
                error('Unsupported operator.');
            end
            self.B = B;
            [self.N, self.S] = size(B);
            self.MaxIterations = self.N ;
            self.NormThreshold = 1e-6;
        end

        function result = solve(self)
            aa = self.A;
            bb = self.B;
            % Initial estimate vectors are all zeros
            xx = zeros(self.N, self.S);
            result = xx;
            self.Iterations = zeros(self.S, 1);
            self.ResidualNorms = zeros(self.S, 1);
            self.Converged = false(self.S, 1);
            % Initial residual vectors
            rr = bb  - aa * xx;
            self.Residuals  = rr;
            % Norm squared of initial residual vectors
            deltas = spx.norm.inner_product_cw(rr, rr);
            % The factor with which the norm needs to be reduced
            epsilon = self.NormThreshold;
            % Target limits on norm squared of residuals
            limits = epsilon^2 * deltas;
            % Maximum number of iterations for which the algorithm
            % is allowed to run
            imax = self.MaxIterations;
            % Number of problems being solved
            ns = self.S;
            for s=1:ns
                % Initialize iteration counter
                i = 0;
                % The quantities for this problem
                limit = limits(s);
                delta = deltas(s);
                % First residual
                r = rr(:, s);
                % First estimate
                x = xx(:, s);
                % Target b
                b = bb(:, s);
                % First direction
                d = r;
                while i < imax && delta > limit
                    % Compute the intermediate variable
                    q = aa * d;
                    % the line search scale factor in current direction
                    alpha = delta / (d' * q);
                    % Update estimate in current direction
                    x = x + alpha * d;
                    if mod(i , 50) == 0
                        % In order to avoid propagation of floating point
                        % errors, we will recompute the value of residual
                        r = b - aa * x;
                    else
                        % Otherwise we use a shortcut
                        r = r  - alpha * q;
                    end
                    % hold the current residual norm squared
                    delta_old = delta;
                    % Update residual norm squared
                    delta = r' * r;
                    % Compute the ratio
                    beta = delta / delta_old;
                    % choose the new direction
                    d = r + beta * d;
                    % Increase iteration counter
                    i = i + 1;
                end
                % The problem has been solved
                result(:, s) = x;
                % Number of iterations taken to solve this problem
                self.Iterations(s) = i;
                self.ResidualNorms(s) = sqrt(delta);
                self.Residuals(:, s) = r;
                self.Converged(s) = delta <= limit;
            end
            % Maintain the result for reference
            self.X = result;
        end

        function result = hasConverged(self)
            % Returns if all the solutions have converged
            result = all(self.Converged);
        end

        function printResults(self)
            ns = self.S;
            nn = self.N;
            for s = 1:ns
                fprintf('Problem: %d\n', s);
                fprintf('Iterations: %d\n', self.Iterations(s));
                fprintf('Residual norm: %.2f, Converged: %d\n', ...
                    self.ResidualNorms(s), self.Converged(s));
                if nn < 10
                    % We will print the solutions too
                    fprintf('Solution vector: ');
                    fprintf('%.4f ', self.X(:, s));
                    fprintf('\n');
                    fprintf('Residual vector: ');
                    fprintf('%.4f ', self.Residuals(:, s));
                    fprintf('\n');
                end
            end
        end
    end

    methods(Access=private)
        % Private methods

    end

    methods(Static)
        % Public static methods

    end

end


function [z,a,it,ord,s,fct] = backcor(n,y,ord,s,fct)

% BACKCOR   Background estimation by minimizing a non-quadratic cost function.
%
%   [EST,COEFS,IT] = BACKCOR(N,Y,ORDER,THRESHOLD,FUNCTION) computes and estimation EST
%   of the background (aka. baseline) in a spectroscopic signal Y with wavelength N.
%   The background is estimated by a polynomial with order ORDER using a cost-function
%   FUNCTION with parameter THRESHOLD. FUNCTION can have the four following values:
%       'sh'  - symmetric Huber function :  f(x) = { x^2  if abs(x) < THRESHOLD,
%                                                  { 2*THRESHOLD*abs(x)-THRESHOLD^2  otherwise.
%       'ah'  - asymmetric Huber function :  f(x) = { x^2  if x < THRESHOLD,
%                                                   { 2*THRESHOLD*x-THRESHOLD^2  otherwise.
%       'stq' - symmetric truncated quadratic :  f(x) = { x^2  if abs(x) < THRESHOLD,
%                                                       { THRESHOLD^2  otherwise.
%       'atq' - asymmetric truncated quadratic :  f(x) = { x^2  if x < THRESHOLD,
%                                                        { THRESHOLD^2  otherwise.
%   COEFS returns the ORDER+1 vector of the estimated polynomial coefficients
%   (computed with n sorted and bounded in [-1,1] and y bounded in [0,1]).
%   IT returns the number of iterations.
%
%   [EST,COEFS,IT] = BACKCOR(N,Y) does the same, but run a graphical user interface
%   to help setting ORDER, THRESHOLD and FCT.
%
% For more informations, see:
% - V. Mazet, C. Carteret, D. Brie, J. Idier, B. Humbert. Chemom. Intell. Lab. Syst. 76 (2), 2005.
% - V. Mazet, D. Brie, J. Idier. Proceedings of EUSIPCO, pp. 305-308, 2004.
% - V. Mazet. PhD Thesis, University Henri Poincaré Nancy 1, 2005.
%
% 22-June-2004, Revised 19-June-2006, Revised 30-April-2010,
% Revised 12-November-2012 (thanks E.H.M. Ferreira!)
% Comments and questions to: vincent.mazet@unistra.fr.

% Check arguments
if nargin < 2, error('backcor:NotEnoughInputArguments','Not enough input arguments'); end;
if nargin < 5, [z,a,it,ord,s,fct] = backcorgui(n,y); return; end; % delete this line if you do not need GUI
if ~isequal(fct,'sh') && ~isequal(fct,'ah') && ~isequal(fct,'stq') && ~isequal(fct,'atq'),
    error('backcor:UnknownFunction','Unknown function.');
end;

% Rescaling
N = length(n);
[n,i] = sort(n);
y = y(i);
maxy = max(y);
dely = (maxy-min(y))/2;
n = 2 * (n(:)-n(N)) / (n(N)-n(1)) + 1;
y = (y(:)-maxy)/dely + 1;

% Vandermonde matrix
p = 0:ord;
T = repmat(n,1,ord+1) .^ repmat(p,N,1);
Tinv = pinv(T'*T) * T';

% Initialisation (least-squares estimation)
a = Tinv*y;
z = T*a;

% Other variables
alpha = 0.99 * 1/2;     % Scale parameter alpha
it = 0;                 % Iteration number
zp = ones(N,1);         % Previous estimation

% LEGEND
while sum((z-zp).^2)/sum(zp.^2) > 1e-9,

    it = it + 1;        % Iteration number
    zp = z;             % Previous estimation
    res = y - z;        % Residual

    % Estimate d
    if isequal(fct,'sh'),
        d = (res*(2*alpha-1)) .* (abs(res)<s) + (-alpha*2*s-res) .* (res<=-s) + (alpha*2*s-res) .* (res>=s);
    elseif isequal(fct,'ah'),
        d = (res*(2*alpha-1)) .* (res<s) + (alpha*2*s-res) .* (res>=s);
    elseif isequal(fct,'stq'),
        d = (res*(2*alpha-1)) .* (abs(res)<s) - res .* (abs(res)>=s);
    elseif isequal(fct,'atq'),
        d = (res*(2*alpha-1)) .* (res<s) - res .* (res>=s);
    end;

    % Estimate z
    a = Tinv * (y+d);   % Polynomial coefficients a
    z = T*a;            % Polynomial

end;

% Rescaling
[~,j] = sort(i);
z = (z(j)-1)*dely + maxy;

    a(1) = a(1)-1;
    a = a*dely;% + maxy;

end

% delete lines below if you do not need GUI

function [z,a,it,ord,s,fct] = backcorgui(n,y)

% BACKCORGUI   Graphical User Interface for background estimation.

% Initialization
z = [];
a = [];
it = [];
ord = [];
s = [];
fct = [];

order = 4;
threshold = 0.01;
costfunction = 'atq';

% Main window
hwin = figure('Visible','off','Position',[0 0 750 400],'NumberTitle','off','Name','Background Correction',...
    'MenuBar','none','Toolbar','figure','Resize','on','ResizeFcn',{@WinResizeFcn});
bgclr = get(hwin,'Color');

% Axes
haxes = axes('Units','pixels');

% Buttons OK & Cancel
hok = uicontrol('Style','pushbutton','String','OK','Position',[600,40,80,25],'Callback',{@OKFcn},'BackgroundColor',bgclr);
hcancel = uicontrol('Style','pushbutton','String','Cancel','Position',[510,40,80,25],'Callback',{@CancelFcn},'BackgroundColor',bgclr);

% Cost functions menu
hfctlbl = uicontrol('Style','text','String','Cost function:','HorizontalAlignment','left','BackgroundColor',bgclr);
hfct = uicontrol('Style','popupmenu','Value',4,'BackgroundColor','white','Callback',{@CostFunctionFcn},...
    'String',{'Symmetric Huber function','Asymmetric Huber function','Symmetric truncated quadratic','Asymmetric truncated quadratic'});

% Threshold text
hthresholdlbl = uicontrol('Style','text','String','Threshold:','HorizontalAlignment','left','BackgroundColor',bgclr);
hthreshold = uicontrol('Style','edit','String',num2str(threshold),'BackgroundColor','white','Callback',{@ThresholdFcn});

% Order slider
horderlbl = uicontrol('Style','text','String','Polynomial order:','HorizontalAlignment','left','BackgroundColor',bgclr);
horder = uicontrol('Style','slider','SliderStep',[0.5 0.5],'Min',0,'Max',10,'Value',order,'SliderStep',[0.1 0.1],'Callback',{@OrderFcn});
horderval = uicontrol('Style','text','String',num2str(order),'BackgroundColor',bgclr);

% Move the GUI to the center of the screen
movegui(hwin,'center');

% Plot a first estimation
[ztmp,atmp,ittmp,order,threshold,costfunction] = compute(n,y,order,threshold,costfunction);

% Make the GUI visible
set(hwin,'Visible','on');

% Callback functions

    function CancelFcn(source,eventdata)
        % Just close the window
        uiresume(gcbf);
        close(hwin);
    end

    function OKFcn(source,eventdata)
        % Return the current estimation and close the window
        z = ztmp;
        a = atmp;
        it = ittmp;
        ord = order;
        s = threshold;
        fct = costfunction;
        uiresume(gcbf);
        close(hwin);
    end

    function CostFunctionFcn(source,eventdata)
        % Change cost function
        cf = get(hfct,'Value');
        if cf == 1,
            costfunction = 'sh';
        elseif cf == 2,
            costfunction = 'ah';
        elseif cf == 3,
            costfunction = 'stq';
        elseif cf == 4,
            costfunction = 'atq';
        end
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function OrderFcn(source,eventdata)
        % Change order
        order = get(horder,'Value');
        set(horderval,'String',num2str(order));
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function ThresholdFcn(source,eventdata)
        % Change threshold
        threshold = get(hthreshold,'String');
        threshold = str2double(threshold);
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function [ztmp,atmp,ittmp,order,threshold,costfunction] = compute(n,y,order,threshold,costfunction)
        % Compute and plot an estimation (need to sort the data)
        [ztmp,atmp,ittmp,order,threshold,costfunction] = backcor(n,y,order,threshold,costfunction);
        [~,i] = sort(n);
        plot(n(i),y(i),'b-',n(i),ztmp(i),'r-');
    end

    function WinResizeFcn(source,eventdata)
        % Resize the window
        pos = get(hwin,'Position');
        w = pos(3);
        h = pos(4);
        if w>400 && h>100,
            set(haxes,'Position',[40,40,w-320,h-70]);
        end;
        set(hok,'Position',[w-90,30,80,25]);
        set(hcancel,'Position',[w-180,30,80,25]);
        set(hfctlbl,'Position',[w-240,h-30,220,20]);
        set(hfct,'Position',[w-240,h-50,220,25]);
        set(hthresholdlbl,'Position',[w-240,h-80,220,20]);
        set(hthreshold,'Position',[w-240,h-100,220,20]);
        set(horderlbl,'Position',[w-240,h-130,220,20]);
        set(horder,'Position',[w-210,h-150,190,20]);
        set(horderval,'Position',[w-240,h-150,20,20]);
    end

uiwait(gcf);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Example code for estimating the HRF using the Inverse-Logit Model, a
% Finte Impluse Response Model and the Canonical HRF with 2 derivatives.
% Also the code illustrates our code for detecting model misspecification.
%
% By Martin Lindquist and Tor Wager
% Created  10/02/09
% Last edited 05/20/13
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Load time course
%

mypath = which('ilogit');
if isempty(mypath), error('Cannot find directory with ilogit.m and other functions. Not on path?'); end
[mydir] = fileparts(mypath)

load(fullfile(mydir,'timecourse'))

tc = (tc- mean(tc))/std(tc);
len = length(tc);

%% Or: create your own
[xBF] = spm_get_bf(struct('dt', .5, 'name', 'hrf (with time and dispersion derivatives)', 'length', 32));
clear Xtrue
for i = 1:1, xx = conv(xBF.bf(:,i), [1 1 1 1 1 1 ]');
    Xtrue(:, i) = xx(1:66);
end
for i = 2:3, xx = conv(xBF.bf(:,i), [1 1]');
    Xtrue(:, i) = xx(1:66);
end
hrf = Xtrue * [1 .3 .2]';
xsecs = 0:.5:32;

hrf = [ 0; 0; hrf];
hrf = hrf(1:length(xsecs));
hrf = hrf ./ max(hrf);
figure; plot(xsecs, hrf, 'k')
%hrf = hrf(1:4:end); % downsample to TR, if TR is > 0.5

R = randperm(640); R = sort(R(1:36));
Run = zeros(640,1);
for i=1:length(R), Run(R(i)) = 1; end;
true_sig = conv(Run, hrf);
true_sig = true_sig(1:640);

% tc_noise = noise_arp(640, [.7 .2]);
% tc = true_sig + 0.1 * tc_noise;
tc = true_sig;
%figure; plot(tc);

Runc{1} = Run;

%%

create_figure; subplot(3,1,1); han = plot(tc);
title('Sample time course'); drawnow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Settings
%

TR = 0.5;
%T = round(30/TR);
T = 30;
t = 1:TR:T;                        % samples at which to get Logit HRF Estimate
FWHM = 4;                       % FWHM for residual scan
pval = 0.01;
df = 600;
alpha = 0.001;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Create stick function (sample event onsets)
% Variable R contains onset times
% Variable Run contains stick (a.k.a. delta or indicator) function

% R = [3, 21, 56, 65, 109, 126, 163, 171, 216, 232, 269, 282, 323, 341, 376, 385, 429, 446, 483, 491, 536, 552, 589, 602];
% Run = zeros(640,1);
% for i=1:length(R), Run(R(i)) = 1; end;
%

try
    hold on;
    hh = plot_onsets(R,'k',-3,1, 1);
    drawnow
catch
    disp('Couldn''t find function to add onset sticks to plot. Skipping.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using IL-function

% Choose mode (deterministic/stochastic)

mode = 0;   % 0 - deterministic aproach
            % 1 - simulated annealing approach
            % Please note that when using simulated annealing approach you
            % may need to perform some tuning before use.

[h1, fit1, e1, param] = Fit_Logit2(tc,TR,Runc,T,mode);
[pv sres sres_ns1] = ResidScan(e1, FWHM);
[PowLoss1] = PowerLoss(e1, fit1, (len-7) , tc, TR, Runc, alpha);

hold on; han(2) = plot(fit1,'r');

disp('Summary: IL_function');

disp('Amplitude:'); disp(param(1));
disp('Time-to-peak:'); disp(param(2)*TR);
disp('Width:'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e1.^2)));
disp('Mis-modeling:'); disp(pv);
disp('Power Loss:'); disp(PowLoss1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using FIR-model

% Choose mode (FIR/sFIR)

mode = 1;   % 0 - FIR
            % 1 - smooth FIR

[h2, fit2, e2, param] = Fit_sFIR(tc,TR,Runc,T,mode);
[pv sres sres_ns2] = ResidScan(e2, FWHM);
[PowLoss2] = PowerLoss(e2, fit2, (len-T) , tc, TR, Runc, alpha);

hold on; han(3) = plot(fit2,'g');

disp('Summary: FIR');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e2.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using Canonical HRF + 2 derivatives

p=1;

[h3, fit3, e3, param, info] = Fit_Canonical_HRF(tc,TR,Runc,30,p);
[pv sres sres_ns3] = ResidScan(e3, FWHM);
[PowLoss3] = PowerLoss(e3, fit3, (len-p) , tc, TR, Runc, alpha);

hold on; han(4) = plot(fit3,'m');

legend(han,{'Data' 'IL' 'sFIR' 'DD'})

disp('Summary: Canonical + 2 derivatives');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e3.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss3);

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

%figure;
%%

subplot(3,2,5); hold on;
plot(xsecs, hrf, 'k')
xsecs1 = xsecs(1:length(h1));
han2 = plot(xsecs1, h1,'r');
xsecs2 = xsecs(1:length(h2));
han2(2) = plot(xsecs2, h2,'g');
xsecs3 = xsecs(1:length(h3));
han2(3) = plot(xsecs3, h3,'m');
legend(han2,{'IL' 'sFIR' 'DD'})
title('Estimated HRF');

subplot(3,1,2); hold on;
hh = plot_onsets(R,'k',-3,1);
drawnow

han3 = plot(sres_ns1,'r');
hold on; han3(2) = plot(sres_ns2,'g');
hold on; han3(3) = plot(sres_ns3,'m');
hold on; plot((1:len),zeros(len,1),'--k');
legend(han3,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (time course)');

subplot(3,2,6); hold on;

[s1] = Fit_sFIR(sres_ns1,TR,Runc,T,0);
[s2] = Fit_sFIR(sres_ns2,TR,Runc,T,0);
[s3] = Fit_sFIR(sres_ns3,TR,Runc,T,0);

han4 = plot(s1(1:T),'r');
hold on; han4(2) = plot(s2(1:T),'g');
hold on; han4(3) = plot(s3(1:T),'m');
hold on; plot((1:T),zeros(T,1),'--k');
legend(han4,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (HRF)');


function q = qinv(p)

% QINV  quaternion inverse
%
%   Q = QINV(P) returns a quaternion Q which is the quaternion inverse of
%     quaternion P.
%     - P is a quaternion. It is a 4-vector or a 4*N array (column i
%        represents quaternion i) where N is the number of quaternions.
%     - Q is the quaternion inverse of quaternion P. It is a 4*N array.

sp = size(p);
if sp == [1 4]
    p = p.';
    sp = size(p);
end

% wrong format
if sp(1) ~= 4
    error('DualQuaternion:Qinv:wrongsize',...
        '%d rows in the P array. It should be 4.',sp(1));
end

normp = qnorm(p);
q = qconj(p)./repmat(normp.^2,4,1);


function res = qdist(a,b)
%QDIST        Implements  q(a,b)  metrical distance
%  Name  qdist  is used to avoid ambiguities with variable  q
%  This functions for non-interval input only for completeness
%
%     res = qdist(a,b)
%
% for real input          abs(a-b)
% for complex input       qdist(real(a),real(b)) + qdist(imag(a),imag(b))
%

% written  03/23/00     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
%

  if isreal(a) & isreal(b)
    res = abs(a-b);
  else
    res = abs(real(a)-real(b)) + abs(imag(a)-imag(b));
  end


function res = computeDirectionWeights3d13(varargin)
%COMPUTEDIRECTIONWEIGHTS3D13 Direction weights for 13 directions in 3D
%
%   C = computeDirectionWeights3d13
%   Returns an array of 13-by-1 values, corresponding to directions:
%   C(1)  = [+1  0  0]
%   C(2)  = [ 0 +1  0]
%   C(3)  = [ 0  0 +1]
%   C(4)  = [+1 +1  0]
%   C(5)  = [-1 +1  0]
%   C(6)  = [+1  0 +1]
%   C(7)  = [-1  0 +1]
%   C(8)  = [ 0 +1 +1]
%   C(9)  = [ 0 -1 +1]
%   C(10) = [+1 +1 +1]
%   C(11) = [-1 +1 +1]
%   C(12) = [+1 -1 +1]
%   C(13) = [-1 -1 +1]
%   The sum of the weights in C equals 1.
%   Some values are equal whatever the resolution:
%   C(4)==C(5);
%   C(6)==C(7);
%   C(8)==C(9);
%   C(10)==C(11)==C(12)==C(13);
%
%   C = computeDirectionWeights3d13(DELTA)
%   With DELTA = [DX DY DZ], specifies the resolution of the grid.
%
%   Example
%   c = computeDirectionWeights3d13;
%   sum(c)
%   ans =
%       1.0000
%
%   c = computeDirectionWeights3d13([2.5 2.5 7.5]);
%   sum(c)
%   ans =
%       1.0000
%
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-10-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Initializations

% grid resolution
delta = [1 1 1];
if ~isempty(varargin)
    delta = varargin{1};
end

% If resolution is [1 1 1], return the pre-computed set of weights
if all(delta == [1 1 1])
    area1 = 0.04577789120476 * 2;
    area2 = 0.03698062787608 * 2;
    area3 = 0.03519563978232 * 2;
    res = [...
        area1; area1; area1; ...
        area2; area2; area2; area2; area2; area2;...
        area3; area3; area3; area3 ];
    return;
end

% Define points of interest in the 26 discrete directions
% format is pt[Xpos][Ypos][Zpos], with [X], [Y] or [Z] being one of
% 'N' (for negative), 'P' (for Positive) or 'Z' (for Zero)

% points below the OXY plane
ptPNN = normalizeVector3d([+1 -1 -1].*delta);
ptPZN = normalizeVector3d([+1  0 -1].*delta);
ptNPN = normalizeVector3d([-1 +1 -1].*delta);
ptZPN = normalizeVector3d([ 0 +1 -1].*delta);
ptPPN = normalizeVector3d([+1 +1 -1].*delta);

% points belonging to the OXY plane
ptPNZ = normalizeVector3d([+1 -1  0].*delta);
ptPZZ = normalizeVector3d([+1  0  0].*delta);
ptNPZ = normalizeVector3d([-1 +1  0].*delta);
ptZPZ = normalizeVector3d([ 0 +1  0].*delta);
ptPPZ = normalizeVector3d([+1 +1  0].*delta);

% points above the OXY plane
ptNNP = normalizeVector3d([-1 -1 +1].*delta);
ptZNP = normalizeVector3d([ 0 -1 +1].*delta);
ptPNP = normalizeVector3d([+1 -1 +1].*delta);
ptNZP = normalizeVector3d([-1  0 +1].*delta);
ptZZP = normalizeVector3d([ 0  0 +1].*delta);
ptPZP = normalizeVector3d([+1  0 +1].*delta);
ptNPP = normalizeVector3d([-1 +1 +1].*delta);
ptZPP = normalizeVector3d([ 0 +1 +1].*delta);
ptPPP = normalizeVector3d([+1 +1 +1].*delta);

%% Spherical cap type 1, direction [1 0 0]

% Compute area of voronoi cell for a point on the Ox axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint = ptPZZ;

% neighbours of chosen point, sorted by CCW angle
neighbors = [ptPNN; ptPNZ; ptPNP; ptPZP; ptPPP; ptPPZ; ptPPN; ptPZN];

% compute area of spherical polygon
area1 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 1, direction [0 1 0]

% Compute area of voronoi cell for a point on the Oy axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint    = ptZPZ;

% neighbours of chosen point, sorted by angle
neighbors   = [ptPPZ; ptPPP; ptZPP; ptNPP; ptNPZ; ptNPN; ptZPN; ptPPN];

% compute area of spherical polygon
area2 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 1, direction [0 0 1]

% Compute area of voronoi cell for a point on the Oz axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint = ptZZP;

% neighbours of chosen point, sorted by angle
neighbors = [ptPZP; ptPPP; ptZPP; ptNPP; ptNZP; ptNNP; ptZNP; ptPNP];

% compute area of spherical polygon
area3 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [1 1 0]

% Compute area of voronoi cell for a point on the Oxy plane, i.e. a point
% in the 18-neighborhood
refPoint = ptPPZ;

% neighbours of chosen point, sorted by angle
neighbors = [ptPZZ; ptPPP; ptZPZ; ptPPN];

% compute area of spherical polygon
area4 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [1 0 1]

% Compute area of voronoi cell for a point on the Oxz plane, i.e. a point
% in the 18-neighborhood
refPoint = ptPZP;
% neighbours of chosen point, sorted by angle
neighbors = [ptPZZ; ptPPP; ptZZP; ptPNP];

% compute area of spherical polygon
area5 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [0 1 1]

% Compute area of voronoi cell for a point on the Oxy plane, i.e. a point
% in the 18-neighborhood
refPoint = ptZPP;
% neighbours of chosen point, sorted by angle
neighbors = [ptZPZ; ptNPP; ptZZP; ptPPP];

% compute area of spherical polygon
area6 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 3 (all cubic diagonals)

% Compute area of voronoi cell for a point on the Oxyz diagonal, i.e. a
% point in the 26 neighborhood only
refPoint = ptPPP;
% neighbours of chosen point, sorted by angle
neighbors = [ptPZP; ptZZP; ptZPP; ptZPZ; ptPPZ; ptPZZ];

% compute area of spherical polygon
area7 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Concatenate results

% return computed areas, formatted as fraction of sphere surface
res = [...
    area1 area2 area3 ...
    area4 area4 area5 area5 area6 area6...
    area7 area7 area7 area7...
    ]/(2*pi);


function wavelet_test07 ( )

%*****************************************************************************80
%
%% WAVELET_TEST07 tests DAUB14_TRANSFORM and DAUB14_TRANSFORM_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'WAVELET_TEST07\n' );
  fprintf ( 1, '  DAUB14_TRANSFORM computes the DAUB14 transform of a vector.\n' );
  fprintf ( 1, '  DAUB14_TRANSFORM_INVERSE inverts it.\n' );
%
%  Random data.
%
  n = 16;
  seed = 123456789;
  [ u, seed ] = r8vec_uniform_01 ( n, seed );

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)       D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Constant signal.
%
  n = 8;
  u(1:n) = 1.0;

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Linear signal.
%
  n = 16;
  a_first = 1.0;
  a_last = n;
  u = linspace ( a_first, a_last, n );

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Quadratic data.
%
  n = 8;
  u(1) = 25.0;
  u(2) = 16.0;
  u(3) = 9.0;
  u(4) = 4.0;
  u(5) = 1.0;
  u(6) = 0.0;
  u(7) = 1.0;
  u(8) = 4.0;

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end

  return
end


function [Y] = Rotation_ForceMoment_FromTireFixed(X,A_K_F,A_Rfl_F,A_Rfr_F,A_down_up)
%% Input parameters:
% A_K_F     [---]   Rotation Matrix from RearAxle Fixed Coordinate System to VehicleFixed Coordinate System
% A_Rfl_F   [---]   Rotation Matrix from RearAxle Fixed Coordinate System to WheelFixed Coordinate System FrontLeft
% A_Rfr_F   [---]   Rotation Matrix from RearAxle Fixed Coordinate System to WheelFixed Coordinate System FrontRight
% A_down_up [...]   Orientation Change Matrix between z-up Orientation and z-down Orientation
% X         [---]   Original Input Vectors [x1 x2 x3 x4] [3x4]
% Output parameters:
% Y         [---]   Transformed Output Vectors [y1 y2 y3 y4] [3x4]

%% Output of transformed vectors
% Quarter Vehicle Model along Vehicle Fixed z-Axis: Rotation from TireFixed Coordinate System into VehicleFixed Coordinate System
% Y=[(A_K_F*transpose(A_Rfl_F)*A_down_up*X(:,1)) (A_K_F*transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_K_F*A_down_up*X(:,3)) (A_K_F*A_down_up*X(:,4))];

% Quarter Vehicle Model along Rear Axle Fixed z-Axis: Rotation from TireFixed Coordinate System into RearAxleFixed Coordinate System
Y=[(transpose(A_Rfl_F)*A_down_up*X(:,1)) (transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

% Quarter Vehicle Model according MATLAB-Documentation
% Y=[(transpose(A_Rfl_F)*A_down_up*X(:,1)) (transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

end


function hermite_cubic_test12 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST12 tests HERMITE_CUBIC_LAGRANGE_INTEGRAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST12:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_LAGRANGE_INTEGRAL returns the integrals\n' );
  fprintf ( 1, '  of the four Lagrange basis functions associated \n' );
  fprintf ( 1, '  with F1, D1, F2 and D2 such that\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  P(X) = F1 * LF1(X) + D1 * LD1(X)\n' );
  fprintf ( 1, '       + F2 * LF2(X) + D2 * LD2(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Lagrange basis function integrals:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        X1          X2          LF1         LD1         LF2         LD2\n' );
  fprintf ( 1, '\n' );

  x2 = 1.0;
  for x1 = -6 : +2
    q = hermite_cubic_lagrange_integral ( x1, x2 );
    fprintf ( 1, '  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f\n', ...
      x1, x2, q(1:4) )
  end

  return
end


function perm=field2d(Nx,Ny,k_avg,V_dp,clx,cly)
% All credits go to http://www.mysimlabs.com/surface_generation.html
% modified by Ali A. Eftekhari
% See the license file
% Note: a very very simple approach! Use it with utmost care :-)
Lx=1.0; % domain length
x=linspace(-Lx/2.0,Lx/2.0,Nx);
y=linspace(-Lx/2.0,Lx/2.0,Ny);
[X,Y]=ndgrid(x,y);
s=-log(1-V_dp); % standard deviation
mu=log(k_avg)-s*s/2.0; % mean for a log-random field
Z=s*randn(Nx,Ny); % normal distribution
F = exp(-(X.^2/(clx*clx/2.0)+Y.^2/(cly*cly/2.0)));
% Gaussian filter
f =2.0/sqrt(pi)*Lx/sqrt(Nx*Ny)/sqrt(clx)/sqrt(cly).*ifft2(fft2(Z).*fft2(F)); % another filter
perm=exp(mu+real(f)); % perm field


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Calibrating kl, ku, mu and nu for a sabr model for Kienitz extrapolation
% the prices are calculated from known sabr parameters

f = 0.03; t =1;                         % forward time
a = 0.25; b = 0.5; r = -.5; n = 0.2;    % sabr parameters

k = 0.0001:0.0001:1;                         % strike range

call = sprice(a, b, r, n, f, k, t,1);   % call prices (standard sabr)
put = sprice(a, b, r, n, f, k, t,0);    % put prices (standard sabr)
put(1) = 0;
call(1) = f;                            % assures forward is matched

Nparam = 4;                             % number of calibrated params

% objective function
of = @(x) of_sabr(a, b, r, n, f, k, t, x(3), x(4), x(1), x(2), call, put);

x0 = [.25*f; 25.5*f; 1.8; 2.4];                   % starting values                  %

A = zeros(Nparam,Nparam); bc= zeros(Nparam,1);
Aeq = A; beq = bc;
lb = [.25*f; f; 1; 3];                        % lower bound
ub = [f; 30*f; 1.5; 5];                     % upper bound
y = fmincon(of,x0,A,bc,Aeq,beq,lb,ub);  % optimization

% verification of results
xval = 0:0.001:.25;                     % x-values
[cl, bl, al, cu, bu, au] = psabr_param_3(a, b, r, n, f, t,y(3),y(4),y(1),y(2));
yval = psabr_5(a, b, r, n, f, xval, t, y(1), y(2), ...
           y(3), cl, bl,al, y(4), cu,bu,au);% y-values calculated

plot(xval,yval);                        % plot the results
Factor = 1000000;                       % used for plotting

yval_call = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 1);    % SABR Call prices
figure; hold on;
    plot(xval,Factor*yval_call,'r');
    plot(k,Factor*call,'g');
hold off;

yval_put = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 0);    % SABR Put prices
figure; hold on;
    plot(xval,Factor*yval_put,'r');
    plot(k,Factor*put,'g');
hold off;

fval = sprice_5(a, b, r, n, f, 0, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 1);    % calculate forward value
y                                       % calibrated values
f - fval                                % display difference to forward


% corrcoef_cell() - compute pairwise correlations using arrays and
%                   cell array inputs.
%
% Usage:
%    >> c = corrcoef_cell( data );
%    >> c = corrcoef_cell( data );
%
% Inputs:
%   data       - [cell array] data consisting of PAIRED arrays to be compared.
%                The last dimension of embeded data arrays is used to compute
%                correlation (see examples).
% Outputs:
%   c   - Correlation values. Same size as data without the last dimension.
%
% Note: the main advantage over the corrcoef Matlab function is the
%       capacity to compute millions of pairwise correlations per second.
%
% Example:
%   a = { rand(1,10) rand(1,10) };
%   c1 = corrcoef_cell(a);
%   c2 = corrcoef(a{1}, a{2});
%   % in this case, c1 is equal to c2(2)
%
%   a = { rand(200,300,100) rand(200,300,100) };
%   c = corrcoef_cell(a);
%   % the call above would require 200 x 300 calls to the corrcoef function
%   % and be about 1000 times slower
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2010

% Copyright (C) Arnaud Delorme
%
% 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 c = corrcoef_cell(a,b);

if nargin < 1
    help corrcoef_cell;
    return;
end;

if nargin < 2
    b = a{2};
    a = a{1};
end;

nd = myndims(a);
if nd == 1
    aa = a-mean(a);
    bb = b-mean(b);
    cv  = aa'*bb/(10-1);
    cva = aa'*aa/(10-1);
    cvb = bb'*bb/(10-1);

    c = cv/sqrt(cva*cvb);
elseif nd == 2
    aa = a-repmat(mean(a,2),[1 size(a,2)]);
    bb = b-repmat(mean(b,2),[1 size(a,2)]);
    cv  = sum(aa.*bb,2);
    cva = sum(aa.*aa,2);
    cvb = sum(bb.*bb,2);

    c = cv./sqrt(cva.*cvb);
elseif nd == 3
    aa = a-repmat(mean(a,3),[1 1 size(a,3)]);
    bb = b-repmat(mean(b,3),[1 1 size(a,3)]);
    cv  = sum(aa.*bb,3);
    cva = sum(aa.*aa,3);
    cvb = sum(bb.*bb,3);

    c = cv./sqrt(cva.*cvb);
elseif nd == 4
    aa = a-repmat(mean(a,4),[1 1 1 size(a,4)]);
    bb = b-repmat(mean(b,4),[1 1 1 size(a,4)]);
    cv  = sum(aa.*bb,4);
    cva = sum(aa.*aa,4);
    cvb = sum(bb.*bb,4);

    c = cv./sqrt(cva.*cvb);
end;

function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end;
    end;


function [Hx,Hy,Ez,time] = Maxwell2D(Hx, Hy, Ez, FinalTime)

% function [Hx,Hy,Ez] = Maxwell2D(Hx, Hy, Ez, FinalTime)
% Purpose :Integrate TM-mode Maxwell's until FinalTime starting with initial conditions Hx,Hy,Ez

Globals2D;
time = 0;

% Runge-Kutta residual storage
resHx = zeros(Np,K); resHy = zeros(Np,K); resEz = zeros(Np,K);

% compute time step size
rLGL = JacobiGQ(0,0,N); rmin = abs(rLGL(1)-rLGL(2));
dtscale = dtscale2D; dt = min(dtscale)*rmin*2/3

% outer time step loop
while (time<FinalTime)

  if(time+dt>FinalTime), dt = FinalTime-time; end

   for INTRK = 1:5
      % compute right hand side of TM-mode Maxwell's equations
      [rhsHx, rhsHy, rhsEz] = MaxwellRHS2D(Hx,Hy,Ez);

      % initiate and increment Runge-Kutta residuals
      resHx = rk4a(INTRK)*resHx + dt*rhsHx;
      resHy = rk4a(INTRK)*resHy + dt*rhsHy;
      resEz = rk4a(INTRK)*resEz + dt*rhsEz;

      % update fields
      Hx = Hx+rk4b(INTRK)*resHx; Hy = Hy+rk4b(INTRK)*resHy; Ez = Ez+rk4b(INTRK)*resEz;
   end;
   % Increment time
   time = time+dt;
end
return


function [inc]=look2inc(la,height,lat)
%LOOK2INC look angle to incidence angle
%
%    [INCIDENCE]=LOOK2INC(LOOK_ANGLE,HEIGHT,LATITUDE)
%
%    LOOK_ANGLE = Look angle (radians) (can be vector or matrix)
%    HEIGHT     = Height of satellite (m)
%    LATITUDE   = mean latitude of ground (degrees)
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%     With permission from Andy Hooper
%
%     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.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
% by Andrew Hooper, 2010

if nargin<3
   lat=40
end

lat1=lat*pi/180;

WGS84_A=6378137.0; % semimajor axis wgs84
WGS84_B=6356752.314; % semiminor axis wgs84
Re=WGS84_A*WGS84_B./sqrt(WGS84_A^2*sin(lat1).^2+WGS84_B^2*cos(lat1).^2);

a=Re+height; % Earth centre to satellite
inc=la;

[R] = fminsearch(@(p) (a^2+p^2-2*a*p*cos(mean(la(:)))-Re^2)^2,[600])
for i=1:length(la(:))
    R = fminsearch(@(p) (a^2+p^2-2*a*p*cos(la(i))-Re^2)^2,R);
    inc(i)=pi-acos((Re^2+R^2-a^2)/2/Re/R);
end


function varths = Get_VarthsPSI_swing(zetaj,ntilj,PSIdj,PSIdjm1)
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here

zetaj2     = zetaj^2; zetaj3 = zetaj*zetaj; zetaj4 = zetaj*zetaj3;

varths = zeros(1,4);
gamms  = zeros(1,3);

gamms(1)  = (1 - zetaj4)/8 + zetaj3/3 - zetaj2/4;
gamms(2)  = 5/12 + zetaj4/4 - zetaj3/3 - zetaj2/2 + zetaj;
gamms(3)  = 1/12 - (1 + zetaj4)/8 + zetaj2/4;

varths(3) = PSIdjm1(ntilj(1)-1)*gamms(1) + PSIdjm1(ntilj(1))*gamms(2) + PSIdjm1(ntilj(1)+1)*gamms(3);

gamms(1) = 1/12 - gamms(1);
gamms(2) = 5/6  - gamms(2);
gamms(3) = 1/12 - gamms(3);

varths(1) = PSIdj(ntilj(1)-1)*gamms(1) + PSIdj(ntilj(1))*gamms(2) + PSIdj(ntilj(1)+1)*gamms(3);

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

gamms(1)  = zetaj4/8 - zetaj3/2 + zetaj2/2;
gamms(2)  = -zetaj4/4 + 2*zetaj3/3;
gamms(3)  = zetaj4/8 - zetaj3/6;

varths(4) = PSIdjm1(ntilj(1))*gamms(1) + PSIdjm1(ntilj(1)+1)*gamms(2) + PSIdjm1(ntilj(1)+2)*gamms(3);

gamms(1) = 1/12 - gamms(1);
gamms(2) = 5/6  - gamms(2);
gamms(3) = 1/12 - gamms(3);

varths(2) = PSIdj(ntilj(1))*gamms(1) + PSIdj(ntilj(1)+1)*gamms(2) + PSIdj(ntilj(1)+2)*gamms(3);

end


% Recursive Least-Squares Algorithm with exponential weighting
%
% From S. Haykin, "Adaptive Filtering Theory (3rd Ed.)", Prentice Hall,
% Chapter 13.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef rls < linear_filter

    properties (GetAccess = 'public', SetAccess = 'private')
        lambda = .99; % forgetting factor
        c = 1E-4; % regularization
    end

    properties (GetAccess = 'public', SetAccess = 'private')
        P = []; % inverse autocorrelation matrix
        w = []; % filter coefficients
    end

    methods

        function obj = rls(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(obj))
                        obj.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(obj,x) % evaluate the algorithm
            if numel(obj.w)>0
                y_est = x*obj.w;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(obj,x,y) % train the algorithm
            if numel(obj.w)==0 % initialize
                m = length(x);
                obj.w = zeros(m,1);
                obj.P = obj.c\eye(m);
            end

            g = obj.P*x'/(obj.lambda+x*obj.P*x'); % gain vector
            err = y - x*obj.w; % instantaneous error
            obj.w = obj.w + g*err; % update filter coefficients
            obj.P = obj.lambda\(obj.P - g*x*obj.P); % update inv. autocorr.
        end

    end
end


function [rs,rp]=reflectionCoeffsVec(e1,e2,k,n,u1,u2)
%%REFLECTIONCOEFFSVEC Reflection coefficients for the reflection of an
%           electromagnetic plane wave from a LOSSLESS medium off a LOSSY
%           medium (or another lossless medium) are computed given a vector
%           in the direction of propagation of the light and a vector
%           normal to the surface. Both media must have real
%           permeabilities. For example, one might approximate the
%           troposphere as lossless while accounting for the loss in sea
%           water. Lossy media have complex permittivities. This function
%           is a different parameterization of reflectionCoeffs.
%
%INPUTS: e1 The permittivity (refraction index) of the lossless medium.
%           The incoming ray is traveling through this prior to reflecting
%           off of the surface with permittivity e2. This must be a real
%           quantity (lossless). e1 and e2 can both be either absolute
%           permittivities or relative permittivities. "Relative" means
%           that the permittivity of the medium has been divided by the
%           permittivity of free space and is a dimensionless quantity.
%        e2 The permittivity of the medium against which the ray reflects.
%           This can be complex (lossy).
%         k A 3X1 vector in the direction of propagation of the light
%           approaching the surface from which it will reflect.
%         n A 3X1 normal vector to the surface of reflection. It does not
%           matter whether n is pointing up or down from the surface.
%        u1 The permeability of the lossless medium. This must be
%           a real quantity.  u1 and u2 can both be either absolute
%           permeabilities or relative permeabilities. If omitted or an
%           empty matrix is passed, a value of 1 is used (the permeability
%           equals that of free space). That is a reasonable approximation
%           for air.
%        u2 The permeability of the lossy medium against which the incoming
%           ray reflects. medium. This must be a real quantity. If omitted
%           or an empty matrix is passed, a value of 1 is used. That is a
%           reasonable approximation for water.
%
%OUTPUTS: rs The complex reflection coefficient for s-polarized light, also
%            known as transverse-Electric (TE) polarized light.
%         rp The complex reflection coefficient for p-polarized light, also
%            known as transverse-Magnetic (TM) polarized light and as
%            tangent-plane polarized light.
%
%This function uses angBetweenVecs to get the angle between the incoming
%vector and the normal to the surface (adjusting in case one should have
%used -n instead of n) and then calls reflectionCoeffs.
%
%July 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(u2))
    u2=1;
end

if(nargin<5||isempty(u1))
    u1=1;
end

thetai=angBetweenVecs(k,n);

if(thetai>pi/2)
    %If -n should have been used instead of n.
    thetai=pi-thetai;
end
[rs,rp]=reflectionCoeffs(e1,e2,thetai,u1,u2);

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 cc_level_compose_animate ( )

%*****************************************************************************80
%
%% CC_LEVEL_COMPOSE_ANIMATE displays the grids that compose one level.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CC_LEVEL_COMPOSE_ANIMATE:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Display the nested 2D Clenshaw-Curtis grids\n' );
  fprintf ( 1, '  that compose one level.\n' );

  dim_num = 2;

  while ( 1 )
%
%  Get user input.
%
    fprintf ( 1, '\n' );
    level = input ( 'Enter the LEVEL or RETURN to exit;' );

    if ( isempty ( level ) )
      break
    end
%
%  Generate the entire set of points.
%
    [ grid_num, point_num ] = cc_levels_minmax_size ( dim_num, ...
      level, level );

    [ grid_level, grid_order, grid_points ] = cc_levels_minmax ( dim_num, ...
      level, level,  grid_num, point_num );

    [ dim_num, grid_points_num ] = size ( grid_points );
%
%  Display the full set of points as filled blue circles.
%
    clf
    axes_handle = axes;
    handle_new = scatter ( grid_points(1,:), grid_points(2,:), 'b', 'filled' );
    axis square
    grid on
    set ( axes_handle, 'xtick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'xticklabel', [] );
    set ( axes_handle, 'ytick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'yticklabel', [] );
    axis ( [ -1.1, 1.1, -1.1, 1.1 ] )
    s = sprintf ( 'Entire grid for LEVEL %d', level );
    title ( s );
    fprintf ( 1, 'Press return\n', level );
    pause

    hold off
%
%  Display the full set of points in gray.
%  Display each contributing grid, one at a time, in red.
%
    for gridd = 1 : grid_num

      order_1d(1:2) = grid_order(1:2,gridd)
      order_nd = prod ( order_1d(1:2) );
      grid_points_new = cc_grid ( dim_num, order_1d, order_nd );

      clf
%
%  We have to name the axes in order to control the grid.
%
      axes_handle = axes;
%
%  Plot the OLD points.
%
      handle_old = scatter ( grid_points(1,:), grid_points(2,:), 'bo' );

      hold on

      handle_new = scatter ( grid_points_new(1,:), grid_points_new(2,:), ...
        'r', 'filled' );
%
%  Force the plotting region to be square, not rectangular.
%
      axis square
%
%  Request grid lines.
%
      grid on
%
%  Specify the location of the grid lines, and suppress labeling.
%
      set ( axes_handle, 'xtick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
      set ( axes_handle, 'xticklabel', [] );
      set ( axes_handle, 'ytick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
      set ( axes_handle, 'yticklabel', [] );
%
%  Make the plotting region slightly bigger than the data.
%
      axis ( [ -1.1, 1.1, -1.1, 1.1 ] )
%
%  Title
%
      s = sprintf ( '+ %d * %d CC grid', grid_level(1,gridd), grid_level(2,gridd) );
      title ( s );

      fprintf ( 1, '+ %d*%d CC grid, Press return\n', ...
        grid_level(1,gridd), grid_level(2,gridd) );
      pause

      hold off

    end
%
%  Display the full set of points as filled blue circles.
%
    clf
    axes_handle = axes;
    handle_new = scatter ( grid_points(1,:), grid_points(2,:), 'b', 'filled' );
    axis square
    grid on
    set ( axes_handle, 'xtick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'xticklabel', [] );
    set ( axes_handle, 'ytick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'yticklabel', [] );
    axis ( [ -1.1, 1.1, -1.1, 1.1 ] )
    s = sprintf ( 'Entire grid for LEVEL %d', level );
    title ( s );
    fprintf ( 1, 'Press return\n', level );
    pause

    hold off

    fprintf ( 1, 'Press return to CLEAR the grid!\n' );
    pause
    clf

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CC_LEVEL_COMPOSE_ANIMATE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ============================================
% FLD.m
%
% Goal:
%   1. Calculate the optimal FLD projection
%   2. For two class problem
%
% Li Shen
% 11/13/2002 - create

function [FLD_basis, FLD_vals] = FLD(Samples,Labels)

d = size(Samples);
if (d(2)>d(1)-2)
    disp('---------------------------------');
    disp(sprintf('N=%d points, M=%d dims (IGNORE the last %d dims): a nonsigular Sb requires M<=N-c (c=2)',d,d(2)-d(1)+2));
    disp('---------------------------------');
    Samples = Samples(:,1:d(1)-2);
end

FLD_basis = [];

Sb = get_Sb(Samples,Labels);
Sw = get_Sw(Samples,Labels);

[V,D] = eig(Sb,Sw);

[eigval,ind] = sort(diag(D));

FLD_basis = V(:,ind(end));

FLD_vals = Samples*FLD_basis;

return;

%
% calculate between class scatter matrix Sb
%

function Sb = get_Sb(Samples,Labels)

c1_ind = find(Labels==1); c2_ind = find(Labels==2);
m = mean(Samples,1); m1_m = mean(Samples(c1_ind,:),1) - m ; m2_m = mean(Samples(c2_ind,:),1) - m;
Sb = length(c1_ind)*(m1_m'*m1_m) + length(c2_ind)*(m2_m'*m2_m);

return;

%
% calculate within class scatter matrix Sw
%

function Sw = get_Sw(Samples,Labels)

c1_ind = find(Labels==1); c2_ind = find(Labels==2);

% need to have at least 2 points in each class
Sw = cov(Samples(c1_ind,:))*(length(c1_ind)-1) + cov(Samples(c2_ind,:))*(length(c2_ind)-1);

return;

%
% calculate within class scatter matrix Sw (directly according to the definition)
%

function Sw = get_Sw_v2(Samples,Labels)

c1_ind = find(Labels==1); c2_ind = find(Labels==2);

% to verify the correctness of Sw
Sw = zeros(size(Samples,2));
m1 = mean(Samples(c1_ind,:),1);
xs1 = Samples(c1_ind,:) - m1(ones(1,length(c1_ind)),:);
for i = 1:size(xs1,1)
    Sw = Sw + xs1(i,:)'*xs1(i,:);
end

m2 = mean(Samples(c2_ind,:),1);
xs2 = Samples(c2_ind,:) - m2(ones(1,length(c2_ind)),:);
for i = 1:size(xs2,1)
    Sw = Sw + xs2(i,:)'*xs2(i,:);
end

return;


function [mfg, mbg] = im_modes(im)
% IM_MODES  Estimate typical values (modes) of foreground/background
% intensities in a greyscale image.
%
% [MFG, MBG] = im_modes(IM)
%
%   IM is an N-array of grayscale intensity values.
%
%   MFG, MBG are scalars with the typical intensity values of foreground
%   (darker) and foreground (lighter) voxels. "Typical" means the mode of
%   the corresponding distributions.
%
%   The modes are estimated by computing histograms with up to 500 bins for
%   the intensity values, and smoothing the distribution until only 1 or 2
%   peaks are visible. In case of 1 peak, it is assume that the image
%   contains only background voxels, and MFG=NaN.

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

DEBUG = 0;

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

% ignore intensity values = 0. Those are considered to be masked out
im2 = im(im > 0);

% if the input image is empty, or too small, we assume that we don't have
% enough information to estimate the background and foreground
if (nnz(im2) < 100)
    mbg = nan;
    mfg = nan;
    return
end

% number of bins to use. We assume that we need at least 10 samples per
% bin, but we don't need more than 500 bins in total. Histograms with more
% bins are slower to process and smooth
nbin = min(500, ceil(nnz(im2)/10));

% compute histogram of the intensity values
[fhist, xhist] = hist(im2, nbin);
fhist = fhist / sum(fhist);

% initial estimation of peaks in the histogram
[pks, loc] = findpeaks(fhist, 'minpeakheight', 0.5e-3);

% number of peaks found
npks = length(pks);

% smooth the histogram until we find just 1 or 2 peaks
tol = 0.5e-10;
while ~((npks == 1) ...
        || ((npks == 2) && (abs(diff(loc)) >= 50)))

    % increase the smoothing parameter
    tol = tol * 2;

    % smooth the histogram
    [~, fhist2] = spaps(xhist, fhist, tol);

    % find the peaks
    [pks, loc] = findpeaks(fhist2, 'minpeakheight', 0.5e-3);

    % number of peaks found
    npks = length(pks);

end

% deal with the number of peaks
if (npks == 2)

    % we have background and tissue. The background is lighter. Sort the
    % peaks in darker to lighter order
    loc = sort(loc, 'ascend');

    % extract typical intensities for background and foreground
    mfg = xhist(loc(1));
    mbg = xhist(loc(2));

    % DEBUG
    if (DEBUG)
        subplot(2, 1, 1)
        hold off
        plot(xhist, fhist, 'b')
        hold on
        plot(xhist, fhist2, 'r')
        plot(mfg*[1 1], [0 max(fhist)], 'g')
        plot(mbg*[1 1], [0 max(fhist)], 'k')

        subplot(2, 1, 2)
        hold off
        imagesc(im(:, :, round((size(im, 3)+1)/2)))
    end

elseif (npks == 1)

    % we assume there's only background (although note that this could be a
    % case with only tissue)
    mfg = nan;
    mbg = xhist(loc);

    % DEBUG
    if (DEBUG)
        hold off
        plot(xhist, fhist, 'b')
        hold on
        plot(xhist, fhist2, 'r')
        plot(mbg*[1 1], [0 max(fhist)], 'r')
    end

else

    error('Assertion fail: The histogram has no peaks')

end

end


function [ sparse_order, sparse_index ] = sparse_grid_mixed_index ( ...
  dim_num, level_max, rule, point_num, point_total_num, sparse_unique_index )

%*****************************************************************************80
%
%% SPARSE_GRID_MIXED_INDEX indexes a sparse grid made from mixed 1D rules.
%
%  Discussion:
%
%    For each "unique" point in the sparse grid, we return its INDEX and ORDER.
%
%    That is, for the I-th unique point P, we determine the product grid which
%    first generated this point, and we return in SPARSE_ORDER the orders of
%    the 1D rules in that grid, and in SPARSE_INDEX the component indexes in
%    those rules that generated this specific point.
%
%    For instance, say P was first generated by a rule which was a 3D product
%    of a 9th order CC rule and a 15th order GL rule, and that to generate P,
%    we used the 7-th point of the CC rule and the 3rh point of the GL rule.
%    Then the SPARSE_ORDER information would be (9,15) and the SPARSE_INDEX
%    information would be (7,3).  This, combined with the information in RULE,
%    is enough to regenerate the value of P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2011
%
%  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.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, integer POINT_NUM, the number of unique points in the grid.
%
%    Input, integer POINT_TOTAL_NUM, the total number of points in the grid.
%
%    Input, integer SPARSE_UNIQUE_INDEX(POINT_TOTAL_NUM), associates each
%    point in the grid with its unique representative.
%
%    Output, integer SPARSE_ORDER(DIM_NUM,POINT_NUM), lists, for each point,
%    the order of the 1D rules used in the grid that generated it.
%
%    Output, integer SPARSE_INDEX(DIM_NUM,POINT_NUM), lists, for each point,
%    its index in each of the 1D rules in the grid that generated it.
%    The indices are 1-based.
%

%
%  Special cases.
%
  if ( level_max < 0 )
    sparse_order = [];
    sparse_index = [];
    return
  end

  if ( level_max == 0 )
    sparse_order(1:dim_num,1) = 1;
    sparse_index(1:dim_num,1) = 1;
    return
  end

  sparse_order = zeros ( dim_num, point_num );
  sparse_index = zeros ( dim_num, point_num );

  point_count = 0;
%
%  The outer loop generates values of LEVEL.
%
  level_min = max ( 0, level_max + 1 - dim_num );

  for level = level_min : level_max
%
%  The middle loop generates a GRID,
%  based on the next partition that adds up to LEVEL.
%
    level_1d = [];
    more_grids = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more_grids, h, t ] = comp_next ( level, dim_num, level_1d, ...
        more_grids, h, t );

      order_1d = level_to_order_default ( dim_num, level_1d, rule );
%
%  The inner loop generates a POINT of the GRID of the LEVEL.
%
      point_index = [];
      more_points = 0;

      while ( 1 )

        [ point_index, more_points ] = vec_colex_next3 ( dim_num, order_1d, point_index, ...
          more_points );

        if ( ~more_points )
          break
        end

        point_count = point_count + 1;
        point_unique = sparse_unique_index(point_count);
        sparse_order(1:dim_num,point_unique) = order_1d(1:dim_num);
        sparse_index(1:dim_num,point_unique) = point_index(1:dim_num);

      end

      if ( ~more_grids )
        break
      end

    end

  end

  return
end


function res = lt(a,b)
%LT           Implements  a < b  elementwise for intervals a and b
%
%  if true,  a  is definitely less than  b
%

% written  10/16/98     S.M. Rump
% modified 11/30/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
%

  if ~isa(a,'intval')
    a = intval(a);
  end
  if ~isa(b,'intval')
    b = intval(b);
  end

  if a.complex | b.complex
    res = real(sup(a)) < real(inf(b)) & imag(sup(a)) < imag(inf(b)) ;
  else
    res = sup(a) < inf(b) ;
  end


function xPol=Cart2DState2PolarState(xCart,systemType)
%%CART2DSSTATE2POLARSTATE Transform a 2D Cartesian state into a state
%                         consisting of position, heading and speed as well
%                         as possibly a turn rate and a linear
%                         acceleration, depending on the choice of
%                         systemType.
%
%INPUTS: xCart A Cartesian state vector consisting of position velocity and
%              possibly acceleration into a state where heading and speed
%              have been separated. xCart has the form
%              [x;y;xdot;ydot;xddot;yddot], where the acceleration terms
%              xddot;yddot can be omitted if the system type is 'ConstVel'.
%   systemType A string constant specifying the desired type of output. In
%              all instances, the heading is measured in terms of radians
%              counterclockwise from the x-axis. Possible values are:
%              'ConstVel'     The target state is [position;heading;speed]
%                             and xCart is [position;velocity]
%              'ConstAccel'   The target state is [position;heading;speed;
%                             speed derivative] and xCart is
%                             [position;velocity;acceleration]
%              'ConstTurn'    The target state is [position;heading;speed;
%                             turn rate] and xCart is
%                             [position;velocity;acceleration]
%              'TurnAndAccel' The target state is [position;heading;speed;
%                             turnrate; speed derivative] and xCart is
%                             [position;velocity;acceleration]
%
%OUTPUTS: xPol The state converted from 2D Cartesian coordinates into the
%              selected 2D coordinate system.
%
%When the system type is 'ConstVel' or 'TurnAndAccel', only a single
%solution is mathematically observable. When the system type is
%'ConstAccel' or 'ConstTurn', the system is overdetermined, but only a
%simple solution is used, not a least squares solution.
%
%The use of 2D states where the heading and speed have been separated is
%discussed in [1] and [2].
%
%The opposite of this function is polar2DState2CartState.
%
%REFERENCES:
%[1] M. Busch and S. Blackman, "Evaluation of IMM filtering for an air
%    defense system application," in Proceedings of SPIE: Signal and Data
%    Processing of Small Targets, vol. 2561, 9 Jul. 1995, pp. 435-447.
%[1] J. L. Gertz, "Multisensor surveillance for improved aircraft
%    tracking," The Lincoln Laboratory Journal, vol. 2, no. 3, pp. 381-396,
%    1989.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Get position
x=xCart(1);
y=xCart(2);
%Get velocity
xDot=xCart(3);
yDot=xCart(4);

switch(systemType)
    case 'ConstVel' %Given position, heading, and speed.
        xPol=[x;
              y;
              atan2(yDot,xDot);
              sqrt(yDot^2+xDot^2)];
    case 'ConstAccel'%Given position, heading, speed, and linear
                     %acceleration
        %Get acceleration
        xDdot=xCart(5);
        yDdot=xCart(6);

        theta=atan2(yDot,xDot);%Heading
        %Determine the sign of the derivative velocity, ignoring the possible
        %effects of noise...
        vDot=sqrt(yDdot^2+xDdot^2);%Linear acceleration

        diff1=(vDot*cos(theta)-xDdot)^2+(vDot*sin(theta)-yDdot)^2;
        diff2=(-vDot*cos(theta)-xDdot)^2+(-vDot*sin(theta)-yDdot)^2;
        if(diff2<diff1)
            vDot=-vDot;
        end
        xPol=[x;
              y;
              theta;
              sqrt(yDot^2+xDot^2);
              vDot];
    case 'ConstTurn' %Given position, heading, speed, and turn rate.
        %Get acceleration
        xDdot=xCart(5);
        yDdot=xCart(6);

        %Turn rate
        omega=(xDot*yDdot-yDot*xDdot)/(xDot^2+yDot^2);

        if(~isfinite(omega))
            omega=0;
        end

        xPol=[x;
              y;
              atan2(yDot,xDot);
              sqrt(yDot^2+xDot^2);
              omega];
    case 'TurnAndAccel'%Given position, heading, speed, turn rate, and
                       %linear acceleration.
        %Get acceleration
        xDdot=xCart(5);
        yDdot=xCart(6);

        theta=atan2(yDot,xDot);%Heading
        v=sqrt(yDot^2+xDot^2);%Speed
        omega=(yDdot*cos(theta)-xDdot*sin(theta))/v;%Turn rate

        %Deal with slow speed targets.
        if(~isfinite(omega))
            omega=0;
        end

        %Linear acceleration
        vDot=xDdot*cos(theta)+yDdot*sin(theta);

        xPol=[x;
              y;
              theta;
              v;
              omega;
              vDot];
    otherwise
        error('Invalid system type given.')
end

end

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


%[2015]-"Multi-verse optimizer: A nature-inspired algorithm for global
%optimization"

% (9/12/2020)

function MVO = jMultiVerseOptimizer(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
p     = 6;      % control TDR
Wmax  = 1;      % maximum WEP
Wmin  = 0.2;    % minimum WEP
type  = 1;

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'p'), p = opts.p; end
if isfield(opts,'Wmin'), Wmin = opts.Wmin; end
if isfield(opts,'Wmax'), Wmax = opts.Wmax; end
if isfield(opts,'ty'), type = opts.ty; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Pre
fit  = zeros(1,N);
fitG = inf;

curve = inf;
t = 1;
% Iterations
while t <= max_Iter
  % Calculate inflation rate
  for i = 1:N
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
    % Best universe
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  % Sort universe from best to worst
  [fitSU, idx] = sort(fit,'ascend');
  X_SU         = X(idx,:);
  % Elitism (first 1 is elite)
  X(1,:) = X_SU(1,:);
  % Either 1-norm or 2-norm
  if type == 1
    % Normalize inflation rate using 2-norm
    NI = fitSU ./ sqrt(sum(fitSU .^ 2));
  elseif type == 2
    % Normalize inflation rate using 1-norm
    NI = fitSU / sum(fitSU);
  end
  % Normalize inverse inflation rate using 1-norm
  inv_fitSU = 1 ./ (1 + fitSU);
  inv_NI    = inv_fitSU / sum(inv_fitSU);
  % Wormhole Existence probability (3.3), increases from 0.2 to 1
  WEP = Wmin + t * ((Wmax - Wmin) / max_Iter);
  % Travelling disrance rate (3.4), descreases from 0.6 to 0
  TDR = 1 - ((t ^ (1 / p)) / (max_Iter ^ (1 / p)));
  % Start with 2 since first is elite
  for i = 2:N
    % Define black hole
    idx_BH = i;
    for d = 1:dim
      % White/black hole tunnels & exchange object of universes (3.1)
      r1 = rand();
      if r1 < NI(i)
        % Random select k with roulette wheel
        idx_WH       = jRouletteWheelSelection(inv_NI);
        % Position update
        X(idx_BH, d) = X_SU(idx_WH, d);
      end
      % Local changes for universes (3.2)
      r2 = rand();
      if r2 < WEP
        r3 = rand();
        r4 = rand();
        if r3 < 0.5
          X(i,d) = Xgb(d) + TDR * ((ub - lb) * r4 + lb);
        else
          X(i,d) = Xgb(d) - TDR * ((ub - lb) * r4 + lb);
        end
      else
        X(i,d) = X(i,d);
      end
    end
    % Boundary
    XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X(i,:) = XB;
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (MVO)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
MVO.sf = Sf;
MVO.ff = sFeat;
MVO.nf = length(Sf);
MVO.c  = curve;
MVO.f  = feat;
MVO.l  = label;
end

%// Roulette Wheel Selection //
function Index = jRouletteWheelSelection(prob)
% Cummulative summation
C = cumsum(prob);
% Random one value, most probability value [0~1]
P = rand();
% Route wheel
for i = 1:length(C)
	if C(i) > P
    Index = i;
    break;
  end
end
end


function hydro = excitationIRF(hydro,tEnd,nDt,nDw,wMin,wMax)
% Calculates the normalized excitation impulse response function:
%
% 	:math:`\overline{K}_{e,i,\theta}(t) = {\frac{1}{2\pi}}\intop_{-\infty}^{\infty}{\frac{X_i(\omega,\theta)e^{i{\omega}t}}{{\rho}g}}d\omega`
%
% Default parameters can be used by inputting [].
% See ``WEC-Sim\examples\BEMIO`` for examples of usage.
%
% Parameters
% ----------
%     hydro : struct
%         Structure of hydro data
%
%     tEnd : float
%         Calculation range for the IRF, where the IRF is calculated from t
%         = 0 to tEnd, and the default is 100 s
%
%     nDt : float
%         Number of time steps in the IRF, the default is 1001
%
%     nDw : float
%         Number of frequency steps used in the IRF calculation
%         (hydrodynamic coefficients are interpolated to correspond), the
%         default is 1001
%
%     wMin : float
%         Minimum frequency to use in the IRF calculation, the default is
%         the minimum frequency from the BEM data
%
%     wMax : float
%         Maximum frequency to use in the IRF calculation, the default is
%         the maximum frequency from the BEM data
%
% Returns
% -------
%     hydro : struct
%         Structure of hydro data with excitation IRF
%

p = waitbar(0,'Calculating excitation IRFs...');  % Progress bar

% Set defaults if empty
if isempty(tEnd)==1;  tEnd = 100;           end
if isempty(nDt)==1;    nDt = 1001;            end
if isempty(nDw)==1;    nDw = 1001;            end
if isempty(wMin)==1;  wMin = min(hydro.w);  end
if isempty(wMax)==1;  wMax = max(hydro.w);  end

% Interpolate to the given t and w
t = linspace(-tEnd,tEnd,nDt);
w = linspace(wMin,wMax,nDw);
N = sum(hydro.dof)*hydro.Nh;

% Calculate the impulse response function for excitation
n = 0;
for i = 1:sum(hydro.dof)
    for j = 1:hydro.Nh
        ex_re = interp1(hydro.w,squeeze(hydro.ex_re(i,j,:)),w);
        ex_im = interp1(hydro.w,squeeze(hydro.ex_im(i,j,:)),w);
        hydro.ex_K(i,j,:) = (1/pi)*trapz(w,ex_re.*cos(w.*t(:))-ex_im.*sin(w.*t(:)),2);
        n = n+1;
    end
    waitbar(n/N)
end

hydro.ex_t = t;
hydro.ex_w = w;
close(p);

end


function [xopt,fopt]=simann(func, x, LB, UB, sa_t, sa_rt, sa_nt, sa_ns,rseed)

% Simulated Annealing programmed for minimization problem
% INPUTS
% func, string variable containing name of function file to be optimized
% x, starting values
% LB, lower bound on optimization parameters
% UB, upper bound on optimization parameters
% sa_t, initial temperature
% sa_rt, temperature reduction factor, 0 < sa_rt < 1, try .85
% sa_nt, number of times through ns loop before temperature reduction (recommended value: 5)
% sa_ns, number of times through function before stepsize adjustment (recommended value: 20)
%
% OUTPUTS
% xopt, the optimal solution
%
%

LB=LB(:)';
UB=UB(:)';

rand('state',rseed);                      %sets seed for random number generator
sa_neps=4;                                %number of times eps
                                          %tolerance is achieved before termination
sa_eps=eps;                              %convergence criteria
sa_maxeval=60;%12000000;                      %maximum number of function evaluations

sa_nargs=length(LB);                      %number of parameters
sa_nobds=0;
sa_nacc=0;                                %number of acceptions
sa_nevals=0;                              %number of evaluations
sa_opteval=0;                             %optimum number of
                                          %function evaluations

fstar=Inf*ones(sa_neps,1);

%x=LB+(UB-LB).*rand(1, sa_nargs);         %starting values for model parameters
f=feval(func,x);                          %function evaluation with parameters x
%disp('initial loss function value:');disp(f);
sa_nevals=sa_nevals+1;
xopt=x;
fopt=f;
xtot=x;
fstar(1)=f;

VM=(UB-LB);%/2;                      %maximum step size

%LOOP
while 1

  nup=0;                                   %number of uphill movements
  nrej=0;                                  %number of rejections
  nnew=0;
  ndown=0;                                 %number of downhill movements
  lnobds=0;
  nacp=zeros(sa_nargs,1);
  C = progress('init','Determine initial hyperparameters for simplex...');
  for m=1:sa_nt
    for j=1:sa_ns
      for h=1:sa_nargs
        if sa_nevals>=sa_maxeval
          %disp('too many function evaluations')
          return
        end
        C = progress(C,sa_nevals/sa_maxeval);
        %workbar(sa_nevals/sa_maxeval,'Determine initial hyperparameters to build grid...','Progress')
        % generate xp, trial value of x
        xp=x;
        xp(h)=x(h)+VM(h)*(2*rand(1,1)-1.0);               %calculate new value for x (xp)
        if (xp(h)<LB(h)) | (xp(h)>UB(h))
          xp(h)=LB(h)+(UB(h)-LB(h))*rand(1,1);
          lnobds=lnobds+1;
          sa_nobds=sa_nobds+1;
        end
        % evaluate at xp and return as fp
        %disp ('current parameter vector:');disp(xp);
        fp=feval(func,xp);                                 %function evaluation with parameters xp
        %disp ('function value');disp(fp);
        sa_nevals=sa_nevals+1;

        % we minimize! accept if the function value decreases
        if fp<=f
          x=xp;
          f=fp;
          sa_nacc=sa_nacc+1;
          nacp(h)=nacp(h)+1;
          nup=nup+1;
          % if smaller than any previous point, record as new optimum
          if fp<fopt
            xopt=xp;
            fopt=fp;
            sa_opteval=sa_nevals;
            nnew=nnew+1;
          end
        else % function value increases
          p=exp((f-fp)/sa_t);                              %random number
          pp=rand(1,1);
          if pp<p
            x=xp;
            f=fp;
            sa_nacc=sa_nacc+1;
            nacp(h)=nacp(h)+1;
            ndown=ndown+1;
          else
            nrej=nrej+1;
          end
        end
      end
    end

    % adjust maximal step size vm
    c=ones(sa_nargs,1)*2;%??
    for i=1:sa_nargs
      ratio=nacp(i)/sa_ns;
      if ratio>0.6
        VM(i)=VM(i) * (1+c(i)*(ratio-0.6)/0.4);
      elseif ratio <0.4
        VM(i)=VM(i)/(1+c(i)*((0.4-ratio)/0.4));
      end
      if VM(i)>(UB(i)-LB(i))
        VM(i)=UB(i)-LB(i);
      end
    end

    % provide statistics about current state of optimization

%     disp('No. of evaluations');disp(sa_nevals);disp('  current temperature');disp(sa_t);
%     disp('current optimum function value');disp(fopt);
%     disp('No. of downhill steps');disp(nup);  % note misnomer in variable declaration!
%     disp('No. of accepted uphill steps');disp(ndown); % we minimize, thus downhill is always accepted!
%     disp('No. of rejections');disp(nrej);
%     disp('current parameter values');disp(xp);
%     disp('current optimum vector');disp(xopt);
%     disp('current step size');disp(VM);
    %disp('Variables used:');whos;

  for i=1:sa_nargs
     nacp(i) = 0;
  end
  end

  % check termination criteria
  fstar(1)=f;
  quit = ((fstar(1)-fopt) <= sa_eps);
  if any(abs(fstar-f)>sa_eps)
    quit=0;
  end

  if quit
    disp(['simulated annealing achieved termination after ', num2str(sa_nevals),' evals']);
    return
  end

  % reduce temperature
  sa_t=sa_t*sa_rt;
  fstar(2:4)=fstar(1:3);
  % continue from current optimum
  x=xopt;
  f=fopt;
end %while