import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.robotics.*;

/**
 *RCXMethods
 * 
 * @author Michael Salley
 * @version 1.20   9 FEB 2009
 * Some code reused from BasicLEGO.java created by Michael Salley and Aaron Folstad 
 */
public class RCXMethods {
 private static Sensor s1;
 private static Sensor s2;
 private static Sensor s3;
 private static Motor a;
 private static Motor c;
 
 private static int sensorMode = -1;
 
    /**
     * Place any statements you wish your robot to execute inside the main() method.
     * Feel free to write your own methods to supplement those provided in the class.
     * Your robot should configureAttachments first and shutDown last, regardless of your operations. 
     */
    public static void main(String args[]) {
    	configureAttachments(0); //0 by default for 2lt1tch, change parameter to 1 to switch configurations
    	
    	/*
    	 * Your code goes here
    	 */
    	
    	
    	
    	shutDown();
    }

    
    //Configuration 1: Two touch sensors and one light sensor. 
    //Configuration 0: Two light sensors and one touch sensor. 
    	//**Note: Parallel touch sensors may be used with just one input. No need for config2 to represent this.
    /**
     * @param mode Specifies which of two sensor configurations the robot should use
     * 
     * When mode==0, the robot is configured with two light sensors on ports S1 and S3 and one touch sensor on S2. 
     * If two touch sensors are in use in this configuration, they should be wired in parallel, with their plugs stacked.
     * In this way, they can be used as if the input from one was logically ORed with the input from another. 
     * 
     * When mode==1, the robot is configured with two touch sensors working independently of one another on ports S1 and S3
     * With a single touch sensor on S2. 
     * 
     * Both configurations provide for two motors on ports A and C. If a third motor is added, add appropriate code here to
     * initialize a new field for the third motor. 
     */
    public static void configureAttachments(int mode) {
    	s1 = Sensor.S1;
		s2 = Sensor.S2;
		s3 = Sensor.S3;
		a = Motor.A;
		c = Motor.C;
		
    	if(mode==0) {
    		//Set up for two light sensors and one touch sensor.
    		//Touch sensors may be compounded.
    		
    		s1.setTypeAndMode(3,0x80);
    		s2.setTypeAndMode(1,0x20);
    		s3.setTypeAndMode(3,0x80);	
    	}
    	else if (!(mode==1)) {
    		//Parameter was invalid
    		System.out.println("Invalid sensor configuration mode specified. Now exiting.");
    		System.exit(1);
    	}
    	else {
    		//Configure for two touch sensors and one light sensor. 
    		s1.setTypeAndMode(1,0x20);
    		s2.setTypeAndMode(3,0x80);
    		s3.setTypeAndMode(1,0x20);	
    	}
    	
    	s1.activate();
		s2.activate();
		s3.activate();
		
		sensorMode = mode;
    }
 
    
     /**
     * Deactivates all motors and "passivates" all sensors. Should be the last
     * executed statement of all robot programs. 
     */
    public static void shutDown()    {

        a.stop();
        c.stop();
        s1.passivate();
        s2.passivate();
        s3.passivate();
    }

    /**
     * sets all motors to the power level 0-7 specified by the parameter.
     * @param powerLevel An integer value representing power levels 0=low through 7=high.
     */
    public static void setAllMotors(int powerLevel)    {

        a.setPower(powerLevel);
        c.setPower(powerLevel);
    }

    /**
     * Sets motor A to the power level specified by the first parameter, 
     * and sets motor C to the power level specified by the second. 
     * @param powerA power level to send to Motor A
     * @param powerC power level to send to Motor C
     */
    public static void setMotors(int powerA, int powerC)   {
  
        a.setPower(powerA);
        c.setPower(powerC);
    }
    
    
    /**
     * Sets both motors to full power.
     */
    public static void fullPower() {
        setAllMotors(7);
    }
    
    /**
     * Sets both motor directions to forward, assuming correct polarity on the wires.
     * NOTE: If your robot is moving backward when it should be moving forward, the solution to this problem
     *	     is NOT to change this method so that it calls backward() instead. We know this from experience. 
     */
    public static void allForward()     {

        a.forward();
        c.forward();
    }

    /**
     * Sets both motors to backward, assuming correct wire polarity.
     */
    public static void allBackward()   {
 
        a.backward();
        c.backward();
    }

    /**
     * Returns the value of the single touch sensor if only one is equipped. 
     * Returns the composite logical OR value of both sensors if both are attached on separate ports.
     */
    public static boolean getTouch() {
    	
    	if(sensorMode==0)
    		return s2.readBooleanValue();
    	else return s1.readBooleanValue()||s3.readBooleanValue();
    }
    /**
     * @param s The sensor object from which to get information
     * Accepts s1, s2, or s3 as input and returns the boolean value of the touch sensor located there.
     * If the sensor is not a touch sensor, method returns false. 
     */
    public static boolean getTouch(Sensor s) {
    	if(s.getId() == 1 && sensorMode==0)
    	return s.readBooleanValue();
    	else if((s.getId() == 0 || s.getId() == 2) && sensorMode==1)
    		return s.readBooleanValue();
    	else return false;
    }
    
  /**
   * 
   * Returns the light sensor reading as a value from 0-100 if the robot only has a single light sensor. 
   * Otherwise, if the robot has two light sensors, it returns the average of both. 
   * If the robot can't find any light sensors in its configuration, return value is -1. 
   */
    public static int getLight() {
    	if(sensorMode==1)
    		return s2.readValue();
    	else if(sensorMode==0)
    		return (s1.readValue() + s3.readValue())/2;
    	else return -1;
    }
    /**
     * 
     *@param s The sensor object from which to read information
     *Returns the light value of the light sensor found in sensor object s. If the sensor is not a light sensor, returns -1.
     */
    public static int getLight(Sensor s) {
    	if(sensorMode==1 && s.getId() == 1)
    		return s.readValue();
    	else if(sensorMode==0 && (s.getId() == 0 || s.getId() == 2))
    			return s.readValue();
    	else return -1;
    }
    
    /**
     * Causes the robot to remain in its current state of motion and logic
     * for the number of milliseconds specified by the parameter. One second 
     * is equal to 1000 milliseconds. 
     * @param milliseconds the number of milliseconds the robot should Sleep. 
     */
    public static void rest(int milliseconds)    {

        try
        {
            Thread.sleep(milliseconds);
        }
        catch(Exception e) { }
    }

    /**
     * Executes a relatively close U-turn in a counterclockwise direction. 
     * Modify the value in the call to rest() to represent the amount of actual
     * turn-around time the robot requires based on its current wheel configuration,
     * power level, "road" surface, and other variables. 
     */
    public static void uTurnLeft()    {

        c.forward();
        a.backward();
        rest(2100);
    }

    /**
     * Executes a relatively close U-turn in a clockwise direction. 
     * Modify the value in the call to rest() to represent the amount of actual
     * turn-around time the robot requires based on its current wheel configuration,
     * power level, "road" surface, and other variables. 
     */
    public static void uTurnRight()    {

        a.forward();
        c.backward();
        rest(2100);
    }

   
}