// Pathfinder.java
//
// A class that demonstrates hwo to find a path in LeJOS.
//
// Simon Parsons
// 26th October 2013
//
// Based on an example from Davide Grossi

public class PathFinder{
    public static void main(String[] args){
	
	// Create lines
	Line[] lines = new Line[3];
	lines[0] = new Line(50, 100, 100, 100);
	lines[1] = new Line(100, 100, 100, 50);
	lines[2] = new Line(50, 100, 100, 50);
	
	// Create bounding box
	Rectangle bounds = new Rectangle(-50, -50, 200, 200);
	
	// Map = lines + bounding box
	LineMap map = new LineMap(lines, bounds);
	
	// Set start and end points --- note they are different objects
	// 270 degrees is pointing along the positive y axis.
	Pose start = new Pose(0, 0, 270);
	Waypoint goal = new Waypoint(125, 150);
	
	// Setup pathfinder
	ShortestPathFinder finder = new ShortestPathFinder(map);
	finder.lengthenLines(5);
	
	// Setup navigator
	PathFollower pFollow = new PathFollower();
	pFollow.newPose(start);
	
	// Find route, ending if the point is unreachable
	try{
	    Path path = finder.findRoute(start, goal);
	    // Setup navigator and off we go
	    pFollow.newPath(path);
	    pFollow.navigate();
	}
	// We are trying to go to an impossible point
	catch(DestinationUnreachableException e){
	    Sound.twoBeeps();
	    System.out.println("Can't get there");
	    System.out.println("From here...");
	    Button.waitForAnyPress();
	}	
    }
}