package LocalNavigation;

import messaging.BumpListener;
import messaging.OdometryListener;
import messaging.SonarListener;

import org.apache.commons.logging.Log;
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 org.ros.namespace.GraphName;
import org.ros.node.Node;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;
import org.ros.node.topic.Subscriber;

/**
 * Solution Dispatcher for Local Navigation
 * @author rss-staff
 *
 */
public abstract class LocalNavigationSolution implements NodeMain{

	private static final String GUI_POINT_MESSAGE = "lab5_msgs/GUIPointMsg";
	private static final String POINT_CHANNEL = "gui/Point";
	private static final String GUI_SEGMENT_MESSAGE = "lab5_msgs/GUISegmentMsg";
	private static final String SEGMENT_CHANNEL = "gui/Segment";
	private static final String GUI_LINE_MESSAGE = "lab5_msgs/GUILineMsg";
	private static final String LINE_CHANNEL = "gui/Line";
	public static final String MOTION_MESSAGE = "rss_msgs/MotionMsg";
	public static final String MOTOR_CHANNEL = "command/Motors";
	public static String ODOMETRY_CHANNEL = "rss/odometry";
	public static String ODOMETERY_MESSAGE = "rss_msgs/OdometryMsg";

	public static String SONAR_CHANNEL = "rss/Sonars";
	public static String SONAR_MESSAGE = "rss_msgs/SonarMsg";

	public static String BUMP_CHANNEL = "rss/BumpSensors";
	public static String BUMP_MESSAGE = "rss_msgs/BumpMsg";

	Publisher<MotionMsg> motionPub;
	Publisher<GUILineMsg> linePub;
	Publisher<GUISegmentMsg> segmentPub;
	Publisher<GUIPointMsg> pointPub;
	Publisher<org.ros.message.std_msgs.String> statePub;

	protected Log log;
	/**
	 * <p> In order to decide whether the solution is provided
	 * by the students or by the staff <p>
	 */
	public static final int STAFF_SOLUTION = 0, STUDENT_SOLUTION = 1;

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

	@Override
	public void onStart(Node node){

		log = node.getLog();

		//Set the STAFF_SOLUTION as the default solution type
		//We can change this to be STUDENT SOLUTION if we want
		int solutionType = STAFF_SOLUTION;

		LocalNavigationSolution localNav;
		switch(solutionType){
			case STAFF_SOLUTION: localNav = new LocalNavigation(); break;
			case STUDENT_SOLUTION: break;
			//Add more cases here if there are more solution types
			// eg: STAFF_SOLUTION_EASY
			// STAFF_SOLUTION_HARD ... etc.
			default: break;
		}

		bumpSub = node.newSubscriber(BUMP_CHANNEL, BUMP_MESSAGE);
		bumpSub.addMessageListener(new BumpListener(this));
		odoSub = node.newSubscriber(ODOMETRY_CHANNEL, ODOMETERY_MESSAGE);
		odoSub.addMessageListener(new OdometryListener(this));
		sonarFrontSub = node.newSubscriber(SONAR_CHANNEL+"/Front", SONAR_MESSAGE);
		sonarFrontSub.addMessageListener(new SonarListener(this));
		sonarRearSub = node.newSubscriber(SONAR_CHANNEL+"/Back", SONAR_MESSAGE);
		sonarRearSub.addMessageListener(new SonarListener(this));

		motionPub = node.newPublisher(MOTOR_CHANNEL, MOTION_MESSAGE);
		linePub = node.newPublisher(LINE_CHANNEL, GUI_LINE_MESSAGE);
		segmentPub = node.newPublisher(SEGMENT_CHANNEL, GUI_SEGMENT_MESSAGE);
		pointPub = node.newPublisher(POINT_CHANNEL, GUI_POINT_MESSAGE);
		statePub = node.newPublisher("/rss/state", "std_msgs/String");
	}

	@Override
	public void onShutdown(Node node){

	}

	@Override
	public void onShutdownComplete(Node node) {
	}

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

	public abstract void handle(OdometryMsg msg);
	public abstract void handle(BumpMsg msg);
	public abstract void handle(double frontRange, double rearRange);

}