/*
 * roombalib -- Roomba C API, part of RoombaComm
 *
 * http://roombahacking.com/
 *
 * Copyright (C) 2006, Tod E. Kurt, tod@todbot.com
 *
 */



#include "roombalib.h"

int roombadebug = 0;

// internal use only
int roomba_init_serialport( char* serialport, speed_t baud );

Roomba* roomba_init( char* portname ) 
{
    int fd = roomba_init_serialport( portname, B57600 );
    if( fd == -1 ) return NULL;
    char cmd[1];
    
    cmd[0] = 128;      // START
    int n = write(fd, cmd, 1);
    if( n!=1 ) {
        perror("open_port: Unable to write to port ");
        return NULL;
    }
    roomba_delay(COMMANDPAUSE_MILLIS);
    
    cmd[0] = 130;   // CONTROL
    n = write(fd, cmd, 1);
    if( n!=1 ) {
        perror("open_port: Unable to write to port ");
        return NULL;
    }
    roomba_delay(COMMANDPAUSE_MILLIS);

    //n = write(fd, "\x86", 1);  // CONTROL
    
    Roomba* roomba = calloc( 1, sizeof(Roomba) );
    roomba->fd = fd;
    roomba->velocity = DEFAULT_VELOCITY;

    return roomba;
}

void roomba_free( Roomba* roomba ) 
{
    if( roomba->fd ) roomba_close( roomba );
    free( roomba );
}

void roomba_close( Roomba* roomba )
{
    close( roomba->fd );
    roomba->fd = 0;
}

void roomba_set_velocity( Roomba* roomba, int vel )
{
    roomba->velocity = vel;
}

int roomba_get_velocity( Roomba* roomba )
{
    return roomba->velocity;
}

void roomba_spin_left( Roomba*  roomba )
{
    roomba_drive( roomba, roomba->velocity, -1 );
}

void roomba_spin_right( Roomba* roomba )
{
    roomba_drive( roomba, roomba->velocity,  1 );
}

void roomba_stop( Roomba* roomba )
{
    roomba_drive( roomba, 0, 0 );
}

void roomba_go_forward( Roomba* roomba )
{
    roomba_drive( roomba, roomba->velocity, 0x8000 );
}

void roomba_go_backward( Roomba* roomba )
{
    roomba_drive( roomba, -roomba->velocity, 0x8000 );
}

void roomba_drive( Roomba* roomba, int velocity, int radius )
{
    char vhi = velocity >> 8;
    char vlo = velocity & 0xff;
    char rhi = radius   >> 8;
    char rlo = radius   & 0xff;
    if(roombadebug) 
        fprintf(stderr,"roomba_drive: %.2hhx %.2hhx %.2hhx %.2hhx\n",
                vhi,vlo,rhi,rlo);
    char cmd[5] = { 137, vhi,vlo, rhi,rlo };  // DRIVE
    int n = write( roomba->fd, cmd, 5);
    if( n!=5 )
        perror("roomba_drive: couldn't write to roomba");
}

int roomba_read_sensors( Roomba* roomba )
{
    char cmd[2] = { 142, 0 };          // SENSOR, get all sensor data
    int n = write( roomba->fd, cmd, 2);
    roomba_delay(COMMANDPAUSE_MILLIS);  //hmm, why isn't VMIN & VTIME working?
    n = read( roomba->fd, roomba->sensor_bytes, 26);
    if( n!=26 ) {
        if(roombadebug)
            fprintf(stderr,"roomba_read_sensors: not enough read (n=%d)\n",n);
        return -1;
    }
    return 0;
}

void roomba_print_raw_sensors( Roomba* roomba )
{
    unsigned char* sb = roomba->sensor_bytes;
    int i;
    for(i=0;i<26;i++) {
        printf("%.2hhx ",sb[i]);
    }
    printf("\n");
}

void roomba_print_sensors( Roomba* roomba )
{
    unsigned char* sb = roomba->sensor_bytes;
    printf("bump: %x %x\n", bump_left(sb[0]), bump_right(sb[0]));
    printf("wheeldrop: %x %x %x\n", wheeldrop_left(sb[0]),
           wheeldrop_caster(sb[0]), wheeldrop_right(sb[0]));
    printf("wall: %x\n", sb[1]);
    printf("cliff: %x %x %x %x\n", sb[2],sb[3],sb[4],sb[5] );
    printf("virtual_wall: %x\n", sb[6]);
    printf("motor_overcurrents: %x %x %x %x %x\n", motorover_driveleft(sb[7]),
           motorover_driveright(sb[7]), motorover_mainbrush(sb[7]),
           motorover_sidebrush(sb[7]),  motorover_vacuum(sb[7]));
    printf("dirt: %x %x\n", sb[8],sb[9]);
    printf("remote_opcode: %.2hhx\n", sb[10]);
    printf("buttons: %.2hhx\n", sb[11]);
    printf("distance: %.4x\n",    (sb[12]<<8) | sb[13] );
    printf("angle: %.4x\n",       (sb[14]<<8) | sb[15] );
    printf("charging_state: %.2hhx\n", sb[16]);
    printf("voltage: %d\n",     (sb[17]<<8) | sb[18] );
    printf("current: %d\n",     ((char)sb[19]<<8) | sb[20] );
    printf("temperature: %d\n",    sb[21]);
    printf("charge: %d\n",      (sb[22]<<8) | sb[23] );
    printf("capacity: %d\n",    (sb[24]<<8) | sb[25] );
}

// 100,000 us == 100 ms == 0.1s
void roomba_delay( int millisecs )
{
    usleep( millisecs * 1000 );
}



// private
// returns valid fd, or -1 on error
int roomba_init_serialport( char* serialport, speed_t baud )
{
    struct termios toptions;
    int fd;

    if(roombadebug)
        fprintf(stderr,"roomba_init_serialport: opening port %s\n",serialport);

    fd = open( serialport, O_RDWR | O_NOCTTY | O_NDELAY );
    if (fd == -1)  {     // Could not open the port.
        perror("roomba_init_serialport: Unable to open port ");
        return -1;
    }
    
    if (tcgetattr(fd, &toptions) < 0) {
        perror("roomba_init_serialport: Couldn't get term attributes");
        return -1;
    }
    
    cfsetispeed(&toptions, baud);
    cfsetospeed(&toptions, baud);

    // 8N1
    toptions.c_cflag &= ~PARENB;
    toptions.c_cflag &= ~CSTOPB;
    toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
    // no flow control
    toptions.c_cflag &= ~CRTSCTS;

    toptions.c_cflag    |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
    toptions.c_iflag    &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

    toptions.c_lflag    &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
    toptions.c_oflag    &= ~OPOST; // make raw

    toptions.c_cc[VMIN]  = 26;
    toptions.c_cc[VTIME] = 2;           // FIXME: not sure about this
    
    if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
        perror("roomba_init_serialport: Couldn't set term attributes");
        return -1;
    }

    return fd;
}