package LocalNavigation;

import Carmen.*;

public class SonarTest implements SonarHandler, BumperHandler, OdometryHandler, HeartbeatHandler, CameraHandler, ArmHandler

{
	/**
	 * <p>DNS name for Carmen central host<\p>
	 */
	public static String centralHost = "localhost";
	
	/**
	 * Run this to test sonar
	 * @param args
	 */
	public static void main(String args[]){

		SonarTest sonarTest = new SonarTest();
		
		Robot.dispatch();
	}
	
	/**
	 * Constructor which takes care of all the common initializing steps which
	 * are independent of the solution implementation
	 */
	public SonarTest(){
		Robot.initialize("SonarTest", centralHost);
		Robot.resetRobotBase();
		Robot.setVelocity(0.0, 0.0);

		BumperMessage.subscribe(this);
		SonarMessage.subscribe(this);
		OdometryMessage.subscribe(this);
		ArmMessage.subscribe(this);
		HeartbeatMessage.subscribe(this);
		CameraMessage.subscribe(this);
	}
	
	
	public void handle(SonarMessage msg) {
		
		
		System.out.println("The number of sonars on this robot are " + msg.num_sonars);

	    double rangeFront = msg.range[0];
	    double rangeRear = msg.range[1];
	    Point robotPose = msg.robot_pose;
	    Point sFront = msg.sonar_positions[0];
	    Point sRear = msg.sonar_positions[1];

	    System.out.println("Sonar Range F: " + rangeFront + "\nSonar Range R: " + rangeRear);
		
	}

	public void handle(BumperMessage msg) {
		
		if(msg.bumper[0] == 0){
			System.out.println("Bumper 0 is at 0, i.e bump detected");
		}
		else{
			System.out.println("Bumper 0 is at 1, i.e bump NOT detected");
		}
		
		if(msg.bumper[1] == 0){
			System.out.println("Bumper 1 is at 0, i.e bump detected");
		}
		else{
			System.out.println("Bumper 1 is at 1, i.e bump NOT detected");
		}
		
	}

	public void handle(OdometryMessage msg) {
		// TODO Auto-generated method stub
		
	}

	public void handle(HeartbeatMessage msg) {
		// TODO Auto-generated method stub
		
	}

	public void handle(CameraMessage msg) {
		// TODO Auto-generated method stub
		
	}

	public void handle(ArmMessage msg) {
		// TODO Auto-generated method stub
		
	}
	
	
	
}