// PathFollower.java
//
// A class that demonstrates some ways to use Navigator to follow a path.
//
// Simon Parsons
// 26th October 2013
//
// Based on an example from Davide Grossi

import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.pathfinding.*;
import lejos.robotics.localization.*;

public class PathFollower{
    // Set up private data members.
    private DifferentialPilot pilot = new DifferentialPilot(3.25, 19.8, Motor.C, Motor.B);
    private PoseProvider posePro = new OdometryPoseProvider(pilot);
    private Navigator navigator = new Navigator(pilot);
    private Waypoint next;
    
    // Public access functions to allow access to the necessary functions of
    // the private members
    public void newWaypoint(int x, int y){
	navigator.addWaypoint(x, y);
    }
    
    public void newPath(Path path){
	navigator.setPath(path);
    }
    
    public void newPose(Pose pose){
	posePro.setPose(pose);
    }
    
    public Pose reportPose(){
	return posePro.getPose();
    }
    
    // The key method. Assumes a path is already set up for the navigator, then
    // has the navigator step through it.
    public void navigate(){
	while(!navigator.pathCompleted()){
	    navigator.followPath();
	    // This bit is just to display what is going on. The navigator will
	    // follow the path without it.
	    next = navigator.getWaypoint();
	    LCD.drawString("Moving to...", 0, 0);
	    LCD.drawString("(" + (int)next.getX() + "," + (int)next.getY() + ")", 0, 1);
	}
    }
}