//----------------------------------------------------------------------------
//--  spaceBall4000FLX   -----------------------------------------------------
//----------------------------------------------------------------------------
void spaceBall4000FLX::init(const string devName, const string portName,
              const int baud)
{
    _devName = devName;
    _portName = portName;
    _baud = baud;
 
    // Byte codes
    DIGITAL_CODE  = '.';
    OLD_DIG_CODE  = 'K';
    ANALOG_CODE   = 'D';
    MODE_CODE     = 'C';
    SENS_CODE     = 'F';
    BEEP_CODE    =  'B';
    ZEROCAP_CODE  = 'Z';
    NULL_CODE     = 'N';
    QUERY_CODE    = '?';//'Q';
    ERROR_CODE    = 'E';
    RESET_CODE    = '@';
    RATE_CODE     = 'P';
    MOVEMENT_MODE = 'M';

    // Packet lengths
    ANALOG_PACKET   = 16;
    DIGITAL_PACKET  = 4;

    _checkControlChars = true;
}

spaceBall4000FLX::~spaceBall4000FLX()
{}

int spaceBall4000FLX::puckConfigure()
{
    puckInit();
    puckMode();
    puckSensitivity();
    puckNullRadius();
    puckZeroCap();
//    puckVersion();
    puckBeep();
    writeSettings();
    return 1;
}

int spaceBall4000FLX::puckOpen()
{
    int options;
#if defined(WIN32) || defined(WIN64)
    options = GENERIC_READ | GENERIC_WRITE;
#else
    options = O_RDWR | O_NONBLOCK | O_NOCTTY;
#endif

    if (!portOpen(_portName.c_str(), options, _file))
     {
        cerr << OPEN_PORT << _portName << endl;
        return 0;
     }

    puckSleep(1);
    
    if (!portConfigure())
        return 0;
    if (!puckConfigure())
        return 0;
    return 1;
}

void spaceBall4000FLX::puckInit()
{
    int n = portWrite(_file, (unsigned char *) "@\r", 2);
    if (n < 0)
        cerr << WRITE_FAIL(2) << endl;
    puckSleep(2);
    _version = "Unknown";
    left_mode = 0;
}

void spaceBall4000FLX::querySettings()
{
}

void spaceBall4000FLX::writeSettings()
{
    cout << endl << " //----------------------- @@ Puck Device Driver @@";
    cout << " ---------------------------//" << endl;
    cout << " //--  A " << _devName << " is connected to the computer." << endl;
    cout << " //--  Serial port: \"" << _portName << "\"" << endl;
    cout << " //--        Speed: " << _baud << " bps" << endl;
    cout << " //--      Version: \"" << _version << "\"" << endl;
    cout << " //--     Settings --------------->" << endl;
    cout << " //--              Left-handed Mode: " << left_mode << endl;
    cout << " //----------------------- @@ Puck Device Driver @@";
    cout << " ---------------------------//" 
         << endl << endl;
}

void spaceBall4000FLX::puckMode()
{
//    int n = write(_file, "CB\rP@r@r\rMSSV\rZ\r", 16);
//    int n = write(_file, "CB\rNT\rFTp\rFRp\rP@r@r\rMSSV\rZ\rBcCcC\r", 33);
//    int n = write(_file, "CB\rNT\rFTp\rFRp\rP32\rMSSV\rZ\rBcCcC\r", 31);
//    int n = write(_file, "CB\rP32\rMSSV\r", 12);
    int n = portWrite(_file, (unsigned char *) "CB\rP32\rMSSV\r", 12);
    if (n < 0)
        cerr << WRITE_FAIL(12) << endl; 
//    n = portRead(_file, buffer, 256);
}

string spaceBall4000FLX::puckVersion()
{
    unsigned char buffer[256];
    int n = portWrite(_file, (unsigned char *) "@\r\r\r", 4);
//    n = portRead(_file, buffer, 256);
    if (n < 1)
        return "";
    buffer[n-1] = '\0';
    if (buffer[0] == '@' && buffer[1] == '2')
    {
        // get rid of carriage return
        buffer[n-1] = '\0';
        _version = (char*)(buffer+3);
    }
//    n = portRead(_file, buffer, 256);
    if (n < 1)
        return "";
    buffer[n-1] = '\0';
    if (buffer[0] == '@' && buffer[1] == '2')
    {
        // get rid of carriage return
        buffer[n-1] = '\0';
        _version = (char*)(buffer+3);
    }
    return "unknown";
}

void spaceBall4000FLX::puckNullRadius()
{
     int n = portWrite(_file, (unsigned char *) "NT\r", 3);
     if (n < 0)
         cerr << WRITE_FAIL(3) << endl;
//    n = portRead(_file, buffer, 256);
}

void spaceBall4000FLX::puckZeroCap()
{
    int n = portWrite(_file, (unsigned char *) "Z\r", 2);
    if (n < 0)
        cerr << WRITE_FAIL(2) << endl;
//    n = portRead(_file, buffer, 256);
}

 
void spaceBall4000FLX::puckBeep()
{
     int n = portWrite(_file, (unsigned char *) "BcCcC\r", 6);
     if (n < 0)
         cerr << WRITE_FAIL(6) << endl;
//    n = portRead(_file, buffer, 256);
}

void spaceBall4000FLX::puckSensitivity()
{
     int n = portWrite(_file, (unsigned char *) "FTp\rFRp\r", 8);
     if (n < 0)
         cerr << WRITE_FAIL(8) << endl;
//    n = portRead(_file, buffer, 256);
}


int spaceBall4000FLX::portConfigure()
{

#if defined(WIN32) || defined(WIN64)

    DCB dcb;
    FillMemory(&dcb, sizeof(dcb), 0);
    dcb.DCBlength = sizeof(dcb);

    // magellan stuff:
    dcb.BaudRate = CBR_9600;
    dcb.fBinary = TRUE;
    dcb.fParity = FALSE;
    dcb.fOutxCtsFlow = FALSE;
    dcb.fOutxDsrFlow = FALSE;
    dcb.fDtrControl = DTR_CONTROL_ENABLE;
    dcb.fOutX = FALSE;
    dcb.fInX = FALSE;
    dcb.fDsrSensitivity = TRUE;//FALSE;
    dcb.fErrorChar = TRUE;
    dcb.fRtsControl = RTS_CONTROL_ENABLE;
    dcb.fAbortOnError = FALSE;
    dcb.ByteSize = 8;
    dcb.Parity = NOPARITY;
    dcb.StopBits = ONESTOPBIT;//TWOSTOPBITS;//
    dcb.ErrorChar = -1;
    dcb.EvtChar = '\r';
    dcb.EofChar = -1;
    dcb.XonChar = -1;
    dcb.XoffChar = -1;
 
    dcb.fNull = FALSE;

    dcb.XonLim = 4096;
    dcb.XoffLim = 0;
    
    if (!SetCommState(_file, &dcb))
    {
        cerr << GENERIC_ERROR << " can't set comm state" << endl;
        return 0;
    }

    if (!SetCommMask(_file, EV_RXFLAG))
    {
        cerr << GENERIC_ERROR << " can't set comm mask" << endl;
        return 0;
    }
    GetCommMask(_file, &_commMask);
 
    // Initialize overlapped event
    _event.Internal = 0;
    _event.InternalHigh = 0;
    _event.Offset = 0;
    _event.OffsetHigh = 0;
    _event.hEvent = 0;

    // Create temporary overlapped event
    OVERLAPPED EVENT;
    EVENT.Internal = 0;
    EVENT.InternalHigh = 0;
    EVENT.Offset = 0;
    EVENT.OffsetHigh = 0;
    EVENT.hEvent = CreateEvent(NULL, TRUE, FALSE, "PuckDeviceSerialEvent");
    if (EVENT.hEvent == INVALID_HANDLE_VALUE)
    {
        cerr << GENERIC_ERROR << " can't create overlapped serial event" << endl;
        return 0;
    }
    else if (EVENT.hEvent == NULL)
    {
        cerr << GENERIC_ERROR << " can't create overlapped serial event 2" << endl;
        return 0;
    }
 
    if (!GetCommTimeouts(_file, &_oldTimeouts))
    {
        cerr << GENERIC_ERROR << " can't get comm timeouts" << endl;
        return 0;
    }
    COMMTIMEOUTS newTimeouts;
    newTimeouts.ReadIntervalTimeout = MAXDWORD;
    newTimeouts.ReadTotalTimeoutMultiplier = 0;//MAXDWORD;
    newTimeouts.ReadTotalTimeoutConstant = 0;//32000;
    newTimeouts.WriteTotalTimeoutMultiplier = 10;
    newTimeouts.WriteTotalTimeoutConstant = 100;
    if (!SetCommTimeouts(_file, &newTimeouts))
    {
        cerr << GENERIC_ERROR << " can't set comm timeouts" << endl;
        return 0;
    }

    puckSleep(1);
    return 1;

#else                              /* #if !defined(WIN32) && !defined(WIN64) */


#if defined(HP) || defined(HPUX)
    mflag m = MRTS | MDTR | MDRS;
    ioctl(_file, MCSETA, &m);
    ioctl(_file, MCSETA, &m);    
#endif

    // Set blocking behavior on read
    fcntl(_file, F_SETFL, 0);

    // get termios structure
    int status = tcgetattr(_file, &_tty_struct);
    // Local options
    // Enable canonical input; disable echo characters
    _tty_struct.c_lflag = ICANON;
//////////////////////    _tty_struct.c_lflag &= ~( ECHO | ECHOE);
    // Input options
    // Ignore parity errors and break conditions
    _tty_struct.c_iflag = IGNBRK | IGNPAR;
    // Output options
    // None
    _tty_struct.c_oflag = 0;
    // Control options
    // Enable receiver; 8 data bits; 1 stop bit; hangup on last close
    // (drop DTR); local line (keep owner same)    
/////////////////////////////    _tty_struct.c_cflag &= ~CSIZE;
    _tty_struct.c_cflag = CREAD | CS8 | HUPCL | CLOCAL; 
//////////////////////////    _tty_struct.c_cflag &= ~CSTOPB;
    // Control characters
    // Disable erase and kill special characters; min & time aren't used
    _tty_struct.c_cc[VEOL] = '\r';
    _tty_struct.c_cc[VERASE] = _POSIX_VDISABLE;
    _tty_struct.c_cc[VKILL] = _POSIX_VDISABLE;
    // Set input = output speed
    cfsetispeed(&_tty_struct, B9600);
    cfsetospeed(&_tty_struct, B9600);

    // Make changes after buffer flush
    tcsetattr(_file, TCSAFLUSH, &_tty_struct);

    return 1;

#endif
}

void spaceBall4000FLX::checkControlChars(unsigned char *buffer, int &len)
{
    for (int i = 0; i < len; i++)
    {
        bool found = false;
        if (buffer[i] == '^')
        {
            found = true;
            for (int j = i; j < len-1; j++)
            {
                if (found)
                {
                    // Make them "un-escaped"
                    buffer[j] = buffer[j+1] &= 31;
                    found = false;
                }
                else
                    buffer[j] = buffer[j+1];
            }
            buffer[len-1] = '\r';
            len--;
        }
    }
}

int spaceBall4000FLX::processBuffer(analogData &ana, digitalData &dig)
{
    const bool checkControlChar = true;

    const int size_buffer = 512;
    unsigned char buffer[size_buffer];
    int rc = portRead(_file, buffer, size_buffer);
    buffer[rc] = '\0';
    if (checkControlChar)
    {
        checkControlChars(buffer, rc);
    }
    if (rc == 0)
        return 0;
    else return handleRead(buffer, rc, ana, dig);
}


int spaceBall4000FLX::handleRead(unsigned char* buffer, int bRead,
                                 analogData &ana, digitalData &dig )
{
    if (buffer[0] == ANALOG_CODE && bRead == ANALOG_PACKET)     // analog data
    {
        processAnalogData(buffer + 1, ana);
        return 2;
    }
    else if (buffer[0] == DIGITAL_CODE && bRead == DIGITAL_PACKET)
    {
        processDigitalData(buffer + 1, dig);
        return 1;
    }
    else if (buffer[0] == OLD_DIG_CODE && bRead == (2 * DIGITAL_PACKET) )       // digital data
    {
        processDigitalData(buffer + 5, dig);
        return 1;
    }
    if (buffer[0] == ERROR_CODE)
    {
        processError(buffer);
    }
    return 0;
}

void spaceBall4000FLX::processError(const unsigned char *buffer)
{
}

void spaceBall4000FLX::processDigitalData(const unsigned char* buffer,
                                         digitalData &dig)
{
    /* Buttons are represented by following bits:

Bit:         7    6    5    4    3    2    1    0
            --------------------------------------
Meaning:     
(First Byte)
             0    1   <L> <12> <11> <10>  <9>  <8>

(Second Byte)
            <7>   1   <6>  <5>  <4>  <3>  <2>  <1>

e.g. The higher 4 bits of the first byte are always 01xy, where
    x = the 'left-handed' mode of the SpaceBall, and
    y = the on/off state of button 12
    */
    int temp = (buffer[1] & 63) |
        ( (buffer[1] & 128) >> 1 ) |
        ( (buffer[0] & 63) << 7 );
    left_mode = (temp & LEFT_MODE) >> 12;

    dig[0] = temp & BUTTON_1;
    dig[1] = temp & BUTTON_2;
    dig[2] = temp & BUTTON_3;
    dig[3] = temp & BUTTON_4;
    dig[4] = temp & BUTTON_5;
    dig[5] = temp & BUTTON_6;
    dig[6] = temp & BUTTON_7;
    dig[7] = temp & BUTTON_8;
    dig[8] = temp & BUTTON_9;
    dig[9] = temp & BUTTON_10;
    dig[10] = temp & BUTTON_11;
    dig[11] = temp & BUTTON_12;
    
    return;
}

void spaceBall4000FLX::processAnalogData(const unsigned char *buffer,
                                         analogData &ana)
{
    int ret[6];
    // 1st 2 char's give a time signal, axes start with 3rd char.
    for (int i = 0; i < 6; i++)
    {   
        ret[i] = (buffer[2*i+2] << 8) |
            buffer[2*i+3];
        // Bit 16 is sign bit for upper 16 bits
        if (ret[i] & (1 << 15))
            ret[i] |= 0xFFFF0000;
        else
            ret[i] &= 0xFFFF;
    }
    ret[2] = -ret[2];
    // switch roll direction
    ret[5] = -ret[5];

    for (int i = 0; i < 6; i++)
    {
        ana[i] = ret[i];
//      cout<<"Axis["<<i<<"] = "<<ret[i]<<endl;
    }
    return;
}

int spaceBall4000FLX::puckClose()
{
    return portClose(_file);
}