%% DEMO_febio_0037_lattice_test_octet_truss_01
% Below is a demonstration for:
%
% * Building the geometry for the octet truss lattive with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement and stress results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * compression, tension, compressive, tensile
% * displacement control, displacement boundary condition
% * hexahedral elements, hex8
% * cube, box, rectangular
% * Lattice
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
% Plot settings
fontSize=15;
faceAlpha1=0.8;
faceAlpha2=1;
edgeColor=0.25*ones(1,3);
edgeWidth=1.5;
markerSize=25;
markerSize2=25;
cMap=gjet(4);

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=fullfile(savePath,[febioFebFileNamePart,'.txt']); %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress
febioLogFileName_stiffness=[febioFebFileNamePart,'_stiffness_out.txt']; %Log file name for exporting stiffness

%Specifying dimensions and number of elements
sampleSize=10;
latticeType=1;
elementType='hex8'; %'hex8'
strutThickness=0.5;

%Define applied displacement
appliedStrain=0.3; %Linear strain (Only used to compute applied stretch)
loadingOption='compression'; % or 'tension'
switch loadingOption
    case 'compression'
        stretchLoad=1-appliedStrain; %The applied stretch for uniaxial loading
    case 'tension'
        stretchLoad=1+appliedStrain; %The applied stretch for uniaxial loading
end
displacementMagnitude=(stretchLoad*sampleSize)-sampleSize; %The displacement magnitude

%Material parameter set
E_youngs1=0.1; %Material Young's modulus
nu1=0.4; %Material Poisson's ratio

% FEA control settings
numTimeSteps=20; %Number of time steps desired
max_refs=50; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=15; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=(1/numTimeSteps); %Maximum time step size
min_residual=1e-20;
symmetric_stiffness=0;
runMode='external'; %'internal' or 'external'

%%

%Specifying dimensions and number of elements
r=0.5; %Radii, results in a width of 1
n=3;
nCopies=n*ones(1,3); %Number of offset copies
d=2*r; %Diameter
w=(n-1)*d; %sampleSize

shrinkFactor=strutThickness./((sampleSize./n).*(sqrt(2)./2));

%% Create lattice

switch latticeType
    case 1 %Octet truss
        [E,V,C,F,CF]=rhombicDodecahedronMesh(r,nCopies);
        V=V./(n-1);
        V=V*sampleSize;

        [indBoundary]=tesBoundary(F);
        cPar.shrinkFactor=shrinkFactor; %Strut sides are formed by shrinking the input mesh faces by this factor
        cPar.meshType='hex'; %desired output mesh type
        cPar.indBoundary=indBoundary; %indices of the boundary faces
        cPar.hexSplit=1;
        cPar.latticeSide=2; %1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
        [Es,Vs,Cs]=element2lattice(E,V,cPar); %Get lattice structure

        logicKeep1=~(Vs(:,1)<=-1e-3);
        logicKeep2=~(Vs(:,2)<=-1e-3);
        logicKeep3=~(Vs(:,3)<=-1e-3);
        logicKeep4=~(Vs(:,1)>=sampleSize+1e-3);
        logicKeep5=~(Vs(:,2)>=sampleSize+1e-3);
        logicKeep6=~(Vs(:,3)>=sampleSize+1e-3);

        logicKeepEs=sum(logicKeep1(Es),2)>=4 &...
            sum(logicKeep2(Es),2)>=4 &...
            sum(logicKeep3(Es),2)>=4 &...
            sum(logicKeep4(Es),2)>=4 &...
            sum(logicKeep5(Es),2)>=4 &...
            sum(logicKeep6(Es),2)>=4;

        Es=Es(logicKeepEs,:);
        Cs=Cs(logicKeepEs,:);
        [Es,Vs,indFix]=patchCleanUnused(Es,Vs);

        % [Es,Vs,~,~]=subHex(Es,Vs,1,1);
        % Cs=repmat(Cs,8,1);

        % Create patch Data for visualization
        [Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting

        %Get new boundary set
        indB=tesBoundary(Fs);
        Fb=Fs(indB,:);
    case 2 %Rhombic dodecahedron mesh ("dual" of octet truss lattice)
        [E,V,C,F,CF]=rhombicDodecahedronMesh(r,nCopies);
        V=V./(n-1);
        V=V*sampleSize;

        [indBoundary]=tesBoundary(F);
        cPar.shrinkFactor=0.15; %Strut sides are formed by shrinking the input mesh faces by this factor
        cPar.meshType='hex'; %desired output mesh type
        cPar.indBoundary=indBoundary; %indices of the boundary faces
        cPar.hexSplit=3;
        cPar.latticeSide=1; %1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
        [Es,Vs,Cs]=element2lattice(E,V,cPar); %Get lattice structure

        logicKeep1=~(Vs(:,1)<=-1e-3);
        logicKeep2=~(Vs(:,2)<=-1e-3);
        logicKeep3=~(Vs(:,3)<=-1e-3);
        logicKeep4=~(Vs(:,1)>=sampleSize+1e-3);
        logicKeep5=~(Vs(:,2)>=sampleSize+1e-3);
        logicKeep6=~(Vs(:,3)>=sampleSize+1e-3);

        logicKeepEs=sum(logicKeep1(Es),2)>=4 &...
            sum(logicKeep2(Es),2)>=4 &...
            sum(logicKeep3(Es),2)>=4 &...
            sum(logicKeep4(Es),2)>=4 &...
            sum(logicKeep5(Es),2)>=4 &...
            sum(logicKeep6(Es),2)>=4;

        Es=Es(logicKeepEs,:);
        Cs=Cs(logicKeepEs,:);
        [Es,Vs,indFix]=patchCleanUnused(Es,Vs);

        % Create patch Data for visualization
        [Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting

        %Get new boundary set
        indB=tesBoundary(Fs);
        Fb=Fs(indB,:);
end

if strcmp(elementType,'hex20')
    [Es,Vs,~,Fb]=hex8_hex20(Es,Vs,{},Fb);
end
%%
% Visualizing input mesh and lattic structures

cFigure;
hs=subplot(1,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.5);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

% Fst=[Fs(:,[1 2 3]); Fs(:,[3 4 1]);];
% indB=tesBoundary(Fst,Vs);
% Fbt=Fst(indB,:);

subplot(1,2,2);
title('Lattice side 1','fontSize',fontSize)
hold on;
gpatch(Fb,Vs,'bw','k',1);
% plotV(Vs(Fb(:),:),'r.');
% patchNormPlot(Fs,Vs);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

drawnow;

%% DEFINE BC's

% Define node set logics
indAll=(1:1:size(Vs,1))';
logicBoundary=ismember(indAll,Fb);

Z=Vs(:,3);
logicTop=Z>=(sampleSize-eps(sampleSize))& logicBoundary;
logicBottom=Z<=eps(sampleSize) & logicBoundary;

X=Vs(:,1);
logicSide1=X>=(sampleSize-eps(sampleSize))& logicBoundary;
logicSide2=X<=eps(sampleSize)& logicBoundary;

Y=Vs(:,2);
logicSide3=Y>=(sampleSize-eps(sampleSize))& logicBoundary;
logicSide4=Y<=eps(sampleSize)& logicBoundary;

bcPrescribeListCell{1}=find(logicSide1)';
bcPrescribeListCell{2}=find(logicSide2)';
bcPrescribeListCell{3}=find(logicSide3)';
bcPrescribeListCell{4}=find(logicSide4)';
bcPrescribeListCell{5}=find(logicTop)';
bcPrescribeListCell{6}=find(logicBottom)';

%% Smoothing lattice

% indKeep=unique([bcPrescribeListCell{:}]);
% [Fb_clean,Vb_clean,indFix]=patchCleanUnused(Fb,Vs);
%
% cPar.Method='HC';
% cPar.n=6;
%
% cPar.RigidConstraints=indFix(indKeep);
% % cPar.RigidConstraints=cPar.RigidConstraints(cPar.RigidConstraints>0);
%
% [Vb_clean]=tesSmooth(Fb_clean,Vb_clean,[],cPar);
% ind=Fb(:);
% ind=unique(ind(:));
% Vs(ind,:)=Vb_clean;

% cFigure; hold on;
% gpatch(Fb,Vs,'bw','k',1);
% % patchNormPlot(Fs,Vs);
% % plotV(Vs(indKeep,:),'k.','MarkerSize',25)
% axisGeom(gca,fontSize);
% camlight headlight; lighting flat;
% drawnow;

%%

%Prescribed displacement nodes
bcPrescribeList=find(logicTop);
bcSupportList=find(logicBottom);

%%
% Visualizing input mesh and lattice structures

cFigure;
hs=subplot(1,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.5);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

subplot(1,2,2);
title('Lattice side 1','fontSize',fontSize)
hold on;
gpatch(Fb,Vs,'bw');
% patchNormPlot(Fs,Vs);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

drawnow;

%%
% Visualize BC's

hf=cFigure; hold on;
title('Boundary conditions model','FontSize',fontSize);
gpatch(Fb,Vs,'w','none',0.5);
hl2(1)=plotV(Vs(bcPrescribeList,:),'r.','MarkerSize',markerSize2);
hl2(2)=plotV(Vs(bcSupportList,:),'b.','MarkerSize',markerSize2);
legend(hl2,{'BC prescribe','BC support'});
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Check porosity
% Note it may be better to use the convex hull volume rather than the cube
% for the octet truss
vol_lattice=sum(hexVol(Es,Vs)); %Volume of hexahedra
porosity_lattice=vol_lattice./sampleSize.^3; %Porosity

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='neo-Hookean';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.E=E_youngs1;
febio_spec.Material.material{1}.v=nu1;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(Vs,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=Vs; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type=elementType; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(Es,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=Es; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

nodeSetName2='bcPrescribeList';
febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

febio_spec.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{2}.dofs='x,y';

febio_spec.Boundary.bc{3}.ATTR.type='prescribe';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{3}.dof='z';
febio_spec.Boundary.bc{3}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{3}.scale.VAL=displacementMagnitude;
febio_spec.Boundary.bc{3}.relative=0;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';

% febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
% febio_spec.Output.logfile.element_data{1}.ATTR.data='sz';
% febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
% febio_spec.Output.logfile.element_data{1}.VAL=1:size(Es,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode=runMode; %Run in external or in matlab terminal

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [timeVec, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp)); %Nodal displacements
    timeVec=[0; timeVec(:)]; %Time

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;
    DN=N_disp_mat(:,:,end);
    DN_magnitude=sqrt(sum(DN(:,3).^2,2));
    Vs_def=Vs+DN;

    %     % Importing element stress from a log file
    %     [time_mat, E_stress_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_stress)); %Nodal forces
    %     time_mat=[0; time_mat(:)]; %Time
    %     stress_cauchy_sim=[0; mean(squeeze(E_stress_mat(:,end,:)),1)'];
    %%
    % Importing nodal forces from a log file

    [dataStruct]=importFEBio_logfile(fullfile(savePath,febioLogFileName_force),1,1); %Nodal forces

    %Access data
    timeVec=dataStruct.time;
    f_sum_x=squeeze(sum(dataStruct.data(bcPrescribeList,1,:),1));
    f_sum_y=squeeze(sum(dataStruct.data(bcPrescribeList,2,:),1));
    f_sum_z=squeeze(sum(dataStruct.data(bcPrescribeList,3,:),1));

    %%
    % Visualize force data

    displacementApplied=timeVec.*displacementMagnitude;

    cFigure; hold on;
    xlabel('$u$ [mm]','Interpreter','Latex');
    ylabel('$F_z$ [N]','Interpreter','Latex');
    hp=plot(displacementApplied(:),f_sum_z(:),'b-','LineWidth',3);
    grid on; box on; axis square; axis tight;
    set(gca,'FontSize',fontSize);
    drawnow;

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    hp=gpatch(Fb,Vs_def,DN_magnitude,'k',1); %Add graphics object to animate
    %     gpatch(Fb,Vs,'kw','none',0.25); %A static graphics object
    hp.FaceColor='interp';

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    clim([0 max(DN_magnitude)]);
    axis([min([Vs_def(:,1);Vs(:,1)]) max([Vs_def(:,1);Vs(:,1)])...
        min([Vs_def(:,2);Vs(:,2)]) max([Vs_def(:,2);Vs(:,2)])...
        min([Vs_def(:,3);Vs(:,3)]) max([Vs_def(:,3);Vs(:,3)]) ]); %Set axis limits statically
    %     view(130,25); %Set view direction
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        DN_magnitude=sqrt(sum(DN.^2,2)); %Current displacement magnitude
        Vs_def=Vs+DN; %Current nodal coordinates

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={Vs_def,DN_magnitude}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

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

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


%COREG_INV invert coreg translations for coefficients
%
%    Andy Hooper, Aug 2005
%
%   ======================================================================
%   04/2008 AH: Updated for compatibility with matlab 2008a
%   11/2008 AH: Deltaline/pixel added for Doris 3.96 compatibility
%   ======================================================================

cpmname=dir('CPM_Data.*');

load coreg_parms
Lrange=coreg_parms(1)-1
Prange=coreg_parms(2)-1
N=coreg_parms(3)^2
%osf=coreg_parms(4)
osf=1
n_ifg=coreg_parms(5)

G=sparse(0,n_ifg*12);
d=zeros(0,1);

for i=1:length(cpmname);

    thisname=cpmname(i).name;
    CPM_Data=load(thisname);

    posL1=((CPM_Data(:,2)-1)*4/Lrange)-2;
    posP1=((CPM_Data(:,3)-1)*4/Prange)-2;
    offL=CPM_Data(:,4);
    offP=CPM_Data(:,5);
    posL2=((CPM_Data(:,2)+offL-1)*4/Lrange)-2;
    posP2=((CPM_Data(:,3)+offP-1)*4/Prange)-2;
    n_pos=size(posL2,1);
    corrf=CPM_Data(:,6);
    stdev=sqrt(3/2/N).*sqrt(1-corrf.^2)/pi./corrf*osf^(3/2);
    weighting=1./stdev;
    ifgs=sscanf(thisname,'CPM_Data.%d.%d');
    ifg1=ifgs(1);
    ifg2=ifgs(2);

    Gblock1=[ones(size(posL1)),posL1,posP1,posL1.^2,posL1.*posP1,posP1.^2];
    Gblock1=Gblock1.*repmat(weighting,1,6);
    Gblock2=[ones(size(posL2)),posL2,posP2,posL2.^2,posL2.*posP2,posP2.^2];
    Gblock2=Gblock2.*repmat(weighting,1,6);
    Gnew=sparse(n_pos*2,n_ifg*12);
    if ifg1~=0
       Gnew(1:n_pos,(ifg1-1)*12+1:(ifg1-1)*12+6)=Gblock1;
       Gnew(n_pos+1:n_pos*2,(ifg1-1)*12+7:ifg1*12)=Gblock1;
    end
    Gnew(1:n_pos,(ifg2-1)*12+1:(ifg2-1)*12+6)=-Gblock2;
    Gnew(n_pos+1:n_pos*2,(ifg2-1)*12+7:ifg2*12)=-Gblock2;

    G=[G;Gnew];
    d=[d;weighting.*offL;weighting.*offP];
    %d=[d;offL;offP];
end

% coeff_s gives mapping of slave to master w.r.t. slave position
coeff_s=G\d;

load slave_corners.txt
deltaL=zeros(4,size(slave_corners,1));
deltaP=zeros(4,size(slave_corners,1));
for i = 1:size(slave_corners,1)
    posL1=((slave_corners(i,1:2))*4/Lrange)-2;
    posL1=[posL1(1);posL1(1);posL1(2);posL1(2)];
    posP1=((slave_corners(i,3:4))*4/Prange)-2;
    posP1=[posP1(1);posP1(2);posP1(1);posP1(2)];
    G=[ones(4,1),posL1,posP1,posL1.^2,posL1.*posP1,posP1.^2];
    deltaL(:,i)=G*coeff_s((i-1)*12+1:(i-1)*12+6);
    deltaP(:,i)=G*coeff_s((i-1)*12+7:(i)*12);
end
corner_offsets=[deltaL(:)';deltaP(:)'];
corner_offsets=corner_offsets(:);
save('corner_offsets.txt','-ascii','corner_offsets')

% we want mapping of slave to master w.r.t. master position
l=[-2:0.05:2];
p=l;
[Ls,Ps]=meshgrid(l,p);
Ls=Ls(:);
Ps=Ps(:);
nsynth=length(Ls);

Gblock=[ones(nsynth,1),Ls,Ps,Ls.^2,Ls.*Ps,Ps.^2];
Gsynth=sparse(2*nsynth,12);
Gsynth(1:nsynth,1:6)=Gblock;
Gsynth(nsynth+1:end,7:12)=Gblock;
coeff_m=zeros(size(coeff_s));

for i=1:n_ifg
    ifg_coeff_s=coeff_s((i-1)*12+1:i*12);
    dsynth=Gsynth*ifg_coeff_s;
    Lm=Ls+dsynth(1:nsynth)*4/Lrange;
    Pm=Ps+dsynth(nsynth+1:end)*4/Prange;
    Gblock=[ones(nsynth,1),Lm,Pm,Lm.^2,Lm.*Pm,Pm.^2];
    Gsynth(1:nsynth,1:6)=Gblock;
    Gsynth(nsynth+1:end,7:12)=Gblock;
    coeff_m((i-1)*12+1:i*12)=Gsynth\-dsynth;
end

save('coreg_coeffs.txt','-ascii','coeff_m')


function EUI_data=riceVST_EUI(Efz_data,sigma_data,VST_ABC)
% Applies exact unbiased inverse of the variance-stabilizing transformation f
% --------------------------------------------------------------------------------------------
%
% SYNTAX
% ------
% EUI_data = riceVST_EUI ( Efz_data , sigma_data , VST_ABC )
%
%
% OUTPUT
% ------
% EUI_data    :  exact unbiased inverse of the input Efz_data
%
%
% INPUTS
% ------
% Efz_data    :  Filtered variance-stabilized data (e.g, output of the denoising filter)
% sigma_data  :  standard-deviation parameter of the Rice distribution;  the method assumes
%                that the data z_data before stabilization is distributed according to
%                z_data ~ Rice(nu,sigma_data), nu being some unknown noise-free signal.
%                EUI_data should coincide with nu if the denoising of fz_data is perfect.
% VST_ABC     :  file-selector for the variance-stabilizing transformation  (default = 'A')
%
%
% --------------------------------------------------------------------------------------------
%
% The software implements the method published in the paper:
%
%  A. Foi, "Noise Estimation and Removal in MR Imaging: the Variance-Stabilization Approach",
%  in Proc. 2011 IEEE Int. Sym. Biomedical Imaging, ISBI 2011, Chicago (IL), USA, April 2011.
%  doi:10.1109/ISBI.2011.5872758
%
% --------------------------------------------------------------------------------------------
%
%
% author:                Alessandro Foi
%
% web page:              http://www.cs.tut.fi/~foi/RiceOptVST
%
% contact:               firstname.lastname@tut.fi
%
% --------------------------------------------------------------------------------------------
% Copyright (c) 2010-2012 Tampere University of Technology.
% All rights reserved.
% This work should be used for nonprofit purposes only.
% --------------------------------------------------------------------------------------------
%
% Disclaimer
% ----------
%
% Any unauthorized use of these routines for industrial or profit-oriented activities is
% expressively prohibited. By downloading and/or using any of these files, you implicitly
% agree to all the terms of the TUT limited license (included in the file Legal_Notice.txt).
% --------------------------------------------------------------------------------------------
%

%% Defaults
if nargin<2
    sigma_data=1;
end
if isempty(sigma_data)
    sigma_data=1;
end

if nargin<3
    VST_ABC='A';
end
if isempty(VST_ABC)
    VST_ABC='A';
end

if ischar(VST_ABC)
    if numel(VST_ABC)==1
        Rice_VST_matFile=['Rice_VST_',VST_ABC];
    else
        Rice_VST_matFile=VST_ABC;
    end
end

%% load variance-stabilizing transformation and its exact unbiased inverse from file
load(Rice_VST_matFile,'Efz','nu','z','f')

%% scale data before applying exact unbiased inverse of the variance-stabilizing transformation (see riceVST.m)
sigma_data_scaling=sigma_data;
a=f(end)-sqrt(sigma_data_scaling^2*max(z)^2/sigma_data^2-1/2);
nu=nu*sigma_data_scaling;
%% apply exact unbiased inverse of the variance-stabilizing transformation
EUI_data=interp1(Efz,nu,Efz_data,'linear','extrap');
%% small values (see Equation 11 in the ISBI2011 paper)
EUI_data(Efz_data<=min(Efz))=min(nu);
%% apply asymptotical exact unbiased inverse of the variance-stabilizing transformation (used only for large values)
maxEfz=max(Efz);
if 0
    EUI_data(Efz_data>maxEfz)=sigma_data*sqrt((Efz_data(Efz_data>maxEfz)-a).^2+0.5);  %% this is the asymptotic inversion to Ez (identical to the above theta_or_Ez==1 case)
    EUI_data(Efz_data>maxEfz)=EUI_data(Efz_data>maxEfz).*(1-0.5*(sigma_data./EUI_data(Efz_data>maxEfz)).^2);  %% this is the asymptotic correction between nu and Ez
else
    EUI_data(Efz_data>maxEfz)=sigma_data*((Efz_data(Efz_data>maxEfz)-a).^2)./sqrt((Efz_data(Efz_data>maxEfz)-a).^2+0.5);   %% this is the result of the two lines above
end


function boolVal=isPermutation(possiblePerm,startIdx)
%%ISPERMUTATION Given an array, determine whether it is a valid
%        permutation of the values startIdx:(startIdx+permLength-1).
%
%INPUTS: possiblePerm A length permLength vector to test for being a
%                     permutation.
%            startIdx The starting index of the permutation values to
%                     consider. If this input is omitted or an empty matrix
%                     is passed, then the default of 1 is used.
%
%OUTPUTS: boolVal This is true if possiblePerm is a permutation vector and
%                 false otherwise. Empty matrices are considered
%                 permutation vectors.
%
%After initial checks, the function just allocates a vector having the
%length of possiblePerm and adds 1 to each index given in possiblePerm. If
%any index is repeated. Afterwards, it checks whether all of the indices
%contain a "1".
%
%EXAMPLE:
% boolVal1=isPermutation([1;4;3;5])
% boolVal2=isPermutation([1;4;3;2;5])
% boolVal3=isPermutation([13;16;15;17;14],13)
%One will find boolVal1=false, boolVal2=true, boolVal3=true.
%
%September 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(startIdx))
    startIdx=1;
end

possiblePerm=possiblePerm(:);

if(~isnumeric(possiblePerm)||~isreal(possiblePerm)||any(~isfinite(possiblePerm))||any(possiblePerm~=fix(possiblePerm)))
    boolVal=false;
    return;
end

numEls=length(possiblePerm);

possiblePerm=possiblePerm-startIdx+1;
if(any(possiblePerm>numEls)||any(possiblePerm<1))
    boolVal=false;
    return;
end

numValsPresent=zeros(numEls,1);
for k=1:numEls
    idx=possiblePerm(k);
    numValsPresent(idx)=numValsPresent(idx)+1;
end

boolVal=all(numValsPresent==1);

end

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


function SE = functionPowerOptimization_maxmin(signal,interference,Pmax,prelogFactor)
%Compute DL power allocation that solves the max-min fairness problem,
%using the algorithm in Theorem 7.1.
%
%This function require additional software packages to be used, which
%need to be downloaded and installed separately. These packages are
%developed independently and are delivered with separate licenses.
%The implementation uses CVX (http://cvxr.com/cvx) and has been tested
%using CVX version 2.1. We recommend the use of the Mosek solver (we
%have tested using version 7.1.0.12).
%
%INPUT:
%signal       = K x L matrix where element (k,j) is a_jk in (7.2)
%interference = K x L x K x L matrix where (l,i,j,k) is b_lijk in (7.3)
%Pmax         = Maximum transmit power per BS
%prelogFactor = Prelog factor
%
%OUTPUT:
%SE = K x L matrix where element (k,j) is the downlink SE of UE k in cell j
%     using the max-min power allocation solution
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2019-03-16)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%Check which UEs that have non-zero channels, because these ones are
%excluded (they are considered inactive)
nonzero = reshape(signal,[K*L 1]);
nonzero = nonzero(nonzero>0);

%Initalize the gamma-variables in Algorithm 1
rateLower = 0;
rateUpper = log2(1+Pmax*min(nonzero));

%Set the accuracy of the bisection
delta = 0.01;

%Prepare to save the power solution
rhoBest = zeros(K,L);

%Solve the max-min problem by bisection - iterate until the difference
%between the lower and upper points in the interval is smaller than delta
while norm(rateUpper - rateLower) > delta

    %Compute the midpoint of the line. Note that we are performing the
    %bisection in the SE domain instead of the SINR domain as in Algorithm
    %1, since we can then specify delta as the SE difference
    rateCandidate = (rateLower+rateUpper)/2;

    %Transform the midpoints into SINR requirements
    gammaCandidate = 2.^(rateCandidate)-1;

    %Solve the feasibility problem using CVX
    [feasible,rhoSolution] = functionFeasibilityProblem_cvx(signal,interference,Pmax,K,L,gammaCandidate);

    %If the problem was feasible, then replace rateLower with
    %gammaCandidate and store rhoSolution as the new best solution.
    if feasible
        rateLower = rateCandidate;
        rhoBest = rhoSolution;
    else
        %If the problem was not feasible, then replace ratePoint with
        %gammaCandidate
        rateUpper = rateCandidate;
    end

end

%Compute the SEs using Theorem 4.6
SE = functionComputeSE_DL_poweralloc(rhoBest,signal,interference,prelogFactor);

function [feasible,rhoSolution] = functionFeasibilityProblem_cvx(signal,interference,Pmax,K,L,SINR)
%Solve the linear feasibility problem in Algorithm 1 using CVX, by adding
%an extra variable so that it becomes a minimization problem with better
%properties.

cvx_begin
cvx_quiet(true); % This suppresses screen output from the solver

variable rho(K,L);  %Variable for the K x L power allocation matrix
variable scaling    %Scaling parameter for power constraints

minimize scaling %Minimize the power indirectly by scaling the power constraints

subject to

for j = 1:L

    for k = 1:K

        if signal(k,j)>0

            %SINR constraints
            SINR*(sum(sum(rho.*interference(:,:,k,j))) + 1) - (rho(k,j)*signal(k,j)) <= 0

        end

        rho(k,j)>=0

    end

    sum(rho(:,j)) <= scaling*Pmax;

end

scaling >= 0; %Power constraints must be positive

cvx_end

%% Analyze the CVX output and prepare the output variables
if isempty(strfind(cvx_status,'Solved')) %Both the power minimization problem and the feasibility problem are infeasible
    feasible = false;
    rhoSolution = [];
elseif scaling>1 %Only the power minimization problem is feasible
    feasible = false;
    rhoSolution = rho;
else %Both the power minimization problem and feasibility problem are feasible
    feasible = true;
    rhoSolution = rho;
end


% l1dantzig_pd.m
%
% Solves
% min_x  ||x||_1  subject to  ||A'(Ax-b)||_\infty <= epsilon
%
% Recast as linear program
% min_{x,u}  sum(u)  s.t.  x - u <= 0
%                         -x - u <= 0
%             A'(Ax-b) - epsilon <= 0
%            -A'(Ax-b) - epsilon <= 0
% and use primal-dual interior point method.
%
% Usage: xp = l1dantzig_pd(x0, A, At, b, epsilon, pdtol, pdmaxiter, cgtol, cgmaxiter)
%
% x0 - Nx1 vector, initial point.
%
% A - Either a handle to a function that takes a N vector and returns a K
%     vector , or a KxN matrix.  If A is a function handle, the algorithm
%     operates in "largescale" mode, solving the Newton systems via the
%     Conjugate Gradients algorithm.
%
% At - Handle to a function that takes a K vector and returns an N vector.
%      If A is a KxN matrix, At is ignored.
%
% b - Kx1 vector of observations.
%
% epsilon - scalar or Nx1 vector of correlation constraints
%
% pdtol - Tolerance for primal-dual algorithm (algorithm terminates if
%     the duality gap is less than pdtol).
%     Default = 1e-3.
%
% pdmaxiter - Maximum number of primal-dual iterations.
%     Default = 50.
%
% cgtol - Tolerance for Conjugate Gradients; ignored if A is a matrix.
%     Default = 1e-8.
%
% cgmaxiter - Maximum number of iterations for Conjugate Gradients; ignored
%     if A is a matrix.
%     Default = 200.
%
% Written by: Justin Romberg, Caltech
% Email: jrom@acm.caltech.edu
% Created: October 2005
%

function xp = l1dantzig_pd(x0, A, At, b, epsilon, pdtol, pdmaxiter, cgtol, cgmaxiter)

largescale = isa(A,'function_handle');

if (nargin < 6), pdtol = 1e-3;  end
if (nargin < 7), pdmaxiter = 50;  end
if (nargin < 8), cgtol = 1e-8;  end
if (nargin < 9), cgmaxiter = 200;  end

N = length(x0);

alpha = 0.01;
beta = 0.5;
mu = 10;

gradf0 = [zeros(N,1); ones(N,1)];

% starting point --- make sure that it is feasible
if (largescale)
  if (max( abs(At(A(x0) - b)) - epsilon ) > 0)
    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
    AAt = @(z) A(At(z));
    [w, cgres] = cgsolve(AAt, b, cgtol, cgmaxiter, 0);
    if (cgres > 1/2)
      disp('A*At is ill-conditioned: cannot find starting point');
      xp = x0;
      return;
    end
    x0 = At(w);
  end
else
  if (max(abs(A'*(A*x0 - b)) - epsilon ) > 0)
    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
    opts.POSDEF = true; opts.SYM = true;
    [w, hcond] = linsolve(A*A', b, opts);
    if (hcond < 1e-14)
      disp('A*At is ill-conditioned: cannot find starting point');
      xp = x0;
      return;
    end
    x0 = A'*w;
  end
end
x = x0;
u = (0.95)*abs(x0) + (0.10)*max(abs(x0));

% set up for the first iteration
if (largescale)
  Atr = At(A(x) - b);
else
  Atr = A'*(A*x - b);
end
fu1 = x - u;
fu2 = -x - u;
fe1 = Atr - epsilon;
fe2 = -Atr - epsilon;
lamu1 = -(1./fu1);
lamu2 = -(1./fu2);
lame1 = -(1./fe1);
lame2 = -(1./fe2);
if (largescale)
  AtAv = At(A(lame1-lame2));
else
  AtAv = A'*(A*(lame1-lame2));
end

% sdg = surrogate duality gap
sdg = -[fu1; fu2; fe1; fe2]'*[lamu1; lamu2; lame1; lame2];
tau = mu*(4*N)/sdg;

% residuals
rdual = gradf0 + [lamu1-lamu2 + AtAv; -lamu1-lamu2];
rcent = -[lamu1.*fu1; lamu2.*fu2; lame1.*fe1; lame2.*fe2] - (1/tau);
resnorm = norm([rdual; rcent]);

% iterations
pditer = 0;
done = (sdg < pdtol) | (pditer >= pdmaxiter);
dispProgress('Newton', 0, pdmaxiter);
while (~done)

  % solve for step direction
  w2 = - 1 - (1/tau)*(1./fu1 + 1./fu2);

  sig11 = -lamu1./fu1 - lamu2./fu2;
  sig12 = lamu1./fu1 - lamu2./fu2;
  siga = -(lame1./fe1 + lame2./fe2);
  sigx = sig11 - sig12.^2./sig11;

  if (largescale)
    w1 = -(1/tau)*( At(A(1./fe2-1./fe1)) + 1./fu2 - 1./fu1 );
    w1p = w1 - (sig12./sig11).*w2;
    hpfun = @(z) At(A(siga.*At(A(z)))) + sigx.*z;
    [dx, cgres, cgiter] = cgsolve(hpfun, w1p, cgtol, cgmaxiter, 0);
    if (cgres > 1/2)
      disp('Cannot solve system.  Returning previous iterate.  (See Section 4 of notes for more information.)');
      xp = x;
      return
    end
    AtAdx = At(A(dx));
  else
    w1 = -(1/tau)*( A'*(A*(1./fe2-1./fe1)) + 1./fu2 - 1./fu1 );
    w1p = w1 - (sig12./sig11).*w2;
    Hp = A'*(A*sparse(diag(siga))*A')*A + diag(sigx);
    opts.POSDEF = true; opts.SYM = true;
    [dx, hcond] = linsolve(Hp, w1p,opts);
    if (hcond < 1e-14)
      disp('Matrix ill-conditioned.  Returning previous iterate.  (See Section 4 of notes for more information.)');
      xp = x;
      return
    end
    AtAdx = A'*(A*dx);
  end
  du = w2./sig11 - (sig12./sig11).*dx;

  dlamu1 = -(lamu1./fu1).*(dx-du) - lamu1 - (1/tau)*1./fu1;
  dlamu2 = -(lamu2./fu2).*(-dx-du) - lamu2 - (1/tau)*1./fu2;
  dlame1 = -(lame1./fe1).*(AtAdx) - lame1 - (1/tau)*1./fe1;
  dlame2 = -(lame2./fe2).*(-AtAdx) - lame2 - (1/tau)*1./fe2;
  if (largescale)
    AtAdv = At(A(dlame1-dlame2));
  else
    AtAdv = A'*(A*(dlame1-dlame2));
  end

  % find minimal step size that keeps ineq functions < 0, dual vars > 0
  iu1 = find(dlamu1 < 0); iu2 = find(dlamu2 < 0);
  ie1 = find(dlame1 < 0); ie2 = find(dlame2 < 0);
  ifu1 = find((dx-du) > 0); ifu2 = find((-dx-du) > 0);
  ife1 = find(AtAdx > 0); ife2 = find(-AtAdx > 0);
  smax = min(1,min([...
    -lamu1(iu1)./dlamu1(iu1); -lamu2(iu2)./dlamu2(iu2); ...
    -lame1(ie1)./dlame1(ie1); -lame2(ie2)./dlame2(ie2); ...
    -fu1(ifu1)./(dx(ifu1)-du(ifu1)); -fu2(ifu2)./(-dx(ifu2)-du(ifu2)); ...
    -fe1(ife1)./AtAdx(ife1); -fe2(ife2)./(-AtAdx(ife2)) ]));
  s = 0.99*smax;

  % backtracking line search
  suffdec = 0;
  backiter = 0;
  while (~suffdec)
    xp = x + s*dx;  up = u + s*du;
    Atrp = Atr + s*AtAdx;  AtAvp = AtAv + s*AtAdv;
    fu1p = fu1 + s*(dx-du);  fu2p = fu2 + s*(-dx-du);
    fe1p = fe1 + s*AtAdx;  fe2p = fe2 + s*(-AtAdx);
    lamu1p = lamu1 + s*dlamu1;  lamu2p = lamu2 + s*dlamu2;
    lame1p = lame1 + s*dlame1; lame2p = lame2 + s*dlame2;
    rdp = gradf0 + [lamu1p-lamu2p + AtAvp; -lamu1p-lamu2p];
    rcp = -[lamu1p.*fu1p; lamu2p.*fu2p; lame1p.*fe1p; lame2p.*fe2p] - (1/tau);
    suffdec = (norm([rdp; rcp]) <= (1-alpha*s)*resnorm);
    s = beta*s;
    backiter = backiter+1;
    if (backiter > 32)
      disp('Stuck backtracking, returning last iterate.  (See Section 4 of notes for more information.)')
      xp = x;
      return
    end
  end

  % setup for next iteration
  x = xp;  u = up;
  Atr = Atrp;  AtAv = AtAvp;
  fu1 = fu1p; fu2 = fu2p;
  fe1 = fe1p; fe2 = fe2p;
  lamu1 = lamu1p; lamu2 = lamu2p;
  lame1 = lame1p; lame2 = lame2p;

  sdg = -[fu1; fu2; fe1; fe2]'*[lamu1; lamu2; lame1; lame2];
  tau = mu*(4*N)/sdg;

  rdual = rdp;
  rcent = -[lamu1.*fu1; lamu2.*fu2; lame1.*fe1; lame2.*fe2] - (1/tau);
  resnorm = norm([rdual; rcent]);

  pditer = pditer+1;
  done = (sdg < pdtol) | (pditer >= pdmaxiter);
  dispProgress('Newton', pditer/pdmaxiter);

%   disp(sprintf('Iteration = %d, tau = %8.3e, Primal = %8.3e, PDGap = %8.3e, Dual res = %8.3e',...
%     pditer, tau, sum(u), sdg, norm(rdual)));
%   if (largescale)
%     disp(sprintf('                CG Res = %8.3e, CG Iter = %d', cgres, cgiter));
%   else
%     disp(sprintf('                  H11p condition number = %8.3e', hcond));
%   end

end
dispProgress('Newton', 'Close');
end


function C = long(d)
%LONG         Long class constructor
%
%  C = long(d)
%
%  C is long scalar or column vector, array input d forced to be column vector
%
%Long representation
%
%  C = C.sign * sum( C.mantissa(i)*beta^(-i) ) * beta^C.exponent +/- C.error
%
%where C.error = C.error.mant * beta^C.error.exp and with
%  summation from 1 to precision (=size(C.mantissa,2)) with
%
%  1 <= precision <= INTLAB_LONG_PRECISION
%
%and integers
%
%  C.sign       in {-1,1}
%  C.mantissa   in 0 .. beta-1
%  C.exponent   representable integer ( -2^52+1 .. 2^52-1 )
%  C.error      nonnegative double, stored by C.error.mant and C.error.exp
%
%Computations can be executed with or w/o error term, see longinit. For
%  computational speed, comparison, min/max and absolute value refer to
%  midpoint.
%  To compare, for example, intervals (for computation with error term)
%  use inf(A)>sup(B) instead of A>B, and so forth.
%
%For control of working precision, see help longprecision. Base beta is a
%  power of 2 so that double precision floating point numbers are stored
%  without error.
%An example of long arithmetic with big cancellation is
%
%  x = long(-20);
%  y = long(1);  t = long(1);  i = 0;
%  while abs(t)>1e-20
%    i = i+1;
%    t = t*x/i;
%    y = y+t;
%  end
%  format long
%  Y = long2intval(y)
%
%producing
%
%  intval Y =
%    1.0e-008 *
%     0.20612_________
%
%The poor accuracy improves with more precision. After
%  longprecision(40);
%and the same statements as above we obtain
%
%  intval Y =
%    1.0e-008 *
%     0.20611536224378
%
%For more information try demolong .
%

% written  12/30/98     S.M. Rump
% modified 02/09/01     S.M. Rump  performance improvement
% modified 09/29/02     S.M. Rump  care for NaN components
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 08/26/12     S.M. Rump  global variables removed
%

  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');
  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');

  if nargin==0
    C.sign = [];
    C.exponent = [];
    C.mantissa = [];
    C.error.mant = [];
    C.error.exp = [];
    C = class(C,'long');
    return
  end

  if isa(d,'long')
    C = d;
  else
    if isempty(d)
      C.sign = [];
      C.exponent = [];
      C.mantissa = [];
      C.error.mant = [];
      C.error.exp = [];
      C = class(C,'long');
      return
    end
    sized = size(d);
    n = prod(sized);
    if n~=sized(1)
      warning('input array for long forced to be column vector')
      d = d(:);
    end
    indexNaN = isnan(d);
    d(indexNaN) = [];
    [s e m] = splitdble(d);

    % get sign
    C.sign = s;

    % get exponent
    q = ceil(e/INTLAB_LONG_LOGBETA);
    C.exponent = q;

    % get mantissa digits
    C.mantissa = zeros(size(d,1),ceil(53/INTLAB_LONG_LOGBETA)+1);
    p = 0;
    while any(m)
      p = p+1;
      m = m*INTLAB_LONG_BETA;
      C.mantissa(:,p) = floor(m);
      m = m - C.mantissa(:,p);
    end

    % treat zero components
    index = ( d==0 );
    if any(index)
      C.sign(index) = 1;
      C.exponent(index) = -inf;
    end

    % adjust mantissa digits to exponent
    r = INTLAB_LONG_LOGBETA*q - e;
    index = ( r~=0 );
    if any(index)
      C.mantissa(index,:) = shiftright(C.mantissa(index,:),r(index));
    end

    % omit trailing zeros (improves performance)
    [m index] = max(C.mantissa(:,end:-1:1)~=0,[],2);
    index(all(C.mantissa'==0)) = size(C.mantissa,2);
    if min(index)~=1
      C.mantissa = C.mantissa(:,1:end-min(index)+1);
    end

    if any(indexNaN)
      Csign = C.sign;
      Cexponent = C.exponent;
      Cmantissa = C.mantissa;
      C.sign = zeros(n,1);
      C.exponent = zeros(n,1);
      C.mantissa = zeros(n,size(Cmantissa,2));
      C.sign(indexNaN) = NaN;
      C.exponent(indexNaN) = NaN;
      C.mantissa(indexNaN) = NaN;
      C.sign(~indexNaN) = Csign;
      C.exponent(~indexNaN) = Cexponent;
      C.mantissa(~indexNaN) = Cmantissa;
    end

    % set error
    C.error.mant = zeros(n,1);
    C.error.exp = zeros(n,1);

    C = class(C,'long');

  end


function [Bx, By, Bz] = igrf(time, latitude, longitude, altitude, coord)

% IGRF Earth's magnetic field from IGRF model.
%
% Usage: [BX, BY, BZ] = IGRF(TIME, LATITUDE, LONGITUDE, ALTITUDE, COORD)
%     or [BX, BY, BZ] = IGRF(COEFS, LATITUDE, LONGITUDE, ALTITUDE, COORD)
%     or B = IGRF(TIME, LATITUDE, LONGITUDE, ALTITUDE, COORD)
%     or B = IGRF(COEFS, LATITUDE, LONGITUDE, ALTITUDE, COORD)
%
% Calculates the components of the Earth's magnetic field using the
% International Geomagnetic Reference Field (IGRF) model. The inputs for
% the position can be scalars or vectors (in the latter case each should
% have the same number of elements or be a scalar), but TIME must be a
% scalar.
%
% When all the coordinate inputs are scalars, the function can be run more
% efficiently by providing the proper IGRF coefficient vector for a given
% time rather than the time itself. This mode is useful when making
% multiple calls to the function while keeping the time the same (meaning
% the coefficients will be the same for each run) as loading the
% coefficients can be the most time-consuming part of the function. The
% coefficient vector can be easily loaded using the function LOADIGRFCOEFS.
% This mode is assumed when all the coordinate inputs are scalars and the
% first input is a vector. In this case, the coefficient vector should be
% formatted as (LOADIGRFCOEFS provides this):
%
%   [g(n=1,m=0) g(n=1,m=1) h(n=1,m=1) g(n=2,m=0) g(n=2,m=1) h(n=2,m=1) ...]
%
% Regardless of the size of the inputs, the outputs will be column vectors.
% If only one output is requested, B = [BX(:), BY(:), BZ(:)] is output.
% Note that the other parameters the IGRF gives can be computed from BX,
% BY, and BZ as:
%
%   Horizonal intensity: hypot(BX, BY) (i.e., sqrt(BX.^2 + BY.^2) )
%   Total intensity: hypot(BX, hypot(BY, BZ))
%   Declination: atan2(BY, BX)
%   Inclination: atan(BZ./hypot(BX, BY))
%
% This function relies on having the file igrfcoefs.mat in the MATLAB
% path to function properly when a time is input. If this file cannot be
% found, this function will try to create it by calling GETIGRFCOEFS.
%
% The IGRF is a spherical harmonic expansion of the Earth's internal
% magnetic field. Currently, the IGRF model is valid between the years 1900
% and 2015. See the health warning for the IGRF model here:
% http://www.ngdc.noaa.gov/IAGA/vmod/igrfhw.html
%
% Reference:
% International Association of Geomagnetism and Aeronomy, Working Group
% V-MOD (2010), International Geomagnetic Reference Field: the eleventh
% generation, _Geophys. J. Int._, _183_(3), 1216-1230,
% doi:10.1111/j.1365-246X.2010.04804.x.
%
% Inputs:
%   -TIME: Time to get the magnetic field values either in MATLAB serial
%   date number format or a string that can be converted into MATLAB serial
%   date number format using DATENUM with no format specified (see
%   documentation of DATENUM for more information).
%   -COEFS: Instead of inputting a time, you can simply specify the proper
%   coefficients for the time you want by inputting in the first argument
%   the proper coefficient vector from igrfcoefs.mat.
%   -LATITUDE: Geocentric or geodetic latitude in degrees.
%   -LONGITUDE: Geocentric or geodetic longitude in degrees.
%   -ALTITUDE: For geodetic coordiates, the height in km above the Earth's
%   surface. For geocentric coordiates, the radius in km from the center of
%   the Earth.
%   -COORD: String specifying the coordinate system to use. Either
%   'geocentric' or 'geodetic' (optional, default is geodetic). Note that
%   only geodetic coordinates have been verified.
%
% Outputs:
%   -BX: Northward component of the magnetic field in nanoteslas (nT).
%   -BY: Eastward component of the magnetic field in nT.
%   -BZ: Downward component of the magnetic field in nT.
%   -B: [BX(:), BY(:), BZ(:)].
%
% See also: LOADIGRFCOEFS, GETIGRFCOEFS, IGRFLINE, DATENUM, IGRF11MAGM.

% Run IGRFS if all position inputs are scalars.
if isscalar(latitude) && isscalar(longitude) && isscalar(altitude)
    if nargin < 5
        [Bx, By, Bz] = igrfs(time, latitude, longitude, altitude);
    else
        [Bx, By, Bz] = igrfs(time, latitude, longitude, altitude, coord);
    end
% Otherwise run IGRFV.
else
    if nargin < 5
        [Bx, By, Bz] = igrfv(time, latitude, longitude, altitude);
    else
        [Bx, By, Bz] = igrfv(time, latitude, longitude, altitude, coord);
    end
end

if nargout <= 1
    Bx = [Bx(:), By(:), Bz(:)];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%        IGRF vector function.        %%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Bx, By, Bz] = igrfv(time, latitude, longitude, altitude, coord)

% Fundamental constant.
Rearth_km = 6371.2;

%%% CHECK INPUT VALIDITY %%%
% Convert time to a datenumber if it is a string.
if ischar(time)
    time = datenum(time);
end

% Make sure time has only one element.
if numel(time) > 1
    error('igrf:timeInputInvalid', ['The input TIME can only have one ' ...
        'element.']);
end

% Check that the inputs all have either one or the same number of elements.
numlat = numel(latitude);
numlon = numel(longitude);
numalt = numel(altitude);
if numlat > 1
    if numlon == 1
        longitude = repmat(longitude, size(latitude));
    end
    if numalt == 1
        altitude = repmat(altitude, size(latitude));
    end
elseif numlon > 1
    latitude = repmat(latitude, size(longitude));
    if numalt == 1
        altitude = repmat(altitude, size(latitude));
    end
elseif numalt > 1
    latitude = repmat(latitude, size(altitude));
    longitude = repmat(longitude, size(altitude));
end
numlat = numel(latitude);
numlon = numel(longitude);
numalt = numel(altitude);
if numlat ~= numlon || numlat ~= numalt || numlon ~= numalt
    error('igrf:inputNotSameSize', ['The input coordinates must have ' ...
        'the same number of elements.']);
end

%%% SPHERICAL COORDINATE CONVERSION %%%
% Convert the latitude, longitude, and altitude coordinates input into
% spherical coordinates r (radius), theta (inclination angle from +z axis),
% and phi (azimuth angle from +x axis). Also, make the coordinates go down
% the rows.
% We want cos(theta) and sin(theta) rather than theta itself.
costheta = cos((90 - latitude(:))*pi/180);
sintheta = sin((90 - latitude(:))*pi/180);

% Convert from geodetic coordinates to geocentric coordinates if necessary.
% This method was adapted from igrf11syn, which was a conversion of the
% IGRF subroutine written in FORTRAN.
if nargin < 5 || isempty(coord) || strcmpi(coord, 'geodetic') || ...
        strcmpi(coord, 'geod') || strcmpi(coord, 'gd')
    a = 6378.137; f = 1/298.257223563; b = a*(1 - f);
    rho = hypot(a*sintheta, b*costheta);
    r = sqrt( altitude(:).^2 + 2*altitude(:).*rho + ...
        (a^4*sintheta.^2 + b^4*costheta.^2) ./ rho.^2 );
    cd = (altitude(:) + rho) ./ r;
    sd = (a^2 - b^2) ./ rho .* costheta.*sintheta./r;
    oldcos = costheta;
    costheta = costheta.*cd - sintheta.*sd;
    sintheta = sintheta.*cd + oldcos.*sd;
elseif strcmpi(coord, 'geocentric') || strcmpi(coord, 'geoc') || ...
        strcmpi(coord, 'gc')
    r = altitude(:);
    cd = 1;
    sd = 0;
else
    error('igrf:coordInputInvalid', ['Unrecognized command ' coord ...
        ' for COORD input.']);
end

% Special case when sin(theta) = 0.
sintheta0 = sintheta == 0;
anysintheta0 = any(sintheta0);
anysinthetanot0 = any(~sintheta0);

% Convert longitude to radians.
phi = longitude(:)*pi/180;

%%% GET PROPER IGRF COEFFICIENTS %%%
[g, h] = loadigrfcoefs(time);
nmax = size(g, 1);

% We need cos(m*phi) and sin(m*phi) multiple times, so precalculate into a
% matrix here:
cosphi = cos(bsxfun(@times, 0:nmax, phi));
sinphi = sin(bsxfun(@times, 0:nmax, phi));

%%% BEGIN MAGNETIC FIELD CALCULATION %%%
% Initialize variables used in for loop below.
Br = zeros(size(r));
Bt = zeros(size(r));
Bp = zeros(size(r));
lastP = 1;
lastdP_1 = 0;
lastdP_2 = 0;

% Sum for each n value.
for n = 1 : nmax

    m = 0 : n;

    % Calculate legendre values. The output of the function has each m
    % value going down the rows, but since m goes along the columns
    % (coordinates go down the rows, remember?), permute it.
    P = legendre(n, costheta, 'sch').';

    % We also need the derivative of the legendre with respect to theta. It
    % is given by a recursive function of both the previous legendre values
    % as well as the previous derivatives. Functionally, it is:
    % dP(0, 0) = 0, dP(1, 1) = cos(theta)
    % dP(n, n) = sqrt(1 - 1/(2n))*(sin(theta)*dP(n-1, n-1) +
    %     cos(theta)*P(n-1, n-1))
    % dP(n, m) = (2n - 1)/sqrt(n^2 - m^2)*(cos(theta)*dP(n-1, m) -
    %     sin(theta)*P(n-1, m)) - sqrt(((n-1)^2 - m^2)/(n^2 - m^2))*
    %     dP(n-2, m)
    dP = [bsxfun(@minus, bsxfun(@times, ...
        (2*n - 1)./sqrt(n^2 - m(1:end-1).^2), ...
        bsxfun(@times, costheta, lastdP_1) - bsxfun(@times, sintheta, ...
        lastP)), bsxfun(@times, sqrt(((n - 1)^2 - m(1:end-1).^2)./...
        (n^2 - m(1:end-1).^2)), lastdP_2)), zeros(size(costheta))];
    if n > 1
        dP(:, end) = sqrt(1 - 1/(2*n))*...
            (sintheta*lastdP_1(end) + costheta*lastP(end));
        lastdP_2 = [lastdP_1 zeros(size(costheta))];
    else
        dP(:, end) = costheta;
        lastdP_2 = lastdP_1;
    end
    lastP = P;
    lastdP_1 = dP;

    % Multiply coefficients by proper longitude trigonemetric term.
    gcos = bsxfun(@times, g(n, m + 1), cosphi(:, m + 1));
    gsin = bsxfun(@times, g(n, m + 1), sinphi(:, m + 1));
    hcos = bsxfun(@times, h(n, m + 1), cosphi(:, m + 1));
    hsin = bsxfun(@times, h(n, m + 1), sinphi(:, m + 1));

    % Calculate the magnetic field components as a running sum. Find
    % explicit expressions for these in Global Earth Physics: a Handbook of
    % Physical Constants by Thomas J. Aherns (1995), pg. 49. Link:
    % http://books.google.com/books?id=aqjU_NHyre4C&lpg=PP1&dq=Global%20
    % earth%20physics%3A%20a%20handbook%20of%20physical%20constants&pg=PA49
    % #v=onepage&q&f=false
    % (except equation 6 is missing a required 1/sin(theta) and m; correct
    % equations on page 5 (equations 3a-3c) of:
    % http://hanspeterschaub.info/Papers/UnderGradStudents/
    % MagneticField.pdf)
    a_r = (Rearth_km./r).^(n + 2);
    Br = Br + a_r.*(n+1).*sum((gcos + hsin).*P, 2);
    Bt = Bt - a_r.*sum((gcos + hsin).*dP, 2);
    % Different case when sin(theta) == 0 for phi component.
    if anysinthetanot0
        Bp(~sintheta0) = Bp(~sintheta0) - 1./sintheta(~sintheta0).*...
            a_r(~sintheta0).*sum(bsxfun(@times, m, ...
            (-gsin(~sintheta0, :) + hcos(~sintheta0, :)).*...
            P(~sintheta0, :)), 2);
    end
    if anysintheta0
        Bp(sintheta0) = Bp(sintheta0) - costheta(sintheta0).*...
            a_r(sintheta0).*sum((-gsin(sintheta0, :) ...
            + hcos(sintheta0, :)).*dP(sintheta0, :), 2);
    end

end

% Convert from spherical to (x,y,z) = (North,East,Down).
Bx = -Bt;
By = Bp;
Bz = -Br;

% Convert back to geodetic coordinates if necessary.
Bx_old = Bx;
Bx = Bx.*cd + Bz.*sd;
Bz = Bz.*cd - Bx_old.*sd;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%        IGRF scalar function.        %%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Bx, By, Bz] = igrfs(time, latitude, longitude, altitude, coord)

% Fundamental constant.
Rearth_km = 6371.2;

%%% CHECK INPUT VALIDITY %%%
% Convert time to a datenumber if it is a string.
if ischar(time)
    time = datenum(time);
end

% Check that the input coordinates are scalars.
if ~isscalar(latitude) || ~isscalar(longitude) || ~isscalar(altitude)
    error('igrf1:inputNotScalar', ...
        'The input coordinates must be scalars.');
end

%%% SPHERICAL COORDINATE CONVERSION %%%
% Convert the latitude, longitude, and altitude coordinates input into
% spherical coordinates r (radius), theta (inclination angle from +z axis),
% and phi (azimuth angle from +x axis).
% We want cos(theta) and sin(theta) rather than theta itself.
costheta = cos((90 - latitude)*pi/180);
sintheta = sin((90 - latitude)*pi/180);

% Convert from geodetic coordinates to geocentric coordinates if necessary.
% This method was adapted from igrf11syn, which was a conversion of the
% IGRF subroutine written in FORTRAN.
if nargin < 5 || isempty(coord) || strcmpi(coord, 'geodetic') || ...
        strcmpi(coord, 'geod') || strcmpi(coord, 'gd')
    a = 6378.137; f = 1/298.257223563; b = a*(1 - f);
    rho = hypot(a*sintheta, b*costheta);
    r = sqrt( altitude.^2 + 2*altitude.*rho + ...
        (a^4*sintheta.^2 + b^4*costheta.^2) ./ rho.^2 );
    cd = (altitude + rho) ./ r;
    sd = (a^2 - b^2) ./ rho .* costheta.*sintheta./r;
    oldcos = costheta;
    costheta = costheta.*cd - sintheta.*sd;
    sintheta = sintheta.*cd + oldcos.*sd;
elseif strcmpi(coord, 'geocentric') || strcmpi(coord, 'geoc') || ...
        strcmpi(coord, 'gc')
    r = altitude;
    cd = 1;
    sd = 0;
else
    error('igrf:coordInputInvalid', ['Unrecognized command ' coord ...
        ' for COORD input.']);
end

% Convert longitude to radians.
phi = longitude*pi/180;

%%% GET PROPER IGRF COEFFICIENTS %%%
if isscalar(time)
    gh = loadigrfcoefs(time);
    nmax = sqrt(numel(gh) + 1) - 1;
% Assume a vector input means the coefficients are the input.
else
    gh = time;
    nmax = sqrt(numel(gh) + 1) - 1;
    % nmax should be an integer.
    if nmax - round(nmax) ~= 0
        error('igrf:timeInputInvalid', ['TIME input should either be ' ...
            'a single date or a valid coefficient vector.']);
    end
end

% We need cos(m*phi) and sin(m*phi) multiple times, so precalculate into a
% vector here:
cosphi = cos((1:nmax)*phi);
sinphi = sin((1:nmax)*phi);

Pmax = (nmax+1)*(nmax+2)/2;

%%% BEGIN MAGNETIC FIELD CALCULATION %%%
% Initialize variables used in for loop below.
Br = 0; Bt = 0; Bp = 0;
 P = zeros(1, Pmax);  P(1) = 1;  P(3) = sintheta;
dP = zeros(1, Pmax); dP(1) = 0; dP(3) = costheta;

% For this initial condition, the first if will result in n = 1, m = 0.
m = 1; n = 0; coefindex = 1;

a_r = (Rearth_km/r)^2;

% Increment through all the n's and m's. gh will be a vector with g
% followed by h for incrementing through n and m except when h would be
% redundant (i.e., when m = 0).
for Pindex = 2:Pmax

    % Increment to the next n when m becomes larger than n.
    if n < m
        m = 0;
        n = n + 1;
        a_r = a_r*(Rearth_km/r); % We need (Rearth_km./r)^(n+2)
    end

    % Calculate P and dP. They are given recursively according to:
    %
    % P(0, 0) = 1, P(1, 1) = sin(theta) <- Specified above
    % P(n, n) = sqrt(1 - 1/(2n))*sin(theta)*P(n-1, n-1)
    % P(n, m) = (2n - 1)/sqrt(n^2 - m^2)*cos(theta)*P(n-1, m) -
    %     sqrt(((n-1)^2 - m^2) / (n^2 - m^2)) * P(n-2, m)
    %
    % dP(0, 0) = 0, dP(1, 1) = cos(theta) <- Specified above
    % dP(n, n) = sqrt(1 - 1/(2n))*(sin(theta)*dP(n-1, n-1) +
    %     cos(theta)*P(n-1, n-1))
    % dP(n, m) = (2n - 1)/sqrt(n^2 - m^2)*(cos(theta)*dP(n-1, m) -
    %     sin(theta)*P(n-1, m)) - sqrt(((n-1)^2 - m^2)/(n^2 - m^2))*
    %     dP(n-2, m)
    if m < n && Pindex ~= 3 % (Pindex=3 is n=1, m=1, initial cond. above)
        last1n = Pindex - n;
        last2n = Pindex - 2*n + 1;
        P(Pindex) = (2*n - 1)/sqrt(n^2 - m^2)*costheta*P(last1n) - ...
            sqrt(((n-1)^2 - m^2) / (n^2 - m^2)) * P(last2n);
        dP(Pindex) = (2*n - 1)/sqrt(n^2 - m^2)*(costheta*dP(last1n) - ...
            sintheta*P(last1n)) - sqrt(((n-1)^2 - m^2) / (n^2 - m^2)) * ...
            dP(last2n);
    elseif Pindex ~= 3
        lastn = Pindex - n - 1;
        P(Pindex) = sqrt(1 - 1/(2*m))*sintheta*P(lastn);
        dP(Pindex) = sqrt(1 - 1/(2*m))*(sintheta*dP(lastn) + ...
            costheta*P(lastn));
    end

    % Calculate the magnetic field components as a running sum. Find
    % explicit expressions for these in Global Earth Physics: a Handbook of
    % Physical Constants by Thomas J. Aherns (1995), pg. 49. Link:
    % http://books.google.com/books?id=aqjU_NHyre4C&lpg=PP1&dq=Global%20
    % earth%20physics%3A%20a%20handbook%20of%20physical%20constants&pg=PA49
    % #v=onepage&q&f=false
    % (except equation 6 is missing a required 1/sin(theta) and m; correct
    % equations on page 5 (equations 3a-3c) of:
    % http://hanspeterschaub.info/Papers/UnderGradStudents/
    % MagneticField.pdf)
    if m == 0 % Implies h = 0, so only coefficient in gh is g
        coef = a_r*gh(coefindex); %*cos(0*phi) = 1
        Br = Br + (n+1)*coef*P(Pindex);
        Bt = Bt - coef*dP(Pindex);
        % Bp is 0 for m = 0.
        coefindex = coefindex + 1; % Only need to skip over g this time.
    else
        coef = a_r*(gh(coefindex)*cosphi(m) + gh(coefindex+1)*sinphi(m));
        Br = Br + (n+1)*coef*P(Pindex);
        Bt = Bt - coef*dP(Pindex);
        if sintheta == 0 % Use different formula when dividing by 0.
            Bp = Bp - costheta*a_r*(-gh(coefindex)*sinphi(m) + ...
                gh(coefindex+1)*cosphi(m))*dP(Pindex);
        else
            Bp = Bp - 1/sintheta*a_r*m*(-gh(coefindex)*sinphi(m) + ...
                gh(coefindex+1)*cosphi(m))*P(Pindex);
        end
        coefindex = coefindex + 2; % Skip over g and h this time.
    end

    % Increment m.
    m = m + 1;

end

% Convert from spherical to (x,y,z) = (North,East,Down).
Bx = -Bt;
By = Bp;
Bz = -Br;

% Convert back to geodetic coordinates if necessary.
Bx_old = Bx;
Bx = Bx.*cd + Bz.*sd;
Bz = Bz.*cd - Bx_old.*sd;


%FEATSELM Feature selection map
%
% [W,R] = FEATSELM(A,CRIT,METHOD,K,T,PAR1,...)
%
% INPUT
%   A      	Training dataset
%   CRIT   	Name of criterion: 'in-in', 'maha-s', 'NN' or others
%           (see FEATEVAL) or an untrained classifier V (default: 'NN')
%   METHOD  - 'forward' : selection by featself (default)
% 	        - 'float'   : selection by featselp
% 	        - 'backward': selection by featselb
% 	        - 'b&b'     : branch and bound selection by featselo
% 	        - 'ind'     : individual
% 	        - 'lr'      : plus-l-takeaway-r selection by featsellr
%           - 'sparse'  : use sparse untrained classifier CRIT
%   K      	Desired number of features (default: K = 0, return optimal set)
%   T      	Tuning set to be used in FEATEVAL (optional)
%   PAR1,.. Optional parameters:
% 	        - L,R       : for 'lr' (default: L = 1, R = 0)
%
% OUTPUT
%   W       Feature selection mapping
%   R       Matrix with step by step results
%
% DESCRIPTION
% Computation of a mapping W selecting K features. This routines offers a
% central interface to all other feature selection methods. W can be used
% for selecting features in a dataset B using B*W.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FEATEVAL, FEATSELO, FEATSELB, FEATSELI,
% FEATSELP, FEATSELF, FEATSELLR

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [w,res] = featselm(a,crit,arg3,ksel,t,par1,par2)

	if (nargin < 2 | isempty(crit))
		prwarning(2,'criterion not specified, assuming NN');
		crit = 'NN';
	end
	if (nargin < 3 | isempty(arg3))
		prwarning(2,'method not specified, assuming forward');
		arg3 = 'forward';
	end
	if (nargin < 4)
		ksel = [];
	end
	if (nargin < 5)
		prwarning(3,'no tuning set supplied (risk of overfit)');
		t = [];
	end
	if (nargin < 6), par1 = []; end;
	if (nargin < 7), par2 = []; end;

	% If no arguments are supplied, return an untrained mapping.

	if (nargin == 0) | (isempty(a))
		w = prmapping('featselm',{crit,arg3,ksel,t,par1,par2});
		w = setname(w,'Feature Selection');
		return
	end

	a = testdatasize(a);
	[m,k] = size(a);

	if (isstr(arg3))
		method = arg3;												% If the third argument is a string,
		switch (method)												%   it specifies the method to use.
		 case {'forward','featself'}
		  [w,res] = featself(a,crit,ksel,t);
		 case {'float','featselp'}
		  [w,res] = featselp(a,crit,ksel,t);
		 case {'backward','featselb'}
		  [w,res] = featselb(a,crit,ksel,t);
		 case {'b&b','featselo'}
		  [w,res] = featselo(a,crit,ksel,t);
		 case {'ind','featseli'}
		  [w,res] = featseli(a,crit,ksel,t);
		 case {'lr','featsellr'}
		  [w,res] = featsellr(a,crit,ksel,par1,par2,t);
		 case {'sparse'}
		  v = a*crit;
		  if isaffine(v)
		  	v = getdata(v,'rot');
		  	w = featsel(size(a,2),find(v(:,1) == 0));
			else
				v = getdata(v,'beta');
		  end
		  w = featsel(size(a,2),find(v(:,1) ~= 0));
		 otherwise
		  error('Unknown method specified.')
		end
	elseif (ismapping(arg3))
		w = arg3;												% If the third argument is a mapping,
		isuntrained(w);									%  assert it is untrained and train
		[w,res] = feval(mfilename,a,crit,w.mapping_file,ksel,t,par1,par2);
	else
		error('Illegal method specified.')
	end

return


% Filtering A Delta Function Input Signal Example Part 2
%
% This example illustrates how numerical aliasing can be avoided by
% spatially smoothing the source mask.
%
% author: Bradley Treeby
% date: 19th January 2010
% last update: 24th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 256;           % number of grid points in the x (row) direction
dx = 10e-3/Nx;      % grid point spacing in the x direction [m]
kgrid = makeGrid(Nx, dx);

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]

% create a time array
num_time_steps = 1024;
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
kgrid.t_array = 0:dt:dt*(num_time_steps - 1);

% define a single element source
source_offset = 50;
source.p_mask = zeros(Nx, 1);
source.p_mask(1 + source_offset, 1) = 1;

% spatially smooth the source mask maintaining the maximum magnitude
source.p_mask = smooth(kgrid, source.p_mask, true);

% threshold out small values
source.p_mask(source.p_mask < 0.05) = 0;

% define a delta function input pulse
temporal_offset = 100;      % [time steps]
source_magnitude = 2;       % [au]
source_func = zeros(size(kgrid.t_array));
source_func(temporal_offset) = source_magnitude;

% assign and scale the input pulse based on the source mask
source.p(1:sum(source.p_mask ~= 0),:) = source.p_mask(source.p_mask ~= 0)*source_func;

% force the source mask to be binary
source.p_mask(source.p_mask ~= 0) = 1;

% define a single element sensor
sensor.mask = zeros(Nx, 1);
sensor.mask(end - source_offset, 1) = 1;

% run the simulation
sensor_data = kspaceFirstOrder1D(kgrid, medium, source, sensor, 'PMLSize', 30);

% compute the amplitude spectra of the recorded time series
[f, output_as] = spect(sensor_data, 1/dt);

% extract the maximum frequency supported by the grid (two points per
% wavelength)
f_max = kgrid.k_max * min(medium.sound_speed(:)) / (2*pi);

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the input and recorded time series
figure;
[t_sc, scale, prefix] = scaleSI(max(kgrid.t_array(:)));
plot(kgrid.t_array*scale, source.p(floor(sum(source.p_mask(:))/2) + 1, :), 'k-', kgrid.t_array*scale, sensor_data, 'b-');
xlabel(['Time [' prefix 's]']);
ylabel('Pressure [au]');
legend('input pulse', 'recorded pulse');

% plot the amplitude spectrum
[f_sc, scale, prefix] = scaleSI(max(f));
figure;
plot(f*scale, output_as, 'b-');
xlabel(['Frequency [' prefix 'Hz]']);
ylabel('Amplitude [au]');

% plot the maximum frequency supported by the grid
ylim = get(gca, 'YLim');
hold on;
line([f_max*scale, f_max*scale], [0, ylim(2)], 'LineStyle','--', 'Color', 'k');
legend('amplitude spectrum of recorded pulse', 'maximum frequency supported by grid');


function M = ptv_params_to_matrix(params, transform_type)
    if isnumeric(transform_type)
        if transform_type == -1
            transform_type = 'translate';
        elseif transform_type == -2
            transform_type = 'rigid';
        elseif transform_type == -3
            transform_type = 'rigid_scale';
        elseif transform_type == -4
            transform_type = 'affine';
        end
    end

    if strcmp(transform_type, 'affine')
        M = [1 + params(3), params(4), params(1); ...
          params(5), 1 + params(6), params(2)];
    elseif strcmp(transform_type, 'rigid')
        theta = params(3);
        M = [cos(theta), -sin(theta), params(1); ...
             sin(theta), cos(theta), params(2)];
    elseif strcmp(transform_type, 'rigid_scale')
        theta = params(3);
        scale = 1 + params(4);
        M = [cos(theta) * scale, -sin(theta) * scale, params(1); ...
             sin(theta) * scale, cos(theta) * scale, params(2)];
    elseif strcmp(transform_type, 'translate')
        M = [1, 0, params(1); ...
             0, 1, params(2)];
    end
end


function [sos,g] = driving_function_imp_nfchoa_pw(N,R,conf)
%DRIVING_FUNCTION_IMP_NFCHOA_PW second-order section representation for a
%plane wave in NFC-HOA
%
%   Usage: sos = driving_function_imp_nfchoa_pw(N,R,conf)
%
%   Input parameters:
%       N       - order of spherical Hankel function
%       R       - radius of secondary source array / m
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       sos     - second-order section representation
%       g       - scalar gain factor
%
%   See also: sound_field_imp, sound_field_imp_nfchoa,
%       driving_function_imp_nfchoa
%
%   References:
%       Spors, Kuscher, Ahrens (2011) - "Efficient realization of model-based
%       rendering for 2.5-dimensional near-field compensated higher order
%       Ambisonics", IEEE Workshop on Applications of Signal Processing to Audio
%       and Acoustics (WASPAA), pp. 61-64,
%       https://doi.org/10.1109/ASPAA.2011.6082325

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 3;
nargmax = 3;
narginchk(nargmin,nargmax);
isargpositivescalar(N,R);
isargstruct(conf);

%% ===== Configuration ==================================================
c = conf.c;
dimension = conf.dimension;
driving_functions = conf.driving_functions;

%% ===== Computation =====================================================
% Find spherical Hankel function zeros
[z,p] = sphbesselh_zeros(N);

% Get the delay and weighting factors
if strcmp('2D',dimension)

    % === 2-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented', ...
            'for a 2D plane wave.'],upper(mfilename),driving_functions);
    end

elseif strcmp('2.5D',dimension) || strcmp('3D',dimension)

    % === 2.5- & 3-Dimensional ==========================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        % 2.5D for a plane wave as source model
        %
        [sos, g] = zp2sos(p,z*c/R,1,'down','none');
        g = g * (-1)^abs(N) * 4*pi * R;
        %
        % Compare Spors et al. (2011), eq. (10)
        %
    otherwise
        error(['%s: %s, this type of driving function is not implemented', ...
            'for a 2.5D plane wave.'],upper(mfilename),driving_functions);
    end

else
    error('%s: the dimension %s is unknown.',upper(mfilename),dimension);
end


function [xUpdate,PUpdate,innov,Pzz,W]=cubKalUpdate(xPred,PPred,z,R,h,xi,w,innovTrans,measAvgFun,stateDiffTrans,stateTrans)
%CUBKALUPDATE Perform the measurement update step in the cubature Kalman
%             filter with additive measurement noise. Unlike the square
%             root version, one can use cubature points with negative
%             weights.
%
%INPUTS: xPred The xDim X 1 predicted target state.
%        PPred The xDim X xDim predicted state covariance matrix.
%            z The zDim X 1 vector measurement.
%            R The zDim X zDim measurement covariance matrix in the native
%              coordinate system of the measurement.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%           xi An xDim X numCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim) is used. It is
%              suggested that xi and w be provided to avoid needless
%              recomputation of the cubature points.
%            w A numCubPoints X 1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. If not transformation is necessary, this
%              parameter can be omitted or an empty matrix passed.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one might generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         PUpdate The updated xDim X xDim state covariance matrix.
%      innov, Pzz The zDimX1 innovation and the zDimXzDim innovation
%                 covariance matrix are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%If the function h needs additional parameters beyond the state, then the
%parameters can be passed by using an anonymous function as the function
%handle. For example, suppose that the measurement function is measFunc and
%it needs the additional parameters param1 and param2. In this instance,
%rather than using
%h=@measFunc
%one should use
%h=@(x)measFunc(x,param1,param2)
%This way, every time cubKalUpdate calls measFunc (via h) with a
%different x, those two parameters are always passed.
%
%The mathematics behind the function cubKalUpdate are described in
%detail in Section IX of [1] and in [2]. Note that this is essentially the
%"unscented Kalman filter" with additive noise. One simply has to provide
%the filter with the appropriate cubature points and weights.
%
%The optional parameters innovTrans and measAvgFun are not described in
%references [1] and [2], but allow for possible modifications to the filter
%as described in [3]. The parameters have been added to allow the filter to
%be used with angular quantities. For example, if the measurement consisted
%of range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%measAvgFun=@(z,w)[calcMixtureMoments(z(1,:),w);
%                  meanAng(z(2,:),w')];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] D. F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%September 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=size(xPred,1);

    if(nargin<6||isempty(xi))
        [xi,w]=fifthOrderCubPoints(xDim);
    end

    if(nargin<8||isempty(innovTrans))
        %The function just returns the input difference.
        innovTrans=@(a,b)bsxfun(@minus,a,b);
    end

    if(nargin<9||isempty(measAvgFun))
        measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
    end

    if(nargin<10||isempty(stateDiffTrans))
        stateDiffTrans=@(x)x;
    end

    if(nargin<11||isempty(stateTrans))
        stateTrans=@(x)x;
    end

    zDim=size(z,1);

    numCubPoints=size(xi,2);

    %cholSemiDef is used instead of chol in case a positive semi-definite
    %covariance matrix is passed.
    SPred=cholSemiDef(PPred,'lower');
    %Predicted cubature state points
    xPredPoints=stateTrans(transformCubPoints(xi,xPred,SPred));

    %Predicted cubature measurement points
    zPredPoints=zeros(zDim,numCubPoints);
    for curP=1:numCubPoints
        zPredPoints(:,curP)=h(xPredPoints(:,curP));
    end

    %Measurement prediction.
    zPred=measAvgFun(zPredPoints,w);

    %The innovation, transformed as necessary to keep values in a desired
    %range.
    innov=innovTrans(z,zPred);

    xPredCenPoints=stateDiffTrans(bsxfun(@minus,xPredPoints,xPred));
    %Centered, predicted cubature measurement points, transformed as
    %necessary to keep the values within a desired range.
    zPredCenPoints=innovTrans(zPredPoints,zPred);
    Pzz=R;
    Pxz=zeros(xDim,zDim);
    for curP=1:numCubPoints
        diff=zPredCenPoints(:,curP);
        Pzz=Pzz+w(curP)*(diff*diff');
        Pxz=Pxz+w(curP)*xPredCenPoints(:,curP)*diff';
    end

    %The filter gain
    W=Pxz/Pzz;

    %Updated state estimate
    xUpdate=stateTrans(xPred+W*innov);

    %Updated state covariance matrix
    %We could just do a simple one-line solution as in [1] and [2].
    %However, that does not guarantee that PUpdate will always be position
    %(semi)definite. Thus, we use an equivalent but more complicated update
    %formula based on Equation in Appendix C of [1].
    PUpdate=W*R*W';
    for curP=1:numCubPoints
        diff=stateDiffTrans(xPredCenPoints(:,curP)-W*zPredCenPoints(:,curP));
        PUpdate=PUpdate+w(curP)*(diff*diff');
    end
    %Ensure symmetry
    PUpdate=(PUpdate+PUpdate')/2;
end

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


function [w,run] = train_dual(x,w,lambda,priority,mod)

% Written by Thomas P Minka

flops(0);
% kernel matrix
c = x'*x;
dc = diag(c);
v = 1/lambda;
% for flop counting, multiplication by lambda is free

[d,n] = size(x);
% this assumes w = 0
alpha = repmat(1e-4,n,1);
% Keerthi-type initialization
alpha = repmat(1/n,n,1);
if nargin < 4
  priority = 1;
end
if nargin < 5
  mod = 0;
end

count = 1;
for iter = 1:2000
  old_alpha = alpha;
  % annealing for expt2
  %v = v + 1e2
  if ~priority
    if mod
      for i = 1:n
	theta = log(alpha(i)./(1-alpha(i)));
	g = v*(c(i,:)*alpha) + theta;
	a = alpha(i)*(1-alpha(i));
	h = v*dc(i) + 1/a;
	%h = h*a*a;
	%h = h*a + g*(1-2*alpha(i));
	if 1
	  a = alpha(i);
	  c2 = ((1+log(a))*a*h-g)/((1+log(a))*a/(1-a) + 1+log(1-a));
	  c1 = a*(h - c2/(1-a));
	  as = linspace(eps,1-eps,100);
	  f = [];
	  f2 = [];
	  f0 = 0.5*v*(alpha'*c*alpha) + ...
	      sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
	  for k = 1:length(as)
	    alpha(i) = as(k);
	    f(k) = 0.5*v*(alpha'*c*alpha) + ...
		sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
	    f2(k) = c1*as(k)*log(as(k))+c2*(1-as(k))*log(1-as(k));
	    f2(k) = f2(k) - (c1*a*log(a)+c2*(1-a)*log(1-a)) + f0;
	  end
	  if 0
	    plot(as,f,as,f2)
	    drawnow
	    ax = axis;
	    line([a a],[ax(3) ax(4)],'Color','r')
	    axis_pct
	    pause
	  end
	end
	theta = theta - g/h;
	alpha(i) = 1/(1+exp(-theta));
      end
      flops(flops + n*(6+2+flops_exp));
    else
      for i = 1:n
	g = v*(c(i,:)*alpha) + log(alpha(i)./(1-alpha(i)));
	h = v*dc(i) + 1/alpha(i)/(1-alpha(i));
	alpha(i) = alpha(i) - g./h;
	if alpha(i) < eps
	  alpha(i) = eps;
	elseif alpha(i) > 1-eps
	  alpha(i) = 1-eps;
	end
      end
    end
    flops(flops + n*(flops_mul(1,n,1)+3+flops_exp + 4 + 2));
  else
    % incremental algorithm
    if iter == 1
      g = v*(c*alpha) + log(alpha./(1-alpha));
      flops(flops + flops_mul(c,alpha)+n*(flops_exp+3));
    end
    for j = 1:n
      [dummy,i] = max(abs(g));
      %disp(['i=' num2str(i) ' g=' num2str(g(i))])
      %i = j;
      o_alpha = alpha(i);
      h = v*dc(i) + 1/alpha(i)/(1-alpha(i));
      if mod
	theta = log(alpha(i)./(1-alpha(i)));
	a = alpha(i)*(1-alpha(i));
	gt = g(i)*a;
	h = h*a*a + gt*(1-2*alpha(i));
	theta = theta - g/h;
	alpha(i) = 1/(1+exp(-theta));
      else
	alpha(i) = alpha(i) - g(i)./h;
      end
      if alpha(i) < eps
	alpha(i) = eps;
      elseif alpha(i) > 1-eps
	alpha(i) = 1-eps;
      end
      % update all g(i)
      da = alpha(i) - o_alpha;  % no cost
      dg = da*c(:,i)*v;
      dg(i) = dg(i) + log(alpha(i)/(1-alpha(i))) - ...
	  log(o_alpha/(1-o_alpha));
      % no cost for second log
      g = g + dg;
    end
    flops(flops + n*(2*n-1 + 4 + 2 + 2) + n*(n + flops_exp+4 + n));
  end

  % computations here don't count
  w = v*x*alpha;
  run.w(:,count) = w;
  run.flops(count) = flops;
  run.e(count) = logProb(x,w) -0.5/v*w'*w;
  e2(count) = 0.5/v*w'*w + sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
  count = count + 1;
  if rem(count,200) == 0
    fprintf('Kernel count %d\n', count)
  end

  if max(abs(alpha - old_alpha)) < 1e-8
    break
  end
end
if iter == 2000
  warning('not enough iters')
end
if nargout > 1
  figure(2)
  % e should go up, e2 go down
  plot(1:length(run.e), run.e, 1:length(e2), e2)
end


% OP_UXN_VXN_2D: assemble the matrix M = [m(i,j)], m(i,j) = (mu u_j x n, v_i x n), with n the exterior normal vector.
%
%   mat = op_uxn_vxn_2d (spu, spv, msh, coeff);
%   [rows, cols, values] = op_uxn_vxn_2d (spu, spv, msh, coeff);
%
% INPUT:
%
%  spu:   structure representing the space of trial functions (see sp_vector/sp_eval_boundary_side)
%  spv:   structure representing the space of test functions  (see sp_vector/sp_eval_boundary_side)
%  msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_eval_boundary_side)
%  coeff: physical parameter
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2009, 2010 Carlo de Falco, Rafael Vazquez
% Copyright (C) 2011, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_uxn_vxn_2d (spu, spv, msh, coeff)

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      shpu_iel = reshape (spu.shape_functions(:, :, :, iel), spu.ncomp, msh.nqn, spu.nsh_max);
      shpv_iel = reshape (spv.shape_functions(:, :, :, iel), spv.ncomp, msh.nqn, spv.nsh_max);

      normal_iel = reshape (msh.normal(:,:,iel), 2, msh.nqn);
      shpu_x_n = bsxfun (@times, shpu_iel(1,:,:), normal_iel(2,:)) - ...
                 bsxfun (@times, shpu_iel(2,:,:), normal_iel(1,:));
      shpv_x_n = bsxfun (@times, shpv_iel(1,:,:), normal_iel(2,:)) - ...
                 bsxfun (@times, shpv_iel(2,:,:), normal_iel(1,:));
      shpu_x_n = reshape (shpu_x_n, msh.nqn, 1, spu.nsh_max);
      shpv_x_n = reshape (shpv_x_n, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = jacdet_weights(:,iel);
      jacdet_shpu = bsxfun (@times, jacdet_iel, shpu_x_n);
      tmp1 = bsxfun (@times, jacdet_shpu, shpv_x_n);
      elementary_values = reshape (sum (tmp1, 1), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_uxn_vxn_2d: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_uxn_vxn_2d: wrong number of output arguments')
  end

end


function optimizeFFT( dimension, dim, kernelSize, fft_planner_method, fftdimPerformed, lambdaCalib, flagZeropadding, sPrecision )
%OPTIMIZEFFT optimize the fft command
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

fprintf('Optimizing fft command\n');
if(strcmp(dimension,'2D'))
    if(dim(4) == 1)
        dimImg = [dim(1), dim(2), dim(5)]; % y-x-cha
        dimKernel = [dimImg(1)+kernelSize(1)-1, dimImg(2)+kernelSize(2)-1, dimImg(3)];
    else
        dimImg = [dim(4), dim(1), dim(2), dim(5)]; % t-y-x-cha
        dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)];
    end
elseif(strcmp(dimension,'3D'))
    dimImg = [dim(1), dim(3), dim(2), dim(5)]; % y-z-x-cha
    dimKernel = [dimImg(1)+kernelSize(1)-1, dimImg(2)+kernelSize(2)-1, dimImg(3)+kernelSize(3)-1, dimImg(4)];
elseif(strcmp(dimension,'4D'))
    dimImg = [dim(4), dim(1), dim(3), dim(2), dim(5)]; % t - y - z - x - cha
    dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)+kernelSize(3)-1, dimImg(5)];
elseif(strcmp(dimension,'5D'))
    dimImg = [dim(4), dim(1), dim(3), dim(2), dim(6), dim(5)]; % t - y - z - x - g - cha
    dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)+kernelSize(3)-1, dimImg(5), dimImg(6)];
end

% check fft along dimensions
% maximal fftdims = {1, 2, 3, [1 2], [1 3], [2 3], 1:3};
fftdims = {fftdimPerformed};
if(lambdaCalib > 0)
    fftdims{end+1} = 1:3; % due to corrKernelKspace -> convKernelImgspace
end

% check if wisdom file already exists
if(evalin('base','exist(''fftw_wisdom'',''var'')'))
    sizesCalculated = evalin('base', 'fftw_wisdom.sizesCalculated');
    doOptimization = false;
    if(size(sizesCalculated,3) ~= length(fftdims) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimImg,'rows')) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimKernel,'rows')))
        evalin('base', 'clear ''fftw_wisdom''');
        doOptimization = true;
    end
else
    doOptimization = true;
end
% if(exist([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat'],'file'))
%     load([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat']);
%     doOptimization = false;
%     if(size(sizesCalculated,3) ~= length(fftdims) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimImg,'rows')) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimKernel,'rows')))
%         delete([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat']);
%         doOptimization = true;
%     end
% else
%     doOptimization = true;
% end

if(doOptimization)
    testImg = complex(randn(dimImg,sPrecision),randn(dimImg,sPrecision));
    if(flagZeropadding && lambdaCalib > 0)
        testKernel = complex(randn(dimKernel,sPrecision),randn(dimKernel,sPrecision));
    end

    sizesCalculated = zeros(2,length(dimImg),7);
    wisdom_str = cell(2,7);
    for i=1:length(fftdims)
        depth = sum(fftdims{i});
        if(length(fftdims{i}) > 1), depth = depth + 1; end;
        % optimize fft for image dimension
        fftw('planner',fft_planner_method);
        tmp = fftnshift(testImg,fftdims{i});
        wisdom_str{1,depth} = fftw('wisdom');
        sizesCalculated(1,:,depth) = dimImg;

        if(flagZeropadding && lambdaCalib > 0)
            % optimize fft for image kernel dimension
            fftw('planner',fft_planner_method);
            tmp = fftnshift(testKernel,fftdims{i});
            wisdom_str{2,depth} = fftw('wisdom');
            sizesCalculated(2,:,depth) = dimKernel;
        end
    end

    fftw_wisdom.sizesCalculated = sizesCalculated;
    fftw_wisdom.wisdom_str = wisdom_str;
    assignin('base', 'fftw_wisdom', fftw_wisdom);
%     save([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat'], 'wisdom_str', 'sizesCalculated');
    clear 'testimg' 'testKernel';
end


function y_buffer = BF_MakeBuffer(y,bufferSize)
% BF_MakeBuffer    Make a buffered version of a time series
%
% y_buffer contains segments (rows) of length bufferSize (columns) corresponding
% to consecutive, non-overlapping segments of the series of length bufferSize

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

N = length(y);

numBuffers = floor(N/bufferSize);

% May need trimming:
y_buffer = y(1:numBuffers*bufferSize);

% Then reshape:
y_buffer = reshape(y_buffer,bufferSize,numBuffers)';

% (Each buffer is a contiguous subsequence of the time series; a row of y_buffer)

end


function [sol, infos] = gsp_wiener_optimization(G, x0, f, psd, psd_noise, param)
%GSP_WIENER_OPTIMIZATION Solve wiener optimization problem
%   Usage:  sol = gsp_wiener_optimization(G, x0, ffid, psd, psd_noise)
%           sol = gsp_wiener_optimization(G, x0, ffid, psd, psd_noise, param)
%           [sol, infos] = gsp_wiener_optimization(...)
%
%   Input parameters:
%         G          : Graph (GSP structure)
%         x0         : Starting point (column vector)
%         f          : Fidelity term - UNLocBox structure
%         psd        : PSD filter (anonymous function)
%         psd_noise  : PSD filter of the noise or single number
%         param      : Optional optimization parameters
%   Output parameters:
%         sol        : Solution
%         infos      : Convergence informations
%
%   This function solves the following wiener optimization problem:
%
%     .. argmin_x f(x) + || w(L) x ||_2^2
%
%     .. math:: arg\min_x f(x) + \| w(L) x \|_2^2
%
%   Please refer to the reference for more information about this problem.
%   This function requires the UNLocBox to work.
%
%   Please refer to the function gsp_filter_analysis and solvep to know how
%   *param* can be set.
%
%   References: perraudin2016stationary

% Author : Nathanael Perraudin
% Date: 6 January 2016

if nargin<6
    param = struct;
end

if isnumeric(psd_noise)
    if sum(abs(psd_noise(:)))==0
        error('This function cannot solve this problem')
    end
    if sum(abs(psd_noise(:)))<1e-10
        warning('This function can prabaly not solve this case');
    end
    wl = @(x) psd_noise./(psd(x)+eps);
    fprox = @(T) @(x) psd(x)./(psd(x)+2*T*psd_noise + eps);
else
    wl = @(x) psd_noise(x)./(psd(x)+eps);
    fprox = @(T) @(x) psd(x)./(psd(x)+2*T*psd_noise(x) + eps);

end

%fprox = @(x) 1./(wl(x)+1);

% In order to be faster
param.stopping_criterion = 'rel_norm_obj';

% Wiener term
fwiener.prox = @(x,T) gsp_filter_analysis(G,fprox(T),x, param);
fwiener.eval = @(x) 0.5*norm(gsp_filter_analysis(G,wl,x,param),'fro')^2;

% Call the solver
[sol , infos ] = solvep(x0,{f,fwiener},param);

end