package LocalNavigation;

import java.awt.Color;
import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;

import org.ros.message.lab5_msgs.ColorMsg;
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.rss_msgs.BumpMsg;
import org.ros.message.rss_msgs.MotionMsg;
import org.ros.message.rss_msgs.OdometryMsg;

import utils.Point;

public class LocalNavigation extends LocalNavigationSolution {

	private static final double ROBOT_WIDTH = .381;

	private static final Point FRONT_SONAR = new Point(-.064, .19, Math.PI/2);

	private static final Point REAR_SONAR = new Point(-.254, .19, Math.PI/2);

	/**
	 * <p>FSM state: stop when bumper is actuated<\p>
	 */
	public static final int STOP_ON_BUMP = 0;

	/**
	 * <p>FSM state: align to object when bumper is actuated<\p>
	 */
	public static final int ALIGN_ON_BUMP = 1;

	/**
	 * <p>FSM state: robot in process of bumper-aligning to object<\p>
	 */
	public static final int ALIGNING = 2;

	/**
	 * <p>FSM state: robot is bumper-aligned to object<\p>
	 */
	public static final int ALIGNED = 3;

	/**
	 * <p>FSM state: robot is aligned and moving away from object<\p>
	 */
	public static final int ALIGNED_AND_BACKING_UP = 4;

	/**
	 * <p>FSM state: robot has aligned, backed away, and rotated 90 degrees
	 * clockwise with respect to object<\p>
	 */
	public static final int ALIGNED_AND_ROTATED = 5;

	/**
	 * <p>FSM state: robot is backward-traversing wall to find wall start<\p>
	 */
	public static final int BACKING_UP = 6;

	/**
	 * <p>FSM state: robot is moving forward to find wall start<\p>
	 */
	public static final int FINDING_WALL = 7;

	/**
	 * <p>FSM state: robot has found wall start and is tracking wall<\p>
	 */
	public static final int TRACKING_WALL = 8;

	/**
	 * <p>FSM state: robot has reached end of wall<\p>
	 */
	public static final int WALL_ENDED = 9;

	/**
	 * <p>FSM state: robot is searching for next wall with CCW search, reacting
	 * to bumper input<\p>
	 */
	public static final int TURNING_CCW_BUMP = 10;

	/**
	 * <p>FSM state: robot has completed model acquisition of polygon<\p>
	 */
	public static final int DONE = 11;

	/**
	 * <p>FSM state: robot has completed the STOP_ON_BUMP behavior<\p>
	 */
	public static final int STOP_ON_BUMP_DONE = 12;

	/**
	 * <p>Right bumper index corresponding to Orc port 8<\p>
	 */
	public static int ROBOT_BUMPER_RIGHT = 0;

	/**
	 * <p>Left bumper index corresponding to Orc port 9<\p>
	 */
	public static int ROBOT_BUMPER_LEFT = 1;

	/**
	 * <p>Front sonar index corresponding to Orc sonar port (4,5)<\p>
	 */
	public static int ROBOT_SONAR_FRONT = 0;

	/**
	 * <p>Rear sonar index corresponding to Orc sonar port (6,7)<\p>
	 */
	public static int ROBOT_SONAR_REAR = 1;

	/**
	 * <p>Constant used to identify robot forward motion<\p>
	 */
	public static int FORWARD = 1;

	/**
	 * <p>Constant used to identify robot backward motion<\p>
	 */
	public static int BACKWARD = -1;

	/**
	 * <p>Classification threshold for sonar points<\p>
	 */
	public static double OBSTACLE_DETECTION_THRESHOLD = 0.4;

	/**
	 * <p>Slow translational velocity in m/s<\p>
	 */
	public static double TRANSLATION_VELOCITY_SLOW = 0.05;//.05

	/**
	 * <p>Slow rotational velocity in rad/s<\p>
	 */
	public static double ROTATION_VELOCITY_SLOW = 0.05;//.05

	/**
	 * <p>Translational velocity for CCW rotation in m/s<\p>
	 */
	public static double CCW_TRANSLATION_VELOCITY = 0.2;//.08

	/**
	 * <p>Rotational velocity for CCW rotation in rad/s<\p>
	 */
	public static double CCW_ROTATION_VELOCITY = 0.08;//.08

	/**
	 * <p>Distance to move from wall after alignment in meters<\p>
	 */
	public static double TRANSLATION_BACKUP = -0.3;

	/**
	 * <p>Desired distance to maintain from wall while tracking in meters<\p>
	 */
	public static double DESIRED_WALL_DISTANCE = 0.3;

	/**
	 * <p>Distance between front and rear sonars in meters<\p>
	 */
	public static double SONAR_BASELINE = 0.356;

	/**
	 * <p>Minimum distance to traverse before transitioning from FINDNG_WALL
	 * state to TRACKING_WALL state<\p>
	 */
	public static double MIN_FINDING_WALL_DISTANCE = 0.2;

	/**
	 * <p>Epsilon threshold to determine if robot is NOT translating<\p>
	 */
	public static double EPS_TRANSLATING = 0.001;

	/**
	 * <p>Epsilon threshold to determine if robot is NOT rotating<\p>
	 */
	public static double EPS_ROTATING = 0.02;

	/**
	 * <p>Epsilon threshold to determine if robot theta is near starting
	 * pose<\p>
	 */
	public static double EPS_THETA_FINISHED = 0.4;

	/**
	 * <p>Epsilon threshold to determine if robot pose is near starting
	 * pose<\p>
	 */
	public static double EPS_DISTANCE_FINISHED = 0.5;

	/**
	 * <p>Epsilon threshold to determine if ending point of previous wall
	 * segment and starting point of current wall segment are coincident<\p>
	 */
	public static double EPS_POLYGON_HOLE_SIZE = 0.5;

	/**
	 * <p>Espilon threshold for exterior angle sum of polygon. Note: sum
	 * should be 0.0, but may vary slightly from math tolerance<\p>
	 */
	public static double EPS_EXTERIOR_THETA_SUM = 0.001;

	/**
	 * <p>Translation velocity gain for wall tracking controller<\p>
	 */
	public static double GAIN_TV = 3;

	/**
	 * <p>Rotational velocity gain for wall tracking controller for wall-robot
	 * theta error contribution<\p>
	 */
	public static double GAIN_RV_THETA = 0.08;

	/**
	 * <p>Rotational velocity gain for wall tracking controller for distance-
	 * from-wall error contribution<\p>
	 */
	public static double GAIN_RV_DIST = 0.04;

	/**
	 * <p>GUI shape index for front sonar points<\p>
	 */
	public static int FRONT_SHAPE = SonarGUI.X_POINT;

	/**
	 * <p>GUI shape index for rear sonar points<\p>
	 */
	public static int REAR_SHAPE = SonarGUI.O_POINT;

	/**
	 * <p>Color to plot obstacle/wall sonar points<\p>
	 */
	public static Color OBSTACLE_COLOR = Color.GREEN;

	/**
	 * <p>Color to plot background sonar points<\p>
	 */
	public static Color BACKGROUND_COLOR = Color.BLACK;

	/**
	 * <p>Holds the current FSM state<\p>
	 */
	private int state;

	/**
	 * <p>DNS name for Carmen central host<\p>
	 */
	public static String centralHost = "localhost";

	/**
	 * <p>Width of the robot<\p>
	 */
	public double robotWidth;

	/**
	 * <p>Starting point of the current wall segment<\p>
	 */
	private Point segmentBegin;

	/**
	 * <p>Ending point of the current wall segment<\p>
	 */
	private Point segmentEnd;

	/**
	 * <p>List of wall segment forming the polygon of the acquired model<\p>
	 */
	private java.util.List<WallSegment> wallSegments;

	/**
	 * <p>Previous x-coordinate of robot pose in world coordinates from last
	 * odometry message<\p>
	 */
	private double prev_x;

	/**
	 * <p>Previous y-coordinate of robot pose in world coordinates from last
	 * odometry message<\p>
	 */
	private double prev_y;

	/**
	 * <p>Previous theta of robot pose in world coordinates from last
	 * odometry message<\p>
	 */
	private double prev_theta;

	/**
	 * <p>Indicates if robot is currently moving<\p>
	 */
	private boolean currentlyMoving = false;

	/**
	 * <p>Parameter 'a' describing linear fit of sonar data in form:
	 *    ax + by + c = 0<\p>
	 */
	private double a_fit;

	/**
	 * <p>Parameter 'b' describing linear fit of sonar data in form:
	 *    ax + by + c = 0<\p>
	 */
	private double b_fit;

	/**
	 * <p>Parameter 'c' describing linear fit of sonar data in form:
	 *    ax + by + c = 0<\p>
	 */
	private double c_fit;

	/**
	 * <p>Number of sonar points for current wall segment<\p>
	 */
	private int numPoints;

	/**
	 * <p>Sum(i) from 0 to numPoints over x(i)<\p>
	 */
	private double sum_x;

	/**
	 * <p>Sum(i) from 0 to numPoints over y(i)<\p>
	 */
	private double sum_y;

	/**
	 * <p>Sum(i) from 0 to numPoints over x(i)*y(i)<\p>
	 */
	private double sum_xy;

	/**
	 * <p>Sum(i) from 0 to numPoints over x(i)^2<\p>
	 */
	private double sum_xx;

	/**
	 * <p>Sum(i) from 0 to numPoints over y(i)^2<\p>
	 */
	private double sum_yy;

	/**
	 * <p>Indicates first point in a linear fit<\p>
	 */
	private boolean firstPoint = true;

	/**
	 * <p>Used to postpone state changes until odometryWait number of odometry
	 * messages have arrives. Allows time for motion commands to be realized<\p>
	 */
	private int odometryWait = 0;

	/**
	 * <p>Used to postpone state changes until sonarWait number of sonar
	 * messages have arrives. Allows time for motion commands to be realized<\p>
	 */
	private int sonarWait = 0;

	/**
	 * <p>Indicates if this is the first wall segment in the model acquisition
	 * procedure<\p>
	 */
	private boolean firstSegment;

	/**
	 * <p>Stores the pose of the robot at the start of model acquisition<\p>
	 */
	private Point startAcquirePosition;

	/**
	 * <p>Used to indicate the first pass through the FINDING_WALL state<\p>
	 */
	private boolean beginFindingWall;

	/**
	 * <p>Stores the first point of a wall segment in world coordinates<\p>
	 */
	private Point beginFindingWallPoint;

	/**
	 * <p>Indicates if error log file should be written<\p>
	 */
	private boolean saveErrors;

	/**
	 * <p>Path of log file<\p>
	 */
	private String logFilename = "out.txt";

	/**
	 * <p>File writer object for logging error data<\p>
	 */
	private FileWriter fileWriter;

	/**
	 * <p>Buffered wrapper for log file writer<\p>
	 */
	private BufferedWriter outFile;

	private double rangeFront;

	private double rangeRear;

	private Point goal;

	private double prev_error = 99;


	/**
	 * <p>Constructs new LocalNavigation object and subscribes
	 * to bump and sonar messages<\p>
	 */
	public LocalNavigation() {
		super();

		robotWidth = ROBOT_WIDTH;
		wallSegments = new java.util.ArrayList<WallSegment>();

		resetLinearFilter();

		beginFindingWall = true;
		firstPoint = true;
		firstSegment = true;

		// 3.2 - Collision Detection
		//state =  STOP_ON_BUMP;

		// 3.3 - align on bump
		state = ALIGN_ON_BUMP;

		// 3.3 - Robot Alignment
		//state = TURNING_CCW_BUMP;
		//		state = BACKING_UP;

		try {
			fileWriter = new FileWriter(logFilename);
			outFile = new BufferedWriter(fileWriter);
		} catch(IOException e) {
			System.err.println("Error Opening File: " + e);
		}


	}

	private void setState(int newstate) {
		state = newstate;

		org.ros.message.std_msgs.String str = new org.ros.message.std_msgs.String();
		switch(newstate) {
			case STOP_ON_BUMP : str.data = "STOP_ON_BUMP"; break;
			case STOP_ON_BUMP_DONE : str.data = "STOP_ON_BUMP_DONE"; break;
			case ALIGN_ON_BUMP : str.data = "ALIGN_ON_BUMP"; break;
			case ALIGNING : str.data = "ALIGNING"; break;
			case ALIGNED : str.data = "ALIGNED"; break;
			case ALIGNED_AND_BACKING_UP : str.data = "ALIGNED_AND_BACKING_UP"; break;
			case ALIGNED_AND_ROTATED :str.data = "ALIGNED_AND_ROTATED"; break;
			case BACKING_UP :  str.data = "BACKING_UP"; break;
			case FINDING_WALL : str.data = "FINDING_WALL"; break;
			case TRACKING_WALL : str.data = "TRACKING_WALL"; break;
			case WALL_ENDED : str.data = "WALL_ENDED"; break;
			case TURNING_CCW_BUMP : str.data = "TURNING_CCW_BUMP"; break;
			case DONE : str.data = "DONE"; break;
			default : str.data = "" + newstate; break;
		}
		statePub.publish(str);
	}


	/**
	 * <p>Handles bumper messages<\p>
	 *
	 * @param a ROS bumper message
	 */
	@Override public void handle(BumpMsg msg) {

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

		// 2.2
		if(state == STOP_ON_BUMP) {
			if(leftBumper || rightBumper) {
				setVelocity(0.0, 0.0);
				setState(STOP_ON_BUMP_DONE);
				log.info("STOP ON BUMP: stopping the robot");
			}
		}
		// 2.3
		else if(state == ALIGN_ON_BUMP) {
			if(leftBumper || rightBumper) {
				setState(ALIGNING);
				log.info("ALIGN ON BUMP: switching to ALIGNING mode");
			}
		}

		// 4 - turn CCW until bump
		if(state == TURNING_CCW_BUMP) {
			if(leftBumper || rightBumper) {
				setState(ALIGNING);
			} else {
				log.info("TURN CCW");
				setVelocity(CCW_TRANSLATION_VELOCITY,
						CCW_ROTATION_VELOCITY);
			}
		}

		if(state == ALIGNING) {
			if(leftBumper && rightBumper) {
				log.info("ALIGNED: stopping the robot");
				setVelocity(0.0, 0.0);
				setState(ALIGNED);
			} else if(leftBumper) {
				// rotate slowly to left
				log.info("ALIGNING: rotate left");
				setVelocity(0.0, ROTATION_VELOCITY_SLOW);
			} else if(rightBumper) {
				// rotate slowly to right
				log.info("ALIGNING: rotate right");
				setVelocity(0.0, -ROTATION_VELOCITY_SLOW);
			} else {
				// move forward slowly
				log.info("ALIGNING: move forward");
				setVelocity(TRANSLATION_VELOCITY_SLOW, 0.0);
			}
		}

	}


	private void setVelocity(double transVel, double rotVel) {
		System.out.println("setting velocity: " + transVel + ", " + rotVel);
		MotionMsg msg = new MotionMsg();
		msg.translationalVelocity = transVel;
		msg.rotationalVelocity = rotVel;
		motionPub.publish(msg);
	}


	/**
	 * <p>Handles odometry messages<\p>
	 */
	@Override public void handle(OdometryMsg msg) {

		//		System.out.println("Pose:\tx: " + msg.x + "\ty:" + msg.y + "\ttheta:" + msg.theta);
		//		System.out.println("Differences:\tx:\t" + Math.abs(msg.x-prev_x) + "y:\t" + Math.abs(msg.y-prev_y) +
		//				"theta:\t" + Math.abs(msg.theta-prev_theta));
		if ((Math.abs(msg.x-prev_x) > EPS_TRANSLATING) ||
				(Math.abs(msg.y-prev_y) > EPS_TRANSLATING) ||
				(Math.abs(msg.theta-prev_theta) > EPS_ROTATING)) {
			currentlyMoving = true;
			System.out.println("Moving");
		} else {
			currentlyMoving = false;
			System.out.println("Not Moving");
		}
		prev_x = msg.x;
		prev_y = msg.y;
		prev_theta = msg.theta;

		if(odometryWait > 0) {
			odometryWait--;
			return;
		}

		// 2.5
		if(state == ALIGNED) {
			System.out.println("ALIGNED");
			if(!currentlyMoving) {

				System.out.println("  -- trying to move " +
						TRANSLATION_BACKUP + " meters with " +
						msg.theta + " change in theta");
				// back up small amount with no change in theta
				//        Robot.moveAlongVector(TRANSLATION_BACKUP, 0);
				double goalx = msg.x + TRANSLATION_BACKUP*Math.cos(msg.theta);
				double goaly = msg.y + TRANSLATION_BACKUP*Math.sin(msg.theta);
				goal = new Point(goalx, goaly);
				setVelocity(-.1,0);
				setState( ALIGNED_AND_BACKING_UP);

				//setState( DONE);
				// wait 3 odometry messages to allow motion to begin
				// before checking state
				odometryWait = 10;
			} else {
				System.out.println("  -- still aligning");
			}
		} else if(state == ALIGNED_AND_BACKING_UP) {
			System.out.println("Goal:\tx:" + goal.x + "\ty:" + goal.y);
			double error = Math.sqrt(Math.pow(msg.x - goal.x, 2) +
					Math.pow(msg.y - goal.y, 2));
			System.out.println("Error:\t" + error);
			if(error > prev_error) {
				System.out.println("  -- trying to rotate");
				prev_error = 99;
				// rotate cw pi/2 rad
				setVelocity(0, -.06);
				goal.theta = msg.theta - Math.PI/2;
				if (goal.theta < 0) goal.theta += Math.PI*2;
				setState( ALIGNED_AND_ROTATED);
				odometryWait = 10;
			} else {
				System.out.println("  -- still backing up");
				prev_error = error;
			}
		} else if(state == ALIGNED_AND_ROTATED) {
			double error = msg.theta - goal.theta;
			if(Math.abs(error) < EPS_ROTATING) {
				setState( BACKING_UP ); // 3.1
				setVelocity(0,0);
				//setState(DONE);
			}
			else {
				System.out.println("  -- still rotating");
				System.out.println("Goal Theta:\t" + goal.theta);
				System.out.println("Current Theta:\t" + msg.theta);
			}
		}

		if(state == DONE) {
			System.out.println("DONE - model acquisition complete");

			try {
				outFile.close();
			} catch(IOException e) {
				System.err.println("Error Closing File: " + e);
			}
		}

	}


	/**
	 * <p>Handles sonar messages<\p>
	 */
	@Override
	public void handle(double rangeFront, double rangeRear) {

		if(sonarWait > 0) {
			sonarWait--;
			return;
		}

		// 2.4 - plot each ping in polyviewer
		// (1) extract sonar range, sonar_positions[i], robot_pose

		// TBD: Could filter sonar data here: average, IIR

		Point robotPose = new Point(prev_x, prev_y);
		Point sFront = FRONT_SONAR;
		Point sRear = REAR_SONAR;

		//log.info("Sonar Range: " + rangeFront + " " + rangeRear);

		// (2) calculate pinged point in real world coordinates
		Point pingPointFront = new Point();
		Point pingPointRear = new Point();

		// Front Sonar
		double rFront = Math.sqrt((Math.pow(sFront.x,2))
				+ (Math.pow(sFront.y,2)));
		double phiFront = Math.atan2(sFront.y, sFront.x);
		double xWorldFront = rFront*Math.cos(phiFront + robotPose.theta)
		+ robotPose.x;
		double yWorldFront = rFront*Math.sin(phiFront + robotPose.theta)
		+ robotPose.y;
		pingPointFront.x = rangeFront*Math.cos(robotPose.theta+sFront.theta)
		+ xWorldFront;
		pingPointFront.y = rangeFront*Math.sin(robotPose.theta+sFront.theta)
		+ yWorldFront;

		// Rear Sonar
		double rRear = Math.sqrt((Math.pow(sRear.x,2))
				+ (Math.pow(sRear.y,2)));
		double phiRear = Math.atan2(sRear.y, sRear.x);
		double xWorldRear = rRear*Math.cos(phiRear + robotPose.theta)
		+ robotPose.x;
		double yWorldRear = rRear*Math.sin(phiRear + robotPose.theta)
		+ robotPose.y;
		pingPointRear.x = rangeRear*Math.cos(robotPose.theta+sRear.theta)
		+ xWorldRear;
		pingPointRear.y = rangeRear*Math.sin(robotPose.theta+sRear.theta)
		+ yWorldRear;

		// (3) classify pinged point as obstacle or background
		boolean frontObstacle = isObstacleDetected(rangeFront);
		boolean rearObstacle = isObstacleDetected(rangeRear);

		// only enable Sonar plotting and line fitting in specific states
		if((state == BACKING_UP) || (state == FINDING_WALL) ||
				(state == TRACKING_WALL)) {

			// Front sonar obstacle detection
			Color color = null;
			if(frontObstacle) {
				if(rearObstacle){
					updateLinearFit(pingPointFront);
					color = OBSTACLE_COLOR;
				} else { color = BACKGROUND_COLOR; }

				GUIPointMsg frontMsg = new GUIPointMsg();
				fillPointMsg(frontMsg, color, pingPointFront.x, pingPointFront.y, FRONT_SHAPE);
				pointPub.publish(frontMsg);

				// Rear sonar obstacle detection
				if(rearObstacle) {
					updateLinearFit(pingPointRear);
					color = OBSTACLE_COLOR;
				} else { color = BACKGROUND_COLOR; }

				GUIPointMsg rearMsg = new GUIPointMsg();
				fillPointMsg(rearMsg, color, pingPointRear.x, pingPointRear.y, REAR_SHAPE);
				pointPub.publish(rearMsg);

				// Plot the updated linear fit
				GUILineMsg lineMsg = new GUILineMsg();
				fillLineMsg(lineMsg, a_fit, b_fit, c_fit);
				linePub.publish(lineMsg);

			}
			// 3.1 - Find start of wall
			if(state == BACKING_UP) {
				System.out.println("BACKING UP");

				System.out.println("obstacles: " + frontObstacle + " " + rearObstacle);
				if(!frontObstacle && !rearObstacle) {
					setVelocity(0.0, 0.0);

					if(firstSegment) {
						startAcquirePosition = new Point();
						startAcquirePosition.x = robotPose.x;
						startAcquirePosition.y = robotPose.y;
						startAcquirePosition.theta = robotPose.theta;
						firstSegment = false;
					} else if(reachedStartPosition(robotPose.x, robotPose.y,
							robotPose.theta)
							&& isValidPolygon()) {
						System.out.println("Stopping Robot, we're done!");
						setVelocity(0.0, 0.0);
						setState( DONE);
						return;
					}

					setState( FINDING_WALL );

				} else {
					// move backward, tracking wall until sonars don't ping wall
					wallTrackController(rangeFront, rangeRear, robotPose,
							BACKWARD);
				}
			}

			if(state == FINDING_WALL) {
				System.out.println("FINDING WALL");

				if(beginFindingWall == true) {
					// store robot pose when beginning first wall
					beginFindingWallPoint = new Point();
					beginFindingWallPoint.x = robotPose.x;
					beginFindingWallPoint.y = robotPose.y;
					beginFindingWall = false;
				}

				// if no obstacle detected move slowly forward
				if(!frontObstacle && !rearObstacle) {
					System.out.println("  - no wall, keep moving forward");
					setVelocity(TRANSLATION_VELOCITY_SLOW, 0.0);
				} else {
					System.out.println("  - wall found, storing point");

					segmentBegin = new Point();
					segmentBegin.x = xWorldFront;
					segmentBegin.y = yWorldFront;

					double xSoFar = robotPose.x - beginFindingWallPoint.x;
					double ySoFar = robotPose.y - beginFindingWallPoint.y;
					double distanceSoFar = Math.sqrt(xSoFar*xSoFar +
							ySoFar*ySoFar);
					System.out.println("  - have we gone 0.2 m yet?");
					if(distanceSoFar > MIN_FINDING_WALL_DISTANCE) {
						System.out.println("    ** yes ** ");
						setState( TRACKING_WALL );
						beginFindingWall = true;
					} else {
						System.out.println("    ** no **");
						setVelocity(TRANSLATION_VELOCITY_SLOW, 0.0);
					}
				}
			}
			if(state == TRACKING_WALL) {
				System.out.println("TRACKING WALL");
				if(!frontObstacle && !rearObstacle) {
					System.out.println("   - wall end found, computing segment");
					setVelocity(0.0, 0.0);
					setState( WALL_ENDED );

					segmentEnd = new Point();
					segmentEnd.x = xWorldRear;
					segmentEnd.y = yWorldRear;

					// find corresponding segment points on fit line
					double normFactor = Math.sqrt(a_fit*a_fit + b_fit*b_fit);
					double a = a_fit / normFactor;
					double b = b_fit / normFactor;
					double c = c_fit / normFactor;
					double xBegin = (a/(a*a+b*b)) *
					((b*b)/a*segmentBegin.x - b*segmentBegin.y - c);
					double yBegin = (-a/b)*xBegin - (c/b);
					double xEnd = (a/(a*a+b*b)) *
					((b*b)/a*segmentEnd.x - b*segmentEnd.y - c);
					double yEnd = (-a/b)*xEnd - (c/b);

					wallSegments.add(new WallSegment(segmentBegin,
							segmentEnd, a, b, c));

					GUISegmentMsg segmentMsg = new GUISegmentMsg();
					fillSegmentMsg(segmentMsg, SonarGUI.makeRandomColor(), xBegin, yBegin, xEnd, yEnd);
					segmentPub.publish(segmentMsg);

					resetLinearFilter();

					// TBD should leave the points
					// (new GUIEraseMessage(SonarGUI.ERASE_POINTS)).publish();

				} else {
					System.out.println("   - tracking wall");
					wallTrackController(rangeFront, rangeRear, robotPose,
							FORWARD);
				}
			}

			// 4 - Model Acquisition
			if(state == WALL_ENDED) {
				setState( TURNING_CCW_BUMP );
			}
		}
	}


	private void fillSegmentMsg(GUISegmentMsg segmentMsg,
			Color c, double xBegin, double yBegin, double xEnd,
			double yEnd) {
		segmentMsg.color = getColorMsg(c);
		segmentMsg.startX = xBegin;
		segmentMsg.endX = xEnd;
		segmentMsg.startY = yBegin;
		segmentMsg.endY = yEnd;
	}


	private void fillLineMsg(GUILineMsg lineMsg, double a, double b,
			double c) {
		lineMsg.lineA = a;
		lineMsg.lineB = b;
		lineMsg.lineC = c;
	}


	private void fillPointMsg(GUIPointMsg pointMsg, Color color, double x,
			double y, int shape) {
		pointMsg.color = getColorMsg(color);
		pointMsg.x = x;
		pointMsg.y = y;
		pointMsg.shape = shape;
	}


	private ColorMsg getColorMsg(Color c) {
		ColorMsg msg = new ColorMsg();
		msg.r = c.getRed();
		msg.g = c.getGreen();
		msg.b = c.getBlue();
		return msg;
	}


	/**
	 * <p>Updates the online linear regression of sonar data<\p>
	 *
	 * @param new point to include in line fit
	 */
	public void updateLinearFit(Point p) {
		numPoints++;
		sum_x += p.x;
		sum_y += p.y;
		sum_xx += p.x * p.x;
		sum_xy += p.x * p.y;
		sum_yy += p.y * p.y;

		// wait until we have at least 2 points
		if(firstPoint) {
			firstPoint = false;
			return;
		}

		double D = sum_xx*sum_yy - sum_xy*sum_xy;
		if(D < 1.0E-8)
			return;

		a_fit = sum_x*sum_yy/D - sum_y*sum_xy/D;
		b_fit = sum_y*sum_xx/D - sum_x*sum_xy/D;
		c_fit = -1;
	}


	/**
	 * <p>Resets state of the linear filter<\p>
	 */
	public void resetLinearFilter() {
		numPoints = 0;
		sum_x = 0;
		sum_y = 0;
		sum_xy = 0;
		sum_xx = 0;
		sum_yy = 0;
		a_fit = 0;
		b_fit = 0;
		c_fit = 0;
	}


	/**
	 * <p>Controller for wall tracking<\p>
	 *
	 * @param current front sonar range value
	 * @param current rear sonar range value
	 * @param current robot pose
	 * @param direction to traverse wall (FORWARD or BACKWARD)
	 */
	public void wallTrackController(double frontRange, double rearRange,
			Point robotPose, int direction) {

		double normFactor = Math.sqrt(a_fit*a_fit + b_fit*b_fit);
		double a = a_fit / normFactor;
		double b = b_fit / normFactor;
		double c = c_fit / normFactor;
		double wallRange = Math.abs(a*robotPose.x + b*robotPose.y + c);

		double distError = DESIRED_WALL_DISTANCE + (robotWidth/2)
		- Math.abs(wallRange);

		System.out.println("wallRange: " + wallRange + " dError: " + distError);

		// compute orientation error
		// (a)
		//		double thetaError = Math.atan(-a/b) - robotPose.theta;
		double rotError = rangeRear- rangeFront;

		// handle 180 degrees off
		/*
		if(Math.abs(thetaError) > Math.PI/2) {
			// if robotPose is negative, add PI
			// if positive, subtract PI
			thetaError += Math.signum(robotPose.theta)*Math.PI;
			System.out.println("Corrected thetaError:" + thetaError);
		}
		 */
		// command translational and rotational velocity
		double command_tv = 0.075 * direction;
		double rv_theta = GAIN_RV_THETA * rotError;

		setVelocity(command_tv, rv_theta);
	}


	/**
	 * <p>Classifies given range as obstacle or background<\p>
	 *
	 * @param sonar range value to classify
	 */
	public boolean isObstacleDetected(double range) {
		if(range < OBSTACLE_DETECTION_THRESHOLD)
			return true;
		else
			return false;
	}


	/**
	 * <p>Write log data<\p>
	 */
	public void writeToFile() {
		/*
		if (saveErrors) {
			try {
				outFile.write(Double.toString(p.x) + " ");
				outFile.write(Double.toString(p.y) + " ");
				outFile.write(Double.toString(m_fit) + " ");
				outFile.write(Double.toString(b_fit));
				outFile.newLine();
			} catch(IOException e) {
				System.err.println("Error Writing File: " + e);
			}
		}
		 */
	}


	/**
	 * <p>Compute exterior angle between two polygon sides<\p>
	 *
	 * @param first wall segment (in CCW motion)
	 * @param second wall segment (in CCW motion)
	 */
	private double computeExteriorAngle(WallSegment ws1, WallSegment ws2) {
		// find difference in theta's relative to x-axis
		return ws2.getTheta() - ws1.getTheta();
	}


	/**
	 * <p>Determine if ending point of one wall segment and starting point
	 * of subsequent segment are sufficiently close together<\p>
	 *
	 * @param first wall segment (in CCW motion)
	 * @param second wall segment (in CCW motion)
	 */
	private boolean isValidSegmentDistance(WallSegment ws1, WallSegment ws2) {
		double x_dist = ws1.getEndPoint().x - ws2.getStartPoint().x;
		double y_dist = ws1.getEndPoint().y - ws2.getStartPoint().y;
		double distance = Math.sqrt(x_dist*x_dist + y_dist*y_dist);

		if(distance < EPS_POLYGON_HOLE_SIZE) {
			return true;
		} else {
			return false;
		}
	}


	/**
	 * <p>Compute the intersection point of two wall segments<\p>
	 *
	 * @param first wall segment
	 * @param second wall segment
	 */
	private Point computeIntersectionPoint(WallSegment ws1, WallSegment ws2) {
		// use the stored linear fit values for each wall segment
		double a1 = ws1.getA();
		double b1 = ws1.getB();
		double c1 = ws1.getC();
		double a2 = ws2.getA();
		double b2 = ws2.getB();
		double c2 = ws2.getC();

		Point p = new Point();
		p.x = (c1/b1 - c2/b2) / (a2/b2 - a1/b1);
		p.y = (1/b1) * (-a1 * p.x - c1);

		return p;
	}


	/**
	 * <p>Determine if the current polygon is valid<\p>
	 */
	public boolean isValidPolygon() {

		if(wallSegments.isEmpty()) {
			return false;
		}

		Point p;
		double theta;
		double thetaSum = 0;

		int i = wallSegments.size() - 1; // handle loop around
		for(int j=0; j < wallSegments.size(); j++) {

			WallSegment ws1 = wallSegments.get(i);
			WallSegment ws2 = wallSegments.get(j);

			if(isValidSegmentDistance(ws1, ws2)) {
				p = computeIntersectionPoint(ws1, ws2);

				// TEST plotting coords

				GUIPointMsg pointMsg = new GUIPointMsg();
				pointMsg.color = getColorMsg(Color.blue);
				pointMsg.shape = FRONT_SHAPE;
				pointMsg.x = p.x;
				pointMsg.y = p.y;
				pointPub.publish(pointMsg);
				//				(new GUIPointMessage(Color.BLUE, p.x, p.y, FRONT_SHAPE)).publish();

				theta = computeExteriorAngle(ws1, ws2);
				thetaSum += theta;
				System.out.println("Exterior Angle ("+i+","+j+") = " + theta
						+ " thetaSum:" + thetaSum);

			} else {
				System.out.println("The distance between wall segments " + i + " and "
						+ j + " is greater than the allowed maximum: "
						+ EPS_POLYGON_HOLE_SIZE);
				return false;
			}

			i = j;
		}

		// check sum of exterior angles, they should add to 0.0
		if(Math.abs(thetaSum) < EPS_EXTERIOR_THETA_SUM) {
			return true;
		} else {
			System.out.println("The total exterior angle of the polygon, " + thetaSum
					+ ", exceeds the tolerance: " + EPS_EXTERIOR_THETA_SUM);
			return false;
		}
	}


	/**
	 * <p>Determine if robot has reached the starting position<\p>
	 */
	private boolean reachedStartPosition(double x, double y, double theta) {
		double thetaError = Math.abs(startAcquirePosition.theta - theta);
		double distanceError = Math.sqrt(Math.pow(startAcquirePosition.x-x, 2) +
				Math.pow(startAcquirePosition.y-y, 2));

		System.out.println("*********************************************");
		System.out.println("Checking if we have reached start location: ");
		System.out.println("  theta error from starting pose: " + thetaError);
		System.out.println("  distance error from staring pose: " + distanceError);

		if( thetaError < EPS_THETA_FINISHED &&
				distanceError < EPS_DISTANCE_FINISHED ) {
			return true;
		} else {
			return false;
		}
	}


}