package ourproject;

import lejos.util.DebugMessages;
import lejos.util.Matrix;
import lejos.geom.Line;
import lejos.geom.Rectangle;
import lejos.nxt.Motor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.localization.PoseProvider;

//TASK 1
//-----------
//import lejos.nxt.LCD;
//
///**
// * A simple hello world program.
// *
// * @author Lawrie Griffiths
// */
//public class Main {
//  public static void main(String[] aArg) throws Exception {
//      LCD.drawString("Hello World", 3, 4);
//      Thread.sleep(2000);
//  }
//}


//TASK 2-3
//---------------

//import lejos.nxt.Button;
//import lejos.nxt.LCD;
//import lejos.nxt.Motor;
//import lejos.nxt.SensorPort;
//import lejos.nxt.addon.GyroSensor;
//import lejos.robotics.Gyroscope;
//import lejos.robotics.navigation.DifferentialPilot;
//import lejos.util.Delay;

/**
 * Test of Gyro Sensor
 * Records the minimum, maximum and current values
 *
 * @author Lawrie Griffiths
 */
/*//public class Main {
//  public static void main(String[] args) {
//      Gyroscope gyro = new GyroSensor(SensorPort.S1);
//      float minValue = 0, maxValue = 0;
//
//      LCD.drawString("Gyro Test:", 0, 0);
//      LCD.drawString("Min:", 0, 2);
//      LCD.drawString("Max:", 0, 3);
//      LCD.drawString("Current:", 0, 4);
//
//      //while(!Button.ESCAPE.isDown()) {
//           DifferentialPilot pilot = new DifferentialPilot(3.26f, 19.5f, Motor.A, Motor.B, false);  // parameters in inches
//          // pilot.setRotateSpeed(30);;  // cm per second
//           pilot.setAcceleration(10000);
//           pilot.setRotateSpeed(10000);
//           Delay.msDelay(10000);
//           pilot.travel(200);         // cm
//           pilot.rotate(90);
//           pilot.travel(580);         // cm
//           pilot.rotate(90);
//           pilot.travel(270);         // cm
//           pilot.rotate(90);
//           pilot.travel(110);         // cm
//           pilot.rotate(90);
//           // degree clockwise
//          // pilot.travel(-50,true);  //  move backward for 50 cm
//           while(pilot.isMoving())Thread.yield();
//          // pilot.rotate(-90);
//          // pilot.rotate
//          // pilot.steer(-50,180,true); // turn 180 degrees to the right
//          // waitComplete();            // returns when previous method is complete
//          // pilot.steer(100);          // turns with left wheel stationary
//           Delay.msDelay(1000);
//           pilot.stop();
//      //}
//  }
//}
*/
//TASK 4
//-------
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.NavigationListener;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.AstarSearchAlgorithm;
import lejos.robotics.pathfinding.FourWayGridMesh;
import lejos.robotics.pathfinding.NodePathFinder;
import lejos.robotics.pathfinding.PathFinder;
import lejos.util.PilotProps;

/**
 * This sample uses the A* search algorithm to find a path from one location to another. The code demonstrates
 * how to create a trivial LineMap, feed it to a GridMesh, and then use a pathfinder
 * to control the robot and allow it to navigate around map obstacles. You will need to construct a simple
 * pilot robot to use this class. No sensors are required.
 *
 * @author BB
 *
 */
public class Main {

    public static void main(String[] args) throws Exception {


    	RConsole.open();
        DifferentialPilot robot = new DifferentialPilot(3.26f, 19.5f, Motor.A, Motor.B, false);  // parameters in inches

        // Create a rudimentary map:
        Line [] lines = new Line[3];
		/*lines [0] = new Line(75f, 100f, 100f, 100f);
		lines [1] = new Line(100, 100, 87, 75);
		lines [2] = new Line(87, 75, 75, 100);*/
        lines [0] = new Line(30, 30, 50, 65);
		lines [1] = new Line(50, 65, 65, 37.5f);
		lines [2] = new Line(65, 37.5f, 30, 30);
		lejos.geom.Rectangle bounds = new Rectangle(-30, -30, 150, 150);
		LineMap myMap = new LineMap(lines, bounds);

		// Use a regular grid of node points. Grid space = 20. Clearance = 15:
		FourWayGridMesh grid = new FourWayGridMesh(myMap, 10, 5);

		// Use A* search:
		AstarSearchAlgorithm alg = new AstarSearchAlgorithm();

		// Give the A* search alg and grid to the PathFinder:
		PathFinder pf = new NodePathFinder(alg, grid);

		PoseProvider posep = new OdometryPoseProvider(robot);

		Navigator nav = new Navigator(robot, posep) ;
		nav.addNavigationListener(new NavigationListener() {

			public void pathInterrupted(Waypoint waypoint, Pose pose, int sequence) {
				// TODO Auto-generated method stub
				RConsole.println("interupted :<");
			}

			public void pathComplete(Waypoint waypoint, Pose pose, int sequence) {
				// TODO Auto-generated method stub
				RConsole.println("Pizza delivered");
			}

			public void atWaypoint(Waypoint waypoint, Pose pose, int sequence) {
				// TODO Auto-generated method stub
				RConsole.println(waypoint.toString());

			}
		});
		System.out.println("Planning path...");
		//nav.followPath(pf.findRoute(posep.getPose(), new Waypoint(130, 130)));
		nav.followPath(pf.findRoute(posep.getPose(), new Waypoint(25, 75)));
		nav.waitForStop();

		RConsole.close();
    }
}