%% findpath(Start,End,Max_steps,Path_links,Path)
%% Finds a Path from Start to End in under Max_steps using the Path_links

%% findpath([0,0],[1,1],3,[[[0,0],[1,0]],[[1,0],[1,1]]],Path).
%% Path = [[0, 0], [1, 0], [1, 1]] ;

findpath(Start,End,Max_steps,Path_links,Path) :-
	findpath(Start,End,Path_links,_,[Start],Path),
	length(Path,Steps), Max_steps>=Steps.

%% findpath(Start,End,Path_links1,Pathlinks2,Path1,Path2)
%% Finds a Path from Start to End using the given Path_links1, returning the new 
%% Path_links, is given Path1, and returns Path2

findpath(End,End,_,_,Path,Path).
findpath(Start,End,Path_links1,Path_links3,Path1,Path2) :-
	findnextstep(Path_links1,Start,Next,Path_links2),
	append(Path1,[Next],Path3),
	findpath(Next,End,Path_links2,Path_links3,Path3,Path2).

%% findnextstep(Path_links,Start,Next,Pathlinks2)
%% Finds the next possible step, Next, starting at Start, from Path_links and returns
%% Path_links2

findnextstep(Path_links1,Start,Next,Path_links2) :-
	delete(Path_links,[Start,Next],Path_links2).
findnextstep(Path_links,Start,Next,Pathlinks2) :-
        delete(Path_links1,[Next,Start],Path_links2).