function pathplanning()
%TODO:
% - simplify uV(refined)
% - distance-dependent obstacle sizing (with a maximum)
% - field line gridding should be @ robot_radius outside the field,
% otherwise possible options can not be found .. for variable radii, this
% will be difficult .. in case not equal, this becomes some
% 'feasibility-indicator' (as is5 the N_grid value)
% - check feasibility should be based on 'small' obstacle radii, whereas
% the final path planning task should be based on conservative 'large'
% obstacle radii (hysteresis) .. the feasible part of the
% previous path is incorporated in the new path .. at the moment, this part
% is (automatically) not refined, (the same as for the
% start point inside an obstacle boundary), hence, the shortest way out of
% the obstacle boundary is sought, which might not be compatibel with the
% subsequent path .. is this desirable?

%init user input
plotAll = 1;
Npp = 0;                    %number of waypoints in the previous path
obstacleMode = 'predefined';    %'auto' or 'manual'
N_obst = 12;                 %number of obstacles
r_obst = .4;                %obstacle radius
dGenV = .1;                 %distance between generalized Voronoi points
minSmoothness = 150;        %[degrees] minimal smoothness of the final path
dPerc = .1;                 %[%/100] path refinement step size in percentage edge lengths

%init general
disp(' '); disp('NEW PATH PLANNING TASK')
format compact;
figure(2); clf; hold on; axis equal;

%init field
FIELDLENGTH = 1;
FIELDWIDTH = 1.5;
plot(FIELDWIDTH*[-1 1 1 -1 -1], FIELDLENGTH*[-1 -1 1 1 -1], 'g','linewidth',2);
% axis([(FIELDWIDTH+r_obst)*[-1 1], (FIELDLENGTH+r_obst)*[-1 1]]);

%% obstacles generation
%initialize N_obst obstacles with radus r_obst
NpObst=100;
Obst=[zeros(2,N_obst); ones(1,N_obst)*r_obst];

switch obstacleMode,
    case 'predefined',
        Obst(1:2,:) = [0 -2
    0 -4
-1.5 -1
1.5 -1
-3 -2
3 -2
0 2
0 4
-1.5 1
1.5 1
-3 2
3 2]';

        %plot obstacles
        for i1=1:N_obst,
            text(Obst(1,i1),Obst(2,i1),num2str(i1),'color','r');
            plot(sin(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(1,i1),cos(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(2,i1),'r');
        end

    case 'auto',
        rand('state',0);
        Obst(1,:) = (rand(1,N_obst)-.5)*2;
        Obst(2,:) = (rand(1,N_obst)-.5)*4;

        %plot obstacles
        for i1=1:length(x),
            text(Obst(1,i1),Obst(2,i1),num2str(i1),'color','r');
            plot(sin(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(1,i1),cos(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(2,i1),'r');
        end

    case 'manual',
        title(['indicate ',num2str(N_obst),' obstacles in the field']);
        for i1=1:N_obst,
            [Obst(1,i1),Obst(2,i1)] = ginput(1);

            %plot obstacles
            text(Obst(1,i1),Obst(2,i1),num2str(i1),'color','r');
            plot(sin(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(1,i1),cos(linspace(0,2*pi,NpObst+1))*Obst(3,i1)+Obst(2,i1),'r');
        end
        title('');
end

%number of vertices per obstacle
NgObst=zeros(1,N_obst);
NgObstTotal=0;
for i1=1:N_obst,
    NgObst(i1) = round(Obst(3,i1)*2*pi/dGenV);
    NgObstTotal = NgObstTotal+NgObst(i1);
end

%grid obstacles
ObstGridded=zeros(2,NgObstTotal);
for i1=1:N_obst,
    ObstGridded(1,(i1*NgObst(i1)-NgObst(i1)+1):i1*NgObst(i1)) = Obst(1,i1)+sin(2*pi/NgObst(i1)*[1:NgObst(i1)])*Obst(3,i1);
    ObstGridded(2,(i1*NgObst(i1)-NgObst(i1)+1):i1*NgObst(i1)) = Obst(2,i1)+cos(2*pi/NgObst(i1)*[1:NgObst(i1)])*Obst(3,i1);
end

%add gridded field boundary (and possible other field lines as well?!)
NgFW = round(2*FIELDWIDTH/dGenV);             %number of vertices per fieldline
NgFL = round(2*FIELDLENGTH/dGenV);
FWgrid = linspace(-1,1,NgFW);
FLgrid = linspace(-1,1,NgFL);
px = [ObstGridded(1,:), FIELDWIDTH*[FWgrid, ones(1,NgFL-2), -FWgrid, -ones(1,NgFL-2)]];
py = [ObstGridded(2,:), FIELDLENGTH*[ones(1,NgFW), FLgrid(2:end-1), -ones(1,NgFW), -FLgrid(2:end-1)]];

%% add start and end points, and previous path (optional)
IOtext={'start','end'};
xIO=zeros(2,1); yIO=zeros(2,1);
for i1=1:2,
    title(['indicate ',IOtext{i1},' point']);
    [xIO(i1),yIO(i1)]=ginput(1);
    plot(xIO(i1),yIO(i1),'r+');
    text(xIO(i1),yIO(i1),IOtext{i1},'horizontalalignment','center','verticalalignment','bottom');
end

%add previous path
xPP=zeros(1,Npp+2); yPP=xPP;

%include start point
xPP=xIO(1); yPP=yIO(1);

%add previous path vertices
for i1=1:Npp,
    title(['indicate ',num2str(Npp),' waypoints of a previous path w.r.t. the start point']);
    [xPP(i1+1),yPP(i1+1)]=ginput(1);
    plot(xPP(i1+1),yPP(i1+1),'r+');
    plot(xPP(i1:i1+1),yPP(i1:i1+1),'r--','linewidth',2);
end

%include end point
xPP(Npp+2)=xIO(2); yPP(Npp+2)=yIO(2);
plot(xPP(Npp+1:Npp+2),yPP(Npp+1:Npp+2),'r--','linewidth',2);
title('');

%% check feasibility of previous path
tic
[prevPath,kPP,xPP,yPP]=checkFeas(xPP,yPP,Obst,dPerc);

%keep track of time
splittime=toc;

%% Voronoi diagram
tic
if ~prevPath,
    %Delaunay triangulation (find natural neighbors)
    TRI = delaunay(px,py);

    %Voronoi diagram
    [vx,vy] = voronoi(px,py,TRI);
    vx_orig = vx; vy_orig = vy;

    %remove vertices outside field
    Ninside=0;
    vx_temp=zeros(size(vx)); vy_temp=vx_temp;
    for i1=1:size(vx,2),
        if abs(vx(1,i1))<FIELDWIDTH && abs(vx(2,i1))<FIELDWIDTH ...
                && abs(vy(1,i1))<FIELDLENGTH && abs(vy(2,i1))<FIELDLENGTH,
            Ninside=Ninside+1;
            vx_temp(:,Ninside)=vx(:,i1);
            vy_temp(:,Ninside)=vy(:,i1);
        end
    end
    vx=vx_temp(:,1:Ninside);
    vy=vy_temp(:,1:Ninside);

    %remove edges crossing obstacle boundaries
    [vx,vy]=filterEdges(vx,vy,Obst);

    %determine unique vertices
    V=[vx(1,:),vx(2,:); vy(1,:),vy(2,:)];
    [b,i_u,j_u]=unique(V','rows');
    uV = V(:,i_u);
    E = [j_u(1:size(vx,2)), j_u(size(vx,2)+1:end)]';

%% add previous path
    %determine closest vertices to connect start- and endpoints to
    dI=inf; dO=inf; i_ndI=0; i_ndO=1;
    for i_nds=1:length(uV),
        if Npp,
            xI=xPP(kPP); yI=yPP(kPP);
        else xI=xIO(1); yI=yIO(1);
        end
        d=sqrt((uV(1,i_nds)-xI)^2+(uV(2,i_nds)-yI)^2);
        if d<dI,
            i_ndI=i_nds;
            dI=d;
        end
        d=sqrt((uV(1,i_nds)-xIO(2))^2+(uV(2,i_nds)-yIO(2))^2);
        if d<dO,
            i_ndO=i_nds;
            dO=d;
        end
    end
    %add start- and endpoint vertices and corresponding edges
    NuV = size(uV,2);
    if Npp,
        uV = [uV, [xPP(kPP), xIO(2); yPP(kPP), yIO(2)]];
    else
        uV = [uV, [xIO'; yIO']];
    end
    E = [E, [NuV+1, NuV+2; i_ndI, i_ndO]];

    %keep track of time
    splittime=toc+splittime;

%% plotting 1/2
    %Voronoi points and lines
    if plotAll,
        plot(px,py,'b*');
        plot(vx_orig,vy_orig,'k--','linewidth',1);
        plot(vx,vy,'k--','linewidth',2);
        plot(vx,vy,'ko');
    end

%% find shortest path and refine path
    tic
    [dist,pathOrig] = dijkstra(uV',E',NuV+1,NuV+2);
    if ~isnan(pathOrig),

        %refine path - skip obsolete vertices
        pathRefined=refinePath(uV,pathOrig,Obst);
        pathRefined1=pathRefined;

        %add preferable init direction to path
        if Npp,
            pathRefined=[size(uV,2)+(1:kPP-1), pathRefined];
            uV=[uV, [xPP(1:kPP-1); yPP(1:kPP-1)]];
        end

        %refine path - smoothen sharp edge-vertex incidents
        k_max=10;               %maximum number of iterations
        uVrefined=uV;
        refined=1; k=0;
        while refined && k<k_max,
            [pathRefined,uVrefined,refined] = refineIncidents(uVrefined,pathRefined,Obst,minSmoothness,dPerc);
            k=k+1;
        end
        %     k
    end
end
%keep track of time
totaltime=toc+splittime
    
%% plotting 2/2
%global path
if prevPath,
    plot(xPP,yPP,'g-','linewidth',2);
    plot(xPP,yPP,'go','linewidth',2);
else
    if isnan(pathOrig),
        error('no feasible path found!')
    else
        if plotAll,
            plot(uV(1,pathOrig),uV(2,pathOrig),'r-','linewidth',2)
            plot(uV(1,pathRefined1),uV(2,pathRefined1),'b-','linewidth',2)
            plot(uV(1,pathRefined1),uV(2,pathRefined1),'bo','linewidth',2)
            title('Voronoi diagram (black), previous + Voronoi path (red), path without obsolete points (blue), refined path (green)')
        end
        plot(uVrefined(1,pathRefined),uVrefined(2,pathRefined),'g-','linewidth',2)
        plot(uVrefined(1,pathRefined),uVrefined(2,pathRefined),'go','linewidth',2)
    end
end