%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  SmoothLandmarks    //Smooths the secondary landmarks in a given loop
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [handles] = SmoothLandmarks(handles)
% if  handles.dat.currentLoopNumber<=0
%         set(handles.txtCurrentStatus, 'ForegroundColor', 'red', 'String', 'Must select a loop to smooth');
%     return;
% end
% handles = TurnOffAll(handles);
elements = get(handles.AAM, 'elements');
ce = elements{handles.dat.currentIndex};

pri = get(ce, 'primaries')';
pri = sort(pri);
pts = get(ce, 'pts')';
if isempty(pri)
    return;
end
pts_orig = pts;
currentLoop = handles.dat.currentLoopNumber;
loops = get(ce, 'loops');
ind = 1:size(pts,1);
if(length(pri) <= 1)
    set(handles.txtCurrentStatus, 'ForegroundColor', 'red', 'String', 'Need at least two primaries points');
    return;
end
SectionsToSmooth = length(pri) - 1;
loop = 1;
% % If loop is complete, loop back to first primary
pri = pri(:)';
if(loop)
    SectionsToSmooth = length(pri);
    pri = [pri pri(1)];
else
    SectionsToSmooth = length(pri) - 1;
end
lastloop = 0;
for k = 1:SectionsToSmooth
    NptsThisSection = pri(k+1) - pri(k) + 1;
    x = zeros(NptsThisSection, 1);
    y = zeros(NptsThisSection, 1);
    if(NptsThisSection < 0)
        lastloop = 1;
        NptsThisSection = size(pts, 1) - pri(k) + 2;
        x(1) = pts(pri(1),1);
        x(NptsThisSection) = pts(pri(k), 1);
        x(2:NptsThisSection-1) = pts(size(pts,1):-1:pri(k)+1,1)';
        y(1) = pts(pri(1), 2);
        y(NptsThisSection) = pts(pri(k), 2);
        y(2:NptsThisSection-1) = pts(size(pts, 1):-1:pri(k)+1, 2)';
    else
        x(1) = pts(pri(k), 1);
        x(NptsThisSection) = pts(pri(k+1), 1);
        x(2:NptsThisSection-1) = pts(pri(k)+1:pri(k)+NptsThisSection-2,1);
        y(1) = pts(pri(k), 2);
        y(NptsThisSection) = pts(pri(k+1), 2);
        y(2:NptsThisSection-1) = pts(pri(k)+1:pri(k)+NptsThisSection-2,2);
    end
    Nint = 50;
    t = 1:NptsThisSection;
    ts = 1:1/Nint:NptsThisSection;
    xfit = spline(t, x, ts); % x-coordinates of interpolating line
    yfit = spline(t, y, ts); % y-coordinates of interpolating line
    % Calculate the length of the line through the secondaries
    xfitS = xfit(2:length(xfit)) - xfit(1:length(xfit)-1);
    yfitS = yfit(2:length(yfit)) - yfit(1:length(yfit)-1);
    LineLength = cumsum(sqrt((xfitS.^2) + (yfitS.^2)));
    % Calculate what the spacing should be between the secondaries.
    DistanceBetweenPoints = LineLength(length(LineLength)) / (NptsThisSection - 1);
    % Clear from last iteration
    newx=[]; newy=[];
    % Find position of new points
    for n = 2:NptsThisSection-1
        idx = min(find(LineLength >= (n-1) * DistanceBetweenPoints)) + 1;
        newx(n-1) = xfit(idx);
        newy(n-1) = yfit(idx);
    end
    if(~lastloop)
        pts(pri(k)+1:pri(k+1)-1, 1) = newx';
        pts(pri(k)+1:pri(k+1)-1, 2) = newy';
    else
        pts(pri(k)+1:size(pts, 1), 1) = flipud(newx');
        pts(pri(k)+1:size(pts, 1), 2) = flipud(newy');
    end
end
pts_orig(ind, :) = pts;
ce = set(ce, 'pts', pts_orig');
elements{handles.dat.currentIndex} = ce;
handles.AAM = set(handles.AAM, 'elements', elements);
handles.dat.targetplotH = PlotModel(handles.targetAxes, ce, handles, handles.dat.targetplotH);
if(loop)
    pri = pri(1:length(pri)-1);
end

function find_segments_to_smooth(pri,ed,pts)
% consider each primary point in turn and create a list of points to which
% it is linked up to and including the next primary point
for i=1:length(pri)
    p=pri(i);
    ind=find(pri~=p);
    remaining_pri=pri(ind);
    segment=p;
    lists=find(ed==p);
    for j=1:length(lists) % chase down each in turn
        % this should be done recursively
    end
end