// RoombaCtrl
// by Tod E. Kurt, tod@todbot.com
// http://roombahacking.com/
//
//
//

// This needs my patched bluetooth library that allows short UUIDs
import processing.bluetooth.*;

final String SOFTKEY_RECORD = "Record";
final String SOFTKEY_STOP   = "Stop";
final String SOFTKEY_PLAY   = "Play";

// state machine sates
final int STATE_START    = 0;
final int STATE_FIND     = 1;
final int STATE_CONFIG   = 2;
final int STATE_DRIVE    = 3;
final int STATE_PLAYBACK = 4;

// state machine state var
int state;

// bluetooth library
Bluetooth bt;
// discovered services
Service[] services;
// status message
String msg;
// connection to Roomba
Client c;

PFont font;

int r_speed = 250;
boolean r_forward = false;
boolean r_vacuum = false;

int[] macro_moves;
int[] macro_times;
int macro_recp;
int macro_playp;
int macro_rectime;
int macro_playtime;
boolean macro_rec = false;

///
void setup() {
  //  framerate(4);
  font = loadFont();
  textFont(font);
  
  bt = new Bluetooth(this, Bluetooth.UUID_SERIALPORT);

  ellipseMode(CORNER);
  macro_moves = new int[100];
  macro_times = new int[100];
  state = STATE_START;
}

void destroy() {
  bt.stop();
}

//
void draw() {
  background(0xcc,0xcc,0xff);

  if (state == STATE_START) {
    fill(0);
    textAlign(LEFT);
    text("Welcome to RoombaCtrl\n\nPress a key to search for a Roomba", 2, 2, width - 4, height - 4);
  } 
  else if (state == STATE_FIND) {
    fill(0);
    textAlign(LEFT);
    if (services == null) {
      text("Looking for Roombas...\n\n" + msg, 2, 2, width-4, height-4);
    }
    else {
      String list = "Ports found:\n";
      for (int i = 0, len = length(services); i < len; i++)
        list += i + ". " + services[i].device.name + "\n";
      text(list, 2, 2, width-4, height-4);
    }
  } 
  else if (state == STATE_DRIVE) {
    noFill();
    text("RoombaCtrl:\n"+
         "Use joystick to drive\n"+
         "push to stop\n"+
         "1,2,3 adjusts speed\n"+
         "* toggles vacuum\n"+
         "0 resets Roomba\n"+
         ((macro_rec) ? "recording event #"+macro_recp+"\n":"")+
         msg, 
         2, 2, width-4, height-4);
  }
  else if (state == STATE_PLAYBACK) {
    // playback when next time delta is < current time - starttime 
    if( millis() - macro_playtime >= macro_times[macro_playp] ) {
      parse_roombakey( macro_moves[macro_playp], 0 );
      macro_playp++;
      if( macro_playp > macro_recp ) {
        state = STATE_DRIVE;
        softkey(SOFTKEY_PLAY);
      }
    } 
    noFill();
    text("RoombaCtrl:\n"+
         "macro playback\n"+
         "event #"+macro_playp+"\n"+
         msg,
         2,2, width-4,height-4);
  }
}

//
void libraryEvent(Object library, int event, Object data) {
  if (library == bt) {
    switch (event) {
    case Bluetooth.EVENT_DISCOVER_DEVICE:
      msg = "Found device at: " + ((Device) data).address + "...";
      break;
    case Bluetooth.EVENT_DISCOVER_DEVICE_COMPLETED:
      msg = "Found " + length((Device[]) data) + " devices, looking for serial port...";
      break;
    case Bluetooth.EVENT_DISCOVER_SERVICE:      
      msg = "Found serial port on " + ((Service[]) data)[0].device.address + "...";
      break;
    case Bluetooth.EVENT_DISCOVER_SERVICE_COMPLETED:
      services = (Service[]) data;
      msg = "Search complete. Pick one.";
      break;
    case Bluetooth.EVENT_CLIENT_CONNECTED:
      c = (Client) data;
      msg = "wtf. client connectd."; 
      break;
    }
  }
}

//
void softkeyPressed(String label) {
  if (label.equals(SOFTKEY_RECORD)) {
    macro_recp = 0;
    macro_rec = true;
    //macro_rectime = millis();
    softkey(SOFTKEY_STOP);
  }
  else if(label.equals(SOFTKEY_STOP)) {
    macro_rec = false;
    state = STATE_DRIVE;
    softkey(SOFTKEY_PLAY);
  }
  else if(label.equals(SOFTKEY_PLAY)) {
    macro_playp = 0;
    macro_playtime = millis();
    state = STATE_PLAYBACK;
    softkey(SOFTKEY_STOP);
  }
}

//
void keyPressed() {
  if (state == STATE_START) {
    services = null;
    bt.find();
    state = STATE_FIND;
    msg = "Looking for devices...";
  } 
  else if (state == STATE_FIND) {
    if (services != null) {
      if ((key >= '0') && (key <= '9')) {
        int i = key - '0';
        if (i < length(services)) {
          msg = "connecting...";
          c = services[i].connect();
          roomba_reset();
          state = STATE_DRIVE;
          softkey(SOFTKEY_RECORD);
          msg = "ready";
        }
      }
    }
  }
  else if (state == STATE_DRIVE) {
    if( macro_rec ) {
      if( macro_recp == 0 ) macro_rectime = millis();
      macro_moves[ macro_recp ] = keyCode;
      macro_times[ macro_recp ] = millis() - macro_rectime;
      macro_recp++;
      if( macro_recp==100 ) {
        macro_rec = false;
        softkey(SOFTKEY_PLAY);
      }
    }
    
    parse_roombakey(keyCode,key);
    
  }
  else if( state == STATE_PLAYBACK ) {

  }
}


void parse_roombakey(int akeyCode, int akey) {
  switch(akeyCode) {
    case UP:
      msg = "forward";
      roomba_drive( r_speed, 0x8000 );
      r_forward = true;
      break; 
    case DOWN:
      msg = "backward";
      roomba_drive( -r_speed, 0x8000 );
      r_forward = false;
      break;
    case LEFT:
      msg = "left";
      if( r_forward ) 
        roomba_drive( r_speed, 127 );
      else 
        roomba_drive( r_speed, 1 );
      break;
    case RIGHT:
      msg = "right";
      if( r_forward ) 
        roomba_drive( r_speed, -127 );
      else 
        roomba_drive( r_speed, -1 );
      break;
    case FIRE:
      msg = "stop";
      roomba_drive( 0, 0 );
      r_forward = false;
      break;
    default:
      switch(akey) {
      case '0':
        msg = "reset";
        roomba_reset();
        macro_recp = 0;
        softkey(SOFTKEY_RECORD);
        break;
      case '1':
        msg = "speed 50";
        r_speed = 50;
        break;
      case '2':
        msg = "speed 250";
        r_speed = 250;
        break;
      case '3':
        msg = "speed 500";
        r_speed = 500;
        break;
      case '*':
        msg = "vacuum "+((r_vacuum)?"on":"off");
        r_vacuum = !r_vacuum;
        roomba_vacuum( r_vacuum );
        break;
      } // switch(key)
    } // switch(keyCode)
}

// ------------------------------------------------------------------
// Roomba functions
// ------------------------------------------------------------------


// a simple sleep function
void pause(long millis) {
  try { Thread.sleep(millis);  }  catch( Exception e) { }
}  

// reset roomba into SAFE mode, ready for commands
void roomba_reset() {
  c.write( R_START );
  c.flush();
  pause(100);
  c.write( R_CONTROL );
  c.flush();
  pause(100);
  roomba_playnote( 72, 10 );
  pause( 200 );
  roomba_playnote( 79, 10 );
  pause( 200 );
  roomba_playnote( 76, 10 );
  pause( 200 );
}  

// drive the roomba at a velocity along a radius
void roomba_drive(int velocity, int radius) {
  byte cmd[] = { 
    (byte)R_DRIVE,
    (byte)(velocity>>>8), (byte)(velocity&0xff),
    (byte)(radius >>> 8), (byte)(radius & 0xff) };
  c.write(cmd);
  c.flush();
}

// play a midi note on the roomba for a duration (in units of 1/64ths of a sec)
void roomba_playnote(int note, int dur) {
  byte cmd[] = {  
    (byte)R_SONG, 15, 1, (byte)note, (byte)dur,  // define song      
    (byte)R_PLAY, 15 };                          // play it back
  c.write(cmd);
  c.flush();
}

// turn on/off the non-drive motors
void roomba_setMotors(boolean mainbrush, boolean vacuum, boolean sidebrush) {
  byte cmd[] = {
    (byte)R_MOTORS,
    (byte)((mainbrush?0x04:0) | (vacuum?0x02:0) | (sidebrush?0x01:0))};
    c.write(cmd);
    c.flush();
}

void roomba_vacuum(boolean state) {
  roomba_setMotors(state,state,state);
}

// Roomba ROI opcodes                                                       
// these should all be bytes, but Java bytes are signed, sucka              
public static final int R_START   =  128;  // 0                               
public static final int R_BAUD    =  129;  // 1                               
public static final int R_CONTROL =  130;  // 0                               
public static final int R_SAFE    =  131;  // 0                               
public static final int R_FULL    =  132;  // 0                               
public static final int R_POWER   =  133;  // 0                               
public static final int R_SPOT    =  134;  // 0                               
public static final int R_CLEAN   =  135;  // 0                               
public static final int R_MAX     =  136;  // 0                               
public static final int R_DRIVE   =  137;  // 4                               
public static final int R_MOTORS  =  138;  // 1                               
public static final int R_LEDS    =  139;  // 3                               
public static final int R_SONG    =  140;  // 2N+2                            
public static final int R_PLAY    =  141;  // 1                               
public static final int R_SENSORS =  142;  // 1                               
public static final int R_DOCK    =  143;  // 0