package GlobalNavigation;

import java.awt.Color;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.awt.geom.Rectangle2D.Double;
import java.io.IOException;
import java.text.ParseException;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;

import org.ros.message.lab5_msgs.ColorMsg;
import org.ros.message.lab5_msgs.GUIEraseMsg;
import org.ros.message.lab5_msgs.GUILineMsg;
import org.ros.message.lab5_msgs.GUIPointMsg;
import org.ros.message.lab5_msgs.GUISegmentMsg;
import org.ros.message.lab6_msgs.GUIPolyMsg;
import org.ros.message.lab6_msgs.GUIRectMsg;
import org.ros.message.rss_msgs.BumpMsg;
import org.ros.message.rss_msgs.MotionMsg;
import org.ros.message.rss_msgs.OdometryMsg;
import org.ros.namespace.GraphName;
import org.ros.node.Node;
import org.ros.node.NodeMain;
import org.ros.node.parameter.ParameterTree;
import org.ros.node.topic.Publisher;
import org.ros.node.topic.Subscriber;

import LocalNavigation.LocalNavigationSolution;
import LocalNavigation.SonarGUI;

/**
 * <p>Driver for the GlobalNavigation lab.</p>
 *
 * @author Marty Vona
 * @author Aisha Walcott
 **/
public class GlobalNavigation implements NodeMain {

	/**
	 * <p>Name to use when run as an application.</p>
	 **/
	public static final String APPNAME = "GlobalNavigation";

	/**
	 * <p>State constant, robot stopped.</p>
	 **/
	public static final int STOP = 0;

	/**
	 * <p>State constant, following path.</p>
	 **/
	public static final int GO = 1;

	/**
	 * <p>Right bumper (orc port 8) index.</p>
	 **/
	public static int ROBOT_BUMPER_RIGHT = 0;

	/**
	 * <p>Left bumper (orc port 9) index.</p>
	 **/
	public static int ROBOT_BUMPER_LEFT = 1;

	/**
	 * <p>Translational velocity while following path (m/s).</p>
	 **/
	public static final double FOLLOW_PATH_TV = 0.2;

	/**
	 * <p>Proportional gain for rotation controller while following path.</p>
	 **/
	public static final double FOLLOW_PATH_RV_GAIN = 0.1;

	/**
	 * <p>Max rotational velocity while following path (rad/s).</p>
	 **/
	public static final double FOLLOW_PATH_MAX_RV = 0.5;

	/**
	 * <p>Rotate before translation threshold (radians)</p>
	 */
	public static final double ROTATE_FIRST_THRESHOLD = 0.2;

	/**
	 * <p>Waypoint reached threshold (m).</p>
	 **/
	public static final double WP_THRESHOLD = 0.03;

	/**
	 * <p>The map, read from file in {@link #main}.</p>
	 **/
	protected PolygonMap map;

	/**
	 * <p>World bounds.</p>
	 **/
	protected Rectangle2D.Double worldRect;

	/**
	 * <p>Start point.</p>
	 **/
	protected Point2D.Double startPoint;

	/**
	 * <p>Goal point.</p>
	 **/
	protected Point2D.Double goalPoint;

	/**
       <p>The graph</p>
	 */
	protected VisibilityGraph visibilityGraph;
	/**
	 * <p>The configuration space.</p>
	 **/
	protected CSpace cspace;

	//This is BAD, the code needs it and I didn't have time to
	//rewrite the odometry handler
	protected Grid grid;
	public static final double GRID_RESOLUTION = 0.075;

	/**
	 * <p>Color of the farthest cell(s).</p>
	 **/
	public static final Color FAR_COLOR = Color.RED;

	/**
	 * <p>Color of the goal cell.</p>
	 **/
	public static final Color GOAL_COLOR = Color.BLUE;

	/**
	 * <p>Color of the path.</p>
	 **/
	public static final Color PATH_COLOR = Color.BLACK;

	private static String mapFileName;

	/**
	 * <p>The current waypoint.</p>
	 **/
	protected Point2D.Double currentWaypoint;
	protected int currentWaypointIndex;
	protected List<Point2D.Double> waypoints;

	/**
	 * <p>Maximum distance from any cell that can reach the goal to the goal
	 * (m).</p>.
	 **/
	protected double maxDist;

	/**
	 * <p>The current robot state.</p>
	 **/
	protected int state = STOP;

	private Publisher<GUIPolyMsg> polyPub;

	private Publisher<GUIRectMsg> rectPub;

	private Publisher<GUIPointMsg> pointPub;

	private Publisher<GUISegmentMsg> segmentPub;

	private Publisher<GUILineMsg> linePub;

	private Publisher<MotionMsg> motionPub;

	private Publisher<GUIEraseMsg> erasePub;

	protected int redrawCounter = 0;

	/**(Solution)
	 *///(Solution)
	static protected double robotRadius = 0.30;//(Solution)

	/**
	 * <p>Display {@link #map} to console and to the GUI.</p>
	 **/
	public void displayMap() {

		GUIRectMsg rectMsg = new GUIRectMsg();
		GUIPolyMsg polyMsg = new GUIPolyMsg();
		fillRectMsg(rectMsg, map.getWorldRect(), null, false);
		rectPub.publish(rectMsg);

		for (PolygonObstacle obstacle : map.getObstacles()) {
			polyMsg = new GUIPolyMsg();
			fillPolyMsg(polyMsg, obstacle, obstacle.color, true, true);
			polyPub.publish(polyMsg);
		}
		GUIPointMsg pointMsg = new GUIPointMsg();
		fillPointMsg(pointMsg, startPoint, Color.BLACK, SonarGUI.O_POINT);
		pointPub.publish(pointMsg);
		pointMsg = new GUIPointMsg();
		fillPointMsg(pointMsg, goalPoint, Color.BLACK, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);
	}

	public void fillPointMsg(GUIPointMsg msg, Point2D.Double point, Color color, long shape) {
		msg.shape = shape;
		msg.x = point.x;
		msg.y = point.y;
		fillColor(msg.color, color);
	}

	public static void fillPolyMsg(GUIPolyMsg msg, PolygonObstacle obstacle, Color c, boolean filled, boolean closed) {
		List<Point2D.Double> vertices = obstacle.getVertices();
		msg.numVertices = vertices.size();
		float[] x = new float[msg.numVertices];
		float[] y = new float[msg.numVertices];
		for (int i = 0; i < msg.numVertices; i++){
			x[i] = (float) vertices.get(i).x;
			y[i] = (float) vertices.get(i).y;
		}
		fillColor(msg.c, c);
		System.out.println("original color:\t" + c);
		//System.out.println(msg.c.r + " " + msg.c.g + " " + msg.c.b);
		msg.x = x;
		msg.y = y;
		msg.closed = closed?1:0;
		msg.filled = filled?1:0;
	}

	private void fillPolyMsg(GUIPolyMsg polyMsg,
			List<java.awt.geom.Point2D.Double> vertices, Color pathColor,
			boolean filled, boolean closed) {
		polyMsg.numVertices = vertices.size();
		float[] x = new float[polyMsg.numVertices];
		float[] y = new float[polyMsg.numVertices];
		for (int i = 0; i < polyMsg.numVertices; i++){
			x[i] = (float) vertices.get(i).x;
			y[i] = (float) vertices.get(i).y;
		}
		polyMsg.x = x;
		polyMsg.y = y;
		polyMsg.closed = closed?1:0;
		polyMsg.filled = filled?1:0;
		fillColor(polyMsg.c, pathColor);
	}

	public static void fillRectMsg(GUIRectMsg msg, Double r, Color c, boolean filled) {
		msg.filled = filled ? 1:0;
		fillColor(msg.c, c);
		msg.height = (float) r.height;
		msg.width = (float) r.width;
		msg.x = (float) r.x;
		msg.y = (float) r.y;
	}

	/**
	 * <p>Display {@link #grid} to the GUI.</p>
	 *
	 * <p>Obstacle cells are not displayed; all others are colored according to
	 * their distance from the goal (see {@link #FAR_COLOR} and {@link
	 * #GOAL_COLOR}).</p>
	 **/
	public void displayGrid() {

		List<Rectangle2D.Double> rects = new ArrayList<Rectangle2D.Double>();
		List<Color> colors = new ArrayList<Color>();

		for (Grid.Cell cell : grid) {

			Color color = Color.YELLOW;

			if (cell.hasPathToGoal())
				color = interpolateColor(maxDist, cell.minDistanceToGoal);

			rects.add(cell.rect);
			colors.add(color);
		}
		publishRects(rects, colors);
	}

	private void publishRects(List<Double> rects, List<Color> colors) {
		GUIRectMsg rectMsg;
		for (int i = 0; i < rects.size(); i++){
			rectMsg = new GUIRectMsg();
			fillRectMsg(rectMsg, rects.get(i), colors.get(1), true);
			rectPub.publish(rectMsg);
		}
	}

	/**
	 * <p>Display {@link #cspace} to the GUI.</p>
	 **/
	public void displayCSpace() {

		for (PolygonObstacle obstacle : cspace.getObstacles()) {
			GUIPolyMsg polyMsg = new GUIPolyMsg();
			fillPolyMsg(polyMsg, obstacle, obstacle.color, false, true);
			polyPub.publish(polyMsg);
		}
	}

	public void displayVisibilityGraph(){
		Color color = Color.GREEN;
		Map<Point2D.Double,List<Point2D.Double>> graph = visibilityGraph.getGraph();
		GUISegmentMsg segmentMsg;
		for(Point2D.Double start : graph.keySet()){
			for(Point2D.Double other : graph.get(start)){
				segmentMsg = new GUISegmentMsg();
				fillSegmentMsg(segmentMsg, color, start, other);
				segmentPub.publish(segmentMsg);
			}
		}
	}

	public void fillSegmentMsg(GUISegmentMsg segmentMsg, Color color, Point2D.Double start,
			Point2D.Double other) {
		fillColor(segmentMsg.color, color);
		segmentMsg.startX = start.getX();
		segmentMsg.startY = start.getY();
		segmentMsg.endX = other.getX();
		segmentMsg.endY = other.getY();
	}

	public static void fillColor(ColorMsg color2, Color color) {
		if (color == null){
			return;
		}
		color2.b = color.getBlue();
		color2.g = color.getGreen();
		color2.r = color.getRed();
	}

	/**
	 * <p>Generate an interpolated color between {@link #FAR_COLOR} and {@link
	 * #GOAL_COLOR}.</p>
	 *
	 * @param maxDist the distance of the farthest cell(s) from the goal (m)
	 * @param dist the distance of the specified cell from the goal (m)
	 **/
	protected static Color interpolateColor(double maxDist, double dist) {

		double where = dist/maxDist;

		double farR = ((FAR_COLOR.getRed()))/255.0;
		double farG = ((FAR_COLOR.getGreen()))/255.0;
		double farB = ((FAR_COLOR.getBlue()))/255.0;

		double goalR = ((GOAL_COLOR.getRed()))/255.0;
		double goalG = ((GOAL_COLOR.getGreen()))/255.0;
		double goalB = ((GOAL_COLOR.getBlue()))/255.0;

		return new Color((float) (where*farR + (1.0-where)*goalR),
				(float) (where*farG + (1.0-where)*goalG),
				(float) (where*farB + (1.0-where)*goalB));
	}

	/**
	 * <p>Display a set of line segments from {@link #currentWaypoint} to the
	 * goal.</p>
	 **/
	public void displayPath() {

		//    System.out.println(currentWaypoint.minDistanceToGoal + " meters to goal");

		//   List<Point2D.Double> waypoints = new ArrayList<Point2D.Double>();
		/*
    for (Point2D.Double waypoint = currentWaypoint;
         waypoint != null;
         waypoint = waypoint.toGoalNext)
      waypoints.add(waypoint.makeCenterPoint());
		 */
		if ( waypoints != null ) {
			GUIPolyMsg polyMsg = new GUIPolyMsg();
			fillPolyMsg(polyMsg, waypoints, PATH_COLOR, false, false);
			polyPub.publish(polyMsg);
		}
	}

	/**
	 * <p>Exercise the convex hull and polygon display code.</p>
	 **/
	public void testConvexHull() {

		List<Point2D.Double> points = new LinkedList<Point2D.Double>();


		double x = -1.0;
		double y = 1.0;
		Point2D.Double p = new Point2D.Double(x, y);
		points.add(p);

		GUIPointMsg pointMsg = new GUIPointMsg();
		fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);

		x = -1.0;
		y = 1.0;
		p = new Point2D.Double(x, y);
		points.add(p);
		fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);

		x = 0.0;
		y = 1.0;
		p = new Point2D.Double(x, y);
		points.add(p);
		fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);

		x = 1.0;
		y = 1.0;
		p = new Point2D.Double(x, y);
		points.add(p);
		fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);

		x = 1.0;
		y = 0.0;
		p = new Point2D.Double(x, y);
		points.add(p);
		fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
		pointPub.publish(pointMsg);

		GUIPolyMsg polyMsg = new GUIPolyMsg();
		fillPolyMsg(polyMsg, GeomUtils.convexHull(points).getVertices(), Color.BLACK, false, true);
		polyPub.publish(polyMsg);

		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			return;
		}

		for (;;) {

			x = (Math.random()-0.5)*6.0;
			y = (Math.random()-0.5)*6.0;
			p = new Point2D.Double(x, y);
			points.add(p);
			fillPointMsg(pointMsg, p, Color.RED, SonarGUI.X_POINT);
			pointPub.publish(pointMsg);

		}
	}

	/**
	 * <p>Implements path following.</p>
	 **/
	public void handle(OdometryMsg msg) {

		GUIPointMsg pointMsg = new GUIPointMsg();
		MotionMsg motionMsg = new MotionMsg();

		if ( redrawCounter++ % 50 == 0 ) {
			displayMap();
			displayCSpace();
			displayPath();
			displayVisibilityGraph();
		}

		switch (state) {
			case GO: {

				//odometry at start point is always (0.0, 0.0, 0.0)
				double x = msg.x + startPoint.x;
				double y = msg.y + startPoint.y;

				double wpX = currentWaypoint.getX();
				double wpY = currentWaypoint.getY();

				//distance to waypoint
				double wpD = Math.sqrt((x-wpX)*(x-wpX) + (y-wpY)*(y-wpY));
				if (wpD < WP_THRESHOLD) {
					fillPointMsg(pointMsg, new Point2D.Double(wpX, wpY), Color.BLACK, SonarGUI.X_POINT);
					pointPub.publish(pointMsg);
					currentWaypointIndex++;
					if (currentWaypointIndex >= waypoints.size()){
						state = STOP;
						motionMsg = new MotionMsg();
						setMotionMsg(motionMsg, 0, 0);
						motionPub.publish(motionMsg);
						System.exit(0);
					}
					//reached current waypoint, move on to next
					currentWaypoint = waypoints.get(currentWaypointIndex);
					/*
        if (currentWaypoint == null) {
          //reached goal
          state = STOP;
          Robot.setVelocity(0.0, 0.0);
          System.exit(0);
        }
					 */
					wpX = currentWaypoint.getX();
					wpY = currentWaypoint.getY();
					/*
        wpX = currentWaypoint.getCenterX();
        wpY = currentWaypoint.getCenterY();
					 */
					wpD = Math.sqrt((x-wpX)*(x-wpX) + (y-wpY)*(y-wpY));
				}

				//      System.out.println(currentWaypoint.minDistanceToGoal+wpD +
				//                 " meters to goal");

				//assume initial heading was actually 0.0
				double heading = msg.theta;

				//cosine and sine of actual heading
				double cActual = Math.cos(heading);
				double sActual = Math.sin(heading);

				//cosine and sine of desired heading
				double cDesired = (wpX-x)/wpD;
				double sDesired = (wpY-y)/wpD;

				//cosine and sine of error angle
				double cError = cDesired*cActual+sDesired*sActual;
				double sError = sDesired*cActual-cDesired*sActual;

				double thetaError = Math.atan2(sError, cError);
				double rv = FOLLOW_PATH_RV_GAIN*thetaError;

				//debug
				//System.out.println("wayX: " + wpX + "wayY: " + wpY );
				//System.out.println("X: " + x + "Y: " + y );
				//System.out.println(" cD: "+cDesired + " sD " + sDesired );
				//System.out.println(" cerror: "+heading + " serror " + sError + "thetaError " + thetaError);
				//

				if (rv > FOLLOW_PATH_MAX_RV)
					rv = FOLLOW_PATH_MAX_RV;

				if (rv < -FOLLOW_PATH_MAX_RV)
					rv = -FOLLOW_PATH_MAX_RV;

				// for large angles, perform rotation before translating
				if(Math.abs(thetaError) > ROTATE_FIRST_THRESHOLD) {
					setMotionMsg(motionMsg, 0.0, rv);
					motionPub.publish(motionMsg);
				} else {
					setMotionMsg(motionMsg, FOLLOW_PATH_TV, rv);
					motionPub.publish(motionMsg);
				}

				break;
			}
			case STOP:
			default:
				setMotionMsg(motionMsg, 0.0, 0.0);
				motionPub.publish(motionMsg);
		}
	}

	public void setMotionMsg(MotionMsg msg, double d, double rv) {
		msg.rotationalVelocity = rv;
		msg.translationalVelocity = d;
	}

	/**
	 * <p>Implements stop on bump.</p>
	 **/
	public void handle(BumpMsg msg) {

		boolean leftBumper = msg.left;
		boolean rightBumper = msg.right;

		if (leftBumper || rightBumper) {
			state = STOP;
			MotionMsg motionMsg = new MotionMsg();
			setMotionMsg(motionMsg, 0.0, 0.0);
			motionPub.publish(motionMsg);
			System.err.println("bumpers report unexpected collision!");
			System.exit(8);
		}
	}



	/**
	 * <p>Driver, calls {@link #instanceMain}.</p>
	 **/
	@Override
	public void onStart(Node node) {
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		ParameterTree paramTree = node.newParameterTree();
		mapFileName = paramTree.getString(node.resolveName("~/mapFileName"));
		System.out.println("filename = " + mapFileName);

		polyPub = node.newPublisher("gui/Poly", "lab6_msgs/GUIPolyMsg");
		rectPub = node.newPublisher("gui/Rect", "lab6_msgs/GUIRectMsg");
		pointPub = node.newPublisher("gui/Point", "lab5_msgs/GUIPointMsg");
		segmentPub = node.newPublisher("gui/Segment", "lab5_msgs/GUISegmentMsg");
		linePub = node.newPublisher("gui/Line", "lab5_msgs/GUILineMsg");
		erasePub = node.newPublisher("gui/Erase", "lab5_msgs/GUIEraseMsg");
		motionPub = node.newPublisher(LocalNavigationSolution.MOTOR_CHANNEL,
				LocalNavigationSolution.MOTION_MESSAGE);

		this.instanceMain(new String[]{mapFileName});

		bumpSub = node.newSubscriber(LocalNavigationSolution.BUMP_CHANNEL,
				LocalNavigationSolution.BUMP_MESSAGE);
		bumpSub.addMessageListener(new BumpListener(this));

		odoSub = node.newSubscriber(LocalNavigationSolution.ODOMETRY_CHANNEL,
				LocalNavigationSolution.ODOMETERY_MESSAGE);
		odoSub.addMessageListener(new OdometryListener(this));

	}

	public Subscriber<org.ros.message.rss_msgs.BumpMsg> bumpSub;
	public Subscriber<org.ros.message.rss_msgs.OdometryMsg> odoSub;

	/**
	 * <p>Displays map, computes cspace and grid, displays grid and path, and
	 * initiates path following.</p>
	 **/
	public void instanceMain(String[] arg) {

		String mapFile = arg[0];

		System.out.println("  map file: " + mapFile);

		try {
			map = new PolygonMap(mapFile);
		} catch (IOException e) {
			System.err.println("I/O error: " + e);
			System.exit(1);
		} catch (ParseException e) {
			System.err.println("Parse error: " + e);
			System.exit(2);
		}

		worldRect = map.getWorldRect();
		startPoint = map.getRobotStart();
		goalPoint = map.getRobotGoal();

		System.out.println("StartPoint: "+startPoint.x+", "+startPoint.y);
		System.out.println("GoalPoint: "+goalPoint.x+", "+goalPoint.y);
		System.out.println("WorldRect: "+
				worldRect.getMinX()+", "+worldRect.getMinY()+" to "+
				worldRect.getMaxX()+", "+worldRect.getMaxY());

		//double robotRadius = map.getRobotRadius();//This is already defined as a protected var.

		//will do this later (for paint order)
		//    displayMap();

		//    testConvexHull();

		//check that start and goal are both in-bounds

		if (!worldRect.contains(startPoint)) {
			System.err.println("start point out of bounds!");
			System.exit(3);
		}

		if (!worldRect.contains(goalPoint)) {
			System.err.println("goal point out of bounds!");
			System.exit(4);
		}

		System.out.println("creating grid");
		Rectangle2D.Double gridRect =
			new Rectangle2D.Double(worldRect.x + robotRadius,
					worldRect.y + robotRadius,
					worldRect.width - 2.0*robotRadius,
					worldRect.height - 2.0*robotRadius);

		//create grid
		grid = new Grid(gridRect, GRID_RESOLUTION);
		System.out.println("grid created");
		//compute and mark cspace obstacles
		cspace = new CSpace(map.getObstacles(), robotRadius);
		System.out.println("created cspace");
		for (PolygonObstacle obstacle : cspace.getObstacles())
			grid.markObstacle(obstacle);


		visibilityGraph = new VisibilityGraph(startPoint,goalPoint,gridRect,cspace);
		System.out.println("created visibility graph");
		for(Point2D.Double p : visibilityGraph.getGraph().get(startPoint)){
			System.out.println("Start Point Connects to: "+p.toString());
		}
		//check that start and goal are both in freespace
		if (!(grid.getCell(startPoint)).free) {
			System.err.println("start point in collision!");
			System.exit(5);
		}
		if (!(grid.getCell(goalPoint)).free) {
			System.out.println("prob with grid goal point");
			System.err.println("goal point in collision!");
			System.exit(6);
		}
		currentWaypointIndex = 0;
		System.out.println("about to compute waypoints");
		waypoints = visibilityGraph.computeShortestPath(startPoint, goalPoint);
		if(waypoints==null){
			System.out.println("No path bro.");
			System.exit(0);

		}
		System.out.println("waypoints computed");
		currentWaypoint = waypoints.get(currentWaypointIndex);
		//compute shortest paths
		//maxDist = grid.computeShortestPaths(goalPoint);

		//find start cell
		//    currentWaypoint = grid.getCell(startPoint);

		//    System.err.println("start cell: " + currentWaypoint);


		//display the grid
		//    displayGrid();

		//display the world bounds and the real obstacles
		System.out.println("displaying map");
		System.out.println(map.toString());
		displayMap();

		//display the cspace obstacles
		System.out.println("displaying cspace");
		displayCSpace();

		//check that a path exists
		//    if (!currentWaypoint.hasPathToGoal()) {
		//    System.err.println("no path!");
		//System.exit(7);
		// }

		//display the path from currentWaypoint
		System.out.println("displaying path");
		displayPath();

		System.out.println("displaying visibility graph");
		displayVisibilityGraph();

		//follow path
		state = GO;
	}

	@Override
	public void onShutdown(Node arg0) {
		// TODO Auto-generated method stub
	}

	@Override public void onShutdownComplete(Node node) {
	}

	@Override public GraphName getDefaultNodeName() {
		return new GraphName("rss/globalnavigation");
	}
}