function Output=MultiWavelengthInterpolator(X_grid,Y_grid,Input_grid,X_vertices,Y_vertices,Elements,varargin)

% MultiWavelengthInterpolator

% Mike Wolovick, 4/4/2022

% This function interpolates a field from a regular grid onto an
% unstructured triangular mesh.  Crucially, this interpolator avoids
% aliasing and achieves approximate conservation.

% The general method is to generate multiple wavelength smoothings of the
% original grid at multiple resolutions.  Then, when interpolating onto the
% mesh, each mesh node interpolates from the grid with an appropriate
% resolution for its mesh size.  

% The result is not exactly conservative, but it is approximately so.  
% A quick test with interpolating ice thickness from the BedMachine 
% grid onto my model mesh for the Filchner-Ronne catchment resulted 
% in a volume error on the order of  10^-4 (measured as a fraction of 
% the volume computed on the input grid).  

% The output can be requested at either mesh vertices (default) or mesh
% faces. 

% Basic Inputs:
% X_grid, Y_grid, Input_grid       grid coordinates and values of input
% X_vertices,Y_vertices              coordinates of mesh vertices
% Elements                                indices defining mesh triangles

% Ouput:
% Output_mesh                output data, can be defined on either vertices
%                                      or faces

% Optional input:
% 'vertices' (default) or 'faces'


%% Check Inputs:

% Get sizes:
xsize=length(X_grid);
ysize=length(Y_grid);
numvertices=length(X_vertices);
numelements=size(Elements,1);

% Re-orient grid vectors:
if size(X_grid,1)>1
    X_grid=X_grid';
end
if size(Y_grid,2)>1
    Y_grid=Y_grid';
end

% Check remaining sizes:
if ~isequal(size(Input_grid),[ysize,xsize])
    error('Input grid size must match input grid vectors')
end
if size(X_vertices,2)>1
    if size(X_vertices,1)==1
        X_vertices=X_vertices';
    else
        error('Input vertex x-coordinates must be a vector')
    end
end
if size(Y_vertices,2)>1
    if size(Y_vertices,1)==1
        Y_vertices=Y_vertices';
    else
        error('Input vertex y-coordinates must be a vector')
    end
end
if length(Y_vertices)~=numvertices
    error('Length of vertex y-coordinates did not match length of vertex x-coordinates')
end
if size(Elements,2)~=3
    if size(Elements,1)==3
        Elements=Elements';
        numelements=size(Elements,1);
    else
        error('Input "Elements" must be either a 3-column or a 3-row matrix')
    end
end

% Parse varargin:
if ~isempty(varargin)
    % Check length:
    if length(varargin)~=1
        error('Too many inputs')
    else
        % Check value:
        if strcmp(varargin{1},'faces')
            outputlocation='faces';
        elseif strcmp(varargin{1},'vertices')
            outputlocation='vertices';
        else
            error(['Unrecognized input parameter value, "',varargin{1},'"'])
        end
    end
end
% Use default:
if ~exist('outputlocation','var')
    outputlocation='vertices';
end

% Check grid spacing:
dx=X_grid(2)-X_grid(1);
if max(abs((diff(X_grid)-dx)/dx))>1e-12
    error('Input gird did not have constant spacing in the x direction')
elseif abs((Y_grid(2)-Y_grid(1)-dx)/dx)>1e-12
    error('Input grid did not have equal spacing in the x and y directions')
elseif max(abs((diff(Y_grid)-dx)/dx))>1e-12
    error('Input gird did not have constant spacing in the y direction')
end

% Check values of elements:
if min(Elements(:))<1
    error('Input "Elements" cannot contain any value less than 1')
elseif max(Elements(:))>numvertices
    error('Input "Elements" cannot contain any values greater than the number of vertices')
elseif max(abs(Elements(:)-round(Elements(:))))>0
    error('Input "Elements" must be integer-valued')
end

%% Analyze Mesh:

% Compute edge lengths:
a=sqrt((X_vertices(Elements(:,1))-X_vertices(Elements(:,2))).^2+(Y_vertices(Elements(:,1))-Y_vertices(Elements(:,2))).^2);
b=sqrt((X_vertices(Elements(:,2))-X_vertices(Elements(:,3))).^2+(Y_vertices(Elements(:,2))-Y_vertices(Elements(:,3))).^2);
c=sqrt((X_vertices(Elements(:,3))-X_vertices(Elements(:,1))).^2+(Y_vertices(Elements(:,3))-Y_vertices(Elements(:,1))).^2);

% Compute element area:
s=(a+b+c)/2;
Area=sqrt(s.*(s-a).*(s-b).*(s-c));

% Compute hydraulic radius:
HydraulicRadius=Area./s;

% Check output location:
if strcmp(outputlocation,'faces')
    % Compute element centers:
    CentX=mean(X_vertices(Elements),2);
    CentY=mean(Y_vertices(Elements),2);
else
    % Average the hydraulic radius onto the vertices:
    % pre-allocate:
    HydraulicRadius_vertices=zeros(numvertices,1);
    Denom_vertices=zeros(numvertices,1);
    % loop through elements:
    for ii=1:numelements
        % add to running sums:
        HydraulicRadius_vertices(Elements(ii,:))=HydraulicRadius_vertices(Elements(ii,:))+Area(ii)*HydraulicRadius(ii);
        Denom_vertices(Elements(ii,:))=Denom_vertices(Elements(ii,:))+Area(ii);
    end
    % finish computing mean:
    HydraulicRadius_vertices=HydraulicRadius_vertices./Denom_vertices;
end

%% Perform Multi-Wavelength Smoothing of Grid:

% Compute number of smothings required:
if strcmp(outputlocation,'faces')
    numwavelengths=ceil(log2(max(HydraulicRadius)/dx))+1;
else
    numwavelengths=ceil(log2(max(HydraulicRadius_vertices)/dx))+1;
end

% Generate wavelengths vector:
Wavelengths=dx*(2.^[0:1:numwavelengths]');

% Pre-allocate:
X_multigrid=cell(numwavelengths,1);
Y_multigrid=cell(numwavelengths,1);
Input_multigrid=cell(numwavelengths,1);

% Assign first wavelength:
X_multigrid{1}=X_grid;
Y_multigrid{1}=Y_grid;
Input_multigrid{1}=Input_grid;

% Clear original variables:
clear *_grid

% Loop through wavelengths:
for ii=2:numwavelengths
    % Smooth from previous wavelength:
    ThisInput=ThreeByThree(Input_multigrid{ii-1},2);
    % Decimate to this wavelength:
    Input_multigrid{ii}=ThisInput(1:2:end,1:2:end);
    % Decimate coordinates:
    X_multigrid{ii}=X_multigrid{ii-1}(1:2:end);
    Y_multigrid{ii}=Y_multigrid{ii-1}(1:2:end);
end

%% Interpolate from Multi-Wavelength Grid to Mesh:

% Pre-allocate:
if strcmp(outputlocation,'faces')
    Output=zeros(numelements,1);
    HasInterpolated=false(numelements,1);
else
    Output=zeros(numvertices,1);
    HasInterpolated=false(numvertices,1);
end

% Loop through wavelengths:
for ii=1:numwavelengths
    % Identify which elements or vertices use this wavelength:
    if ii<numwavelengths
        if strcmp(outputlocation,'faces')
            TheseOnes=~HasInterpolated&HydraulicRadius<Wavelengths(ii);
        else
            TheseOnes=~HasInterpolated&HydraulicRadius_vertices<Wavelengths(ii);
        end
    else
        TheseOnes=~HasInterpolated;
    end
    % Interpolate to these elements or vertices:
    if strcmp(outputlocation,'faces')
        Output(TheseOnes)=interp2(X_multigrid{ii},Y_multigrid{ii},Input_multigrid{ii},CentX(TheseOnes),CentY(TheseOnes));
    else
        Output(TheseOnes)=interp2(X_multigrid{ii},Y_multigrid{ii},Input_multigrid{ii},X_vertices(TheseOnes),Y_vertices(TheseOnes));
    end
    % Flag that we've interpolated:
    HasInterpolated(TheseOnes)=1;
end


