package LocalNavigation;
import Carmen.BumperMessage;

/**
 * Abstraction for the data contained in a bumper message which masks
 * the bumpers into left and right, and pressed and not-pressed states.
 * 
 * This works for only 2 bumpers - but can be extended to N bumpers by
 * using a switch case.
 * 
 * @author Vinayak Ranade
 */
public class BumperData {
	
	public static final int NUM_BUMPERS = 2;
	
	private int leftBumper, rightBumper;
	
	private int[] bumpers = new int[NUM_BUMPERS];
	
	/**
	 * Left and Right Bumpers
	 */
	public static int RIGHT_BUMPER = 1, LEFT_BUMPER = 0;
	
	/**
	 * Tells us whether a bumper is pressed or not pressed
	 */
	public static final int PRESSED = 0, NOT_PRESSED = 1;

	/**
	 * Takes in a Carmen Bumper message and constructs a
	 * BumperData object
	 * 
	 * @param msg
	 */
	public BumperData(BumperMessage msg){
		
		for(int i =0; i<NUM_BUMPERS; i++){
			bumpers[i] = msg.bumper[i];
		}

		leftBumper = msg.bumper[LEFT_BUMPER];
		rightBumper = msg.bumper[RIGHT_BUMPER];
	}
	
	/**
	 * True is the specified bumper is pressed, false if not.
	 * 
	 * @param bumperIndex : The bumper number that we want the status for
	 */
	public boolean isBumperPressed(int bumperIndex){	

		if(bumperIndex < NUM_BUMPERS){
			return bumpers[bumperIndex] == 0;
		}
		else{
			throw new RuntimeException("Invalid Bumper Type");
		}
	}
	
	/**
	 * True if both the bumpers are pressed, false otherwise
	 * ONLY use if NUM_BUMPERS ==2 
	 */
	public boolean bothBumpersPressed(){
		assert NUM_BUMPERS == 2;
		return isBumperPressed(RIGHT_BUMPER) && isBumperPressed(LEFT_BUMPER);		
	}
	
	
	/**
	 * True if exactly one of the bumpers is pressed, false otherwise/
	 */
	public boolean onlyOneBumperPressed(){
		
		boolean onePressed = false;
		
		for(int i = 0; i<NUM_BUMPERS; i++){
			if(!onePressed && isBumperPressed(i)){
				onePressed = true;
			}
			else if(onePressed && isBumperPressed(i)){
				onePressed = false;
				return onePressed;
			}
		}
		return onePressed;
	}

	/**
	 * True iff left bumper is pressed
	 * @return
	 */
	public boolean leftBumperPressed(){
		return isBumperPressed(LEFT_BUMPER);
	}
	
	/**
	 * True iff right bumper is pressed
	 * @return
	 */
	public boolean rightBumperPressed(){
		return isBumperPressed(RIGHT_BUMPER);
	}
	
	/**
	 * True iff at least one bumper is pressed
	 * @return
	 */
	public boolean atLeastOneBumperPressed(){
		
		for(int i = 0; i<NUM_BUMPERS; i++){
			if(isBumperPressed(i)){
				return true;
			}
		}
		return false;
	}

	/**
	 * True if the specified bumper is the only bumper that is pressed
	 * False otherwise
	 * 
	 * Eg: onlyBumperPressed(RIGHT_BUMPER) will only return true if
	 * 			rightBumperPressed() == true AND
	 * 			leftBumperPressed() == false
	 * 
	 * 
	 * @param bumper
	 * @return
	 */
	public boolean onlyBumperPressed(int bumper){
		
		assert bumper < NUM_BUMPERS;
		
		if(onlyOneBumperPressed()){
			return isBumperPressed(bumper);
		}
		else{
			return false;
		}

	}
}