function [x,y_int,time] = sp_interpolate(x1,y1,y2,Settings)
% Shortest-path interpolation scheme which takes a single x-vector with two
% y-vectors and output the interpolated curve(s).

% Preamble
disp('# SHORTEST-PATH INTERPOLATION SCHEME:');
disp(' - Author:   Jesper Toft Falkenberg')
disp('             Theoretical nanoelectronics group')
disp('             Technical University of Denmark')
disp('             jestr@nanotech.dtu.dk')
disp(' - Journal:  Beilstein J. Nanotechnol. 2015, 6, 1603–1608.')
disp('             DOI: 10.3762/bjnano.6.164')
disp('             Please cite when using our method.')
disp(' - Version:  1.0.1')
disp(' - Date:     13-08-2015')
disp(' ');

% Default settings and used values
DefSettings.verbose       = 1;
DefSettings.figures       = 0;
DefSettings.weightx       = 1;
DefSettings.weighty       = 1;
DefSettings.broadening    = 0.2;
DefSettings.alphanum      = 3;
DefSettings.numpoints     = -2;
DefSettings.addpoints     = [];
zline = 1.05;
disp('# Settings:')
for i=fieldnames(DefSettings)'
    extra = ' ';
    if exist('Settings','var') && isfield(Settings,char(i))
        if (Settings.(char(i))~=DefSettings.(char(i)))
            extra = [' (default value = ' num2str(DefSettings.(char(i)),'%+ 8.2f') ')'];
            DefSettings.(char(i)) = Settings.(char(i));
        end
    end
    disp(sprintf(' - %-14s = %+ 8.2f %s', char(i),DefSettings.(char(i)),extra));
end
Settings = DefSettings;

% Check settings
if Settings.verbose~=0 && Settings.verbose~=1
    error('Error: Setting ''verbose'' is defined incorrectly.');
end
if Settings.figures~=0 && Settings.figures~=1
    error('Error: Setting ''figures'' is defined incorrectly.');
end
if Settings.weightx<0 || Settings.weightx>1
    error('Error: Setting ''weightx'' is defined incorrectly.');
end
if Settings.weighty<0 || Settings.weighty>1
    error('Error: Setting ''weighty'' is defined incorrectly.');
end
if Settings.broadening<0 || Settings.broadening>1
    error('Error: Setting ''broadening'' is defined incorrectly.');
end
if Settings.alphanum<1
    error('Error: Setting ''alphanum'' is defined incorrectly.');
end
if Settings.numpoints==0
    error('Error: Setting ''numpoints'' is defined incorrectly.');
end
if ~isnumeric(Settings.addpoints)
    error('Error: Setting ''add_points'' is defined incorrectly.');
end

if Settings.verbose
	disp(' ');
    disp(' - ''verbose'':       Print additional information when running routine.');
    disp(' - ''figures'':       Make figures in intermediate steps - great for debugging.');
    disp(' - ''weightx'':       The weight of all x-values when calculating distances.');
    disp(' - ''weighty'':       The weight of all y-values when calculating distances.');
    disp(' - ''broadening'':    Span of off-diagonal matrix to be examined. A smaller value');
    disp('                    speeds up calculations but can result in wrong interpolations.');
    disp(' - ''alphanum'':      The used number of intermediate curves when interpolating.');
    disp(' - ''numpoints'':     A negative value is used a multiplyer for the median point distance.');
    disp('                    which gives curves of a max length. A positive value linearly interpolates');
    disp('                    the original data to the specified amount of points (with equidistant x-values).');
    disp(' - ''addpoints'':     Add point in original data (for testing purposes).');
end
disp('# Settings: Done.');


% START ALGORITHM
disp(' ');
disp('# Run routine')
timings = [];


% STEP: Read in data from vectors and rotate
disp(' - Read data and rotate');
x1 = x1(:); x2 = x1; y1 = y1(:); y2 = y2(:);
x_orig = x1; y1_orig = y1;  y2_orig = y2;
if Settings.figures
    figure; plot(x1,y1,'.-',x2,y2,'.-'); axis tight; drawnow;
    title('\bf Original data curves');
end


% STEP: Refine data
tic
disp(' - Refining data-points');

if ~isempty(Settings.addpoints)
    for i=1:size(Settings.addpoints,1);
        data = Settings.addpoints(i,:);
        % Remove old point data
        id1 = find(x1==data(1)); x1(id1) = []; y1(id1) = [];
        id2 = find(x2==data(1)); x2(id2) = []; y2(id2) = [];
        % Insert new data
        x1 = [x1; data(1)]; y1 = [y1; data(2)];
        [x1,id1] = sort(x1); y1 = y1(id1);
        x2 = [x2; data(1)]; y2 = [y2; data(2)];
        [x2,id2] = sort(x2); y2 = y2(id2);
    end
end

maxdist1 = median(sqrt(diff(x1).^2+diff(y1).^2));
maxdist2 = median(sqrt(diff(x2).^2+diff(y2).^2));
maxdist  = abs(Settings.numpoints)*min(maxdist1,maxdist2);
if (Settings.numpoints<0)
    dist1 = sqrt(diff(x1).^2+diff(y1).^2);
    while max(dist1)>maxdist
        [~,i] = max(dist1);
        x1 = [x1(1:i); sum(x1(i:i+1))/2; x1(i+1:end)];
        y1 = [y1(1:i); sum(y1(i:i+1))/2; y1(i+1:end)];
        dist1 = sqrt(diff(x1).^2+diff(y1).^2);
    end
    dist2 = sqrt(diff(x2).^2+diff(y2).^2);
    while max(dist2)>maxdist
        [~,i] = max(dist2);
        x2 = [x2(1:i); sum(x2(i:i+1))/2; x2(i+1:end)];
        y2 = [y2(1:i); sum(y2(i:i+1))/2; y2(i+1:end)];
        dist2 = sqrt(diff(x2).^2+diff(y2).^2);
    end
    disp('   - Changing segment lengths');
else
    xnew = linspace(x1(1),x1(end),Settings.numpoints);
    y1 = interp1(x1,y1,xnew);
    y2 = interp1(x2,y2,xnew);
    x1 = xnew;
    x2 = xnew;
    disp('   - Changing original grid linearly');
end
if Settings.figures
    figure; plot(x1,y1,'.-',x2,y2,'.-'); axis tight; drawnow;
    title('\bf Refined data curves');
end
timings(end+1).time = toc;
timings(end).step = 'Refine data';


% STEP: Determine weighted distance landscape
tic
disp(' - Determining weighted distance landscape');
[X1,X2] = meshgrid(x1,x2);
[Y1,Y2] = meshgrid(y1,y2);
distance = sqrt(Settings.weightx*(X1-X2).^2 + Settings.weighty*(Y1-Y2).^2);
dims = size(distance);
if Settings.figures
    figure; surf(distance); 
    shading flat; axis tight; view([0 90]); colorbar; drawnow;
    title('\bf Weighted distance landscape');
end
timings(end+1).time = toc;
timings(end).step = 'Calculate weighted distance';

% STEP: Set illegal points in off-diagonal
tic
disp(' - Restricting phase-space by applying broadening');
illegal = ones(size(distance));
ratio = ceil(size(distance,2)*Settings.broadening);
for i=1:min(size(distance,1))
    j = ceil(i/size(distance,1)*size(distance,2));
    dj = [max(1,j-ratio) j min(size(distance,2),j+ratio)];
    illegal(i,dj(1):dj(3)) = 0;
end
% Output reduction in phase-space
A = numel(illegal);
B = sum(sum(illegal<1));
C = B/A*100;
disp(['   - Reduction found: ',num2str(A),' -> ',num2str(B),' (',num2str(C,'%.2f'),'%)']);
% Apply illegality to distance matrix
illdistance = distance.*~illegal;
node_ids = 1:numel(distance); node_ids = reshape(node_ids,size(distance,1),[]);
node_ids = sparse(node_ids.*~illegal);
if Settings.figures
    figure; surf(illegal); 
    shading flat; axis tight; view([0 90]); colorbar; drawnow;
    title('\bf Restricted phase-space'); drawnow;
    figure; surf(illdistance); 
    shading flat; axis tight; view([0 90]); colorbar; drawnow;
    title('\bf Restricted distance landscape'); drawnow;
end
timings(end+1).time = toc;
timings(end).step = 'Restrict phase-space';


% STEP: Implementation of shortest-path search
disp(' - Finding shortest path');
start  = [1 1];
target = size(distance);
closed_set = [];                                  % We have not examined any sites yet
open_set   = [full(node_ids(start(1),start(2)))]; % We begin at our starting point
came_from  = ([]);                                % So far we have no path
g_score = (zeros(size(node_ids)));
f_score = (zeros(size(node_ids)));
g_score(start) = 0;
f_score(start) = g_score(start);
tic
while ~isempty(open_set)
    [~,current] = min(f_score(open_set));
    curr = open_set(current);
    open_set(current) = [];
    closed_set = sort([closed_set curr]);
    [row,col] = find(node_ids==curr);
    if row==target(1) && col==target(2)
        break; % Let's quit if we have arrived at our destination
    end
    
    % loop trough neighbours -- we allow left, left up, up (if legal!)
    neighbour_list = [];
    if col+1<=dims(2) && ~illegal(row,col+1)
        neighbour_list(end+1,:) = [row col+1];
    end
    if row+1<=dims(1) && ~illegal(row+1,col)
        neighbour_list(end+1,:) = [row+1 col];
    end
    if row+1<=dims(1) && col+1<=dims(2) && ~illegal(row+1,col+1)
        neighbour_list(end+1,:) = [row+1 col+1];
    end
    %     if col>1 && ~illegal(row,col-1)
    %         neighbour_list(end+1,:) = [row col-1];
    %     end
    %     if row>1 && ~illegal(row-1,col)
    %         neighbour_list(end+1,:) = [row-1 col];
    %     end
    %     if row>1 && col+1<=dims(2) && ~illegal(row-1,col+1)
    %         neighbour_list(end+1,:) = [row-1 col+1];
    %     end
    %     if row+1<=dims(1) && col>1 && ~illegal(row+1,col-1)
    %         neighbour_list(end+1,:) = [row+1 col-1];
    %     end
    %     if row>1 && col>1 && ~illegal(row-1,col-1)
    %         neighbour_list(end+1,:) = [row-1 col-1];
    %     end
    
    for nid=1:size(neighbour_list,1)
        ncurr = neighbour_list(nid,:);
        nodeid = full(node_ids(ncurr(1),ncurr(2)));
        if any(find(nodeid==closed_set, 1))
            continue
        end
        tentative_g_score = g_score(current) + distance(ncurr(1),ncurr(2));
        if isempty(find(nodeid==open_set, 1)) || tentative_g_score < g_score(ncurr(1),ncurr(2))
            came_from(nodeid) = curr;
            g_score(nodeid)   = tentative_g_score;
            f_score(nodeid)   = g_score(nodeid);
            if isempty(find(nodeid==open_set, 1))
                open_set = sort([open_set nodeid]);
            end
        end
    end
end
if Settings.figures
    figure; surf(f_score);
    shading flat; axis tight; view([0 90]); colorbar;
    title('\bf Investigated sites in distance landscape'); drawnow;
end
timings(end+1).time = toc;
timings(end).step = 'Calculate shortest path';


% STEP: Reconstruct path
disp(' - Reconstructing path from source to destination');
tic
total_path = size(distance);
current = came_from(end);
while current>1
    current = came_from(current);
    [A,B] = find(node_ids==current);
    total_path(end+1,:) = [A B];
end
I = total_path(:,1); J = total_path(:,2);
I = I(end:-1:1); J = J(end:-1:1);
if Settings.figures
    figure; surf(illdistance); 
    shading flat; axis tight; view([0 90]); colorbar;
    hold all,plot3(J,I,zline*ones(size(I)),'-k','LineWidth',5)
    title('\bf Suggested shortest path through distance landscape'); drawnow;
end
timings(end+1).time = toc;
timings(end).step = 'Reconstruct path';


% STEP: Use path to interpolate curves
disp(' - Using path to interpolate intermediate curves');
tic
Path = [J(:) I(:)];
if Settings.figures
    figure; plot(x1,y1,'.-',x2,y2,'.-'); axis tight;
    title('\bf Original data curves with interpolated curves');
end
alpha = linspace(0,1,Settings.alphanum+2); alpha([1 end]) = [];
for ialpha = alpha;
    Curve = zeros(length(Path),2);
    for i=1:length(Path)
        j = Path(i,:);
        xi = (1-ialpha)*x1(j(1))+(ialpha)*x2(j(2));
        yi = (1-ialpha)*y1(j(1))+(ialpha)*y2(j(2));
        Curve(i,:) = [xi yi];
    end
    if Settings.figures
        hold all; plot(Curve(:,1),Curve(:,2),'.-'); hold off; drawnow;
    end
end
disp(['   - ',num2str(Settings.alphanum),' curves created']);
timings(end+1).time = toc;
timings(end).step = 'Interpolate curves';


% STEP: Debug plot - connect all interpolation points with dashed lines
disp(' - Debugging');
tic
if Settings.figures
    figure; plot(x1,y1,'.-',x2,y2,'.-');
    for i=1:length(Path)
        j = Path(i,:);
        xi = [x1(j(1)) x2(j(2))];
        yi = [y1(j(1)) y2(j(2))];
        if Settings.figures
            hold all; plot(xi,yi,':k'); hold off;
        end
    end
    title('\bf Debug plot - see where points connect to'); drawnow;
end
timings(end+1).time = toc;
timings(end).step = 'Debug';


% STEP: Get average of interpolated curves
disp(' - Calculating average of curves');
tic
Path = [J(:) I(:)];
if Settings.figures
    figure; plot(x1,y1,'.-',x2,y2,'.-');
end
alpha = linspace(0,1,Settings.alphanum+2); alpha([1 end]) = [];
Curve = zeros(length(alpha),length(Path),2);
Curve2 = zeros(length(alpha),length(x1));
for ialpha = 1:length(alpha);
    for i=1:length(Path)
        j = Path(i,:);
        xi = (1-alpha(ialpha))*x1(j(1))+(alpha(ialpha))*x2(j(2));
        yi = (1-alpha(ialpha))*y1(j(1))+(alpha(ialpha))*y2(j(2));
        Curve(ialpha,i,:) = [xi yi];
    end
    [~,id,~] = unique(Curve(ialpha,:,1));
    
    Curve2(ialpha,:) = interp1(Curve(ialpha,id,1),Curve(ialpha,id,2),x1);
    if Settings.figures
        hold all; plot(x1,Curve2(ialpha,:),'.-'); hold off
    end
end
Curve = mean(Curve2,1);
if Settings.figures
    hold all; plot(x1,Curve,'.-k','LineWidth',5); hold off; drawnow;
end
title('\bf Result of interpolation');
timings(end+1).time = toc;
timings(end).step = 'Calculate average';


% STEP: Map to original grid
disp(' - Mapping to original data grid');
tic
x = x_orig(:);
Curve3 = zeros(size(Curve2,1),length(x));
for i=1:size(Curve2)
    Curve3(i,:) = interp1(x1,Curve2(i,:),x);
    
    % Remove NAN values
    id = isnan(Curve3(i,:));
    Curve3(i,id) = y1_orig(id);
end
y_int = [y1_orig Curve3' y2_orig];
timings(end+1).time = toc;
timings(end).step = 'Map to original grid';
disp('# Run routine: Done.');

% STEP: Write timings
time = timings;
disp(' ');
disp('# Timings');
for i=1:length(time)
    disp(sprintf(' - %-30s: % 10.3f',time(i).step,time(i).time));
end
disp('   ------------------------------------------');
disp(sprintf(' - %-30s: % 10.3f','Total',sum(cat(1,time(:).time))));
disp('# Timings: Done.');
disp(' ');
disp('# SHORTEST-PATH INTERPOLATION SCHEME: Done.');