import java.lang.*;
//import java.util.Date;

public synchronized class RobotController extends java.lang.Object
{
    int[] stateArray;

    public final static int XPOS = 17;
    public final static int YPOS = 18;
    public final static int THETA = 19;
    public final static int RVEL = 20;
    public final static int LVEL = 21;
    public final static int BUMPER = 22;


    /** Creates a new RobotController.

        @param outputArea A place for the controller to output
             debugging statements (currently there are none). */
    public RobotController() {
      	stateArray = new int[23];
    }


    /** open and close the serial port **/

    public int serialOpen() {
        this.openSerial();
        return 1;
    }

    public int serialClose() {
        this.closeSerial();
        return 1;
    }

    public int turnSonarsOn() {
        this.sonarsOn(stateArray);
        return stateArray[0];
    }

    public int turnSonarsOff() {
        this.sonarsOff(stateArray);
        return stateArray[0];
    }

    public int GS() {
        this.getState(stateArray);
        return stateArray[0];
    }

    /** [robot command] Sets the wheel velocity.

        @param leftVel The new left wheel velocity in 10 x inches/s.
        @param rightVel The new right wheel velocity in 10 x inches/s.
        @return STATUS_OK if successful. */
    public int setVel(int leftVel, int rightVel) {
        rMove(rightVel, leftVel, stateArray);
        return stateArray[0];
    }

    /** [robot command] Kills the left and right wheel motors (typically causing
        the robot to coast to a halt).

        @return STATUS_OK if successful. */
    public int killMotors() {
        limp(stateArray);
        return stateArray[0];
    }

    /** [robot command] Sets the wheel acceleration.
        Default is 100 10xin / sec / sec
        @param leftAccel The new acceleration of the left wheel in mm/s/s.
        @param rightAccel The new acceleration of the right wheel in mm/s/s.
        @return STATUS_OK if successful. */
    public int setAccel(int leftAccel, int rightAccel) {
        confAc(rightAccel, leftAccel, 0, stateArray);
        return stateArray[0];
    }

    /** [robot command] Reconfigures the sonars.
        Do not use this function yet!
        @param period If this value is x, the time delay between firing of
            two consecutive sonars will be 4 * x ms.  0 < x < 256.
        @param sonarOrder sonarOrder[i] will be the ith sonar fired
            in the new firing sequence.
        @param numSonars The length of the new firing sequence.
        @return STATUS_OK if successful. */
    public int configureSonars(int period, int[] sonarOrder) {
        confSonars(period, sonarOrder, stateArray);
        return stateArray[0];
    }

    /** [robot command] Sets the command timeout.  If no commands are issued to the
        robot for a time equal to the timeout, the robot will internally
        issue a killMotors() command.
        DEFAULT is 2 seconds
        timeout is in seconds.
        @return STATUS_OK if successful. */
    public int setTimeout(int timeout) {
        confTimeout(timeout, stateArray);
        return stateArray[0];
    }

    // ***************************************************************** //
    // the following are C language DLL calls! //
    // opens serial port, returns 0 if successful

    native int openSerial();
    native int closeSerial();

    // All of the below functions take an array that MUST be allocated
	// already, as a 23-element integer array in Java. See above for
	// examples of how to do that. Here is what gets returned in this array
	// index 0: return value of the last command
	// index 1 - 16: sonars 1 - 16 values (ranges in inches)
	// index 17: integrated X position of the robot
	// index 18: integrated Y position of the robot
	// index 19: angle of the robot, 0 - 3600
	// index 20: right wheel measured velocity of the robot
	// index 21: left wheel measured velocity of the robot
	// index 22: bumper state of the robot-- if this is zero, no
	//              bumper is depressed. Else, it will be greater than 0.

    // turns on all 16 sonars in default pattern
	native int sonarsOn(Object stateArray);
	// turns off all sonars
	native int sonarsOff(Object stateArray);
	native int zero(Object stateArray); // zero the encoders
	native int limp(Object stateArray); // limp the motors
	native int confAc(int ta, int sa, int ra, Object stateArray); //configure accelerations
	native int confTimeout(int timeout, Object stateArray); // configure motor timeout
	native int getState(Object stateArray); // update the state array
	native int rMove(int rightspeed, int leftspeed, Object stateArray);
	native int confSonars(int rate, Object sonarOrder,Object stateArray);

    // .dll loading command //
	static {
	    System.loadLibrary("cscout");
	}
}