function [path] = findpath(pathout,p, pfreq)
% function [path] = findpath(pathout,p)
% THIS FUNCTION FINDS THE PATH AND PRESCRIBED VELOCITY VECTORS GIVEN A
% POINT P ON THE FINAL LEVELSET IN PATHOUT
%
% INPUTS:
%           PATHOUT:        Structure containing level set information
%   `        PATHOUT.MAG    Maximum magnitude relative velocity of vehicle
%            PATHOUT.N      Number of assets (vehicles)
%            PATHOUT.C      Cell array points defining level set curves (curves
%                            created using contourc.
%            PATHOUT.P      Points tracing current asset path (used for
%                            comparison and plots)
%            PATHOUT.T      Time at which each curve in pathout.c was generated
%            PATHOUT.V      Velocity vectors of flowfield at points in
%                            pathout.c
%           P:              Point to where you want asset to go. If not
%                            specified, take the maximum y valued point. 
%           PFREQ:          Frequency of output plots
%
% OUTPUTS:
%           PATH:            Structure containing the resultant path and
%                             predicted velocities (minus flow field)
%            PATH.p          Starting point
%            PATH.P          Entire predicted path of vehicle in fixed
%                             coordinates
%            PATH.V          Velocity predictions at each time [u,v,T]
%            PATH.MAG        Maximum magnitude relative velocity of vehicle
%
% See also: intersection, contourc
%
% Written by: Matt Ueckermann

if nargin < 3
    pfreq = 20;
end

%Find number of level set curve section
N = length(pathout.C);

%The curve extracted from contourc gives families of segmented curves, we
%want to insert NaN in spaces that gives the information about those
%families, and we also want to find the number of families
num=pathout.C{N}(2);
numS = 1;  %Where to start in array
numC=0;    %Number of families, current guess is 0
CC=[];     %Modified curve with NaN's inserted
Tact = 0;  %Actual time to reach point -- used for error checking
while numS < size(pathout.C{N},2)
    num = pathout.C{N}(2,numS);
    CC = [CC [NaN NaN]' pathout.C{N}(:,numS+1:numS+num)];
    numS = num+numS+1;
    numC=numC+1;
end

% Handle case is user did not input a point
if nargin<2 || isempty(p)
    ids = find(CC(2,:)==max(CC(2,:)));
    idsv= ids - sum(sum(isnan(CC(:,1:ids-1))))/2;
    p  = (CC(:,ids)'+CC(:,ids+1)')/2;
else
    d   = sqrt((CC(1,:)-p(1)).^2+(CC(2,:)-p(2)).^2);
    [p1 ids] = min(d);
    minids = ids + (-1:1);
    if ids < 3
        minids(1:3) = [size(CC, 2)-1,2,3];
    end
    if minids(3) > size(CC, 2)-1;
        minids(3) = 2;
    end
    [p tf] = project_a2line(p, CC(:, minids)');
    if tf(2) && ~tf(1)
        minids = [minids(3), minids(2), minids(1)];
    end    
    idsv(1) = minids(1) - sum(sum(isnan(CC(:,1:minids(1)))))/2;
    idsv(2) = minids(2) - sum(sum(isnan(CC(:,1:minids(2)))))/2;
%     %Project the user-given point onto the level set
%     d   = sqrt((CC(1,:)-p(1)).^2+(CC(2,:)-p(2)).^2);
%     [p1 ids] = min(d);
%     minids = ids + (-1:1);
%     if minids(1) < 1
%         minids(1) = size(CC, 2);
%     end
%     if minids(3) > size(CC, 2);
%         minids(3) = 1;
%     end
%     idsv= ids - sum(sum(isnan(CC(:,1:ids-1))))/2;
%     idsv = [idsv, idsv+1];
%     p = project_a2line(p, CC(:, minids)');
end

plot(p(1),p(2),'pk','MarkerSize',15,'MarkerFaceColor','k')

%Loop to generate path
for i = N:-1:2
    %Record path
    path.p(i,:) = p;

    if ~mod(i,pfreq)
    
    %Plot current contour and path-generated point, and previous path
    plot(CC(1,:),CC(2,:),'b'),hold on, axis equal
    plot(path.p(i,1),path.p(i,2),'ro-')
%    plot(pathout.p(i,1),pathout.p(i,2),'r.')
    title('Intermediate Contours and optimal path','FontSize',14) 
    end

    
    %Need to find the normal direction to the curve to know in which
    %direction the vehicle moved
    d1 = sqrt(sum(CC(:,minids(2))-p').^2);
    d2 = sqrt(sum(CC(:,minids(1))-p').^2);
    d = CC(:,minids(2))-CC(:,minids(1));
%     POLY=polyfit(CC(1,ids-1:ids+2),CC(2,ids-1:ids+2),2);
%     x = linspace(CC(1,ids-1),CC(1,ids+2),30);
%     plot(x,polyval(POLY,x),'r')

    %The point sits between two vertexes in the contour, so the normal can
    %be calculated using a straight line
    nrml = [d(2),-d(1)] ;
    nrml = nrml/norm(nrml);
    nrml1 = nrml;
    %However, the straight line normal is not the best. Consider the case
    %if the point is at a vertex, in that case you want to take an average
    %of the normals on either side of the vertex. The following blends the
    %normal from the other edge closes to the point with the normal
    %calculated above
    if d1<d2
        nrml2 = CC(:,minids(3))-CC(:,minids(2));
    else
        if minids(1) > 2
            nrml2 = CC(:,minids(1))-CC(:,minids(1)-1);
        else
            nrml2 = CC(:,minids(1))-CC(:,end-1);
        end
    end
        
    nrml2 = [nrml2(2),-nrml2(1)];
    nrml2 = nrml2/norm(nrml2);
    
    %The blending is such that the weights are 1/2 for each normal if the
    %point is a vertex, and 1 for the the straight-line normal if the point
    %is at the midpoint of the line
    nrml = nrml*(0.5+min(d1,d2)/(d1+d2)) + nrml2*(0.5-min(d1,d2)/(d1+d2));    
    nrml = nrml/norm(nrml);
    
    if isnan(nrml)
        nrml = nrml1;
    end
    
    if ~mod(i,pfreq)
    %Plot the calculated normal
%    quiver(p(1),p(2),nrml(1),nrml(2),0.05,'g')
    end
    
    %Need to figure out which way the normal is pointing, so perturb it and
    %see if it is inside or outside the curve
    d = sqrt(sum(d.^2));
    d = p+d*nrml;
    d = inpolygon(d(1),d(2),CC(1,:),CC(2,:));
    % If the point is NOT in the polygon, we want to reverse the normal
    % direction, because we're moving backwards, we're looking for the
    % inward pointing normal. 
    nrml = nrml*sign(d-0.5);
    
    
    %Now find the flowfield velocity at the current point
    d1 = sqrt(sum(CC(:,minids(2))-p').^2);
    d2 = sqrt(sum(CC(:,minids(1))-p').^2);
    v = (pathout.v{i}(:,idsv(2))*d1/(d1+d2)+ pathout.v{i}(:,idsv(1))*d2/(d1+d2))';
    %Plot total vector direction
%   tmp = (nrml*pathout.mag-v);
%%%%    tmp=tmp/norm(tmp);% Matt: Instead of normalizing, advance p by tmp*dt
%    if ~mod(i,pfreq)  %  dt is the dt of saving the level set.  May need to
%      quiver(p(1),p(2),tmp(1),tmp(2),0.05,'b') % reproject if don't hit next curve.
%    end
    
    
    %Now we have to advance the point in this direction till we hit the
    %next curve!
    dt = pathout.t(i) - pathout.t(i-1);
    p2 = p + (nrml*pathout.mag-v)*dt; %We need to project this point p2, unto the next level set curve. 
                            %(i.e. use same projection as used for p at start of this script)
%     if ~mod(i,pfreq)
%        plot(p2(1),p2(2),'x')
%     end
    
    %Save the velocity heading
    path.v(i,:) = -nrml*pathout.mag;

    %Fix the curve for the next timestep
    num=pathout.C{i-1}(2);
    numS = 1;
    numC=0;
    CC=[];
    while numS < size(pathout.C{i-1},2)
        num = pathout.C{i-1}(2,numS);
        CC = [CC [NaN NaN]' pathout.C{i-1}(:,numS+1:numS+num)];
        numS = num+numS+1;
        numC=numC+1;        
    end
%     if ~mod(i,pfreq)
%        plot(CC(1,:),CC(2,:),'m:x')
%     end
    
%%%% Now we can do the projection of p2
     %Since this is the same code as at the top of the script, I will
     %rename p2 -> p
     p = p2;
% doing the projection here
    %Project the user-given point onto the level set
    d   = sqrt((CC(1,:)-p(1)).^2+(CC(2,:)-p(2)).^2);
    [p1 ids] = min(d);
    minids = ids + (-1:1);
    if ids < 3
        minids(1:3) = [size(CC, 2)-1,2,3];
    end
    if minids(3) > size(CC, 2) - 1;
        minids(3) = 2;
    end
    [p tf] = project_a2line(p, CC(:, minids)', false);
    if tf(2) && ~tf(1)
        minids = [minids(3), minids(2), minids(1)];
    end
    
    idsv(1) = minids(1) - sum(sum(isnan(CC(:,1:minids(1)))))/2;
    idsv(2) = minids(2) - sum(sum(isnan(CC(:,1:minids(2)))))/2;
%     i
%     if i  <85
%     keyboard
%     end
%     if idsv (1) < 1
%         idsv = [1, 2];
%     end
    %keyboard
    
end

%Finalize outputs
path.v(1,:) = path.v(2,:);
path.p(1,:) = pathout.p(1,1:2);
path.P = path.p;
path.p = path.p(1,:);
path.mag = pathout.mag;
path.v = [path.v pathout.t'];
path.Tact = Tact;
