function pathConfs = prm2
close all;
javaaddpath('C:\Documents and Settings\Mr. Hyde\My Documents\Grad School\Grad School Work\Robotic Motion Planning');
import java.util.*;
load hw6_2.mat
basepos = [5,5];
%line([basepos(1)-1,basepos(1)+1],[basepos(2) basepos(2)])
minNNDist = 1.2;
linkLengths = 1*ones(4,1);
numNeighbors = 3;

figure(1)
hold on
axis([0 10 0 10]);
for m = 1:length(obstacles)
    xVals = obstacles(m).xVals;
    yVals = obstacles(m).yVals;
    fill(xVals,yVals,'b');
end

goodStart = false;
while ~goodStart
    startConfig = struct('alpha',2*pi*rand(1),'beta',2*pi*rand(1),'gamma',2*pi*rand(1),...
        'kappa',2*pi*rand(1),'basepos',basepos,'linkLengths',linkLengths,'neighbors',[]);
    goodStart = checkConfig(startConfig, obstacles);
end
goodEnd = false;
while ~goodEnd
    endConfig = struct('alpha',2*pi*rand(1),'beta',2*pi*rand(1),'gamma',2*pi*rand(1),...
        'kappa',2*pi*rand(1),'basepos',basepos,'linkLengths',linkLengths,'neighbors',[]);
    goodEnd = checkConfig(endConfig, obstacles);
end


%|         1     b      2     g      3       k       4       e
%|alpha---------beta--------gamma----------kappa-------------
%|

configs = [];
if checkConfig(startConfig, obstacles) %collision free config
    configs = [configs; startConfig];
    e = calculate_e(startConfig);
    plot(e(1),e(2),'go')
end
if checkConfig(endConfig, obstacles) %collision free config
    configs = [configs; endConfig];
    e = calculate_e(endConfig);
    plot(e(1),e(2),'rx')
end
for n = 1:100
    %create configuration
    alpha = 2*pi*rand(1);
    beta = 2*pi*rand(1);
    gamma = 2*pi*rand(1);
    kappa = 2*pi*rand(1);

    new_config = struct('alpha',alpha,'beta',beta,'gamma',gamma,'kappa',kappa,'basepos',basepos,...
        'linkLengths',linkLengths,'neighbors',[]);
    
    if checkConfig(new_config, obstacles) %collision free config
        configs = [configs; new_config];
        e = calculate_e(new_config);
        plot(e(1),e(2),'k.')
    end
end

nneighbors = [];
for m = 1:length(configs)
    nneighbors = [];
    for n = 1:length(configs)
        if n ~= m
            new_config = configs(n);
            nneighbors = [nneighbors; [n configDist(new_config,configs(m))]];    
        end
    end
    nneighbors = sortrows(nneighbors,2);
    valid_neighbors = 0;
    ind = 1;
    while valid_neighbors < numNeighbors && ind < length(nneighbors)
        if localPlanner(configs(m), configs(nneighbors(ind,1)), obstacles)
            if ~ismember(m, configs(nneighbors(ind,1)).neighbors )
                %we don't store two edges, just one
                configs(m).neighbors = [configs(m).neighbors; nneighbors(ind,1)];
                e = calculate_e(configs(m));
                e_n = calculate_e(configs(nneighbors(ind,1)));
                plot([e(1) e_n(1)],[e(2) e_n(2)],'k:');
            end
            %but it is still a valid neighbor
            valid_neighbors = valid_neighbors + 1;
        end
        ind = ind + 1;
    end
%    cur_config = configs(m);
%    e = calculate_e(cur_config);
%     for k = 1:numNeighbors
%         cur_neigh = configs(nneighbors(k,1));
%         e_n = calculate_e(cur_neigh);
%         line([e(1) e_n(1)],[e(2) e_n(2)],'Color','k');
%     end
end
disp 'planning path'
unsort_pathConfs = planpath(configs)
if unsort_pathConfs(1) ~= 2
    unsort_pathConfs(2) = unsort_pathConfs(1);
    unsort_pathConfs(1) = 2;
end
pathConfs = myUnique(unsort_pathConfs)

fig_hand = figure(2);
hold on
axis([0 10 0 10]);
for m = 1:length(obstacles)
    xVals = obstacles(m).xVals;
    yVals = obstacles(m).yVals;
    fill(xVals,yVals,'b');
end
movie_index = 1;
for m = 1:length(pathConfs)
    cur_conf = configs(pathConfs(end-m+1));
    %draw arm
    b = cur_conf.basepos + cur_conf.linkLengths(1)*[cos(cur_conf.alpha),sin(cur_conf.alpha)];
    g = b + cur_conf.linkLengths(2)*[cos(cur_conf.beta),sin(cur_conf.beta)];
    k = g + cur_conf.linkLengths(3)*[cos(cur_conf.gamma),sin(cur_conf.gamma)];
    e = k + cur_conf.linkLengths(4)*[cos(cur_conf.kappa),sin(cur_conf.kappa)];
    plot([b(1);g(1);k(1);e(1)],[b(2);g(2);k(2);e(2)],'b.');
    line([cur_conf.basepos(1) b(1)],[cur_conf.basepos(2) b(2)],'Color','b')
    line([b(1) g(1)],[b(2) g(2)],'Color','b')
    line([g(1) k(1)],[g(2) k(2)],'Color','b')
    line([k(1) e(1)],[k(2) e(2)],'Color','b')
    for frame_count = 1:2
        Mv(movie_index) = getframe(fig_hand);
        movie_index = movie_index + 1;
    end
    plot([b(1);g(1);k(1);e(1)],[b(2);g(2);k(2);e(2)],'w.');
    line([cur_conf.basepos(1) b(1)],[cur_conf.basepos(2) b(2)],'Color','w')
    line([b(1) g(1)],[b(2) g(2)],'Color','w')
    line([g(1) k(1)],[g(2) k(2)],'Color','w')
    line([k(1) e(1)],[k(2) e(2)],'Color','w')
end
movie2avi(Mv,'arm_movie');


function cdist = configDist(config1, config2)

cdist = angleDist(config1.alpha, config1.beta, config1.gamma, config1.kappa,...
    config2.alpha, config2.beta, config2.gamma, config2.kappa);


function adist = angleDist(a1, b1, g1, k1, a2, b2, g2, k2)

adist = sqrt((a2-a1)^2 + (b2-b1)^2 + (g2-g1)^2 + (k2-k1)^2);


function unList = myUnique(theList)

unList = [];
for m = 1:length(theList)
    if ~ismember(theList(m), unList)
        unList = [unList; theList(m)];
    end
end



function success = localPlanner(config1, config2, obstacles)
step_inc = .4;
success = true;
%step through all possible configurations between config1 and config2 and
%check collisions.

for a_step = min(config1.alpha,config2.alpha):step_inc:max(config1.alpha,config2.alpha)

    for b_step = min(config1.beta,config2.beta):step_inc:max(config1.beta,config2.beta)
    
            for g_step = min(config1.gamma,config2.gamma):step_inc:max(config1.gamma,config2.gamma)
                
                    for k_step = min(config1.kappa,config2.kappa):step_inc:max(config1.kappa,config2.kappa)
                        
                        tempConfig = struct('alpha',a_step,'beta',b_step,'gamma',g_step,...
                            'kappa',k_step,'basepos',config1.basepos,...
                            'linkLengths',config1.linkLengths,'neighbors',[]);
                        
                        success = checkConfig(tempConfig,obstacles);
                        if ~success
                            return
                        end
                    end
            end
    end
                        
end


function okConfig = checkConfig(new_config, obstacles)

alpha = new_config.alpha; beta = new_config.beta; gamma = new_config.gamma; kappa = new_config.kappa;
linkLengths = new_config.linkLengths; basepos = new_config.basepos;

okConfig = true;

b = basepos + linkLengths(1)*[cos(alpha),sin(alpha)];
g = b + linkLengths(2)*[cos(beta),sin(beta)];
k = g + linkLengths(3)*[cos(gamma),sin(gamma)];
e = k + linkLengths(4)*[cos(kappa),sin(kappa)];

%check for collision at this orientation.

%for each link, check collision with each object

%Link1: line from basepos -- basepos+[cos(alpha),sin(alpha)]*10
%b = basepos + linkLengths(1)*[cos(alpha),sin(alpha)];

collision_b = false;
collision_g = false;
collision_k = false;
collision_e = false;
    
for m = 1:length(obstacles)
    x_values = obstacles(m).xVals;
    y_values = obstacles(m).yVals;
    %for all obstacle segments
    g_m = (b(2) - basepos(2))/(b(1) - basepos(1));
    for n = 1:(length(x_values) - 1)
        slope = (y_values(n+1)-y_values(n))/(x_values(n+1)-x_values(n));
        if (slope == Inf || slope == -Inf)
            intersection = [x_values(n);g_m*(x_values(n) - b(1)) + b(2)];
        else
            intersection = [-(g_m),1;-slope,1]...
                \[b(2) - g_m*b(1);y_values(n) - slope*x_values(n)];
        end
        %if intersection is on the object (endpoints ok) and between the

        if( ((intersection(1) < max([x_values(n),x_values(n+1)]))&&...
                (intersection(1) > min([x_values(n),x_values(n+1)])) &&...
                (intersection(2) < max([y_values(n),y_values(n+1)])) &&...
                (intersection(2) > min([y_values(n),y_values(n+1)]))) && ...
                ( (intersection(1) < max([basepos(1),b(1)]))  &&...
                (intersection(1) > min([basepos(1),b(1)]))  &&...
                (intersection(2) < max([basepos(2),b(2)]))  &&...
                (intersection(2) > min([basepos(2),b(2)]))) )

            collision_b = true;
        end
    end
    if collision_b
        break;
    end
    %Link2:
    %g = b + linkLengths(2)*[cos(beta),sin(beta)];
    
    %collision_g = false;

    g_m = (g(2) - b(2))/(g(1) - b(1));
    
    for n = 1:(length(x_values) - 1)
        slope = (y_values(n+1)-y_values(n))/(x_values(n+1)-x_values(n));
        if (slope == Inf || slope == -Inf)
            intersection = [x_values(n);g_m*(x_values(n) - g(1)) + g(2)];
        else
            intersection = [-(g_m),1;-slope,1]...
                \[g(2) - g_m*g(1);y_values(n) - slope*x_values(n)];
        end
        %if intersection is on the object (endpoints ok) and between the

        if( ((intersection(1) < max([x_values(n),x_values(n+1)]))&&...
                (intersection(1) > min([x_values(n),x_values(n+1)])) &&...
                (intersection(2) < max([y_values(n),y_values(n+1)])) &&...
                (intersection(2) > min([y_values(n),y_values(n+1)]))) && ...
                ( (intersection(1) < max([b(1),g(1)]))  &&...
                (intersection(1) > min([b(1),g(1)]))  &&...
                (intersection(2) < max([b(2),g(2)]))  &&...
                (intersection(2) > min([b(2),g(2)]))) )

            collision_g = true;
        end
    end
    if collision_g
        break;
    end

    %Link3:
    %k = g + linkLengths(3)*[cos(gamma),sin(gamma)];

    %collision_k = false;

    g_m = (k(2) - g(2))/(k(1) - g(1));
    %for all obstacle segments
    for n = 1:(length(x_values) - 1)
        slope = (y_values(n+1)-y_values(n))/(x_values(n+1)-x_values(n));
        if (slope == Inf || slope == -Inf)
            intersection = [x_values(n);g_m*(x_values(n) - k(1)) + k(2)];
        else
            intersection = [-(g_m),1;-slope,1]...
                \[k(2) - g_m*k(1);y_values(n) - slope*x_values(n)];
        end
        %if intersection is on the object (endpoints ok) and between the

        if( ((intersection(1) < max([x_values(n),x_values(n+1)]))&&...
                (intersection(1) > min([x_values(n),x_values(n+1)])) &&...
                (intersection(2) < max([y_values(n),y_values(n+1)])) &&...
                (intersection(2) > min([y_values(n),y_values(n+1)]))) && ...
                ( (intersection(1) < max([g(1),k(1)]))  &&...
                (intersection(1) > min([g(1),k(1)]))  &&...
                (intersection(2) < max([g(2),k(2)]))  &&...
                (intersection(2) > min([g(2),k(2)]))) )

            collision_k = true;
        end
    end
    if collision_k
        break;
    end
    %Link4:
    %e = k + linkLengths(4)*[cos(kappa),sin(kappa)];

     %   collision_e = false;

    g_m = (e(2) - k(2))/(e(1) - k(1));
    %for all obstacle segments
    for n = 1:(length(x_values) - 1)
        slope = (y_values(n+1)-y_values(n))/(x_values(n+1)-x_values(n));
        if (slope == Inf || slope == -Inf)
            intersection = [x_values(n);g_m*(x_values(n) - e(1)) + e(2)];
        else
            intersection = [-(g_m),1;-slope,1]...
                \[e(2) - g_m*k(1);y_values(n) - slope*x_values(n)];
        end
        %if intersection is on the object (endpoints ok) and between the

        if( ((intersection(1) < max([x_values(n),x_values(n+1)]))&&...
                (intersection(1) > min([x_values(n),x_values(n+1)])) &&...
                (intersection(2) < max([y_values(n),y_values(n+1)])) &&...
                (intersection(2) > min([y_values(n),y_values(n+1)]))) && ...
                ( (intersection(1) < max([k(1),e(1)]))  &&...
                (intersection(1) > min([k(1),e(1)]))  &&...
                (intersection(2) < max([k(2),e(2)]))  &&...
                (intersection(2) > min([k(2),e(2)]))) )

            collision_e = true;
        end
    end
    if collision_e
        break;
    end
    if p_poly_dist(e(1),e(2),x_values,y_values) < 0
        collision_e = true;
    end
       
end
if collision_b || collision_g || collision_k || collision_e
    okConfig = false;
end

function e = calculate_e(config1)
b = config1.basepos + config1.linkLengths(1)*[cos(config1.alpha),sin(config1.alpha)];
g = b + config1.linkLengths(2)*[cos(config1.beta),sin(config1.beta)];
k = g + config1.linkLengths(3)*[cos(config1.gamma),sin(config1.gamma)];
e = k + config1.linkLengths(4)*[cos(config1.kappa),sin(config1.kappa)];


function pathConfigs = planpath(configs)
javaaddpath('C:\Documents and Settings\Mr. Hyde\My Documents\Grad School\Grad School Work\Robotic Motion Planning');
import java.util.*;
vertexes = ArrayList();
edges = ArrayList();

for m = 1:length(configs)
    e = calculate_e(configs(m));
    vertexes.add(Vertex(e(1), e(2)));
end
%addedNeighbors = [];
for m = 1:length(configs)
    cur_config = configs(m);
    for n = 1:length(cur_config.neighbors)
        cur_neigh_num = cur_config.neighbors(n);
        cur_neigh = configs(cur_neigh_num);
        %This implementation of Dijkstras was really designed for
        %directionless edges.  It gets confused when edges are added twice.
        %if ~(ismember([m cur_neigh_num], addedNeighbors,'rows') ||...
        %        ismember([cur_neigh_num m], addedNeighbors,'rows'))
        %    addedNeighbors = [addedNeighbors; [m cur_neigh_num]];
            cost = configDist(cur_config, cur_neigh);
            edges.add(Edge(vertexes.get(m-1),vertexes.get(cur_config.neighbors(n)-1),cost));
        %end
    end
    %addedNeighbors
end
    
worldMap = AdjListGraph(vertexes, edges);
algs = GraphAlgorithms(worldMap);
worldMap.vertices;
algs.calculateDijkstras(worldMap.vertices.get(0));
path = algs.shortestPathTo(worldMap.vertices.get(1));

if path.size() == 0
    return
end

pathEdges = path.toArray();   
pathVertices = [];
for m = 1:length(pathEdges)
    v1 = pathEdges(m).vertex1;
    v2 = pathEdges(m).vertex2;
    pathVertices = [pathVertices; [v1.getX() v1.getY() v2.getX() v2.getY()]];
end

for m = 1:length(pathVertices(:,1))
    plot([pathVertices(m,1);pathVertices(m,3)],[pathVertices(m,2);pathVertices(m,4)],'g-','LineWidth',2);
end
pathConfigs = [];
for m = 1:length(pathVertices(:,1))
    e = [pathVertices(m,1) pathVertices(m,2)];
    for n = 1:length(configs)
        cur_e = calculate_e(configs(n));
        if cur_e == e
            pathConfigs = [pathConfigs; n];
            n = Inf;
        end
    end
    e = [pathVertices(m,3) pathVertices(m,4)];
    for n = 1:length(configs)
        cur_e = calculate_e(configs(n));
        if cur_e == e
            pathConfigs = [pathConfigs; n];
            n = Inf;
        end
    end
end