#use "xbcserial.ic"

/* This file contais a number of useful files for running an iRobot Roomba from an XBC.
   There is a separate but similar library for the Roomba.
   Note, an XBC v3 with a XBC-Roomba cable is needed, along with IC 7.08 or higher.
   Written by David Miller, 4/07
*/


#define twopi 6.28318531
//ROOMBA_BUSY keeps any  roomba commands in other processes from interfering with one another.
#define ROOMBA_BUSY while(g_roomba_busy) defer(); g_roomba_busy = 1;
#define ROOMBA_FREE g_roomba_busy = 0;

int g_roomba_busy = 0;
int g_roomba_connected = 0;

// this function turns on a roomba if it is off
void roomba_wake()
{
    set_digital_output_value(8,0);
    msleep(200L);
    set_digital_output_value(8,1);
    sleep(0.5);
}

// This pusts the roomba back to sleep
void roomba_sleep() {ROOMBA_BUSY; serial_write_byte(133); ROOMBA_FREE;}


// This routine establishes serial link
// Note that this routine should be called once, and must have been called before 
// any of the other roomba functions will work.
// Once this function is called, you cannot talk to the XBC over the USB connection
// until the roomba_disconnect function ahs been called or the GBA on the XBC has been powercycled.
// If Roomba is connected, power light goes yellow and play led turns on
void roomba_connect()
{
    roomba_wake();
    serial_set_mode(BAUD_57K);
    serial_set_extra(1);
    beep(); sleep(0.1);
    ROOMBA_BUSY;
    serial_write_byte(128);//send SCI start
    sleep(0.5);
    serial_write_byte(130);//send SCI control
    sleep(0.5);
    ROOMBA_FREE;
    roomba_clean_led(1);
    roomba_power_led(100,255);
    g_roomba_connected = 1;
    beep();
}

// Same as above but for serial XBCs (versions 1 & 2)
void roomba_connect_s()
{
    roomba_wake();
    serial_set_mode(BAUD_57K);
    beep(); sleep(0.1);
    ROOMBA_BUSY;
    serial_write_byte(128);//send SCI start
    sleep(0.5);
    serial_write_byte(130);//send SCI control
    sleep(0.5);
    ROOMBA_FREE;
    roomba_clean_led(1);
    roomba_power_led(100,255);
    g_roomba_connected = 1;
    beep();
}


// returns the serial connections on XBC to normal communications over the USB port.
// Turns of roomba
void roomba_disconnect() {
    roomba_clean_led(0);
    roomba_power_led(0,255);
    roomba_sleep();
    g_roomba_connected =0; serial_set_mode(0); 
    serial_set_extra(0);
}

// Same as above but for serial XBCs
void roomba_disconnect_s() {
    roomba_clean_led(0);
    roomba_power_led(0,255);
    roomba_sleep();
    g_roomba_connected =0; serial_set_mode(0); 
}



// See the roomba open interface document for more information on these functions
void roomba_safe() {ROOMBA_BUSY; s_write_byte(131); ROOMBA_FREE;}

void roomba_full() {ROOMBA_BUSY; s_write_byte(132); ROOMBA_FREE;}

void roomba_spot() {ROOMBA_BUSY; s_write_byte(134); ROOMBA_FREE;}

void roomba_clean() {ROOMBA_BUSY; s_write_byte(135); ROOMBA_FREE;}

void roomba_max() {ROOMBA_BUSY; s_write_byte(136); ROOMBA_FREE;}

void roomba_cover_dock() {ROOMBA_BUSY; s_write_byte(143); ROOMBA_FREE;}


//////////////Roomba Sensor Routines///////////////



//globals are updated by the functions below to give access to many key Roomba sensors
int gr_lbump, gr_rbump, gr_ldrop, gr_rdrop, gr_fdrop, gr_rcliff, gr_rfcliff, gr_lcliff, gr_lfcliff;
int gr_distance, gr_angle, gr_total_angle, gr_power_button, gr_spot_button,gr_clean_button,gr_max_button; 
int gr_wall, gr_v_wall, gr_motor_oc, gr_dirt_left, gr_dirt_right, gr_remote, gr_total_angle_diff;
int gr_charging_state, gr_voltage, gr_current, gr_temperature, gr_charge, gr_capacity;

//this function will update the values for all of the sensor globals above
void roomba_sensor_update()
{
    int b;
    ROOMBA_BUSY;
    s_write_byte(142);
    s_write_byte(0);//downloads all 26 bytes of sensor data
    while(serial_buffer_count()==0);
    b=s_read_byte();
    gr_fdrop=(b >> 4) & 0x1;
    gr_ldrop=(b >> 3) & 0x1;
    gr_rdrop=(b >> 2) & 0x1;
    gr_lbump=(b >> 1) & 0x1;
    gr_rbump=b & 0x1;
    while(serial_buffer_count()==0);
    gr_wall=s_read_byte();
    while(serial_buffer_count()==0);
    gr_lcliff=s_read_byte();
    while(serial_buffer_count()==0);
    gr_lfcliff=s_read_byte();
    while(serial_buffer_count()==0);
    gr_rfcliff=s_read_byte();
    while(serial_buffer_count()==0);
    gr_rcliff=s_read_byte();
    while(serial_buffer_count()==0);
    gr_v_wall=s_read_byte();
    while(serial_buffer_count()==0);
    gr_motor_oc=s_read_byte();
    while(serial_buffer_count()==0);
    gr_dirt_left=s_read_byte();
    while(serial_buffer_count()==0);
    gr_dirt_right=s_read_byte();
    while(serial_buffer_count()==0);
    gr_remote=s_read_byte();
    while(serial_buffer_count()==0);
    b=s_read_byte();
    gr_max_button=b & 0x1;
    gr_clean_button=(b >> 1) & 0x1;
    gr_spot_button=(b >> 2) & 0x1;
    gr_power_button=(b >> 3) & 0x1;
    while(serial_buffer_count()<2);
    gr_distance=gr_distance+(256*s_read_byte() + s_read_byte());
    while(serial_buffer_count()<2);
    gr_total_angle_diff=gr_total_angle_diff+(256*s_read_byte() + s_read_byte());
    while(serial_buffer_count()==0);
    gr_charging_state=s_read_byte();
    while(serial_buffer_count()<2);
    gr_voltage=(256*s_read_byte() + s_read_byte());
    while(serial_buffer_count()<2);
    gr_current=(256*s_read_byte() + s_read_byte());
    while(serial_buffer_count()==0);
    gr_temperature=s_read_byte();
    while(serial_buffer_count()<2);
    gr_charge=(256*s_read_byte() + s_read_byte());
    while(serial_buffer_count()<2);
    gr_capacity=(256*s_read_byte() + s_read_byte());
    ROOMBA_FREE;
    roomba_update_angle_globals();
}



// this function updates gr_angle which stores a normalized angle between 0 and 359 degrees
// and the global gr_total_angle which is not normalized and can be larger than 360 and less than 0.
// CCW angles are positive and CW turns decrement the angle value.
void roomba_update_angle_globals() {
    gr_total_angle = (int)((360.0*(float)gr_total_angle_diff)/(3.14159*258.0));
    gr_angle=gr_total_angle % 360;
    if(gr_angle < 0) gr_angle = gr_angle + 360;
}




////////////////////ROOMBA MOVEMENT FUNCTIONS///////////////////////////////////////////////

// This command drives the robot along a curve with radius (in mm) radius; and at a speed (mm/sec) of speed
// a radius of 32767 will drive the robot straight
// a radius of 1 will spin the robot CCW
// a radius of -1 will spin the robot CW
// Negative radii will be right turns, positive radii left turns
void roomba_drive (int speed, int radius)
{
    ROOMBA_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(radius));
    serial_write_byte(get_low_byte(radius));
    ROOMBA_FREE;
}

// special version of command above drives robot at speed speed.  Negative speed will drive robot backwards
void roomba_drive_straight (int speed)
{
    ROOMBA_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(32767));
    serial_write_byte(get_low_byte(32767));
    ROOMBA_FREE;
}

// special version of command spins robot CW with the wheels turning at speed speed
void roomba_spin_CW (int speed)
{
    ROOMBA_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(-1));
    serial_write_byte(get_low_byte(-1));
    ROOMBA_FREE;
}

// special version of command spins robot CCW with the wheels turning at speed speed
void roomba_spin_CCW (int speed)
{
    ROOMBA_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(1));
    serial_write_byte(get_low_byte(1));
    ROOMBA_FREE;
}

// turns on and off the main brush, vacuum and side brush motors (1 on 0 off)
void roomba_cleaning_motors(int brush, int vac, int side)
{
    int motors = 0;
    motors=side + 2*vac + 4*brush;
    ROOMBA_BUSY;
    serial_write_byte(138);
    serial_write_byte(motors);
    ROOMBA_FREE;
}


/////////////////////////ROOMBA LEDs///////////////////

int gRoomba_leds[3]={0,0,0};

void roomba_spot_led(int on) 
{
    ROOMBA_BUSY;
    serial_write_byte(139);
    if(on){
        if(!(gRoomba_leds[0] & 0b00001000)) gRoomba_leds[0]=gRoomba_leds[0]+ 8;
    }
    else{
        if(gRoomba_leds[0] & 0b00001000) gRoomba_leds[0]=gRoomba_leds[0]- 8;
    }
    serial_write_byte(gRoomba_leds[0]);            
    serial_write_byte(gRoomba_leds[1]);            
    serial_write_byte(gRoomba_leds[2]);  
    ROOMBA_FREE;
}

void roomba_clean_led(int on) 
{
    ROOMBA_BUSY;
    serial_write_byte(139);
    if(on){
        if(!(gRoomba_leds[0] & 0b00000100)) gRoomba_leds[0]=gRoomba_leds[0]+ 4;
    }
    else{
        if(gRoomba_leds[0] & 0b00000100) gRoomba_leds[0]=gRoomba_leds[0]- 4;
    }
    serial_write_byte(gRoomba_leds[0]);            
    serial_write_byte(gRoomba_leds[1]);            
    serial_write_byte(gRoomba_leds[2]);           
    ROOMBA_FREE;
}

void roomba_max_led(int on) 
{
    ROOMBA_BUSY;
    serial_write_byte(139);
    if(on){
        if(!(gRoomba_leds[0] & 0b00000010)) gRoomba_leds[0]=gRoomba_leds[0]+ 2;
    }
    else{
        if(gRoomba_leds[0] & 0b00000010) gRoomba_leds[0]=gRoomba_leds[0]- 2;
    }
    serial_write_byte(gRoomba_leds[0]);            
    serial_write_byte(gRoomba_leds[1]);            
    serial_write_byte(gRoomba_leds[2]);           
    ROOMBA_FREE;
}

// takes state value between 0 and 3: 0=off, 1=red, 2= green, 3=amber
void roomba_status_led(int state) 
{
    if(state>=0 && state <4){
        gRoomba_leds[0]=(gRoomba_leds[0]& 0b00000111)+ 16*state;
        ROOMBA_BUSY;
        serial_write_byte(139);
        serial_write_byte(gRoomba_leds[0]);            
        serial_write_byte(gRoomba_leds[1]);            
        serial_write_byte(gRoomba_leds[2]);        
        ROOMBA_FREE;
    }
    else printf("illegal value given to roomba_status_led\n");
}

void roomba_dirt_led(int on) 
{
    ROOMBA_BUSY;
    serial_write_byte(139);
    if(on){
        if(!(gRoomba_leds[0] & 0b00000001)) gRoomba_leds[0]=gRoomba_leds[0]+ 1;
    }
    else{
        if(gRoomba_leds[0] & 0b00000001) gRoomba_leds[0]=gRoomba_leds[0]- 1;
    }
    serial_write_byte(gRoomba_leds[0]);            
    serial_write_byte(gRoomba_leds[1]);            
    serial_write_byte(gRoomba_leds[2]);        
    ROOMBA_FREE;
}


//sets roomba power led. Color =0 is green and color = 255 is red -- with intermediate colors
//brightness of 0 is off and 255 is fully bright
void roomba_power_led(int color, int brightness) 
{
    ROOMBA_BUSY;
    serial_write_byte(139);
    gRoomba_leds[1]=color;
    gRoomba_leds[2]=brightness;
    serial_write_byte(gRoomba_leds[0]);            
    serial_write_byte(gRoomba_leds[1]);            
    serial_write_byte(gRoomba_leds[2]);      
    ROOMBA_FREE;
}

/////////// ROMBA MUSIC////////////
int gr_song_array[16][33];

// this loads a song into the robot's memory. 
// song can be numbered 0 to 15. the first element in each row of the array should be
// the number of notes (1-16) the subsequent pairs of bytes should be tone and duration
// see the roomba SCI manual for note codes.
// user's program should load the song data into the array before calling this routine
void roomba_load_song(int num)
{
    int i,numnotes;
    numnotes=gr_song_array[num][0];
    if(num >= 0 && num <=15 && numnotes > 0 && numnotes <= 16){
        ROOMBA_BUSY;
        serial_write_byte(140);
        serial_write_byte(num);
        serial_write_byte(numnotes);
        for(i=1; i< 2*numnotes+1; i++)
          serial_write_byte(gr_song_array[num][i]);
        ROOMBA_FREE;
    }
    else printf("illegal song being written to memory\n");
}

// see the roomba SCI manual for note codes.
void roomba_play_song(int num)
{
    int i;
    if(num >= 0 && num <=15 ){
        ROOMBA_BUSY;
        serial_write_byte(141);
        serial_write_byte(num);
        ROOMBA_FREE;
    }
    else printf("Song array reference is out of bounds\n");
}

/////////// The functions below are communication and byte packing utitilities
int get_high_byte(int v)
{
    return (v >>8);
}

int get_low_byte(int v)
{
    return (v & 0b0000000011111111);
}

void clear_serial_buffer()
{
    while (serial_buffer_count()) serial_read_byte();
}

void s_write_byte(int byte)
{
    callml(181, byte);
}

//Returns 0 if there is nothing to be read
int s_read_byte() 
{
    return callml(180, 0);
}