function Main()
% This program is a stand-alone toy example of radiation treatment planning
% (RTP) optimization for a brain tumor case.
%     The program generates a toy patient head model using scaled and shifted
% ellipsoids and p-norm sublevel sets to represent all the volumes of
% interest (VOIs), including skin, eyes, optic nerves, brain stem, a tumor
% and artificial "shell" around the tumor. These VOI sublevel sets are then
% discretized into PointClouds, by retaining the points in a discrete 3-D
% grid of volume elements (voxels) that lie in the 1-sublevel set of the
% VOI's p-norm model.
%     Then we compute candidate beam directions by firing beams at random
% points on the tumor surface from a set of about 150 nodes, located
% uniformly in a spherical region of radius 80cm from the patient's head.
%     Then we compute the dose vectors associated with each beam in each of
% the VOIs, by evaluating a dose function in a cylindrical region around each
% beam direction. The dose function is a crude model of the roll-off with
% depth, and radial diffusion or scatter. For each candidate beam, this
% rolloff/scatter function is evaluated for each voxel in each VOI. Thus
% each beam results in a column of the patient dose matrix.
% Finally, we set the min and max dosages for each VOI.
%     We then pass the dose matrices and min and max specs to one of two
% solvers (CPLEX or ADMM), which find the optimal set of beams and
% intensities (beam weights) for this patient case. This is the optimal
% "plan". The optimization formulation is based on a problem from the
% Stanford EE364b final exam from 2011, by Stephen Boyd and Eric Chu.
%     Then we visualize the optimal plan by plotting the dose levels for
% all the voxels of each VOI, and the dose volume histograms.
%     The program consists of this Main() function and several "classes":
% (1) Patient: cell array of VOIs, names, etc
% (2) VOI: p-norm model and associated point cloud and boundary, dose specs
% (3) Beams: beam heads,tails,dose vector computation + nodes + collimators
% (4) Model: p-norm models and gradients
% (5) Point Cloud & 3-D geometry functions
% (6) Optimizer functions: CPLEX wrapper and ADMM solver (ala EE364b)
%
% Reference:
% H. Hindi, "A Tutorial on Optimization Methods for Cancer Radiation
% Treatment Planning," Proc. American Contr. Conf, 2013.
%
% Written by Haitham Hindi, 2012/01/18
%
% DISCLAIMER: This code is intended for algorithm research purposes
% only!! Author makes no claims as to the realism, accuracy, or correctness
% of ANY part of this code, including (but not limited to):
% patient anatomy and dimensions; plan safety; beam weights,doses,physics;
% dose units, dose specs; plan quality, evaluation metrics. The author is
% an engineer with no medical training whatsoever! This code is supplied
% as-is, with no guarantee of correctness, and the author accepts no
% responsibility for any damage or false conclusions from the use of this
% code. Of course, we would appreciate hearing about any bugs you might find.

    clear, clc, close all
    rng('default');
    rng(2);

    %====================================================
    % data for patient and beams
    %====================================================
    % patient specs
    PatientName     = 'Toy Brain Tumor Case';
    PatientThetaxyz = [pi/2;0;0];
    PatientOffset   = [0;0;0];
    PatientAxLims   = [-15 15];

    % VOI specs (VOIs are scaled shifted p-norm sublevel sets)
    Target=1;Shell=2;BrStm=3;EyeL=4;EyeR=5;OptNrvL=6;OptNrvR=7;Skin=8;
    VoxSizeSkin      = 1;
    VoxSizeOrgans    = 0.2;%0.15%0.113;
    SampFactorSkin   = 1;
    SampFactorOrgans = 1;
    VOINames      ={'Target','Shell','BrStm','EyeL','EyeR','OptNrvL','OptNrvR','Skin'};
    dmaxs         =[50      , 25    ,20     , 20   , 20   , 20      , 20      , 30];
    dmins         =[40      , 0     , 0     ,  0   ,  0   ,  0      ,  0      ,  0];
    VOINumbers    =[ 1,       2,      3,      4,     5,     6,        7,        8];
    VOITypes      ={'v',     's',    'v',    'v',   'v',   'v',      'v',      's'};
    VOIColors     ={'k',     'r',    'g',    'b',   'b',   'c',      'c',      'y'};
    VOILineStyles ={'-',     '-',    '-',    '-',   '--',  '-',      '--',     '-'};
    VOIVoxSizes   =[VoxSizeOrgans*ones(7,1);VoxSizeSkin];
    VOISampFactors=[SampFactorOrgans*ones(7,1);SampFactorSkin];
    VOIxyzScales  =[1.2*[1;1;1],1.56*[1;1;1],[1.5;1.5;6],1.7*[1;1;1],1.7*[1;1;1],[0.7;4;0.7],[0.7;4;0.7],[11;11;11]];
    VOIpNorms     =[2;  2;  3;  2;  2;  2;  2;  3];
    VOIThetaxyz   =[[0;0;0],[0;0;0],[-pi/8;0;0],[0;0;0],[0;0;0],[pi/6;-pi/6;0],[pi/6;pi/6;0],[0;0;0]];
    VOIOffsets    =[[3;-3;0],[3;-3;0],[0;-3;-5],[-4;8;4],[4;8;4],[-2;3;2],[2;3;2],[0;0;0]];
    VOIHistBins   = 0:max(dmaxs)/50:1.4*max(dmaxs);

    % beam specs
    NPtsSphereParam = 7%8%18
    NodeLocs = GetPointsOnUnitSphere1000(NPtsSphereParam);
    NodeLocs = NodeLocs(find(NodeLocs(:,3)>=-0.6),:);% remove bottom (can't go under patient)
    NodeLocs = NodeLocs(find(NodeLocs(:,2)<= 0.6),:);% remove front  (can't go through patient)
    NodeLocs = 80*NodeLocs;% scale to 80cm radius
    NNodes = size(NodeLocs,1)
    NodesAxLims = 85*[-1 1 -1 1 -1 1];
    Collimators = [5; 7.5; 10; 12.5; 15; 20; 25];%; 30; 35; 40; 50; 60];
    Collimators = Collimators/10; % scale to cm
    NCollimators= length(Collimators);
    NBeamlets = 5;
    NPtsPerAx = 50;
    AxLength = 15;

    %====================================================
    % create patient and beams, solve optimization problem, visualize results
    %====================================================
    % get patient
    disp(['Computing patient model...'])
    Patient = PatientCreate();
    Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                          VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                          VOIVoxSizes,VOISampFactors,VOIHistBins,...
                          VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets);
    Patient = PatientRotateTranslate(Patient,PatientThetaxyz,PatientOffset);

    figure
    PatientPlot(Patient)

    % get beams
    disp(['Computing beam directions...'])
    Beams = BeamsCreate();
    Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,Patient.VOIs{Skin}.Model.f,Patient.VOIs{Target}.PointCloudBdy);

    PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators);
    PlotPatientBeamDirectionsAndNodes(Patient,Beams);
    %figure,hold on
    %PatientPlot(Patient)
    %[GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength);
    %PlotFewRandomBeamDoseMaps(Beams.Tails,Beams.Heads,Beams.Widths,3,GridPts)

    % compute dose matrices of all VOIs and set upper and lower bounds
    Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs);

    % construct the Linear Program matrices and solve for optimal beam
    % weights using CLEX or ADMM
    Patient = PatientApplyOptimization(Patient);

    % plot optimal beam weights, constraints, and dose volume histograms
    PatientEvaluate(Patient);

return

%====================================================
% Patient Class: mainly cell array of VOIs, names, axis limits
%====================================================
function Patient = PatientCreate()
    Patient.Name     = [];
    Patient.VOINames = [];
    Patient.NumVOIs  = [];
    Patient.VOIs     = [];
    Patient.AxLims   = [];
    Patient.xBeamWeights = [];
return

function Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                               VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                               VOIVoxSizes,VOISampFactors,VOIHistBins,...
                               VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets)
    Patient.Name = PatientName;
    Patient.VOINames = VOINames;
    Patient.NumVOIs =length(VOINames);
    Patient.VOIs = {};
    for i=1:Patient.NumVOIs
        Modi            = ModelInit(ModelCreate(),VOIxyzScales(:,i),VOIpNorms(i));
        VOIi            = VOIInit(VOICreate(),VOINames{i},VOINumbers(i),VOITypes{i},VOIColors{i},VOILineStyles{i},VOIVoxSizes(i),VOISampFactors(i),Modi,VOIHistBins);
        Patient.VOIs{i} = VOIRotateTranslate(VOIi,VOIThetaxyz(:,i),VOIOffsets(:,i));
    end
    Patient.AxLims = PatientAxLims;
return

function Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDoseAMatrix(Patient.VOIs{i},Beams);
        Patient.VOIs{i} = VOISetDoseMinMax(Patient.VOIs{i},dmins(i),dmaxs(i));
    end
return

function Patient = PatientApplyOptimization(Patient)
    % construct LP matrices
    A    = [];dmin = [];dmax = [];
    for i=1:Patient.NumVOIs
        A    = [A;Patient.VOIs{i}.ADose];
        dmin = [dmin;Patient.VOIs{i}.DoseMin];
        dmax = [dmax;Patient.VOIs{i}.DoseMax];
    end

    % compute optimal beam weights
    %%%Patient.xBeamWeights = OptimizeBeamsCPLEX(A,dmin,dmax);
    Patient.xBeamWeights = OptimizeBeamsADMM(A,dmin,dmax);

    % compute resulting dose on each VOI
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDose(Patient.VOIs{i},Patient.xBeamWeights);
    end
return

function PatientEvaluate(Patient)
    figure,stem(Patient.xBeamWeights),title('Beam Weights')
    figure,PatientPlotConstraints(Patient);
    figure,PatientPlotDVH(Patient);
return

function Patient = PatientRotateTranslate(Patient,Thetaxyz,Offset)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIRotateTranslate(Patient.VOIs{i},Thetaxyz,Offset);
    end
return

function Patient = PatientAffineTransform(Patient,A,b)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIAffineTransform(Patient.VOIs{i},A,b);
    end
return

function PatientPlot(Patient)
    hold on
    for i=1:Patient.NumVOIs
        VOIPlot(Patient.VOIs{i},Patient.AxLims);
    end
    title(Patient.Name),legend(Patient.VOINames)
    xlabel('x'),ylabel('y'),zlabel('z')
    view(127.5,30)
    %view(3)
return

function PatientPlotConstraints(Patient)
    nPlotsPerCol = ceil(Patient.NumVOIs/2);
    for i=1:Patient.NumVOIs
        subplot(nPlotsPerCol,2,i),VOIPlotMinMaxConstraints(Patient.VOIs{i});
    end
return

function PatientPlotDVH(Patient)
    hold on,title('DVHs'),grid on, axis([0,max(Patient.VOIs{1}.DVHBins),0,1.1])
    for i=1:Patient.NumVOIs
        [DVHi,Histi] = GetDVH(Patient.VOIs{i}.Dose,Patient.VOIs{i}.DVHBins);
        plot(Patient.VOIs{i}.DVHBins,DVHi,Patient.VOIs{i}.Color);
    end
    legend(Patient.VOINames)
return

function PlotPatientBeamDirectionsAndNodes(Patient,Beams)
    figure,hold on
    PatientPlot(Patient)
    PlotNodes(Beams.NodeLocs,Beams.NodesAxLims);
    PlotBeams(Beams.Tails0,Beams.Heads,1);
    figure,hold on
    PatientPlot(Patient)
    PlotBeams(Beams.Tails,Beams.Heads,1);
return

%====================================================
% VOI Class: has name etc, point cloud model, point cloud, dose matrix, dose specs
%====================================================
function VOI = VOICreate()
    VOI.Name          = [];
    VOI.Number        = [];
    VOI.Type          = [];
    VOI.Color         = [];
    VOI.LineStyle     = [];
    VOI.VoxSize       = [];
    VOI.SubSampFactor = [];
    VOI.Model         = [];
    VOI.PointCloud    = [];
    VOI.PointCloudBdy = [];
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = [];
return

function VOI = VOIInit(VOI,Name,Number,Type,Color,LineStyle,VoxSize,SubSampFactor,Model,HistBins)
    VOI.Name          = Name;
    VOI.Number        = Number;
    VOI.Type          = Type;
    VOI.Color         = Color;
    VOI.LineStyle     = LineStyle;
    VOI.VoxSize       = VoxSize;
    VOI.SubSampFactor = SubSampFactor;
    VOI.Model         = Model;
    VOI.PointCloud    = ModelToPointCloud(Model,VoxSize,SubSampFactor);
    VOI.PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor);
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = HistBins;
return

function VOI = VOIAffineTransform(VOI,A,b)
    VOI.Model         = ModelAffineTransform(VOI.Model,A,b);
    VOI.PointCloud    = PointCloudAffineTransform(VOI.PointCloud,A,b);
    VOI.PointCloudBdy = PointCloudAffineTransform(VOI.PointCloudBdy,A,b);
return

function VOI = VOIRotateTranslate(VOI,Thetaxyz,Offset)
    VOI.Model         = ModelRotateTranslate(VOI.Model,Thetaxyz,Offset);
    VOI.PointCloud    = PointCloudRotateTranslate(VOI.PointCloud,Thetaxyz,Offset);
    VOI.PointCloudBdy = PointCloudRotateTranslate(VOI.PointCloudBdy,Thetaxyz,Offset);
return

function VOIPlot(VOI,AxisLimits)
    Symbol = ['.',VOI.Color];
    PlotCloud(VOI.PointCloudBdy,Symbol,AxisLimits);
return

function VOI = VOIGetDoseAMatrix(VOI,Beams)
    if VOI.Type =='v' % full volume
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloud);
    else              % boundary shell
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloudBdy);
    end
    disp(['NPts-',VOI.Name,'=',num2str(size(VOI.ADose,1))])
return

function VOI = VOISetDoseMinMax(VOI,dmin,dmax)
    if VOI.Type =='v' % full volume
        NPtsVOI = size(VOI.PointCloud,1);
    else              % boundary shell
        NPtsVOI = size(VOI.PointCloudBdy,1);
    end
    VOI.DoseMin = dmin*ones(NPtsVOI,1);
    VOI.DoseMax = dmax*ones(NPtsVOI,1);
return

function VOI = VOIGetDose(VOI,xBeamWeights)
    VOI.Dose = VOI.ADose*xBeamWeights;
return

function VOIPlotMinMaxConstraints(VOI)
    nVox = length(VOI.DoseMin);
    vox  = 1:nVox;
    plot(vox,VOI.Dose,'*b',...
         vox,VOI.DoseMin,'--r',...
         vox,VOI.DoseMax,'--r')
    axis([0 nVox  0  1.2*max(VOI.DoseMax) ])
    legend(VOI.Name)
    xlabel('voxels'),ylabel('dose')
return

function [DVH,Hist] = GetDVH(DosageProfile,Bins);
    Hist = hist(DosageProfile,Bins);
    Hist = Hist/sum(Hist);
    HistCum = cumsum(Hist);
    DVH = 1-HistCum;
    DVH = [1;DVH(1:(length(DVH)-1))'];
return

%====================================================
% Beam Class: has beams, nodes, collimators, dose computation
%====================================================
function Beams = BeamsCreate()
    Beams.NodeLocs    = [];
    Beams.NodesAxLims = [];
    Beams.Collimators = [];
    Beams.NBeamlets   = [];
    Beams.Tails0= [];
    Beams.Tails = [];
    Beams.Heads = [];
    Beams.Widths= [];
    Beams.Nodes = [];
return

function Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,SkinModelf,BdyTarget)
    Beams.NodeLocs    = NodeLocs;
    Beams.NodesAxLims = NodesAxLims;
    Beams.Collimators = Collimators;
    Beams.NBeamlets   = NBeamlets;
    [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets);
    Beams.Tails0= BeamTails0;
    Beams.Tails = BeamTails;
    Beams.Heads = BeamHeads;
    Beams.Widths= BeamWidths;
    Beams.Nodes = BeamNodes;
return

function [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets)
% compute candidate beam directions by firing from each Node, NBeamlets for each collimator size, onto random points on the target boundary
% also calculate the point of entry into the patient (skin) boundary
    NNodes = size(NodeLocs,1);
    NColls = length(Collimators);
    NBdyTar = size(BdyTarget,1);
    NBisections = 10;
    BeamTails0= zeros(NColls*NNodes*NBeamlets,3);
    BeamTails = zeros(NColls*NNodes*NBeamlets,3);
    BeamHeads = zeros(NColls*NNodes*NBeamlets,3);
    BeamWidths= zeros(NColls*NNodes*NBeamlets,1);
    BeamNodes = zeros(NColls*NNodes*NBeamlets,1);
    % for each node
    for i=1:NNodes
        % make that node the tail for all collimators and beamlets
        ii     = (i-1)*NColls*NBeamlets;
        iiPlus = ii +  NColls*NBeamlets;
        BeamTails0(ii+1:iiPlus,:) = repmat(NodeLocs(i,:),NColls*NBeamlets,1);
        BeamNodes(ii+1:iiPlus,:)  = repmat(i,NColls*NBeamlets,1);
        % for each collimator size
        for j=1:NColls
            % get NBeamlets random heads on target boundary
            jj     = (j-1)*NBeamlets;
            jjPlus = jj  + NBeamlets;
            BeamHeads(ii+jj+1:ii+jjPlus,:) = BdyTarget(GetRandomIndecesNoRepetition(NBeamlets,NBdyTar),:);
            BeamWidths(ii+jj+1:ii+jjPlus,:)= repmat(Collimators(j),NBeamlets,1);
            % for each beamlet
            for k=1:NBeamlets
                % compute intersection with patient surface
                BeamTails(ii+jj+k,:) = IntersectionSegmentLevelset(SkinModelf,BeamTails0(ii+jj+k,:),BeamHeads(ii+jj+k,:),NBisections);
            end
        end
    end
return

function DosageVecs = GetBeamDosageVectors8(Tails,Heads,Widths,PointCloud)%,Collimators)
% compute beam dose matrix on the given PointCloud, and quantizes it to accuracy 1e-3
    NBeams     = size(Tails,1);
    NPts       = size(PointCloud,1);
    DosageVecs = zeros(NPts,NBeams);
    disp(['Computing fake beam dosage vectors...'])
    tic
    for j=1:NBeams
        BeamRadius = Widths(j)/2;
        [DistOnRay,DistToRay]   = ComputeDistancesOnAxisAndToAxis(PointCloud,Tails(j,:)',Heads(j,:)');
        [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay);
        DosageVecs(SelectIdx,j) = ComputeFakeBeamRolloffAndDiffusion(DistOnRaySelect,DistToRaySelect,BeamRadius);
    end
    DosageVecs = 1e-3 * round(1e3*DosageVecs); % quantize to 1e-3 accuracy, anything smaller gets set to zero
    toc
return

function DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius,BeamDiffusionSigma)
% for single beam, compute fake dose vector as:
% - on-axis build-up then roll-off
% - radial diffusion that broadens a bit with distance along beam direction
    if nargin < 4, BeamDiffusionSigma = 0.2;end
    % [build up then roll off] .* [convolve(rect_aperture,gaussian) = difference of erf's]
    DoseVector =  (DistOnRay+0.02).^(1/8) .* exp(-(DistOnRay+0.02)/15).*...
                  (  erf( (DistToRay+BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) ) - ...
                     erf( (DistToRay-BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) )   );
return

function [DistOnRay,DistToRay] = ComputeDistancesOnAxisAndToAxis(PointCloud,tail,head)
% vectorized computation of cylindrical coordinates along beam direction, for all points in VOI PointCloud,
% used for calculation of VOI dose vector
    NPts        = size(PointCloud,1);
    x0              = tail;
    u0              = ComputeUnitVector(head,tail);
    PointCloudShift = PointCloud-repmat(x0',NPts,1);
    DistOnRay       = PointCloudShift*u0;
    DistToRay       = sqrt( sum(  (PointCloudShift - DistOnRay*u0').^2  ,2)  );
return

function [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay)
% keep only points in the positive direction of the beam from the tail0 and
% with distance no more than 5cm from the axis (since largest collimator is 6cm diameter = 3cm radius)
    SelectIdx       = find( (DistOnRay > 0) & (DistToRay <=5) );
    DistOnRaySelect = DistOnRay(SelectIdx);
    DistToRaySelect = DistToRay(SelectIdx);
return

function u = ComputeUnitVector(head,tail)
    u = (head-tail);
    u = u/norm(u);
return

function IndexRand = GetRandomIndecesNoRepetition(NumSamples,NumTotal)
% gets NumSamples random indexes from index set {1,...,NumTotal}, with no repetition
    IndexPerm = randperm(NumTotal);
    IndexRand = IndexPerm(1:NumSamples);
return

function PointOfInt = IntersectionSegmentLevelset(LevelSetf,tail,head,NBisections)
% finds approximate intersection of segment with levelset of f using bisection
% assumes head is inside level set and tail is outside; crashes if not.
    if LevelSetf(tail(:)) <=1, error('tail already inside sublevel set!'),end
    if LevelSetf(head(:)) > 1, error('head is not inside sublevel set!'),end
    for r=1:NBisections
        PointOfInt = 0.5*(tail+head);
        % if PointOfInt inside the 1-level set
        if LevelSetf(PointOfInt(:)) <=1
            head = PointOfInt; % move away from target
        else
            tail = PointOfInt; % move toward target
        end
    end
return

function PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators)
% plot first the roll off of beam intensity along the center of a beam
% then plot a matrix of plots showing the roll off from the axis for all collimators
    BeamRadius  = 0.75/2;
    DistOnRay=0:0.1:50;
    DistToRay=0;
    DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius);
    figure
    plot(DistOnRay,DoseVector),grid
    title('On axis beam roll off')
    xlabel('on axis distance [cm]'),ylabel('normalized intensity')

    DistOnRay=0:5:40;
    DistToRay=0:0.1:8;
    NColl = length(Collimators);
    figure,
    for j=1:NColl
        Collj = Collimators(j);
        subplot(4,3,j)
        hold on
        for i=1:length(DistOnRay)
            DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay(i),DistToRay,Collj/2);
            plot(DistToRay,DoseVector), grid on
        end
        title(['Radial roll off ',num2str(Collj/2),'cm radius beam'])
        xlabel('off axis distance [cm]'),ylabel('normalized intensity')
    end
return

function PlotBeams(BeamTails,BeamHeads,LineWidth)
% plots dotted lines as the directions of the beams (not the full dose vectors)
    if nargin<3, LineWidth=4; end
    for i=1:size(BeamTails,1)
        h = plot3([BeamTails(i,1);BeamHeads(i,1)],...
              [BeamTails(i,2);BeamHeads(i,2)],...
              [BeamTails(i,3);BeamHeads(i,3)]);
        %set(h,'LineWidth',4);
        set(h,'LineWidth',LineWidth,'LineStyle',':');
    end
return

function PlotFewRandomBeamDoseMaps(Tails,Heads,Widths,nBeamsToPlot,GridPts)
% plot few random beams over the points GridPts - rarely do more than 3-5
% beams because can be really slow for a 100x100x100 grid
    NBeams      = length(Widths);
    Select      = GetRandomIndecesNoRepetition(nBeamsToPlot,NBeams);
    TailsSelect = Tails(Select,:);
    HeadsSelect = Heads(Select,:);
    WidthsSelect= Widths(Select,:)
    DosageVecs  = GetBeamDosageVectors8(TailsSelect,HeadsSelect,WidthsSelect,GridPts);
    DosageVecsThresh = 0.01;
    PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh);
return

function PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh)
% plot the DosageVecs on the grid GridPoints, plotting only values greater
% than DosageVecsThresh
    if nargin < 3, DosageVecsThresh = 0;end
    NDosageVecs = size(DosageVecs,2);
    disp(['Plotting beam dosage vectors...'])
    for j=1:NDosageVecs
        iThresh = find(DosageVecs(:,j) > DosageVecsThresh);
        scatter3(GridPts(iThresh,1),GridPts(iThresh,2),GridPts(iThresh,3),3,DosageVecs(iThresh,j),'filled');
    end
return

function PlotNodes(NodeLocs,NodesAxLims)
    hold on
    plot3(NodeLocs(:,1),NodeLocs(:,2),NodeLocs(:,3),'r.'), grid on
    xlabel('x'),ylabel('y'),zlabel('z')
    axis(NodesAxLims)
    %view(3)
return

%====================================================
% Model Class: for generating point clouds as sublevel sets of scaled/shifted p-norms (1 < p < Inf)
%====================================================
function Model = ModelCreate()
    Model.f = [];
    Model.fGrad    = [];
    Model.AxLims = [];
return

function Model = ModelInit(Model,xyzScale,p)
    A            = diag(1./xyzScale);
    Model.f      = @(x) norm(A*x,p); % scaled p-norm (where 1 < p < Inf)
    Model.fGrad  = @(x) A*(sum((A*x).^p))^((1/p)-1)*(A*x).^(p-1); % grad of scaled p-norm
    Model.AxLims = [xyzScale(1)*[-1 1] xyzScale(2)*[-1 1] xyzScale(3)*[-1 1]]; % range for plotting
return

function PointCloud = ModelToPointCloud(Model,VoxSize,SubSampFactor)
    PointCloud = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
return

function PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor)
    PointCloud    = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
    PointCloudBdy = PointCloudBdyFromfAndfGrad(Model.f,Model.fGrad,PointCloud,VoxSize,SubSampFactor);
return

function Model = ModelRotateTranslate(Model,Thetaxyz,Offset)
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Model  = ModelAffineTransform(Model,RotMtx,Offset);
return

function Model = ModelAffineTransform(Model,A,b)
    Model.f      = fAffineTransform(Model.f,A,b);
    Model.fGrad  = fGradAffineTransform(Model.fGrad,A,b);
    Model.AxLims = AxLimsAffineTransform(Model.AxLims,A,b);
return

%====================================================
% Point Cloud 3D geometry stuff
%====================================================
function [GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength)
    %AxPts = linspace(0,AxLength,NPtsPerAx);
    AxPts = linspace(-AxLength,AxLength,NPtsPerAx);
    [X,Y,Z] = meshgrid(AxPts,AxPts,AxPts);
    GridPts = [X(:),Y(:),Z(:)];
return

function [Cloud,nPtsCloud] = PointCloudFromf(f,AxLim,VoxSize)
% returns point cloud of 1-sublevel set of f, sampled over a box specified
% by AxLim, with granularity VoxSize
    xMin=AxLim(1);xMax=AxLim(2);yMin=AxLim(3);yMax=AxLim(4);zMin=AxLim(5);zMax=AxLim(6);
    [X,Y,Z] = meshgrid([xMin:VoxSize:xMax],[yMin:VoxSize:yMax],[zMin:VoxSize:zMax]);
    GridPts = [X(:),Y(:),Z(:)];
    nPts    = size(GridPts,1);
    InOrOut = zeros(nPts,1);
    for i=1:nPts
        InOrOut(i) = ( f(GridPts(i,:)') <= 1);
    end
    IndexCloudPts = find(InOrOut > 0);
    Cloud         = GridPts(IndexCloudPts,:);
    nPtsCloud     = length(IndexCloudPts);
return

function [CloudBdy,nPtsCloudBdy] = PointCloudBdyFromfAndfGrad(f,fGrad,Cloud,VoxSize,SubSampFactor)
% returns boundary points of point cloud of 1-sublevel set of f,
% assumed to be differentiable with gradient fGrad, also assumed
% be computed with granularity VoxSize, and will be downsampled by factor SubSampFactor
    if nargin < 4, VoxSize = 0.1;end
    if nargin < 5, SubSampFactor = 1; end
    n       = size(Cloud,1);
    InOrOut = zeros(n,1);
    for i=1:SubSampFactor:n
        xi = Cloud(i,:)';
        InOrOut(i) = ( abs(f(xi)-1)  <=  norm(fGrad(xi))*VoxSize );% point on shell
    end
    IndexBdyPts = find( InOrOut > 0 );
    CloudBdy    = Cloud(IndexBdyPts,:);
    nPtsCloudBdy= size(CloudBdy,1);
return

function f = fAffineTransform(f,A,b)
% update f so that its 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    f = @(x) f(AInv*(x-b));
return

function fGrad = fGradAffineTransform(fGrad,A,b)
% update fGrad to correspond to the f whose 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    fGrad = @(x) AInv'*fGrad(AInv*(x-b));
return

function AxLims = AxLimsAffineTransform(AxLims,A,b)
    Vertices = AxLimsToVertices(AxLims);
    Vertices = PointCloudAffineTransform(Vertices,A,b);
    AxLims   = AxLimsFromVertices(Vertices);
return

function AxLims = AxLimsFromVertices(Vertices)
% computes box around set of Vertices sorted row-wise [x1';x2';...;xN'];
    Dimensions = size(Vertices,2);
    AxLims = zeros(1,2*Dimensions);
    for i=1:Dimensions
        AxLims(2*i)  = max(Vertices(:,i));
        AxLims(2*i-1)= min(Vertices(:,i));
    end
return

function Vertices = AxLimsToVertices(AxLims)
    Vertices = GenerateVertices([],0,AxLims);
return

function V = GenerateVertices(V,i,AxLims)
% computes Vertices sorted row-wise [x1';x2';...;xN'] from Matlab axis limits AxLims,
% via recursively doubling up the coordinates matrix, backwards from N to 1,
% adding new coordinate column each step.
    n = length(AxLims)/2;
    i = i+1;
    if i <= n
        V = [AxLims(2*(n-i+1))  *ones(2^(i-1),1),V;
             AxLims(2*(n-i+1)-1)*ones(2^(i-1),1),V];
        V = GenerateVertices(V,i,AxLims);
    else
        V = V;
    end
return

function Cloud = PointCloudAffineTransform(Cloud,A,b)
% Apply A*x+b to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    nPtsCloud = size(Cloud,1);
    Cloud     = Cloud*A' + repmat(b(:)',nPtsCloud,1);
return

function Cloud = PointCloudRotateTranslate(Cloud,Thetaxyz,Offset)
% Apply rotation (Thetaxyz) and Offset to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Cloud = PointCloudAffineTransform(Cloud,RotMtx,Offset(:));
return

function [RotMtx,Rx,Ry,Rz] = AnglesToRotMtx(Thetaxyz)
% Rotation about x, y, and z axes - in that order!
    cx = cos(Thetaxyz(1)); sx = sin(Thetaxyz(1));
    cy = cos(Thetaxyz(2)); sy = sin(Thetaxyz(2));
    cz = cos(Thetaxyz(3)); sz = sin(Thetaxyz(3));
    Rx  = [1    0   0;
           0    cx -sx;
           0    sx  cx];
    Ry  = [cy   0   sy;
           0    1   0;
          -sy   0   cy];
    Rz  = [cz  -sz  0;
           sz   cz  0;
           0    0   1];
    RotMtx = Rz*Ry*Rx;
return

function PlotCloud(CloudGridPts,Symbol,AxPts)
    plot3(CloudGridPts(:,1),CloudGridPts(:,2),CloudGridPts(:,3),Symbol);
    AxMin = min(AxPts); AxMax = max(AxPts);
    axis([AxMin,AxMax,AxMin,AxMax,AxMin,AxMax])
    grid on
return

function Points = GetPointsOnUnitSphere1000(N)
% get evenly distributed 1000 points on UNIT sphere
% taken off the web
    if nargin <1
        N=14;
    end
    Beta = 0.5*pi/N;
    Points = [];
    % line segment length
    A = 2*sin(Beta/2);
    % endcap
    Points = [Points;[0 0 1]];
    Points = [Points;[0 0 -1]];
    % rings
    for i=1:N
      R = sin(i*Beta);
      Z = cos(i*Beta);
      M = round(R*2*pi/A);
      for j=0:M-1
          Alpha = j/M * 2 * pi;
          X = cos(Alpha)*R;
          Y = sin(Alpha)*R;
          Points = [Points;[X Y Z]];
          if i~=N
              Points = [Points;[X Y -Z]];
          end
      end
    end
return

%====================================================
% Optimization functions: CPLEX and ADMM
%====================================================
function xBmWtsStar = OptimizeBeamsCPLEX(AAll,dMin,dMax);
% solve using CPLEX
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    INVoxAll = speye(NVoxAll);
    ZNVoxAll = sparse(NVoxAll,NVoxAll);
    ZNVoxAllNBeams = sparse(NVoxAll,NBeams);
    AEq = [AAll, -INVoxAll, ZNVoxAll];
    AEq = sparse(AEq);
    bEq = [zeros(NVoxAll,1)];
    AIneq = [ZNVoxAllNBeams, -INVoxAll, -INVoxAll;
             ZNVoxAllNBeams,  INVoxAll, -INVoxAll];
    AIneq = sparse(AIneq);
    bIneq = [-dMin;dMax];
    lb = zeros(NBeams+NVoxAll+NVoxAll,1);
    ub = [300*ones(NBeams,1);1e6*ones(2*NVoxAll,1)];
    cLP = [zeros(NBeams,1);zeros(NVoxAll,1);ones(NVoxAll,1)];
    disp('Solving optimization with CPLEX ...')
    tic
    options = cplexoptimset('Simplex','on');
    [xStar,Obj,exitflag,output,lambda] = ...
    cplexlp(cLP',AIneq,bIneq,AEq,bEq,lb,ub,[],options); %, x0);
    exitflag = exitflag
    xBmWtsStar = xStar(1:NBeams);
    fstar = Obj
    toc
return

function xBmWtsStar = OptimizeBeamsADMM(AAll,dMin,dMax);
% solve using ADMM following Stanford EE364b final exam problem (Boyd&Chu)
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    tic
    disp(' ')
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    disp('doing ADMM as in EE364b 2011 Final (Stephen Boyd, Eric Chu)')
    disp('computing pseudo-inverse of [A;I]...')
    tic
    AAllI    = [AAll;eye(NBeams)];
    %AAllIPinv = pinv(AAllI);
    Delta = pinv(speye(NBeams)+AAll'*AAll);
    save('Delta','Delta');
    %load Delta
    toc
    ud  = zeros(NVoxAll,1);
    ux  = zeros(NBeams,1);
    dose = zeros(NVoxAll,1);
    xBm  = zeros(NBeams,1);
    z    = zeros(NBeams,1);
    zOld = z;
    disp('doing ADMM iterations')
    MaxIters = 300
    rho     = 1.25;%0.5
    mu      = 10;%1.5
    tauIncr = 2;%1.1
    tauDecr = 1/tauIncr;
    rNorm = zeros(MaxIters,1);
    sNorm = zeros(MaxIters,1);
    rhos  = zeros(MaxIters,1);
    for i=1:MaxIters

        % do ADMM update
        AAllzOld = AAll*zOld;
        dose = DoubleHingeProx(dMin,dMax,rho,AAllzOld-(ud/rho));
        xBm  = min(max(z-(ux/rho),0),300);
        z    = Delta*(  AAll'*(dose+(ud/rho))  +  (xBm+(ux/rho))  );
        AAllz= AAll*z;
        ux   = ux + rho*(xBm-z);
        ud   = ud + rho*(dose-AAllz);

        % compute primal and dual residuals for stopping criterion and rho updating
        r        = [dose;xBm] - [AAllz;z];
        s        = rho*([AAllz-AAllzOld;z-zOld]);
        rNorm(i) = norm(r,2);
        sNorm(i) = norm(s,2);

        % update z
        zOld = z;

        % "adaptive" rho update (doesn't always work ==> safer to switch off)
        rhos(i) = rho;
%         if     (rNorm(i) > mu*sNorm(i))
%             rho = tauIncr*rho
%         elseif (rNorm(i) < (1/mu)*sNorm(i))
%             rho = tauDecr*rho
%         end

    end
    disp('done!')
    xBmWtsStar = xBm;
    toc

    figure
    subplot(3,1,1),plot(1:MaxIters,log10(rNorm)),grid,ylabel('log10(rNorm)')
    subplot(3,1,2),plot(1:MaxIters,log10(sNorm)),grid,ylabel('log10(sNorm)')
    subplot(3,1,3),plot(1:MaxIters,log10(rhos)),grid,ylabel('log10(rho)')
return

function xProx = DoubleHingeProx(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xProx = 1/rho + x - max(x-(aa-1/rho),0) + max(x-aa,0) - max(x-bb,0) + max(x-(bb+1/rho),0);
return

function xProx = DoubleHingeProx2(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xTemp = SoftThresh(1/2/rho,x-(aa-1/2/rho)) + aa;
    xProx = SoftThresh(1/2/rho,xTemp-(bb+1/2/rho))+bb;
return

function xST = SoftThresh(k,x)
    if min(k) <= 0, error('k <=0'),end
    xST = max(x-k,0) - max(-x-k,0);
return

function y = DoubleHinge(xMin,xMax,x)
    if min(xMax-xMin)<0, error('xMin > xMax!'),end
    y = max(xMin-x,0) + max(x-xMax,0);
return


function d=dsphere(p,xc,yc,zc,r)

%   Copyright (C) 2004-2006 Per-Olof Persson. See COPYRIGHT.TXT for details.

d=sqrt((p(:,1)-xc).^2+(p(:,2)-yc).^2+(p(:,3)-zc).^2)-r;


%% Setting of the problem
global s
pde = fracLapdata9;
% pde = checkboarddata;
pde.L = 1;
option.maxIt = 4;
option.maxN = 1e6;
option.elemType = 'P1P2';
option.solver = 'mg';
option.gNquadorder = 4;
option.tol = 1e-6;
[node,elem] = squaremesh([-1 1 -1 1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');

%% s = 0.2
s = 0.2;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.4
s = 0.4;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
femfracLap(node,elem,pde,bdFlag,option);


% Image construction from overlapping patches
function [result] = overlap_add(patches, img_size, grid)

result = zeros(img_size);
weight = zeros(img_size);

for i = 1:size(grid, 3)
    patch = reshape(patches(:, i), size(grid, 1), size(grid, 2));
    result(grid(:, :, i)) = result(grid(:, :, i)) + patch;
    weight(grid(:, :, i)) = weight(grid(:, :, i)) + 1;
end

I = logical(weight);
result(I) = result(I) ./ weight(I);


function a = dec2bin(d)
i=1;
a=zeros(1,65535);
while d >= 2
    r=rem(d,2);
    if r==1
        a(i)=1;
    else
        a(i)=0;
    end
    i=i+1;
    d=floor(d/2);
end
if d == 2
    a(i) = 0;
else
    a(i) = 1;
end
x=[a(16) a(15) a(14) a(13) a(12) a(11) a(10) a(9) a(8) a(7) a(6) a(5) a(4) a(3) a(2) a(1)];


function i4row_sort2_d_test ( )

%*****************************************************************************80
%
%% I4ROW_SORT2_D_TEST tests I4ROW_SORT2_D;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 6;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4ROW_SORT2_D_TEST\n' );
  fprintf ( 1, '  For a rectangular integer matrix:\n' );
  fprintf ( 1, '  I4ROW_SORT2_D sorts the elements of the rows.\n' );

  seed = 123456789;

  for i = 1 : m
    for j = 1 : n
      a(i,j) = 10 * i + j;
    end
  end

  i4mat_print ( m, n, a, '  The original matrix:' );

  [ a, seed ] = i4mat_perm2_uniform ( m, n, a, seed );

  i4mat_print ( m, n, a, '  The matrix, permuted by I4MAT_PERM2_UNIFORM:' );

  a = i4row_sort_d ( m, n, a );

  i4mat_print ( m, n, a, '  The row-sorted matrix:' );

  a = i4row_sort2_d ( m, n, a );

  i4mat_print ( m, n, a, '  The element-sorted row-sorted matrix:' );

  return
end


% test_remove_vertex_from_tri.m

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 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/>.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Remove opposite vertices from a cube
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% create a surface mesh that is a cube
x = [
    0 0 0
    0 0 1
    0 1 0
    0 1 1
    1 0 0
    1 0 1
    1 1 0
    1 1 1
    ];

tri = [
    1     3     5
    2     1     5
    2     3     1
    4     3     2
    6     2     5
    4     2     6
    7     6     5
    3     7     5
    4     7     3
    8     6     7
    4     8     7
    4     6     8
    ];

% plot mesh
hold off
trisurf(tri, x(:,1), x(:,2), x(:,3), ones(1, size(x, 1)))
axis equal

% vertices to remove
idx = [2 7];

% plot vertices that are going to be removed
hold on
plot3(x(idx, 1), x(idx, 2), x(idx, 3), 'ro')

% remove vertices
[tri2, x2] = remove_vertex_from_tri(tri, x, idx);

% plot result
hold off
trisurf(tri2, x2(:,1), x2(:,2), x2(:,3), ones(1, size(x2, 1)))
hold on
plot3(x(idx, 1), x(idx, 2), x(idx, 3), 'ro')
axis equal

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Remove neighbour vertices from a cube
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% create a surface mesh that is a cube
x = [
    0 0 0
    0 0 1
    0 1 0
    0 1 1
    1 0 0
    1 0 1
    1 1 0
    1 1 1
    ];

tri = [
    1     3     5
    2     1     5
    2     3     1
    4     3     2
    6     2     5
    4     2     6
    7     6     5
    3     7     5
    4     7     3
    8     6     7
    4     8     7
    4     6     8
    ];

% plot mesh
hold off
trisurf(tri, x(:,1), x(:,2), x(:,3), ones(1, size(x, 1)))
axis equal

% vertices to remove
idx = [1 2];

% plot vertices that are going to be removed
hold on
plot3(x(idx, 1), x(idx, 2), x(idx, 3), 'ro')

% remove vertices
[tri2, x2] = remove_vertex_from_tri(tri, x, idx);

% plot result
hold off
trisurf(tri2, x2(:,1), x2(:,2), x2(:,3), ones(1, size(x2, 1)))
hold on
plot3(x(idx, 1), x(idx, 2), x(idx, 3), 'ro')
axis equal


function pass = test_sample( )
% Test spherefun sample() command

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

% Function to test
f = spherefun(@(x,y,z) sin(pi*x.*y));

% Ensure the matrix of sampled values is correct.
[m,n] = length(f);
[nn,mm] = size(sample(f));
pass(1) = (m == mm) && (n == nn);

% Sample on fixed grids of various sizes to make sure the right size output
% is given.
m = 120;
n = 121;
[nn,mm] = size(sample(f, m, n));
pass(2) = (m == mm) && (n == nn);

m = 121;
n = 120;
[nn,mm] = size(sample(f, m, n));
pass(3) = (m == mm) && (n == nn);

% Check samples are correct.
% m and n even
m = 30;
n = 20;
[lam,th] = meshgrid(trigpts(m, [-pi, pi]), linspace(0, pi, n));
F = f(lam, th);
G = sample(f, m, n);
pass(4) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, n);
G = U * D * V.';
pass(5) = norm(F(:) - G(:), inf) < tol;

% m even and n odd
m = 30;
n = 21;
[lam, th] = meshgrid(trigpts(m, [-pi, pi]), linspace(0, pi, n));
F = f(lam, th);
G = sample(f, m, n);
pass(6) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, n);
G = U * D * V.';
pass(7) = norm(F(:) - G(:), inf) < tol;

% m odd and n even
m = 31;
n = 20;
[lam, th] = meshgrid(trigpts(m, [-pi, pi]), linspace(0, pi, n));
F = f(lam, th);
G = sample(f, m, n);
pass(8) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, n);
G = U * D * V.';
pass(9) = norm(F(:) - G(:), inf) < tol;

% m odd and n odd
m = 31;
n= 21;
[lam, th] = meshgrid(trigpts(m, [-pi, pi]), linspace(0, pi, n));
F = f(lam, th);
G = sample(f, m, n);
pass(10) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, n);
G = U * D * V.';
pass(11) = norm(F(:) - G(:), inf) < tol;

% Sample should return all ones for the function 1.
f = spherefun(@(x,y,z) 1 + 0*x);
F = sample(f, 128, 128);
pass(12) = norm(F(:) - 1, inf) < tol;

% Check that errors are caught
try
    F = sample(f, 0, 20);
    pass(13) = false;
catch ME
    pass(13) = strcmp(ME.identifier, 'CHEBFUN:SPHEREFUN:sample:inputs');
end

try
    F = sample(f, 20, 0);
    pass(14) = false;
catch ME
    pass(14) = strcmp(ME.identifier, 'CHEBFUN:SPHEREFUN:sample:inputs');
end

end


function ui = ic_spike ( x )

%*****************************************************************************80
%
%% IC_SPIKE evaluates the initial condition for a spike function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the node coordinates.
%
%    Output, real UI(*), the value of the initial condition at each node.
%
  ui = max ( 1.0 - 3.0 * abs ( x ), 0.0 );

  return
end


function prod = tensor_array_innerprod( X1, X2 )

prod = 0;
N = length(X1);
for i = 1:N
    prod = prod + innerprod(X1{i},X2{i});
end

end


function f = rcnn_scale_features(f, feat_norm_mean)
% My initial experiments were conducted on features with an average norm
% very close to 20. Using those features, I determined a good range of SVM
% C values to cross-validate over. Features from different layers end up
% have very different norms. We rescale all features to have an average norm
% of 20 (why 20? simply so that I can use the range of C values found in my
% initial experiments), to make the same search range for C reasonable
% regardless of whether these are pool5, fc6, or fc7 features. This strategy
% seems to work well. In practice, the optimal value for C ends up being the
% same across all features.
target_norm = 20;
f = f .* (target_norm / feat_norm_mean);


function [D]=logic2levelset(varargin)

% function [D]=logic2levelset(logicInside,voxelSize,voxelSizeResample)
% ------------------------------------------------------------------------
% This function converts the logic image logicInside to a level set image.
% The level set image intensities define the distance of the voxels to the
% boundaries of the logic image.
%
%
% Change log:
%
% ------------------------------------------------------------------------
%%

switch nargin
    case 1
        logicInside=varargin{1};
        voxelSize=[];
        voxelSizeResample=[];
    case 2
        logicInside=varargin{1};
        voxelSize=varargin{2};
        voxelSizeResample=[];
    case 3
        logicInside=varargin{1};
        voxelSize=varargin{2};
        voxelSizeResample=varargin{3};
end

if isempty(voxelSize)
    voxelSize=ones(1,3);
end

if isempty(voxelSizeResample)
    voxelSizeResample=mean(voxelSize);
end

if numel(voxelSizeResample)==1
    voxelSizeResample=voxelSizeResample*ones(1,3);
end

%%

siz=size(logicInside);
%Resample input image isotropically (isotropic voxels)
if ~max(voxelSize-voxelSizeResample)< max(eps(voxelSize))
    resampleOn=1;
    [logicInside]=imageResample(logicInside,voxelSize,voxelSizeResample);
    logicInside=logicInside>0; %Forces logic and fixes potential NaN's
    scaleFactor=voxelSizeResample(1);
else
    resampleOn=0;
    scaleFactor=voxelSize(1);
end
logicInside=logicInside>0; %Force to be a logic (in case resampling altered it)

%Remove interior from logic
try
    logicOn = bwmorph3(logicInside,'remove'); %New in R2018a
catch
    logicOn=logicRemoveInterior(logicInside); %GIBBON alterative
end

%Do distance transform on isotropic image
D = double(bwdist(logicOn,'euclidean')); %Compute distance
D(logicInside)=-D(logicInside); %Negate distance inside
D=D.*scaleFactor; %Scale by voxel size

if resampleOn==1
    sizNew=size(D); %Size of distance data in resampled state
    [J,I,K]=meshgrid(1:1:siz(2),1:1:siz(1),1:1:siz(3)); %Image coordinate grid of original image
    [X,Y,Z]=im2cart(I,J,K,voxelSize); %Spatial coordinate of original image grid
    [I,J,K]=cart2im(X,Y,Z,voxelSizeResample); %Image coordinates of original grid in resampled image
    I=round(I); J=round(J); K=round(K); %Rounding
    IND=reshape(sub2indn(sizNew,[I(:) J(:) K(:)],1),size(I)); %Convert to linear indices
    D=D(IND); %Override D to be data sampled at original image points
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


% EKF/UKF toolbox for Matlab 7.x
% Version 1.3, August 12, 2011
%
% Copyright (C) 2005-2011 Simo S�rkk�, <simo.sarkka@hut.fi>
%               2007-2011 Jouni Hartikainen <jmjharti@cc.hut.fi>
%               2010-2011 Arno Solin <arno.solin@tkk.fi>
% History:
%   12.08.2011 JH & AS & SS Updated to version 1.3
%   04.09.2007 JH & SS Updated for version 1.1
%   06.08.2007 JH Updated for version 1.0
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
%
%
% Kalman filtering
%   KF_PREDICT    Perform Kalman Filter prediction step
%   KF_UPDATE     Kalman Filter update step
%   KF_LHOOD      Kalman Filter measurement likelihood
%   RTS_SMOOTH    Rauch-Tung-Striebel Smoother
%   TF_SMOOTH     Smoother based on combination of two Kalman filters
%
% Extended Kalman filtering
%   EKF_PREDICT1  1st order Extended Kalman Filter prediction step
%   EKF_UPDATE1   1st order Extended Kalman Filter update step
%   EKF_PREDICT2  2nd order Extended Kalman Filter prediction step
%   EKF_UPDATE2   2nd order Extended Kalman Filter update step
%   ERTS_SMOOTH1  1st order Extended RTS Smoother
%   ETF_SMOOTH1   Smoother based on two 1. order extended Kalman filters
%
% Nonlinear transform based filtering
%   UT_WEIGHTS    Generate weights for sigma points using the summation form
%   UT_MWEIGTS    Generate weights for sigma points using the matrix form
%   UT_SIGMAS     Generate Sigma Points for Unscented Transformation
%   UT_TRANSFORM  Makes the Unscented Transformation of x and y
%   UKF_PREDICT1  Nonaugmented UKF prediction step
%   UKF_UPDATE1   Nonaugmented UKF update step
%   UKF_PREDICT2  Augmented (state and process noise) UKF prediction step
%   UKF_UPDATE2   Augmented (state and measurement noise) UKF update step
%   UKF_PREDICT3  Augmented (state, process and measurement noise) UKF prediction step
%   UKF_UPDATE3   Augmented (state, process and measurement noise) UKF update step
%   URTS_SMOOTH1  Nonaugmented unscented RTS-smoother
%   URTS_SMOOTH2  Augmented unscented RTS-smoother
%   UTF_SMOOTH    Smoother based on combination of two unscented Kalman filters
%   GH_TRANSFORM  Gauss-Hermite transform of random variables
%   GHKF_PREDICT  Gauss-Hermite Kalman filter prediction step
%   GHKF_UPDATE   Gauss-Hermite Kalman filter update step
%   GHRTS_SMOOTH  Additive form Gauss-Hermite Rauch-Tung-Striebel smoother
%   CKF_TRANSFORM Cubature Kalman filter transform of random variables
%   CKF_PREDICT   Cubature Kalman filter prediction step
%   CKF_UPDATE    Cubature Kalman filter update step
%   CRTS_SMOOTH - Additive form cubature Rauch-Tung-Striebel smoother
%
% Multiple Model Filtering
%   IMM_PREDICT   IMM filter prediction step
%   IMM_UPDATE    IMM filter update step
%   IMM_SMOOTH    IMM smoothing
%   EIMM_PREDICT  IMM-EKF filter prediction step
%   EIMM_UPDATE   IMM-EKF filter update step
%   EIMM_SMOOTH   IMM-EKF smoothing
%   UIMM_PREDICT  IMM-UKF filter prediction step
%   UIMM_UPDATE   IMM-UKF filter update step
%   UIMM_SMOOTH   IMM-UKF smoothing
%
%
% Misc.
%   GAUSS_PDF     Multivariate Gaussian PDF
%   GAUSS_RND     Multivariate Gaussian random variables
%   LTI_INT       Integrate LTI ODE with Gaussian Noise
%   LTI_DISC      Discretize LTI ODE with Gaussian Noise
%   RK4           Runge-Kutta integration
%   DER_CHECK     Check derivatives using finite differences
%   SCHOL         Positive semidefinite matrix Cholesky factorization
%   RESAMPSTR     Stratified resampling
%
% /DEMOS/
%
%   /KF_CWPA_DEMO/
%      KF_CWPA_DEMO       CWPA model demonstration with Kalman filter
%
%   /EKF_SINE_DEMO/
%      EKF_SINE_F         Dynamic model function (needed by the augmented UKF)
%      EKF_SINE_H         Measurement model function
%      EKF_SINE_DH_DX     1st order derivative of the measurement model
%      EKF_SINE_D2H_DX2   2nd order derivative of the measurement model
%      EKF_SINE_DEMO      Random Sine Signal demonstration
%
%   /UNGM_DEMO/
%      UNGM_F             Dynamic model function
%      UNGM_DF_DX         1st order derivative of the dynamic model
%      UNGM_D2F_DX2       2nd order derivative of the dynamic model (not used)
%      UNGM_H             Measurement model function
%      UNGM_DH_DX         1st order derivative of the measurement model
%      UNGM_D2H_DX2       2nd order derivative of the measurement model (not used)
%      UNGM_DEMO          UNGM model demonstration
%
%   /BOT_DEMO/
%      BOT_H              Measurement model function
%      BOT_DH_DX          1st order derivative of the measurement model
%      BOT_D2H_DX2        2nd order derivative of the measurement model
%      BOT_DEMO_ALL       BOT demo with EKF and UKF
%      EKFS_BOT_DEMO      BOT demo with EKF
%      UKFS_BOT_DEMO      BOT demo with UKF
%      GHKFS_BOT_DEMO     BOT demo with GHKF
%      CKFS_BOT_DEMO      BOT demo with CKF
%
%   /REENTRY_DEMO/
%      REENTRY_F          Dynamic model function
%      REENTRY_DF         Derivative of the dynamic model
%      REENTRY_H          Measurement model function
%      REENTRY_DH         Derivative of the measurement model
%      REENTRY_IF         Inverse prediction of the dynamic model
%      REENTRY_COND       Generates condition numbers for simulation data
%      MAKE_REENTRY_DATA  Generates the simulation data for reentry dynamics
%      REENTRY_DEMO       Reentry Vehicle Tracking demonstration
%
%   /IMM_DEMO/
%      IMM_DEMO           Tracking a Target with Simple Manouvers demonstration
%
%   /EIMM_DEMO/
%      F_TURN             Dynamic model function for the coordinated turn model
%      F_TURN_DX          Jacobian of the coordinated turn model's dynamic model
%      F_TURN_INV         Inverse dynamics of the coordinated turn model
%      CT_DEMO            Coordinated Turn Model demonstration
%      BOT_H              Measurement model function
%      BOT_DH_DX          1st order derivative of the measurement model
%      BOT_D2H_DX2        2nd order derivative of the measurement model
%      BOTM_DEMO          Bearings Only Tracking of a Manouvering Target Demonstration
%
% Demos currently included in the toolbox, but not documented:
%
% /KF_SINE_DEMO/
%      KF_SINE_DEMO       Sine signal demonstration with Kalman filter


function [forecast_estimates]=olsforecast(data_endo_a,data_exo_p,Fperiods,betahat,Bhat,sigmahat,n,m,p,k,const,Fband)

% function [forecast_estimates]=olsforecast(data_endo_a,data_exo_p,Fperiods,betahat,Bhat,sigmahat,n,m,p,k,const,Fband)
% computes unconditional forecast values (point estimates and confidence bands) for the OLS VAR model
% inputs:  - matrix 'data_endo_a': matrix of pre-forecast endogenous data
%          - matrix 'data_exo_p': predicted values for the exogenous variables over the forecast periods
%          - integer 'Fperiods': number of forecast periods
%          - vector 'betahat': OLS VAR coefficients in vectorised form (defined in 1.1.15)
%          - matrix 'Bhat': OLS VAR coefficients, in non vectorised form (defined in 1.1.9)
%          - matrix 'sigmahat': OLS VAR variance-covariance matrix of residuals (defined in 1.1.10)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'const': 0-1 value to determine if a constant is included in the model
%          - scalar 'Fband': confidence level for forecasts
% outputs: - cell 'forecast_estimates': lower bound, point estimates, and upper bound for the unconditional forecasts

% this function implements the chain rule of forecast, desibed p38 of the technical guide

% point estimates for forecasts are obtained using the chain rule for forecast in Lutkepohl (1991), equation (2.2.3) p 29
% approximate confidence interval are obtained from Lutkepohl (1991), formula (3.5.15) p 89, based on the sample mean squared error matrix (2.2.10)

% generate the matrix of predicted exogenous variables
% if the constant has been retained, augment the matrices of exogenous with a column of ones:
if const==1
data_exo_p=[ones(Fperiods,1) data_exo_p];
% if no constant was included, do nothing
else
end

% then generate the point estimates
% recover the lagged endogenous required to produce the forecasts
temp=data_endo_a(end-p+1:end,:);
% repeat the process for periods T+1 to T+h
for ii=1:Fperiods
% Define the matrix of regressors X by using lagX on temp; retain only the last row of the matrix
   % if no exogenous variable is present at all in the model (neither constant nor other exogenous), define X from the endogenous variables only
   if isempty(data_exo_p)
   X=bear.lagx(temp,p-1);
   X=X(end,:);
   % if there are exogenous vaiables, concatenate them next to the endogenous
   else
   X=bear.lagx(temp,p-1);
   X=[X(end,:) data_exo_p(ii,:)];
   end
% obtain predicted value for T+ii
yp=X*Bhat;
% concatenate the transpose of yp to the top of temp
temp=[temp;yp];
% repeat until values are obtained for T+h
end

% finally, generate the confidence bands
% this requires to estimate the forecast error matrix sigmaf for each forecast period
% to do so, it is first necessary to obtain irfs
[irfmatrix,~]=bear.irfsim(betahat,eye(n),n,m,p,k,Fperiods);
% then initiate sigmaf for period 1
sigmaf(:,:,1)=irfmatrix(:,:,1)*sigmahat*irfmatrix(:,:,1)';
% and increment for each forecast period
for ii=2:Fperiods
sigmaf(:,:,ii)=sigmaf(:,:,ii-1)+irfmatrix(:,:,ii)*sigmahat*irfmatrix(:,:,ii)';
end
% with the sigmaf series, it is possible to compute the confidence intervals
% first compute the percentile of the normal distribution corresponding to size of the confidence interval
c_low=norminv((1-Fband)/2,0,1);
c_high=norminv(Fband+(1-Fband)/2,0,1);

% finally, create and fill the forecast_estimates cell
forecast_estimates=cell(n,1);
for ii=1:n
% record forecast, point estimate
forecast_estimates{ii,1}(2,:)=temp(p+1:end,ii)';
   % then loop over forecast periods
   for jj=1:Fperiods
   % record forecast, lower bound
   forecast_estimates{ii,1}(1,jj)=forecast_estimates{ii,1}(2,jj)+c_low*sigmaf(ii,ii,jj)^0.5;
   % record forecast, upper bound
   forecast_estimates{ii,1}(3,jj)=forecast_estimates{ii,1}(2,jj)+c_high*sigmaf(ii,ii,jj)^0.5;
   end
end


function [fusion,w0] = qfuser_v6(w,scores,wfuse)

if nargin==0
    test_this();
    return;
end

% block 1
f1 = linear_fuser([],scores.scores);
w1 = wfuse;
[whead,wtail] = splitvec_fh(length(w1));
f1 = f1(whead);

% block 2
modelQ = scores.modelQ;
[q,n1] = size(modelQ);
modelQ = [modelQ;ones(1,n1)];
segQ = scores.segQ;
[q2,n2] = size(segQ);
segQ = [segQ;ones(1,n2)];
assert(q==q2);
q = q + 1;

wq = q*(q+1)/2;
r = AWB_fh(modelQ',segQ,tril_to_symm_fh(q));
[whead,wtail] = splitvec_fh(wq,wtail);
r = r(whead);
w2 = zeros(wq,1);w2(end) = -5;

% block 3
s = AWB_fh(modelQ',segQ,tril_to_symm_fh(q,wtail));
w3 = w2;

% assemble
rs = stack([],r,s);
fusion = scalibration_fh(stack(w,f1,rs));
w0 = [w1;w2;w3];

end

function test_this()

m = 3;
k = 2;
n1 = 4;
n2 = 5;

scores.scores = randn(m,n1*n2);
scores.modelQ = randn(k,n1);
scores.segQ = randn(k,n2);

wfuse = [1,2,3,4]';

[fusion,w0] = qfuser_v6([],scores,wfuse);

test_MV2DF(fusion,w0);

[fusion(w0),linear_fuser(wfuse,scores.scores)]

%fusion(w0)

end


%****************************************************
% Function to calculate sub-band energy(SBC) parameters
% from enframed signal
% modifided on 17 jan 2008
% *********sarikaya paper algorithm***********
% fs samapling rate 8000
%   frame size used 192
%s signal
%
%
%***************************************************
function feature= sbc_2(s,fs)

frames=enframe(s,hamming(192),192);
no_offrames=size(frames);
no_of_frames=no_offrames(:,1);
feature=zeros(no_of_frames,12);
energy=zeros(24,1);

fe_frame=0;  % This counter is used to avoid frames with no energy is a sub band

f=statusbar('Extracting SBC Feature');% Creates status bar

for i=1:no_of_frames
    energy=ones(24);

    f=statusbar((i/no_of_frames),f);% updates status bar
    [coef,len]=wavedec(frames(i,:),6,'db3');
    s_no=cumsum(len);

    %% next two gives the last node in wavelet pacet

    energy(1)=en(coef(1:s_no(1)));

    energy(2)=en(coef((s_no(1)+1):s_no(2)));
   % coef((s_no(1)+1):s_no(2))
            %refer diagram in sarikaya paper
            [coef1,len1]=wavedec(coef(s_no(2):s_no(3)),1,'db3');
            len1=cumsum(len1);
                energy(3)=en(coef1(1:len1(1)) );
                energy(4)=en(coef1((len1(1)+1):len1(2)));

                    [coef2,len2]=wavedec(coef((s_no(3)+1):s_no(4)),2,'db3');
                        len2=cumsum(len2);
                        energy(5)=en(coef2(1:len2(1)));

                        energy(6)=en(coef2( (len2(1)+1):len2(2) ) );
                            [coef3,len3]=wavedec(coef2((len2(2)+1):len2(3)),1,'db3');
                            len3=cumsum(len3);
                                    energy(7)=en(coef3(1:len3(1)));
                                    energy(8)=en(coef3((len3(1)+1):len3(2)));

                % all nodes at level six are over

           [coef4,len4]=wavedec(coef(s_no(4):s_no(5)),2,'db3');
            len4=cumsum(len4);
                        energy(9)=en(coef4(1:len4(1)));
                        energy(10)=en(coef4((len4(1)+1):len4(2)));

                        [coef5,len5]=wavedec(coef4((len4(2)+1):len4(3)),1,'db3');
                        len5=cumsum(len5);
                                    energy(11)=en(coef5(1:len5(1)));
                                    energy(12)=en(coef5((len5(1)+1):len5(2)));

            [coef7,len7]=wavedec(coef(s_no(5):s_no(6)),3,'db3');
            len7=cumsum(len7);
                       energy(13)=en(coef7(1:len7(1)));
                       energy(14)=en(coef7((len7(1)+1):len7(2)));

                        [coef8,len8]=wavedec(coef7((len7(2)+1):len7(3)),1,'db3');
                        len8=cumsum(len8);
                           energy(15)=en(coef8(1:len8(1)));
                           energy(16)=en(coef8((len8(1)+1):len8(2)));

             [coef9,len9]=wavedec(coef7((len7(3)+1):len7(4)),2,'db3');
              len9=cumsum(len9);
                        energy(17)=en(coef9(1:len9(1)));
                        energy(18)=en(coef9((len9(1)+1):len9(2)));

                 energy(19)=en(coef9((len9(2)+1):len9(3)));

  [coef10,len10]=wavedec(coef((s_no(6)+1):s_no(7)),3,'db3');
  len10=cumsum(len10);

           energy(20)=en(coef10(1:len10(1)));
           energy(21)=en(coef10((len10(1)+1):len10(2)));
           energy(22)=en(coef10((len10(2)+1):len10(3)));

     [coef11,len11]=wavedec(coef10((len(3)+1):len10(4)),1,'db3');
     len11=cumsum(len11);
            energy(23)=en(coef11(1:len11(1)));
            energy(24)=en(coef11((len11(1)+1):len11(2)));

% Taking 12 filter bank equalent
%
%f=rdct(feature); function not woring
% 19 jan DCT modified file in dessai cmtr
if all(energy>0)
  fe_frame=fe_frame+1;
    log_en=log(energy.*1E+06);

   for j=1:12
   for k=1:24

        feature(fe_frame,j)=feature(fe_frame,j)+log_en(k)*cos((j*(k-0.5)*pi)/24);
   end
   end
 % feature=abs(feature); this needs to be checked
end
end
 delete(statusbar)


classdef GrEA < ALGORITHM
% <many> <real/integer/label/binary/permutation>
% Grid-based evolutionary algorithm
% div --- --- The number of divisions in each objective

%------------------------------- Reference --------------------------------
% S. Yang, M. Li, X. Liu, and J. Zheng, A grid-based evolutionary algorithm
% for many-objective optimization, IEEE Transactions on Evolutionary
% Computation, 2013, 17(5): 721-736.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            Div = [0 45 15 10 9 9 8 8 10 12];
            div = Algorithm.ParameterSet(Div(min(Problem.M,10)));

            %% Generate random population
            Population = Problem.Initialization();

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = MatingSelection(Population.objs,div);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Population = EnvironmentalSelection([Population,Offspring],Problem.N,div);
            end
        end
    end
end


function [clusters,subclusters] = cluster_princomp(clusters,varargin)
% function [clusters,subclusters] = cluster_princomp(clusters,[behavioral score vector],[corr flag],[plotflag],[locflag])
%
% ALSO TRY: subcluster_montage(subclusters{1}) % to plot the output
%
% clusters is structure of clusters from tor_extract_rois.m
% behavioral vector is row vector of behavioral or other scores to correlate
% corr flag:  *1 = work on correlations among voxels, 2 = work on covariance
% plotflag:   *1 = yes, 0 = no.  plots.
% locflag:    1 yes, *0 no; add XYZ voxel locations (scaled) to data submitted to clustering
%             pushes voxels closer in space to be classified in the same cluster
%
% try this to test the program on random data:
% cl(1).all_data = randn(23,30);cl(1).numVox = 30;cl = cluster_princomp(cl,EXPT.behavior,1,1);
% cl(1).all_data(:,1:10) = cl(1).all_data(:,1:10) + 10; cl = cluster_princomp(cl,EXPT.behavior,1,1);
% cl(1).all_data(:,25:30) = cl(1).all_data(:,25:30) + repmat((EXPT.behavior .* 3)',1,6);
% cl(1).all_data(:,21:24) = cl(1).all_data(:,21:24) + repmat((1:23)',1,4);
% cl = cluster_princomp(cl,EXPT.behavior,1,1);
% mean-center everything now:
% cl.PCA = []; cl.all_data - cl.all_data - repmat(mean(cl.all_data),size(cl.all_data,1),1);
% cl = cluster_princomp(cl,EXPT.behavior,1,1);
% add another correlated group:
% cl.all_data(:,1:5) = cl.all_data(:,1:5) + repmat(rand(23,1)*5,1,5);
% cl = cluster_princomp(cl,EXPT.behavior,1,1);
%
% if component scores are used and correlated with behavior, this means that the subjects
% tend to show the behavioral effect who also show the pattern associated with comp. x.
% this may mean high on a number of voxels, or high on some and low on others.
% the weights may be used to interpret what the components mean, and this can be done
% graphically.
%
% t-tests on component scores have ambiguous interpretations, because a high t-score
% may indicate negative values or close-to-zero values on some voxels.
% a component could have the interpretation, "high on this component means high on V1
% and low on V2."
%
% classifying voxels is done using cluster analysis (hierarchical, centroid linkage)
% on the voxels (observations) using the PCA weights (eigenvectors) as variables.
% This lets the clustering algorithm work in the reduced variable space with dimensionality
% equal to the number of components.
% The max number of clusters is restricted based on the gradient of the eigenvalues in the PCA
% maxclusters = 1 + the number of eigenvalues with gradient at least 20% of the initial drop
% from 1 to 2 eigenvalues.
%
% Requires clustering library in Matlab.
% Robust option also uses the robust PCA algorithm RAPCA,
% created by:
% Hubert, M., Rousseeuw, P.J., Verboven, S. (2002),
%  "A fast method for robust principal components with applications to chemometrics", by Mia Hubert, Peter J. Rousseeuw,
%  Chemometrics and Intelligent Laboratory Systems, 60, 101-111.
%
%

corrflag = 1; plotflag = 1; robustflag = 1; locflag = 0;
if length(varargin) > 1, corrflag = varargin{2};, end
if length(varargin) > 2, plotflag = varargin{3};, end
if length(varargin) > 3, locflag = varargin{4};, end

for i = 1:length(clusters)
    subclusters{i} = []; clusters(i).PCA = [];

    % check for NaNs and remove those voxels
    tst = any(isnan(clusters(i).all_data),1);
    clusters(i).all_data(:,tst) = [];
    clusters(i).XYZ(:,tst) = [];
    clusters(i).XYZmm(:,tst) = [];
    clusters(i).Z(:,tst) = [];
    if any(tst),disp(['Warning! Removed ' num2str(sum(tst)) ' voxels with NaN values.']),end

    a = clusters(i).all_data;

    % scale here, because robust pca doesn't use correlations, does it?
    % but robust PCA seems to be unaffected by scale changes on some variables
    if corrflag, a = scale(a);,end

    % if we choose to add the XYZ flag to add locations to clustering criteria
    if locflag,
        wfactor = round(size(a,1) ./ 3);                       % weight for loc; higher = more weight on location
        xyztmp = repmat(scale(clusters(i).XYZ')',wfactor,1);   % scale to make comparable to img values,
                                                                % but multiply by weighting factor
        a = [a; xyztmp];
    end

    if size(a,2) > 2    % must have 3 voxels to try clustering

    % -------------------------------------------------------------------------------
    % * All the real work is done here.  Compute pc's
    % -------------------------------------------------------------------------------

    if ~robustflag
        [clusters(i).PCA.pcomps,clusters(i).PCA.weights,clusters(i).PCA.eigval,clusters(i).PCA.class] = pc(a,corrflag);

        % automatically pick number of clusters, based on gradient in eigenvalues
        g = abs(gradient(clusters(i).PCA.eigval));
        maxclusters = sum(g > g(1).*.2) + 1;
    else

        % pick number of dimensions by hand
        disp(' ')
        disp([num2str(clusters(i).numVox) ' voxels in main cluster'])
        fprintf(1,'%3.2f Observations on %3.2f voxels\n',size(a,1),size(a,2))
        fprintf(1,'Save at least 2 eigenvectors to do clustering');
        % doing this on a' means voxels are observations, conditions/subj scores are variables
        % we'll use the scores, which has voxels as rows and components as columns, to classify
        out = rapca(a');
        clusters(i).PCA.pcomps = out.T;
        clusters(i).PCA.weights = out.P;
        clusters(i).PCA.eigval = out.L;
        maxclusters = length(out.L);
    end

    % -------------------------------------------------------------------------------
    % * All the real work is done here.  Classify
    % -------------------------------------------------------------------------------

    if size(clusters(1).all_data,1) > 12,
        disp('More than 12 dimensions (observations per voxel) in original data - using eigenvectors to classify')
        close all; try, pack, catch, end
        % we pick the number of CLASSES separately by hand, because # components not a good indicator
        % of how many classes there are
        clusters(i).PCA.class = docluster(out.T',[],plotflag);
    else
        clusters(i).PCA.class = docluster(out.T',[],plotflag);
    end

    % clean up if using locflag
    clusters(i).PCA.locflag = locflag;
    if locflag,
        clusters(i).PCA.pcomps = clusters(i).PCA.pcomps(1:size(clusters(i).all_data,1),:);
        %clusters(i).PCA.weights = clusters(i).PCA.weights(1:size(clusters(i).all_data,1),:);
        %clusters(i).PCA.avgs = clusters(i).PCA.avgs(1:size(clusters(i).all_data,1),:);
    end

    % -------------------------------------------------------------------------------
    % for each group, separate into contiguous clusters
    % -------------------------------------------------------------------------------

    grps = unique(clusters(i).PCA.class(clusters(i).PCA.class~=0)); % values are component of origin
    for j = 1:length(grps),
        wh = find(clusters(i).PCA.class==grps(j));
        XYZ = clusters(i).XYZ(:,wh);
        cl_index = spm_clusters(XYZ) ./ 100;
        clusters(i).PCA.class(wh) = clusters(i).PCA.class(wh) + cl_index;
    end
    ngrps = length(unique(clusters(i).PCA.class));
    fprintf(1,'%3.0f contiguous clusters separated by class',ngrps)

    % -------------------------------------------------------------------------------
    % average within classes / contiguous regions
    % -------------------------------------------------------------------------------

    grps = unique(clusters(i).PCA.class(clusters(i).PCA.class~=0)); % values are component of origin
    for j = 1:length(grps),
        clusters(i).PCA.avgs(:,j) = mean(a(:,find(clusters(i).PCA.class==grps(j))),2);,
        freq(j) = sum(clusters(i).PCA.class == grps(j));
    end

    disp(['Cluster ' num2str(i) ', ' num2str(clusters(i).numVox) ' voxels: ' num2str(size(clusters(i).PCA.pcomps,2)) ' components'])
    fprintf(1,'\tMean\tEigval\tcorrel\t')

    % -------------------------------------------------------------------------------
    % * display each component and correlation with behavior
    % -------------------------------------------------------------------------------

    for j = 1:size(clusters(i).PCA.pcomps,2)

        %[H,P,CI,STATS] = TTEST(clusters(i).PCA.pcomps(:,j),0,.05,0);
        % skip the t-test.  t-tests on component scores don't make a lot of sense.
        fprintf(1,'\n\t%3.3f\t%3.3f\t',mean(clusters(i).PCA.pcomps(:,j)),clusters(i).PCA.eigval(j))

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.pcomps(:,j),varargin{1});
                co = co(1,2);
                fprintf(1,'%3.3f\t',co)
            end
        end

    end
    fprintf(1,'\n')

    % -------------------------------------------------------------------------------
    % * display classification info
    % -------------------------------------------------------------------------------
    disp(['Classified into ' num2str(ngrps) ' groups:'])
    fprintf(1,'\tClass\tVoxels\tMean\tt\tp\tcorrect. p\tcorrel\t')

    % for each component, test mean value and correlation with behavior
    for j = 1:length(grps)

        [H,P,CI,STATS] = TTEST(clusters(i).PCA.avgs(:,j),0,.05,0);
        fprintf(1,'\n\t%3.0f\t%3.0f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t',j,freq(j),mean(clusters(i).PCA.avgs(:,j)),STATS.tstat,P,P .* size(clusters(i).PCA.avgs,2))

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.avgs(:,j),varargin{1});
                co = co(1,2);
                fprintf(1,'%3.3f\t',co)
            end
        end

    end
    fprintf(1,'\n')

    % -------------------------------------------------------------------------------
    % * Plot, if requested
    % -------------------------------------------------------------------------------

    if plotflag,
        figure('Color','w'), subplot(1,3,1), imagesc(a), title(['Cl ' num2str(i) ': Data']), xlabel('Voxels'),ylabel('Subjects')
        subplot(1,3,2), imagesc(clusters(i).PCA.weights'), title(['Weights (eigenvectors)']), xlabel('Voxels'),ylabel('Eigenvectors')
        subplot(1,3,3), imagesc(clusters(i).PCA.pcomps), title(['Component scores (predictions)']), xlabel('Voxels'),ylabel('Subjects')

        a = [clusters(i).PCA.class' a']; a=sortrows(a,1); a = a(:,2:end)';
        figure;subplot 131; imagesc(a),title(['Cl ' num2str(i) ':Data sorted by class']), xlabel('Class'),ylabel('Subjects'),
        xlab = [sort(clusters(i).PCA.class(clusters(i).PCA.class~=0)) clusters(i).PCA.class(clusters(i).PCA.class==0)];
        set(gca,'XTick',1:length(clusters(i).PCA.class)); set(gca,'XTickLabel',xlab)
        subplot 132; imagesc(clusters(i).PCA.avgs),title('Class averages'), xlabel('Class'),ylabel('Subjects'),
        subplot 133; if length(varargin) > 0, if ~isempty(varargin{1}), imagesc(varargin{1}'), title('Behavior'),end,end

    end

    else
        disp(['Cluster ' num2str(i) ' has less than 3 voxels.'])
        clusters(i).PCA.class = ones(1,clusters(i).numVox);
        clusters(i).PCA.avgs = clusters(i).timeseries;
        grps = 1;
        if ~isfield(clusters,'correl'), clusters(1).correl = [];, end
    end

    % -------------------------------------------------------------------------------
    % * separate into subclusters, based on class membership
    % -------------------------------------------------------------------------------

    if ~isfield(clusters,'correl'), clusters(i).correl = [];, end
    disp('Recomputing correlations and z-scores and saving in subclusters')
    if ~isfield(clusters,'XYZ'), clusters(i).XYZ = ones(3,size(clusters(i).all_data,2));, end
    if ~isfield(clusters,'Z'), clusters(i).Z = ones(1,size(clusters(i).XYZ,2));, end
    clear subc

    for j = 1:length(grps)

        try
            subc(j) = clusters(i);
        catch
            warning('clusters does not have all required fields.'); break
        end

        wh = find(clusters(i).PCA.class == grps(j));

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.avgs(:,j),varargin{1});
                subc(j).correl = co(1,2);
            end
        end

        if isfield(subc(j),'title'), subc(j).title = [subc(j).title '_SUBCL_' num2str(j)];, end
        if isfield(subc(j),'name'), subc(j).name = [subc(j).name '_SUBCL_' num2str(j)];, end
        if isfield(subc(j),'numVox'), subc(j).numVox = length(wh);, end
        if isfield(subc(j),'Z'), subc(j).Z = subc(j).Z(wh);, end
        if isfield(subc(j),'XYZmm'), subc(j).XYZmm = subc(j).XYZmm(:,wh);, end
        if isfield(subc(j),'XYZ'), subc(j).XYZ = subc(j).XYZ(:,wh);, end

        if isfield(subc(j),'timeseries'), subc(j).timeseries = nanmean(subc(j).all_data(:,wh)')';, end
        %if isfield(subc(j),'snr'), subc(j).snr = subc(j).snr(wh);, end
        if isfield(subc(j),'center'), subc(j).center = center_of_mass(subc(j).XYZ,subc(j).Z);, end
        if isfield(subc(j),'mm_center'), subc(j).mm_center = center_of_mass(subc(j).XYZmm,subc(j).Z);, end

        if isfield(subc(j),'all_data'),
            subc(j).all_data = subc(j).all_data(:,wh);,
            for k = 1:size(subc(j).all_data,2)
                [H,P,CI,STATS] = ttest(subc(j).all_data(:,k),0,.05,0);
                subc(j).Z(k) = spm_t2z(STATS.tstat,STATS.df);
            end
        end

    end

    subclusters{i} = subc;

end

return

function [b,v,d,class] = pc(a,corrflag)
% a is original matrix, b is principal components, v is eigenvectors
% (weights on columns, which = weights on voxels)
% class is classification of voxels into groups based on component loadings

if corrflag, [v,d]=eig(corrcoef(a));, else, [v,d]=eig(cov(a));,end
b = (pinv(v) * a')' ./ repmat((diag(d)').^.5,size(a,1),1);
% i made this up: think of rptating each subject's scores (in cols of a')
% by the rotation matrix pinv(v), and normalizing by the sqrt of the eigenvalues
% pinv(v) and v are rotation matrices because det = 1, no shearing or dilation
%
% this appears to work to give scores as well
% both methods (above,below) are scaled versions of the splus factor scores
% the problem is that doing it two different ways in splus flips the signs
% of some components and not others (gui vs cmd line).

%X = a; R = corrcoef(a); A = v * (d^.5); B = inv(R) * A;
%scores = X * B;
% X is data, A is factor loading matrix, B is factor score coeff matrix
% this method, from the text, and the one giving b above produce identical results

A = v * (d^.5);

b = fliplr(b); v = fliplr(v); A = fliplr(A); %scores = fliplr(scores);

num = min(10,sum(diag(d) >= 1));
b = b(:,1:num); v = v(:,1:num); A = A(:,1:num);
origd = diag(d);
d = diag(d)'; d= fliplr(d); d = d(1:num);

if num == 0, warning('No eigenvalues above 1!');, origd, class = [];

else
    % classify each voxel into a group based on loading
    % use A, which re-introduces the comp variance, because we
    % want relationships with more variance to count more.
    % This just doesn't work so hot.  See docluster, below.

    %wh = A' == repmat(max(A'),size(A,2),1);
    %for i = 1:size(wh,2), tmp = find(wh(:,i));, class(i) = tmp(1); end
    %class(max(A') < .3) = 0;

end

%figure;plot(b,'r'),hold on;plot(a,'k'), hold on; plot(mean(a,2),'g--'),legend({'eig' 'orig' 'avg'})

return

function class = docluster(a,maxclusters,doplot)

    Y = pdist(a','euclid');     % transpose so the voxels are observations, eigenvectors the variables
    Z = linkage(Y,'complete');
    if maxclusters > 1
        class = cluster(Z,maxclusters)';

        if doplot,
            dendrogram(Z,0); title('Dendrogram for clustering')
        end

    else
        dendrogram(Z,0); title('Dendrogram for clustering')
        set(gcf,'Position',[10   601   800   500])
        maxclusters = input('Pick number of classes to save: ');

        if maxclusters == 1,
            class = ones(1,size(a,2));
        else
            class = cluster(Z,maxclusters)';
        end

    end

return


function r = cot(a)
%COT          Hessian (elementwise) cotangent
%

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

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

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

    r.x = cot(a.x);
    f = -1 - sqr(r.x);
    r.dx = f * a.dx;
    r.hx = f * ( a.hx - reshape( (r.x*a.dx) * a.dx.' , size(a.hx) ) );

  else                      % matrix hessian

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

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

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

    else                            % input full

      r.x = cot(a.x);
      rx = r.x(:).';
      f = -1 - sqr(rx);
      f = f(ones(N*N,1),:);
      r.dx = a.dx .* f(1:N,:);
      adx = repmat(rx,N,1) .* a.dx;
      r.hx = f .* ( a.hx - adx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:) );

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


function gX = ratquadKernGradX(kern, X, X2)

% RATQUADKERNGRADX Gradient of RATQUAD kernel with respect to input locations.
% FORMAT
% DESC computes the gradident of the rational quadratic
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO ratquadKernParamInit, kernGradX, ratquadKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
for i = 1:size(X, 1);
  gX(:, :, i) = ratquadKernGradXpoint(kern, X(i, :), X2);
end

function gX = ratquadKernGradXpoint(kern, x, X2)

% RATQUADKERNGRADXPOINT Gradient with respect to one point of x.

gX = zeros(size(X2));
n2 = dist2(X2, x);
wi2 = (.5/(kern.lengthScale*kern.lengthScale*kern.alpha));
ratquadPart = kern.variance*(1+n2*wi2).^-(kern.alpha+1)/(kern.lengthScale*kern.lengthScale);
for i = 1:size(x, 2)
  gX(:, i) =(X2(:, i) - x(i)).*ratquadPart;
end


function [rVect, vVect] = getStateAtTime(bodyInfo, time, gmu)
%getStateAtTime Summary of this function goes here
%   Detailed explanation goes here
    if(isstruct(bodyInfo) || (isprop(bodyInfo,'propTypeEnum') && bodyInfo.propTypeEnum == BodyPropagationTypeEnum.TwoBody) || (numel(time) == 1 && time == bodyInfo.epoch))
        numTimes = length(time);

        oneArray = (zeros(1, numTimes)+1);

        sma = bodyInfo.sma * oneArray;
        ecc = bodyInfo.ecc * oneArray;
        inc = AngleZero2Pi(deg2rad(bodyInfo.inc)) * oneArray;
        raan = AngleZero2Pi(deg2rad(bodyInfo.raan)) * oneArray;
        argp = AngleZero2Pi(deg2rad(bodyInfo.arg)) * oneArray;
        M0 = deg2rad(bodyInfo.mean) * oneArray;

        n = computeMeanMotion(sma, gmu);
        deltaT = time - bodyInfo.epoch;
        M = (M0(:) + n(:).*deltaT(:))';
        tru = computeTrueAnomFromMean(M, ecc);

        if(length(tru) > 1)
            [rVect, vVect] = vect_getStatefromKepler(sma, ecc, inc, raan, argp, tru, gmu, true);
        else
            [rVect, vVect] = getStatefromKepler_Alg(sma, ecc, inc, raan, argp, tru, gmu);
%             [rVect, vVect] = vect_getStatefromKepler(sma, ecc, inc, raan, argp, tru, gmu, true);
        end

    elseif(bodyInfo.propTypeEnum == BodyPropagationTypeEnum.Numerical)
        [rVect, vVect] = bodyInfo.numIntStateCache.getCachedBodyStateAtTime(time);

    else
        error('Unknown celestial body prop sim type.');
    end
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: various distances and Multi-Level Parametric Image Registration
%
%   - data                 PETCT, Omega=(0,140)x(0,151), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             {'SSD','NCC','MI','NGF'}
%   - transformation       affine2D
% see also E7_PETCT_MLPIR_ext
%==============================================================================

clear, close all, help(mfilename);
setup2DPETCTData;                                         % load data

% a list of distance measures to be used
DM = {'SSD','NCC','MI','NGF'};

OPTpara = FAIRcell2struct(optPara('PIR-GN'));

for dm = 1:length(DM), % run over all distance measures

  % initialize interpolation, using a smooth representation (theta=1e0)
  imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e0);

  % initialize transformation, create initial guess and reference for stopping
  trafo('reset','trafo','affine2D'); wStop = trafo('w0'); w0 = wStop;

  % initialize distance and display options
  distance('reset','distance',DM{dm}); distance('disp')

  % run MLPIR using sufficient amount of details (level=5)
  wSmooth =  MLPIR(ML,'minLevel',5,'plotIter',0,'plotMLiter',0);

  % refine interpolation (theta=1e-3)
  imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e-3);
  level = length(ML); omega = ML{level}.omega; m = ML{level}.m;
  [T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);

  % start PIR, using the result from the smooth problem as starting guess

  % initialize plots
  FAIRplots('set','mode','PIR');
  FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

  % optimize
  xc = getCellCenteredGrid(omega,m);
  Rc = imgModel(R,omega,xc);
  fctn = @(wc) PIRobjFctn(T,Rc,omega,m,0,[],[],xc,wc); fctn([]);
  [wc,his] = GaussNewton(fctn,w0,OPTpara{:});

  % visualize results
  yc = trafo(wc,xc);
  R0 = imgModel(R,omega,xc);
  T0 = imgModel(T,omega,xc);
  Tc = imgModel(T,omega,yc);

  figure(11); clf;
  viewImage(T0,omega,m,'axis','off'); hold on;
  plotGrid(yc,omega,m,'spacing',ceil(m/32),'linewidth',2,'color','w');

  figure(12); clf;
  overlayImage2D(Tc,R0,omega,m); axis off;
end;

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


function [equality center] = doquality(Xcx,X)
% :Usage:
% ::
%
%     [equality] = doquality(Xcx,X)
%
% :Inputs:
%
%   **Xcx:**
%        binary indicator matrix of cluster assignments,
%
%   **X:**
%        stimulus coordinates in group space
%
%        also: takes group spaces with zeros;

if size(Xcx>1);
    for i = 1:size(Xcx,2)    % for each class
        tmp = mean(X(find(Xcx(:,i)),:),1);     % get center of this class
        center(i,:) = tmp;
        % dist of all points in X from each center
        % number of cols of X is the number of dimensions; sums squared
        % vals across dims, takes sqrt to get Euclidean distance in N-d
        % space
        d(:,i) = sum((repmat(tmp,size(X,1),1) - X).^2,2).^0.5;
    end

    % rows of d are objects, columns are classes, values are dist from
    % class center
    for i=1:size(d,1)                   % i is the object (point)

        myclass = find(Xcx(i,:)==1);    % index of which class it is

        edist(i) = d(i,myclass);        %distance to center of own class

        otherclass = find(Xcx(i,:)==0);     % indices of columns for other classes

        otherdist(i) = min(d(i,otherclass));  %distance to center of cluster

    end

    % for each point, quality is distance to nearest neighbor - dist to own
    % cl / mak of those two
    equality = (otherdist - edist) ./ max([edist;otherdist]);

else
end


function sensor_out=la_sen(acc, gyro, gyro_der, sen_la, sen_or, sen_typ)
nsen=size(sen_typ,1);
sensor_out=zeros(nsen,1);
for in=1:nsen;
    %lever arm
    Csm=sen_or(:,:,in);
    la=sen_la(:,in);

    %Acceleration
    acc_sen=Csm'*(acc+cross(gyro,cross(gyro,la))+cross(gyro_der,la));

    %Rotation rate
    gyro_sen=Csm'*gyro;

    if (sen_typ(in)==1)
        sensor_out(in)=acc_sen(1);
    elseif (sen_typ(in)==2)
        sensor_out(in)=gyro_sen(1);
    end
end


function [imgPyr, maskPyr, scaleImgPyr] = sc_create_pyramid(img, mask, optS)

% SC_CREAT_IMG_PYRAMID
%
% Create image pyramid with linear or log scale for coarse to fine image
% completion
%
% Input:
%   - img:  input image with hole
%   - mask: Hole mask
%   - optS: options
% Output:
%   - imgPyr:      Image pyramid
%   - maskPyr:     Mask pyramid
%   - scaleImgPyr: Image dimensions in each level

% Image size in the high-resolution image
[imgHeight, imgWidth, nCh] = size(img);
img = sc_init_coarsest_level(img, logical(mask));

% =========================================================================
% Create pyramid: scale
% =========================================================================
scaleImgPyr = sc_create_scale_pyramid(imgHeight, imgWidth, optS);

% =========================================================================
% Create pyramid: mask
% =========================================================================
maskPyr = sc_create_image_pyramid(mask, scaleImgPyr, 'mask', optS);

% =========================================================================
% Create pyramid: image
% =========================================================================
imgPyr = sc_create_image_pyramid(img, scaleImgPyr, 'image', optS);

% =========================================================================
% Recover image boundary
% =========================================================================

% nCh = 3;
% for iLvl = 1: optS.numPyrLvl
%     maskCur  = maskPyr{iLvl};
%     bdRegion = maskCur < 0.99 & maskCur > 0.1;
%     bdRegionC = bdRegion(:,:,ones(nCh,1));
%
%     imgCur   = imgPyr{iLvl};
%     imgCurBd = bsxfun(@rdivide, imgCur, maskCur);
%     imgCur(bdRegionC) = imgCurBd(bdRegionC);
%     imgPyr{iLvl} = imgCur;
% end

% Initialize the coarsest level
imgPyr{optS.numPyrLvl} = sc_init_coarsest_level(imgPyr{optS.numPyrLvl}, maskPyr{optS.numPyrLvl});

% Convert to single type
imgPyr = cellfun(@im2single, imgPyr, 'UniformOutput', false);

end

function img = sc_init_coarsest_level(img, mask)

% Get the inital solution
[~, idMap] = bwdist(~mask, 'euclidean');

% Intepolate only in the interior to avoid dark values near the image borders
maskInt = mask;
maskInt(1,:) = 0;   maskInt(end,:) = 0;
maskInt(:,1) = 0;   maskInt(:,end) = 0;

for ch = 1: 3
    imgCh = img(:,:,ch);
    imgCh = imgCh(idMap);
    img(:,:,ch) = roifill(imgCh, maskInt);
end

end

function imgPyr = sc_create_image_pyramid(img, scaleImgPyr, imageType, optS)

% h = fspecial('gaussian', 5, 1);

% Initialize image pyramid
imgPyr  = cell(optS.numPyrLvl, 1);

% The finest level
imgPyr{1} = img;

%
for iLvl = 2: optS.numPyrLvl
    imgHCurLvl = scaleImgPyr{iLvl}.imgSize(1);
    imgWCurLvl = scaleImgPyr{iLvl}.imgSize(2);

    % Previous layer
    imgCur = imgPyr{iLvl - 1};

    % Anti-alising by blurring
%     imgCur   = imfilter(imgCur, h, 'same', 'replicate', 'conv');

    % Resampling
    imgPyr{iLvl} = imresize(imgCur,  [imgHCurLvl, imgWCurLvl], optS.resampleKernel);
end

if(strcmp(imageType, 'mask'))
    % Convert resampled masks into logical type
    for iLvl = 1: optS.numPyrLvl
        imgPyr{iLvl} = imgPyr{iLvl} > 0.5;
    end
end

end

function scaleImgPyr = sc_create_scale_pyramid(imgHeight, imgWidth, optS)

% Compute the coarsest image scale
imgSizeMin   = min(imgHeight, imgWidth);
coarestScale = optS.coarestImgSize/imgSizeMin;

% Compute the scale in each layer in the image pyramid
if(optS.useLogScale)      % use log scale
    scalePyr = 2.^linspace(0, log2(coarestScale), optS.numPyrLvl);
else                      % use linear scale
    scalePyr = linspace(1, coarestScale, optS.numPyrLvl);
end

% Image size in each layer
imgHPyr = round(imgHeight *scalePyr);
imgWPyr = round(imgWidth  *scalePyr);

% Initialize scales
scaleImgPyr = cell(optS.numPyrLvl, 1);

% Finest level
scaleImgPyr{1}.imgScale = 1;
scaleImgPyr{1}.imgSize = [imgHeight, imgWidth];

% Downsampled images
for k = 2: optS.numPyrLvl
    scaleImgPyr{k}.imgScale = scalePyr(k);
    scaleImgPyr{k}.imgSize  = [imgHPyr(k), imgWPyr(k)];
end

end


% TOEPLITZ_BLOCK - Constructs a block-Toeplitz matrix.
%
%   Y = TOEPLITZ_BLOCK(X)
%
% X = [X_1, X_2, X_3, ..., X_N]
%
% Y = [X_1 X_2 X_3 ... X_N
%      X_2 X_1 X_2 ... ...
%      X_3 X_2 X_1 ... ...
%      ... ... ... ... ...
%      X_N ... X_3 X_2 X_1]

function Y = toeplitz_block(X)

[M,N] = size(X);

if mod(N,M) ~= 0
  error('The number of columns must be a multiple of the number of rows.')
end

D = N/M;

if issparse(X)
  t = cputime();

  nonzeros = zeros(D,1);
  i = cell(D,1);
  j = cell(D,1);
  v = cell(D,1);
  [i{1},j{1},v{1}] = find(X(:,1:M));
  nonzeros(1) = D*length(v{1});
  for d=2:D
    k = (d-1)*M+1;
    l = d*M;
    [i{d},j{d},v{d}] = find(X(:,k:l));
    nonzeros(d) = 2 * (D-d+1) * length(v{d});
  end
  nzs = sum(nonzeros);

  %time = cputime() - t, t = cputime();

  I = zeros(nzs,1);
  J = zeros(nzs,1);
  values = zeros(nzs,1);

  %time = cputime() - t, t = cputime();

  z = 1;
  for d2=1:D
    for d1=1:D
      ind = abs(d1-d2) + 1;
      jnd = z:(z+length(v{ind})-1);
      I(jnd) = i{ind} + (d1-1)*M;
      J(jnd) = j{ind} + (d2-1)*M;
      values(jnd) = v{ind};
      z = z + length(v{ind});
    end
  end

  %time = cputime() - t, t = cputime();

  Y = sparse(I,J,values,N,N,nzs);

  %time = cputime() - t, t = cputime();

else

  Y = zeros(N,N);
  for i=1:D
    for j=1:D
      ind = abs(i-j) + 1;
      k = (i-1)*M+1;
      l = (j-1)*M+1;
      m = (ind-1)*M+1;
      Y(k:(k+M-1),l:(l+M-1)) = X(:,m:(m+M-1));
    end
  end

end


function edge = lineToEdge3d(line)
%LINETOEDGE3D Convert a 3D straight line to a 3D finite edge.
%
%   EDGE = lineToEdge3d(LINE)
%   Returns the edge with same origin as the line LINE, and with second
%   extremity corresponding to the addition of line origin and direction.
%   LINE is represented as [X0 Y0 Z0  DX DY DZ]
%   EDGE is represented as [X1 Y1 Z1  X2 Y2 Z2]
%
%   Example
%     line = [3 4 5  1 2 3];
%     edge = lineToEdge3d(line)
%     edge =
%          3   4   5   4   6   8
%
%   See also
%     lines3d, edges3d, edgeToLine3d

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-05-07, using Matlab 9.6.0.1072779 (R2019a)
% Copyright 2019-2022 INRA - Cepia Software Platform

edge = [line(:, 1:3) line(:,1:3)+line(:,4:6)];


% resample the set of particles.
% A particle has a probability proportional to its weight to get
% selected. A good option for such a resampling method is the so-called low
% variance sampling, Probabilistic Robotics pg. 109
function newParticles = resample(particles)

numParticles = length(particles);

w = [particles.weight];

% normalize the weight
w = w / sum(w);

% consider number of effective particles, to decide whether to resample or not
useNeff = false;
%useNeff = true;
if useNeff
  neff = 1. / sum(w.^2);
  neff
  if neff > 0.5*numParticles
    newParticles = particles;
    for i = 1:numParticles
      newParticles(i).weight = w(i);
    end
    return;
  end
end

newParticles = struct;

% TODO: implement the low variance re-sampling

% the cummulative sum
cs = cumsum(w);
weightSum = cs(length(cs));

% initialize the step and the current position on the roulette wheel
step = weightSum / numParticles;
position = unifrnd(0, weightSum);
idx = 1;

% walk along the wheel to select the particles
for i = 1:numParticles
  position += step;
  if (position > weightSum)
    position -= weightSum;
    idx = 1;
  end
  while (position > cs(idx))
    idx++;
  end
  newParticles(i) = particles(idx);
  newParticles(i).weight = 1/numParticles;
end

end