/*
 *  RoombaComm Interface
 *
 *  Copyright (c) 2006 Tod E. Kurt, tod@todbot.com, ThingM
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General
 *  Public License along with this library; if not, write to the
 *  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
 *  Boston, MA  02111-1307  USA
 *
 */


package roombacomm;

/**
 * The abstract base for all Roomba communications.
 * 
 * <h2> Overview </h2>
 * This class contains the communications layer-independent parts of
 * how to communicate with a Roomba.  It does assume a very serial port-like
 * interaction, as the only working subclass is for serial ports.
 * Thus, this objects form may change in the future.
 *
 * Standard lifecyle of this object (and its subclasses) <pre>
 *   RoombaComm roomba = new RoombaCommSubClass();  // (e.g. RoombaCommSerial)
 *   roomba.listports();               // if implemented
 *   roomba.connect("someportid");
 *   roomba.startup();
 *   roomba.updateSensors();
 *   while( ... ) {
 *      roomba.sensors();
 *      roomba.playNote( 53, 12 );
 *      roomba.goForward( 400 );
 *      roomba.spinRight( 45 );
 *      if( roomba.bump() ) roomba.goBackward( 100 );
 *   }    
 *   roomba.disconnect();
 * </pre>
 *
 * <h2> API levels </h2>
 * Describe different API levels
 *
 * <h2> Sensor Functions </h2>
 * Describe sensor functions
 *
 * <h2> Sublass behavior </h2>
 * Describe subclassing strategries
 *
 *
 * @author Tod E. Kurt
 *
 */
public abstract class RoombaComm
{
    /** version of the library */
    static public final String VERSION = "0.95";

    /** turns on/off various debugging messages */
    public boolean debug = false;
    
    /** distance between wheels on the roomba, in millimeters */
    public static final int wheelbase = 258; 
    /** mm/deg is circumference distance divided by 360 degrees */
    public static final float 
        millimetersPerDegree = (float)(wheelbase * Math.PI / 360.0);
    /** mm/rad is a circumference distance divied by two pi */
    public static final float 
        millimetersPerRadian = (float)(wheelbase/2);

    /** default speed for movement operations if speed isn't specified */
    public static final int defaultSpeed  =  200;

    /** default update time in ms for auto sensors update */
    public static final int defaultSensorsUpdateTime = 200;
       
    /** current mode, if known */
    int mode;

    /** current speed for movement operations that don't take a speed */
    public int speed = defaultSpeed;

    /** computed boolean for when Roomba is errored out of safe mode */
    boolean safetyFault = false;
    /** if sensor variables have been updated successfully */
    boolean sensorsValid = false;
    /** Set to true to make sensors auto-update (at expense of serial b/w) */
    boolean sensorsAutoUpdate = false;
    /** Time in milliseconds between sensor updates  */
    int sensorsUpdateTime = 200;
    /** last time (System.currentTimeMillis) that the sensors were updated */
    long sensorsLastUpdateTime;
    /** how many bytes we expect to read from the sensor command */
    int readRequestLength;

    /** internal storage for all roomba sensor data */
    byte[] sensor_bytes = new byte[1024];
  
    /** connected to a serial port or not, not necessarily to roomba */
    boolean connected = false;

    public RoombaComm() {
        connected = false;
        mode = MODE_UNKNOWN;
    }

    public RoombaComm(boolean autoUpdate) {
        this();
        if( autoUpdate )
            startAutoUpdate();
    }

    public RoombaComm(boolean autoUpdate, int updateTime) {
        this(autoUpdate);
        sensorsUpdateTime = updateTime;
    }

    public void startAutoUpdate() {
        new Thread( new Runnable() {
                public void run() {
                    try { 
                        while( sensorsUpdateTime > 0 ) {
                            if( connected() ) sensors();
                            Thread.sleep( sensorsUpdateTime );
                        }
                    } catch(InterruptedException ex) {}
                }
            }).start();
    }

    /**
     * List available ports
     * @return a list available portids, if applicable
     * or empty set if no ports, 
     * or return null if list is not enumerable
     */
    public abstract String[] listPorts();

    /**
     * Connect to a port
     * (for serial, portid is serial port name, for net, portid is url?)
     * @return true on successful connect, false otherwise
     */
    public abstract  boolean connect(String portid);
    /**
     * Disconnect from a port, clean up any memory in use
     */
    public abstract  void disconnect();


    /**
     * Send given byte array to Roomba. 
     * @param bytes byte array of ROI commands to send
     * @return true on successful send
     */
    public abstract boolean send(byte[] bytes);

    /** 
     * Send a single byte to the Roomba 
     * (defined as int because of stupid java signed bytes)
     * @param b byte of an ROI command to send
     * @return true on successful send
     */
    public abstract boolean send(int b);

    /**
     * Query Roomba for sensor status and sync its state with this object's
     * Subclasses should query Roomba and fill up 'sensor_bytes' with the full
     * sensor data set
     * If a RooombaComm object is constructed with 'autoUpdate' true, 
     * calling this method is not required because a separate thread is created
     * to do sensor updating.
     *
     * @return true on successful sensor update, false otherwise
     */
    public abstract boolean updateSensors();

    /**
     * Wake's Roomba up, if possible, thus optional
     * To wake up the Roomba requires twiddling its DD line, often
     * hooked up to the RS-232 DTR line, which may not be available in some 
     * implementations
     */
    public void wakeup() {
        logmsg("subclass has not implemented");
    }

    /**
     * Put Roomba in safe mode.
     * As opposed to full mode.  Safe mode is the preferred working state
     * when playing with the Roomba as it provides some measure of 
     * autonomous self-preservation if it encounters a cliff or is picked up
     * If that happens it goes into passive mode and must be 'reset()'.
     * @see #reset()
     */
    public void startup() {
        logmsg("startup");
        speed = defaultSpeed;
        start();
    }

    /** 
     * Reset Roomba after a fault.  This takes it out of whatever mode it was
     * in and puts it into safe mode.
     * This command also syncs the object's sensor state with the Roomba's
     * by calling updateSensors()
     * @see #startup()
     * @see #updateSensors()
     */
    public void reset() {
        logmsg("reset");
        stop();
        startup();
        control();
        updateSensors();
    }

    /**  Send START command  */
    public void start() { 
        logmsg("start");
        mode = MODE_PASSIVE;
        send( START );
    }
    /**  Send CONTROL command  */
    public void control() { 
        logmsg("control");
        mode = MODE_SAFE;
        send( CONTROL );
        // set blue dirt LED on so we know roomba is powered on & under control
        // (and we don't forget to turn it off, and run it's batteries flat)
        // FIXME: first time after a poweron, the lights flash then turn off
        setLEDs(false, false, false, false, false, true, 128, 255);
    }
    /**  Send SAFE command  */
    public void safe() { 
        logmsg("safe");
        mode = MODE_SAFE;
        send( SAFE );
    }
    /**  Send FULL command  */
    public void full() { 
        logmsg("full");
        mode = MODE_FULL;
        send( FULL );
    }

    /**
     * Power off the Roomba.  Once powered off, the only way to wake it
     * is via wakeup() (if implemented) or via a physically pressing
     * the Power button
     * @see #wakeup()
     */
    public void powerOff() {
        logmsg("powerOff");
        mode = MODE_UNKNOWN;
        send( POWER );
    }

    /** Send the SPOT command */
    public void spot() {
        logmsg("spot");
        mode = MODE_PASSIVE;
        send( SPOT );
    }
    /** Send the CLEAN command */
    public void clean() {
        logmsg("clean");
        mode = MODE_PASSIVE;
        send( CLEAN );
    }
    /** Send the max command */
    public void max() {
        logmsg("max");
        mode = MODE_PASSIVE;
        send( MAX );
    }
    /** 
     * Send the SENSORS command 
     * with one of the SENSORS_ arguments
     * Typically, one does "sensors(SENSORS_ALL)" to get all sensor data
     * @param packetcode one of SENSORS_ALL, SENSORS_PHYSICAL, 
     *                   SENSORS_INTERNAL, or SENSORS_POWER, or for roomba 5xx, it
     *                   is the sensor packet number (from the spec)
     */
    public void sensors(int packetcode ) {
        logmsg("sensors:"+packetcode);
        switch (packetcode) {
        case 0: readRequestLength = 26; break;
        case 1: readRequestLength = 10; break;
        case 2: readRequestLength = 6; break;
        case 3: readRequestLength = 10; break;
        case 4: readRequestLength = 14; break;
        case 5: readRequestLength = 12; break;
        case 6: readRequestLength = 52; break;
        case 100: readRequestLength = 80; break;
        case 101: readRequestLength = 28; break;
        case 106: readRequestLength = 12; break;
        case 107: readRequestLength = 9; break;
        case 19:
        case 20:
        case 22:
        case 23:
        case 25:
        case 26:
        case 27:
        case 28:
        case 29:
        case 30:
        case 39:
        case 40:
        case 41:
        case 42:
        case 43:
        case 44:
        case 46:
        case 47:
        case 48:
        case 49:
        case 50:
        case 51:
        case 54:
        case 55:
        case 56:
        case 57: readRequestLength = 2; break;
        default: readRequestLength = 1; break;
        }
        
        byte cmd[] = { (byte)SENSORS, (byte)packetcode};
        send(cmd);
    }
    
    /** 
     * get all sensor data
     */
    public void sensors() {
    	readRequestLength = 26;
        sensors( SENSORS_ALL );
    }

    /**
     * Query a list of sensors. This is a roomba 5xx only command.
     * @param sensorList A byte array containing the sensor groups requested to be read
     * @param returnLen The number of bytes of data expected to be returned from roomba
     */
    public void queryList(byte[] sensorList, int returnLen)
    {
    	int i = 0;
    	
    	readRequestLength = returnLen;
    	byte cmd[] = new byte[1+sensorList.length];
    	cmd[i++] = (byte) QUERYLIST;
    	for (i=0; i<sensorList.length; i++)
    		cmd[i+1] = sensorList[i];
    	send(cmd);
    }

    //
    // basic functions
    //

    /** 
     * Alias to pause
     * @see #pause(int)
     */
    public void delay( int millis ) {  pause( millis );  }

    /** 
     * Just a simple pause function. 
     * Makes the thread block with Thread.sleep()
     * @param millis number of milliseconds to wait
     */
    public void pause( int millis ) {
        try { Thread.sleep(millis); } catch(Exception e) { }
    }


    //
    // higher-level functions
    //

    /**
     * Stop Rooomba's motion.
     * Sends drive(0,0)
     */
    public void stop() {
        logmsg("stop");
        drive( 0, 0 );
    }

    /** Set speed for movement commands */
    public void setSpeed( int s ) { speed = Math.abs(s); }
    /** Get speed for movement commands */
    public int  getSpeed() { return speed; }

    /**
     * Go straight at the current speed for a specified distance.
     * Positive distance moves forward, negative distance moves backward.
     * This method blocks until the action is finished.
     * @param distance distance in millimeters, positive or negative
     */
    public void goStraight( int distance ) {
        float pausetime = Math.abs(distance / speed);  // mm/(mm/sec) = sec
        if (distance > 0)
        	goStraightAt( speed );
        else
        	goStraightAt( -speed);
        pause( (int)(pausetime*1000) );
        stop();
    }

    /**
     * @param distance distance in millimeters, positive 
     */
    public void goForward( int distance ) {
        if( distance < 0 ) return;
        goStraight( distance );
    }

    /**
     * @param distance distance in millimeters, positive 
     */
    public void goBackward( int distance ) {
        if( distance < 0 ) return;
        goStraight( -distance );
    }

    /**
     *
     */
    public void turnLeft() {
        turn(129);
    }
    public void turnRight() {
        turn(-129);
    }
    public void turn( int radius ) {
        drive( speed, radius );
    }

    /**
     * Spin right or spin left a particular number of degrees
     * @param angle angle in degrees, 
     *              positive to spin left, negative to spin right
     */
    public void spin( int angle ) {
        if( angle > 0 )       spinLeft( angle );
        else if( angle < 0 )  spinRight( -angle );
    }

    /**
     * Spin right the current speed for a specified angle 
     * @param angle angle in degrees, positive
     */
    public void spinRight( int angle ) {
        if( angle < 0 ) return;
        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
        spinRightAt( Math.abs(speed) );
        pause( (int)(pausetime*1000) );
        stop();
    }

    /**
     * Spin left a specified angle at a specified speed
     * @param angle angle in degrees, positive
     */
    public void spinLeft( int angle ) {
        if( angle<0 ) return;
        //float pausetime = 
        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
        spinLeftAt( Math.abs(speed) );
        pause( (int)(pausetime*1000) );
        stop();
    }

    /** 
     * Spin in place anti-clockwise, at the current speed
     */
    public void spinLeft() {
        spinLeftAt( speed );  
    }
    /** 
     * Spin in place clockwise, at the current speed
     */
    public void spinRight() {
        spinRightAt( speed );  
    }

    /**
     * Spin in place anti-clockwise, at the current speed.
     * @param aspeed speed to spin at
     */
    public void spinLeftAt(int aspeed) {
        drive( aspeed, 1 ); 
    }

    /**
     * Spin in place clockwise, at the current speed.
     * @param aspeed speed to spin at, positive
     */
    public void spinRightAt(int aspeed) {
        drive( aspeed, -1 ); 
    }

    //
    // mid-level movement, no blocking, parameterized by speed, not distance
    //

    /** 
     * Go straight at a specified speed.  
     * Positive is forward, negative is backward
     * @param velocity velocity of motion in mm/sec
     */
    public void goStraightAt( int velocity ) {
        //System.out.println("goStraightAt: velocity:"+velocity);
        if( velocity > 500 ) velocity = 500;
        if( velocity < -500 ) velocity = -500;
        drive( velocity, 0x8000 );
    }

    /**
     * Go forward the current (positive) speed
     */
    public void goForward() {
        goStraightAt( Math.abs(speed) );
    }

    /**
     * Go backward at the current (negative) speed
     */
    public void goBackward() {
        goStraightAt( - Math.abs(speed) );
    }

    /**
     * Go forward at a specified speed
     */
    public void goForwardAt( int aspeed ) {
        if( aspeed < 0 ) return;
        goStraightAt( aspeed );
    }

    /**
     * Go backward at a specified speed
     */
    public void goBackwardAt( int aspeed ) {
        if( aspeed < 0 ) return;
        goStraightAt( -aspeed );
    }


    //
    // low-level movement and action
    //

    /**
     * Move the Roomba via the low-level velocity + radius method.
     * See the 'Drive' section of the Roomba ROI spec for more details.
     * Low-level command.
     * @param velocity  speed in millimeters/second, 
     *                  positive forward, negative backward
     * @param radius    radius of turn in millimeters
     */
    public void drive( int velocity, int radius ) {
        byte cmd[] = { (byte)DRIVE,(byte)(velocity>>>8),(byte)(velocity&0xff), 
                       (byte)(radius >>> 8), (byte)(radius & 0xff) };
        logmsg("drive: "+hex(cmd[0])+","+hex(cmd[1])+","+hex(cmd[2])+","+
               hex(cmd[3])+","+hex(cmd[4]));
        send( cmd );
    }

    /**
     * Play a musical note 
     * Does it via the hacky method of defining a one-note song & playing it
     * Uses up song slot 15.
     * If another note is played before one is finished, the new note cuts off
     * the old one.
     * @param note     a note number from 31 (G0) to 127 (G8)
     * @param duration duration of note in 1/64ths of a second
     */
    public void playNote( int note, int duration ) {
        logmsg("playnote: "+note+":"+duration);
        byte cmd[] = {
            (byte)SONG, 3, 1, (byte)note, (byte)duration,  // define song
            (byte)PLAY, 3 };                               // play it back
        send( cmd );
    }

    public void playSong( int songnum ) {
        byte cmd[] = { (byte)PLAY, (byte)songnum };
        send(cmd);
    }

    /**
     * Make a song
     * @param songnum number of song to define
     * @param song  array of songnotes, 
     *              even entries are notenums, odd are duration of 1/6ths
     */
    public void createSong( int songnum, int song[] ) {
        int len = song.length;
        int songlen = len/2;
        logmsg("createSong: songnum:"+songnum+", songlen:"+songlen);
        byte cmd[] = new byte[len+3]; 
        cmd[0] = (byte) SONG;
        cmd[1] = (byte) songnum;
        cmd[2] = (byte) songlen;
        for( int i=0; i < len; i++ ) {
            cmd[3+i] = (byte)song[i];
        }
        send(cmd);
    }
    /**
     * Make a song
     * @param songnum number of song to define
     * @param song  array of Notes
     */
    public void createSong( int songnum, Note song[] ) {
        int songlen = song.length;
        logmsg("createSong: songnum:"+songnum+", songlen:"+songlen);
        byte cmd[] = new byte[songlen+3]; 
        cmd[0] = (byte) SONG;
        cmd[1] = (byte) songnum;
        cmd[2] = (byte) songlen;
        int j=3;
        for( int i=0; i < songlen; i++ ) {
            cmd[j++] = (byte)song[i].notenum;
            cmd[j++] = (byte)song[i].toSec64ths();
        }
        send(cmd);
    }



    /**
     * Turns on/off the non-drive motors (main brush, vacuum, sidebrush).
     * Sort of low-level.
     * @param mainbrush  mainbrush motor on/off state
     * @param vacuum     vacuum motor on/off state
     * @param sidebrush  sidebrush motor on/off state
     */
    public void setMotors(boolean mainbrush,boolean vacuum,boolean sidebrush) {
        byte cmd[] = { 
            (byte)MOTORS, 
            (byte)((mainbrush?0x04:0) | (vacuum?0x02:0) | (sidebrush?0x01:0))};
        send( cmd );
    }

    /** 
     * Turns on/off the various LEDs.
     * Low-level command.
     * FIXME: this is too complex
     */
    public void setLEDs( boolean status_green, boolean status_red, 
                         boolean spot,boolean clean,boolean max,boolean dirt, 
                         int power_color, int power_intensity ) {
        int v = (status_green?0x20:0) | (status_red?0x10:0) | 
            (spot?0x08:0) | (clean?0x04:0) | (max?0x02:0) | (dirt?0x01:0);
        logmsg("setLEDS: "+binary(v));
        byte cmd[] = { (byte)LEDS, (byte)v,
                       (byte)power_color, (byte)power_intensity };
        send(cmd);
    }


    /**
     * Turn all vacuum motors on or off according to state
     * @param state true to turn on vacuum function, false to turn it off
     */
    public void vacuum(boolean state) {
        logmsg("vacuum: "+state);
        setMotors(state,state,state);
    }


    //
    // sensor functions
    //


    /**
     * Compute possible safety fault.
     * Called on every successful updateSensors().
     * In normal use, call updateSensors() then check safetyFault().
     * @return  true if indicates we had an event that took the Roomba out of
     *          safe mode
     * @see #updateSensors()
     */
    public boolean computeSafetyFault() {
        safetyFault = (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROP_MASK) != 0 ||
            sensor_bytes[CLIFFLEFT]==1  || sensor_bytes[CLIFFFRONTLEFT]==1 ||
            sensor_bytes[CLIFFRIGHT]==1 || sensor_bytes[CLIFFFRONTRIGHT]==1;

        if( safetyFault && (mode == MODE_SAFE) ) mode = MODE_PASSIVE;

        return safetyFault;
    }

    /** 
     * Returns current connected state.  
     * It's up to subclasses to ensure this variable is correct.
     * @return current connected state
     */
    public boolean connected() { return connected; }

    /** current ROI mode RoombaComm thinks the Roomba is in */
    public int mode() { return mode; }
    /** mode as String */
    public String modeAsString() {
        String s=null;
        switch(mode) {
        case MODE_UNKNOWN: s = "unknown"; break;
        case MODE_PASSIVE: s = "passive"; break;
        case MODE_SAFE:    s = "safe"; break;
        case MODE_FULL:    s = "full"; break;
        }
        return s;
    }

    /** */
    public boolean sensorsAutoUpdate() { return sensorsAutoUpdate; }
    /** */
    public void setSensorsAutoUpdate(boolean b) { sensorsAutoUpdate=b; }
    /** */
    public int sensorsUpdateTime() { return sensorsUpdateTime; }
    /** */
    public void setSensorsUpdateTime(int i) { sensorsUpdateTime=i; }

    /**
     *
     */
    public boolean safetyFault() { return safetyFault; } 

    /**
     * 
     */
    public boolean sensorsValid() {
        // FIXME: 
        if( sensorsValid ) {  // may be valid but stale
            long difftime = System.currentTimeMillis() - sensorsLastUpdateTime;
            if( difftime > 2*sensorsUpdateTime ) { // give it some space
                return false;
            }
            else return true;
        }
        return false;
    }

    /** 
     * @return all sensor data as a string
     */
    public String sensorsAsString() {
	String sd="";
	if( debug ) {
	    sd = "\n";
	    for( int i=0; i<26; i++ )
		sd += " "+hex(sensor_bytes[i]);
	}
        return
            "bump:" + 
            (bumpLeft()?"l":"_") + 
            (bumpRight()?"r":"_") +
            " wheel:" +
            (wheelDropLeft()  ?"l":"_") +
            (wheelDropCenter()?"c":"_") +
            (wheelDropLeft()  ?"r":"_") +
            " wall:" + (wall() ?"Y":"n") + 
            " cliff:" +
            (cliffLeft()       ?"l":"_") +
            (cliffFrontLeft()  ?"L":"_") +  
            (cliffFrontRight() ?"R":"_") +
            (cliffRight()      ?"r":"_") +
            " dirtL:"+dirtLeft()+
            " dirtR:"+dirtRight()+
            " vwal:" + virtual_wall() +
            " motr:" + motor_overcurrents() + 
            " dirt:" + dirt_left() + "," + dirt_right() +
            " remo:" + hex(remote_opcode()) +
            " butt:" + hex(buttons()) +
            " dist:" + distance() + 
            " angl:" + angle() +
            " chst:" + charging_state() + 
            " volt:" + voltage() +
            " curr:" + current() +
            " temp:" + temperature() +
            " chrg:" + charge() +
            " capa:" + capacity() +
            sd;
    }

    /** Did we bump into anything */
    public boolean bump() {
        return (sensor_bytes[BUMPSWHEELDROPS] & BUMP_MASK) !=0;
    }
    /** Left bump sensor */
    public boolean bumpLeft() {
        return (sensor_bytes[BUMPSWHEELDROPS] & BUMPLEFT_MASK) !=0;
    }
    /** Right bump sensor */
    public boolean bumpRight() {
        return (sensor_bytes[BUMPSWHEELDROPS] & BUMPRIGHT_MASK) !=0;
    }
    /** Left wheeldrop sensor */
    public boolean wheelDropLeft() {
        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPLEFT_MASK) !=0;
    }
    /** Right wheeldrop sensor */
    public boolean wheelDropRight() {
        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPRIGHT_MASK) !=0;
    }
    /** Center wheeldrop sensor */
    public boolean wheelDropCenter() {
        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPCENT_MASK) !=0;
    }
    /** Can we see a wall? */
    public boolean wall() {
        return sensor_bytes[WALL] != 0;
    }

    /**
     * @return true if dirt present
     */
    public boolean dirt() {
        int dl = sensor_bytes[DIRTLEFT] & 0xff;
        int dr = sensor_bytes[DIRTRIGHT] & 0xff;
        //if(debug) println("Roomba:dirt: dl,dr="+dl+","+dr);
        return (dl > 100) || (dr > 100);
    }
    /**
     * amount of dirt seen by left dirt sensor 
     */
    public int dirtLeft() {
        return dirt_left();  // yeah yeah
    }
    /**
     * amount of dirt seen by right dirt sensor 
     */
    public int dirtRight() {
        return dirt_right();
    }

    /** left cliff sensor */
    public boolean cliffLeft() {
        return (sensor_bytes[CLIFFLEFT] != 0);
    }  
    /** front left cliff sensor */
    public boolean cliffFrontLeft() {
        return (sensor_bytes[CLIFFFRONTLEFT] != 0);
    }  
    /** front right cliff sensor */
    public boolean cliffFrontRight() {
        return (sensor_bytes[CLIFFFRONTRIGHT] != 0);
    }  
    /** right cliff sensor */
    public boolean cliffRight() {
        return sensor_bytes[CLIFFRIGHT] != 0;
    }
  
    /** overcurrent on left drive wheel */
    public boolean motorOvercurrentDriveLeft() {
        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERDRIVELEFT_MASK) != 0;
    }
    /** overcurrent on right drive wheel */
    public boolean motorOvercurrentDriveRight() {
        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERDRIVERIGHT_MASK) != 0;
    }
    /** overcurrent on main brush */
    public boolean motorOvercurrentMainBrush() {
        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERMAINBRUSH_MASK) != 0;
    }
    /** overcurrent on vacuum */
    public boolean motorOvercurrentVacuum() {
        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERVACUUM_MASK) != 0;
    }
    /** overcurrent on side brush */
    public boolean motorOvercurrentSideBrush() {
        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERSIDEBRUSH_MASK) !=0;
    }

    /** 'Power' button pressed state */
    public boolean powerButton() {
        return (sensor_bytes[BUTTONS] & POWERBUTTON_MASK) != 0;
    }
    /** 'Spot' button pressed state */
    public boolean spotButton() {
        return (sensor_bytes[BUTTONS] & SPOTBUTTON_MASK) != 0;
    }
    /** 'Clean' button pressed state */
    public boolean cleanButton() {
        return (sensor_bytes[BUTTONS] & CLEANBUTTON_MASK) != 0;
    }
    /** 'Max' button pressed state */
    public boolean maxButton() {
        return (sensor_bytes[BUTTONS] & MAXBUTTON_MASK) != 0;
    }


    //
    // lower-level sensor access
    //
    /** lower-level func, returns raw byte */
    public int bumps_wheeldrops() {
        return sensor_bytes[BUMPSWHEELDROPS];
    }
    /** lower-level func, returns raw byte */
    public int cliff_left() {
        return sensor_bytes[CLIFFLEFT];
    }
    /** lower-level func, returns raw byte */
    public int cliff_frontleft() {
        return sensor_bytes[CLIFFFRONTLEFT];
    }
    /** lower-level func, returns raw byte */
    public int cliff_frontright() {
        return sensor_bytes[CLIFFFRONTRIGHT];
    }
    /** lower-level func, returns raw byte */
    public int cliff_right() {
        return sensor_bytes[CLIFFRIGHT];
    }
    /** lower-level func, returns raw byte */
    public int virtual_wall() {
        return sensor_bytes[VIRTUALWALL];
    }
    /** lower-level func, returns raw byte */
    public int motor_overcurrents() {
        return sensor_bytes[MOTOROVERCURRENTS];
    }
    /**  */
    public int dirt_left() {
        return sensor_bytes[DIRTLEFT] & 0xff;
    }
    /** */
    public int dirt_right() {
        return sensor_bytes[DIRTRIGHT] & 0xff;
    }
    /** lower-level func, returns raw byte */
    public int remote_opcode() {
        return sensor_bytes[REMOTEOPCODE];
    }
    /** lower-level func, returns raw byte */
    public int buttons() {
        return sensor_bytes[BUTTONS];
    }

    /** 
     * Distance traveled since last requested
     * units: mm
     * range: -32768 - 32767
     */
    public short distance() {
        return toShort(sensor_bytes[DISTANCE_HI],
                       sensor_bytes[DISTANCE_LO]);
    }
    /** 
     * Angle traveled since last requested
     * units: mm, diff in distance traveled by two drive wheels
     * range: -32768 - 32767
     */
    public short angle() {
        return toShort(sensor_bytes[ANGLE_HI],
                       sensor_bytes[ANGLE_LO]);
    }  
    /**
     * angle since last read, but in degrees
     */
    // FIXME I think this should be (360 * angle())/(258 * PI)
    public float angleInDegrees() {
        return (float) angle() / millimetersPerDegree;
    }
    /**
     * angle since last read, but in radians
     */
 // FIXME I think this should be (2 * angle())/258
    public float angleInRadians() {
        return (float) angle() / millimetersPerRadian;
    }

    /** 
     * Charging state
     * units: enumeration
     * range: 
     */
    public int charging_state() {
        return sensor_bytes[CHARGINGSTATE] & 0xff;
    }
    /** 
     * Voltage of battery
     * units: mV
     * range: 0 - 65535
     */
    public int voltage() {
        return toUnsignedShort(sensor_bytes[VOLTAGE_HI],
                               sensor_bytes[VOLTAGE_LO]);
    } 
    /**
     * Current flowing in or out of battery
     * units: mA
     * range: -332768 - 32767
     */
    public short current() {
        return toShort(sensor_bytes[CURRENT_HI],
                       sensor_bytes[CURRENT_LO]);
    }
    /**
     * temperature of battery
     * units: degrees Celcius
     * range: -128 - 127
     */
    public byte temperature() {
        return sensor_bytes[TEMPERATURE];
    }
    /**
     * Current charge of battery
     * units: mAh 
     * range: 0-65535
     */
    public int charge() {
        return toUnsignedShort(sensor_bytes[CHARGE_HI],
                               sensor_bytes[CHARGE_LO]);
    }
    /**
     * Estimated charge capacity of battery
     * units: mAh
     * range: 0-65535
     */
    public int capacity() {
        return toUnsignedShort(sensor_bytes[CAPACITY_HI],
                               sensor_bytes[CAPACITY_LO]);
    }

    // possible modes
    public static final int MODE_UNKNOWN = 0;
    public static final int MODE_PASSIVE = 1;
    public static final int MODE_SAFE    = 2;
    public static final int MODE_FULL    = 3;

    // Roomba ROI opcodes
    // these should all be bytes, but Java bytes are signed, sucka
    public static final int START   =  128;  // 0
    public static final int BAUD    =  129;  // 1
    public static final int CONTROL =  130;  // 0
    public static final int SAFE    =  131;  // 0
    public static final int FULL    =  132;  // 0
    public static final int POWER   =  133;  // 0
    public static final int SPOT    =  134;  // 0
    public static final int CLEAN   =  135;  // 0
    public static final int MAX     =  136;  // 0
    public static final int DRIVE   =  137;  // 4
    public static final int MOTORS  =  138;  // 1
    public static final int LEDS    =  139;  // 3
    public static final int SONG    =  140;  // 2N+2
    public static final int PLAY    =  141;  // 1
    public static final int SENSORS =  142;  // 1
    public static final int DOCK    =  143;  // 0
    public static final int PWMMOTORS = 144; // 3
    public static final int DRIVEWHEELS = 145; 	// 4
    public static final int DRIVEPWM = 146;  // 4
    public static final int STREAM  =  148;  // N+1
    public static final int QUERYLIST = 149; // N+1
    public static final int STOPSTARTSTREAM = 150;  // 1
    public static final int SCHEDULINGLEDS = 162; 	// 2
    public static final int DIGITLEDSRAW = 163; 	// 4
    public static final int DIGITLEDSASCII = 164;	// 4
    public static final int BUTTONSCMD  =  165; // 1
    public static final int SCHEDULE =  167;  // n
    public static final int SETDAYTIME = 168; // 3

    // offsets into sensor_bytes data
    public static final int BUMPSWHEELDROPS     = 0;
    public static final int WALL                = 1;
    public static final int CLIFFLEFT           = 2;
    public static final int CLIFFFRONTLEFT      = 3;
    public static final int CLIFFFRONTRIGHT     = 4;
    public static final int CLIFFRIGHT          = 5;
    public static final int VIRTUALWALL         = 6;
    public static final int MOTOROVERCURRENTS   = 7;
    public static final int DIRTLEFT            = 8;
    public static final int DIRTRIGHT           = 9;
    public static final int REMOTEOPCODE        = 10;
    public static final int BUTTONS             = 11;
    public static final int DISTANCE_HI         = 12;
    public static final int DISTANCE_LO         = 13;  
    public static final int ANGLE_HI            = 14;
    public static final int ANGLE_LO            = 15;
    public static final int CHARGINGSTATE       = 16;
    public static final int VOLTAGE_HI          = 17;
    public static final int VOLTAGE_LO          = 18;  
    public static final int CURRENT_HI          = 19;
    public static final int CURRENT_LO          = 20;
    public static final int TEMPERATURE         = 21;
    public static final int CHARGE_HI           = 22;
    public static final int CHARGE_LO           = 23;
    public static final int CAPACITY_HI         = 24;
    public static final int CAPACITY_LO         = 25;

    // bitmasks for various thingems
    public static final int WHEELDROP_MASK      = 0x1C;
    public static final int BUMP_MASK           = 0x03;
    public static final int BUMPRIGHT_MASK      = 0x01;
    public static final int BUMPLEFT_MASK       = 0x02;
    public static final int WHEELDROPRIGHT_MASK = 0x04;
    public static final int WHEELDROPLEFT_MASK  = 0x08;
    public static final int WHEELDROPCENT_MASK  = 0x10;

    public static final int MOVERDRIVELEFT_MASK = 0x10;
    public static final int MOVERDRIVERIGHT_MASK= 0x08;
    public static final int MOVERMAINBRUSH_MASK = 0x04;
    public static final int MOVERVACUUM_MASK    = 0x02;
    public static final int MOVERSIDEBRUSH_MASK = 0x01;

    public static final int POWERBUTTON_MASK    = 0x08;  
    public static final int SPOTBUTTON_MASK     = 0x04;  
    public static final int CLEANBUTTON_MASK    = 0x02;  
    public static final int MAXBUTTON_MASK      = 0x01;  

    // which sensor packet, argument for sensors(int)
    public static final int SENSORS_ALL         = 0;
    public static final int SENSORS_PHYSICAL    = 1;
    public static final int SENSORS_INTERNAL    = 2;
    public static final int SENSORS_POWER       = 3;

    public static final int REMOTE_NONE         = 0xff;
    public static final int REMOTE_POWER        = 0x8a;
    public static final int REMOTE_PAUSE        = 0x89;
    public static final int REMOTE_CLEAN        = 0x88;
    public static final int REMOTE_MAX          = 0x85;
    public static final int REMOTE_SPOT         = 0x84;
    public static final int REMOTE_SPINLEFT     = 0x83;
    public static final int REMOTE_FORWARD      = 0x82;
    public static final int REMOTE_SPINRIGHT    = 0x81;

        /*
no button = -1
power = -118 8a 
pause = -119 89
clean = -120 88
max = -123 85
spot = -124 84
spinleft = -125 81  (8d keyup?)
forward = -126  82   (8c?)
spinright = -127 83
        */

    //
    // utility methods
    //

    /**
     *
     */
    static public final short toShort(byte hi, byte lo) {
        return (short)((hi << 8) | (lo & 0xff));
    }
    /**
     *
     */
    static public final int toUnsignedShort(byte hi, byte lo) {
        return (int)(hi & 0xff) << 8 | lo & 0xff;
    }
    
    public void println(String s) {
        System.out.println(s);
    }
    
    public String hex(byte b) {
        return Integer.toHexString(b&0xff);
    }

    public String hex(int i) {
        return Integer.toHexString(i);
    }
    
    
    public String binary(int i) {
        return Integer.toBinaryString(i);
    }

    /**
     * just a little debug 
     */
    public void logmsg(String msg) {
        if(debug) 
            System.err.println("RoombaComm ("+System.currentTimeMillis()+"):"+
                               msg);
    }

    /**
     * General error reporting, all corraled here just in case
     * I think of something slightly more intelligent to do.
     */
    public void errorMessage(String where, Throwable e) {
        e.printStackTrace();
        throw new RuntimeException("Error inside Serial." + where + "()");
    }

}