File content as of revision 8:6fa15d4e31fb:

#include "glibr.h"
#include "mbed.h"




glibr::glibr(PinName sda, PinName scl):i2c(sda, scl){
          gesture_ud_delta_ = 0;
    gesture_lr_delta_ = 0;
    
    gesture_ud_count_ = 0;
    gesture_lr_count_ = 0;
    
    gesture_near_count_ = 0;
    gesture_far_count_ = 0;
    
    gesture_state_ = 0;
    gesture_motion_ = DIR_NONE;
}    
   
glibr::~glibr(){
       
} 

 uint8_t  glibr::ginit(){
    uint8_t id;
    
 //   if( !wireReadDataByte(APDS9960_ID, id) ) {
        
     id=I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ID);
    
    if( (!(id == APDS9960_ID_1 || id == APDS9960_ID_2))||id==ERROR) {
         return false;
    }
    
    if(!setMode(ALL, OFF)) {
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ATIME, DEFAULT_ATIME)){
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_WTIME, DEFAULT_WTIME)){
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PPULSE, DEFAULT_PROX_PPULSE)){
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR)){
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL)){
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG1, DEFAULT_CONFIG1)){
        return false;
    }
    
    if( !setLEDDrive(DEFAULT_LDRIVE) ) {
        return false;
    }
    
    if( !setProximityGain(DEFAULT_PGAIN) ) {
        return false;
    }
    if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
        return false;
    }
    if( !setProxIntLowThresh(DEFAULT_PILT) ) {
        return false;
    }
    if( !setProxIntHighThresh(DEFAULT_PIHT) ) {
        return false;
    }
    if( !setLightIntLowThreshold(DEFAULT_AILT) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG2, DEFAULT_CONFIG2) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG3, DEFAULT_CONFIG3) ) {
        return false;
    }
    
    if( !setGestureEnterThresh(DEFAULT_GPENTH) ) {
        return false;
    }
    if( !setGestureExitThresh(DEFAULT_GEXTH) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF1, DEFAULT_GCONF1) ) {
        return false;
    }
    if( !setGestureGain(DEFAULT_GGAIN) ) {
        return false;
    }
    if( !setGestureLEDDrive(DEFAULT_GLDRIVE) ) {
        return false;
    }
    if( !setGestureWaitTime(DEFAULT_GWTIME) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) {
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GPULSE, DEFAULT_GPULSE) ) {
        return false;
    }
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF3, DEFAULT_GCONF3) ) {
        return false;
    }
    if( !setGestureIntEnable(DEFAULT_GIEN) ) {
        return false;
    }
    
    
    
    return id;
}

#if 0
    /* Gesture config register dump */
    uint8_t reg;
    uint8_t val;
  
    for(reg = 0x80; reg <= 0xAF; reg++) {
        if( (reg != 0x82) && \
            (reg != 0x8A) && \
            (reg != 0x91) && \
            (reg != 0xA8) && \
            (reg != 0xAC) && \
            (reg != 0xAD) )
        {
         val= I2CreadByte(APDS9960_I2C_ADDR, reg);
          if(val==ERROR){
              printf("ERROR");
          }
            /*
            print(reg, HEX);
            print(": 0x");
            println(val, HEX);*/
        }
    }

    for(reg = 0xE4; reg <= 0xE7; reg++) {
        val= I2CreadByte(APDS9960_I2C_ADDR, reg);
     /*   Serial.print(reg, HEX);
        Serial.print(": 0x");
        Serial.println(val, HEX);*/
    }
#endif 

  //  return true;
//}



/**
 * @brief Enables or disables a feature in the APDS-9960
 *
 * @param[in] mode which feature to enable
 * @param[in] enable ON (1) or OFF (0)
 * @return True if operation success. False otherwise.
 */
bool glibr::setMode(uint8_t mode, uint8_t enable)
{
    uint8_t reg_val;

    /* Read current ENABLE register */
    reg_val = getMode();
    if( reg_val == ERROR ) {
        return false;
    }
    
    /* Change bit(s) in ENABLE register */
    enable = enable & 0x01;
    if( mode >= 0 && mode <= 6 ) {
        if (enable) {
            reg_val |= (1 << mode);
        } else {
            reg_val &= ~(1 << mode);
        }
    } else if( mode == ALL ) {
        if (enable) {
            reg_val = 0x7F;
        } else {
            reg_val = 0x00;
        }
    }
    
    /* Write value back to ENABLE register */     
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE, reg_val)){
        return false;
    }
   

        
    return true;
}

uint8_t glibr::getMode()
{
    uint8_t val;
    val= I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
    if(val==ERROR){
        return ERROR;
    }
    return val;
}



bool glibr::enableLightSensor(bool interrupts)
{
    
    /* Set default gain, interrupts, enable power, and enable sensor */
    if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
        return false;
    }
    if( interrupts ) {
        if( !setAmbientLightIntEnable(1) ) {
            return false;
        }
    } else {
        if( !setAmbientLightIntEnable(0) ) {
            return false;
        }
    }
    if( !enablePower() ){
        return false;
    }
    if( !setMode(AMBIENT_LIGHT, 1) ) {
        return false;
    }
    
    return true;

}

/**
 * @brief Ends the light sensor on the APDS-9960
 *
 * @return True if sensor disabled correctly. False on error.
 */
bool glibr::disableLightSensor()
{
    if( !setAmbientLightIntEnable(0) ) {
        return false;
    }
    if( !setMode(AMBIENT_LIGHT, 0) ) {
        return false;
    }
    
    return true;
}

/**
 * @brief Starts the proximity sensor on the APDS-9960
 *
 * @param[in] interrupts true to enable hardware external interrupt on proximity
 * @return True if sensor enabled correctly. False on error.
 */
bool glibr::enableProximitySensor(bool interrupts)
{
    /* Set default gain, LED, interrupts, enable power, and enable sensor */
    if( !setProximityGain(DEFAULT_PGAIN) ) {
        return false;
    }
    if( !setLEDDrive(DEFAULT_LDRIVE) ) {
        return false;
    }
    if( interrupts ) {
        if( !setProximityIntEnable(1) ) {
            return false;
        }
    } else {
        if( !setProximityIntEnable(0) ) {
            return false;
        }
    }
    if( !enablePower() ){
        return false;
    }
    if( !setMode(PROXIMITY, 1) ) {
        return false;
    }
    
    return true;
}

/**
 * @brief Ends the proximity sensor on the APDS-9960
 *
 * @return True if sensor disabled correctly. False on error.
 */
bool glibr::disableProximitySensor()
{
    if( !setProximityIntEnable(0) ) {
        return false;
    }
    if( !setMode(PROXIMITY, 0) ) {
        return false;
    }

    return true;
}


/**
 * @brief Starts the gesture recognition engine on the APDS-9960
 *
 * @param[in] interrupts true to enable hardware external interrupt on gesture
 * @return True if engine enabled correctly. False on error.
 */
bool glibr::enableGestureSensor(bool interrupts)
{
    
    /* Enable gesture mode
       Set ENABLE to 0 (power off)
       Set WTIME to 0xFF
       Set AUX to LED_BOOST_300
       Enable PON, WEN, PEN, GEN in ENABLE 
    */

    resetGestureParameters();
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_WTIME, 0xFF) ) {
        return false;
    }
    if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PPULSE, DEFAULT_GESTURE_PPULSE) ) {
        return false;
    }
    if( !setLEDBoost(LED_BOOST_300) ) {
        return false;
    }
    if( interrupts ) {
        if( !setGestureIntEnable(1) ) {
            return false;
        }
    } else {
        if( !setGestureIntEnable(0) ) {
            return false;
        }
    }
    if( !setGestureMode(1) ) {
        return false;
    }
    if( !enablePower() ){
        return false;
    }
    if( !setMode(WAIT, 1) ) {
        return false;
    }
    if( !setMode(PROXIMITY, 1) ) {
        return false;
    }
    if( !setMode(GESTURE, 1) ) {
        return false;
    }
    
    return true;
}

/**
 * @brief Ends the gesture recognition engine on the APDS-9960
 *
 * @return True if engine disabled correctly. False on error.
 */
bool glibr::disableGestureSensor()
{
    resetGestureParameters();
    if( !setGestureIntEnable(0) ) {
        return false;
    }
    if( !setGestureMode(0) ) {
        return false;
    }
    if( !setMode(GESTURE, 0) ) {
        return false;
    }
    
    return true;
}


/**
 * @brief Determines if there is a gesture available for reading
 *
 * @return True if gesture available. False otherwise.
 */
bool glibr::isGestureAvailable()
{
    uint8_t val;
    
    /* Read value from GSTATUS register */
    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
    if( val==ERROR) {
        return ERROR;
    }
    
    /* Shift and mask out GVALID bit */
    val &= APDS9960_GVALID;
    
    /* Return true/false based on GVALID bit */
    if( val == 1) {
        return true;
    } else {
        return false;
    }
}

int glibr::readGesture()
{
    uint8_t fifo_level = 0;
   // uint8_t bytes_expected= 0;
    int check;
    uint8_t fifo_data[128];
    uint8_t gstatus;
    int motion;
    int i;
    
    /* Make sure that power and gesture is on and data is valid */
    if( !isGestureAvailable() || !(getMode() & 0x41) ) {
        return DIR_NONE;
    }
    
    /* Keep looping as long as gesture data is valid */
    while(1) {
       
        /* Wait some time to collect next batch of FIFO data */
        wait(FIFO_PAUSE_TIME);
        
        /* Get the contents of the STATUS register. Is data still valid? */
        
        gstatus=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
        if( gstatus==ERROR ) {
            return ERROR;
        }
        /* If we have valid data, read in FIFO */
        if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
        
            /* Read the current FIFO level */
            fifo_level=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GFLVL);
            if( fifo_level==ERROR ) {
                return ERROR;
            }
            

//#if DEBUG
//            Serial.print("FIFO Level: ");
//            Serial.println(fifo_level);
//#endif

            /* If there's stuff in the FIFO, read it into our data block */                 //NEED TO FIGURE OUT WHAT THIS IS DOING.
        
            if( fifo_level > 0) {
                check = I2CReadDataBlock(APDS9960_I2C_ADDR,APDS9960_GFIFO_U, 
                                                (uint8_t*)fifo_data, 
                                                (fifo_level * 4) );
                
                if( check == -1 ) {
                    return ERROR;
                }
#if DEBUG
                //Serial.print("FIFO Dump: ");
                for ( i = 0; i < (fifo_level * 4); i++ ) {
                 //   Serial.print(fifo_data[i]);
                  //  Serial.print(" ");
                }
                //Serial.println();
#endif

                /* If at least 1 set of data, sort the data into U/D/L/R */
                if((fifo_level * 4)  >= 4 ) {
                    for( i = 0; i < (fifo_level * 4); i += 4 ) {
                        gesture_data_.u_data[gesture_data_.sindex] = \
                                                            fifo_data[i + 0];
                        gesture_data_.d_data[gesture_data_.sindex] = \
                                                            fifo_data[i + 1];
                        gesture_data_.l_data[gesture_data_.sindex] = \
                                                            fifo_data[i + 2];
                        gesture_data_.r_data[gesture_data_.sindex] = \
                                                            fifo_data[i + 3];
                        gesture_data_.sindex++;
                        gesture_data_.total_gestures++;
                    }
                    
#if DEBUG
               // Serial.print("Up Data: ");
                for ( i = 0; i < gesture_data_.total_gestures; i++ ) {
                 //   Serial.print(gesture_data_.u_data[i]);
                  //  Serial.print(" ");
                }
              //  Serial.println();
#endif

                    /* Filter and process gesture data. Decode near/far state */
                    if( processGestureData() ) {
                        if( decodeGesture() ) {
                            //***TODO: U-Turn Gestures
#if DEBUG
                            //Serial.println(gesture_motion_);
#endif
                        }
                    }
                    
                    /* Reset data */
                    gesture_data_.sindex = 0;
                    gesture_data_.total_gestures = 0;
                }
            }
        } else {
    
            /* Determine best guessed gesture and clean up */
            wait(FIFO_PAUSE_TIME);
            decodeGesture();
            motion = gesture_motion_;
#if DEBUG
        //    Serial.print("END: ");
        //    Serial.println(gesture_motion_);
#endif
            resetGestureParameters();
            return motion;
        }
    }
}
/**
 * Turn the APDS-9960 on
 *
 * @return True if operation successful. False otherwise.
 */
bool glibr::enablePower()
{
    if( !setMode(POWER, 1) ) {
        return false;
    }
    
    return true;
}

/**
 * Turn the APDS-9960 off
 *
 * @return True if operation successful. False otherwise.
 */
bool glibr::disablePower()
{
    if( !setMode(POWER, 0) ) {
        return false;
    }
    
    return true;
}

/*******************************************************************************
 * Ambient light and color sensor controls
 ******************************************************************************/

/**
 * @brief Reads the ambient (clear) light level as a 16-bit value
 *
 * @param[out] val value of the light sensor.
 * @return True if operation successful. False otherwise.
 */
bool glibr::readAmbientLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CDATAL);
    if( val_byte==ERROR) {
        return false;
    }
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
   
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CDATAH);
    if( val_byte==ERROR) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

/**
 * @brief Reads the red light level as a 16-bit value
 *
 * @param[out] val value of the light sensor.
 * @return True if operation successful. False otherwise.
 */
bool glibr::readRedLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_RDATAL);
    if( val_byte==ERROR) {
        return false;
    }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_RDATAH);
    if( val_byte==ERROR) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

/**
 * @brief Reads the green light level as a 16-bit value
 *
 * @param[out] val value of the light sensor.
 * @return True if operation successful. False otherwise.
 */

bool glibr::readGreenLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GDATAL);
    if( val_byte==ERROR) {
        return false;
    }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GDATAH);
    if( val_byte==ERROR) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

/**
 * @brief Reads the red light level as a 16-bit value
 *
 * @param[out] val value of the light sensor.
 * @return True if operation successful. False otherwise.
*/

bool glibr::readBlueLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_BDATAL);
    if( val_byte==ERROR) {
        return false;
    }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_BDATAH);
    if( val_byte==ERROR) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

/*******************************************************************************
 * Proximity sensor controls
 ******************************************************************************/

/**
 * @brief Reads the proximity level as an 8-bit value
 *
 * @param[out] val value of the proximity sensor.
 * @return True if operation successful. False otherwise.
 */
bool glibr::readProximity(uint8_t &val)
{
    val = 0;
    
    /* Read value from proximity data register */
     val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PDATA);
    
    if(val==ERROR){
        return false;   
    }
    
    return true;
}

/*******************************************************************************
 * High-level gesture controls
 ******************************************************************************/

/**
 * @brief Resets all the parameters in the gesture data member
 */
void glibr::resetGestureParameters()
{
    gesture_data_.sindex = 0;
    gesture_data_.total_gestures = 0;
    
    gesture_ud_delta_ = 0;
    gesture_lr_delta_ = 0;
    
    gesture_ud_count_ = 0;
    gesture_lr_count_ = 0;
    
    gesture_near_count_ = 0;
    gesture_far_count_ = 0;
    
    gesture_state_ = 0;
    gesture_motion_ = DIR_NONE;
}

bool glibr::processGestureData()
{
    uint8_t u_first = 0;
    uint8_t d_first = 0;
    uint8_t l_first = 0;
    uint8_t r_first = 0;
    uint8_t u_last = 0;
    uint8_t d_last = 0;
    uint8_t l_last = 0;
    uint8_t r_last = 0;
    int ud_ratio_first;
    int lr_ratio_first;
    int ud_ratio_last;
    int lr_ratio_last;
    int ud_delta;
    int lr_delta;
    int i;

    /* If we have less than 4 total gestures, that's not enough */
    if( gesture_data_.total_gestures <= 4 ) {
        return false;
    }
    
    /* Check to make sure our data isn't out of bounds */
    if( (gesture_data_.total_gestures <= 32) && \
        (gesture_data_.total_gestures > 0) ) {
        
        /* Find the first value in U/D/L/R above the threshold */
        for( i = 0; i < gesture_data_.total_gestures; i++ ) {
            if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
                
                u_first = gesture_data_.u_data[i];
                d_first = gesture_data_.d_data[i];
                l_first = gesture_data_.l_data[i];
                r_first = gesture_data_.r_data[i];
                break;
            }
        }
        
        /* If one of the _first values is 0, then there is no good data */
        if( (u_first == 0) || (d_first == 0) || \
            (l_first == 0) || (r_first == 0) ) {
            
            return false;
        }
        /* Find the last value in U/D/L/R above the threshold */
        for( i = gesture_data_.total_gestures - 1; i >= 0; i-- ) {
/* #if DEBUG
            Serial.print(F("Finding last: "));
            Serial.print(F("U:"));
            Serial.print(gesture_data_.u_data[i]);
            Serial.print(F(" D:"));
            Serial.print(gesture_data_.d_data[i]);
            Serial.print(F(" L:"));
            Serial.print(gesture_data_.l_data[i]);
            Serial.print(F(" R:"));
            Serial.println(gesture_data_.r_data[i]);
#endif */
            if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
                (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
                
                u_last = gesture_data_.u_data[i];
                d_last = gesture_data_.d_data[i];
                l_last = gesture_data_.l_data[i];
                r_last = gesture_data_.r_data[i];
                break;
            }
        }
    }
    
    /* Calculate the first vs. last ratio of up/down and left/right */
    ud_ratio_first = ((u_first - d_first) * 100) / (u_first + d_first);
    lr_ratio_first = ((l_first - r_first) * 100) / (l_first + r_first);
    ud_ratio_last = ((u_last - d_last) * 100) / (u_last + d_last);
    lr_ratio_last = ((l_last - r_last) * 100) / (l_last + r_last);
       
/* #if DEBUG
    Serial.print(F("Last Values: "));
    Serial.print(F("U:"));
    Serial.print(u_last);
    Serial.print(F(" D:"));
    Serial.print(d_last);
    Serial.print(F(" L:"));
    Serial.print(l_last);
    Serial.print(F(" R:"));
    Serial.println(r_last);

    Serial.print(F("Ratios: "));
    Serial.print(F("UD Fi: "));
    Serial.print(ud_ratio_first);
    Serial.print(F(" UD La: "));
    Serial.print(ud_ratio_last);
    Serial.print(F(" LR Fi: "));
    Serial.print(lr_ratio_first);
    Serial.print(F(" LR La: "));
    Serial.println(lr_ratio_last);
#endif */
       
    /* Determine the difference between the first and last ratios */
    ud_delta = ud_ratio_last - ud_ratio_first;
    lr_delta = lr_ratio_last - lr_ratio_first;
    
/* #if DEBUG
    Serial.print("Deltas: ");
    Serial.print("UD: ");
    Serial.print(ud_delta);
    Serial.print(" LR: ");
    Serial.println(lr_delta);
#endif */

    /* Accumulate the UD and LR delta values */
    gesture_ud_delta_ += ud_delta;
    gesture_lr_delta_ += lr_delta;
    
/* #if DEBUG
    Serial.print("Accumulations: ");
    Serial.print("UD: ");
    Serial.print(gesture_ud_delta_);
    Serial.print(" LR: ");
    Serial.println(gesture_lr_delta_);
#endif */
    
    /* Determine U/D gesture */
    if( gesture_ud_delta_ >= GESTURE_SENSITIVITY_1 ) {
        gesture_ud_count_ = 1;
    } else if( gesture_ud_delta_ <= -GESTURE_SENSITIVITY_1 ) {
        gesture_ud_count_ = -1;
    } else {
        gesture_ud_count_ = 0;
    }
    
    /* Determine L/R gesture */
    if( gesture_lr_delta_ >= GESTURE_SENSITIVITY_1 ) {
        gesture_lr_count_ = 1;
    } else if( gesture_lr_delta_ <= -GESTURE_SENSITIVITY_1 ) {
        gesture_lr_count_ = -1;
    } else {
        gesture_lr_count_ = 0;
    }
    
    /* Determine Near/Far gesture */
    if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 0) ) {
        if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
            (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
            
            if( (ud_delta == 0) && (lr_delta == 0) ) {
                gesture_near_count_++;
            } else if( (ud_delta != 0) || (lr_delta != 0) ) {
                gesture_far_count_++;
            }
            
            if( (gesture_near_count_ >= 10) && (gesture_far_count_ >= 2) ) {
                if( (ud_delta == 0) && (lr_delta == 0) ) {
                    gesture_state_ = NEAR_STATE;
                } else if( (ud_delta != 0) && (lr_delta != 0) ) {
                    gesture_state_ = FAR_STATE;
                }
                return true;
            }
        }
    } else {
        if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
            (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
                
            if( (ud_delta == 0) && (lr_delta == 0) ) {
                gesture_near_count_++;
            }
            
            if( gesture_near_count_ >= 10 ) {
                gesture_ud_count_ = 0;
                gesture_lr_count_ = 0;
                gesture_ud_delta_ = 0;
                gesture_lr_delta_ = 0;
            }
        }
    }
    
/* #if DEBUG
    Serial.print("UD_CT: ");
    Serial.print(gesture_ud_count_);
    Serial.print(" LR_CT: ");
    Serial.print(gesture_lr_count_);
    Serial.print(" NEAR_CT: ");
    Serial.print(gesture_near_count_);
    Serial.print(" FAR_CT: ");
    Serial.println(gesture_far_count_);
    Serial.println("----------");
#endif */
    
    return false;
}

/**
 * @brief Determines swipe direction or near/far state
 *
 * @return True if near/far event. False otherwise.
 */
bool glibr::decodeGesture()
{
    /* Return if near or far event is detected */
    if( gesture_state_ == NEAR_STATE ) {
        gesture_motion_ = DIR_NEAR;
        return true;
    } else if ( gesture_state_ == FAR_STATE ) {
        gesture_motion_ = DIR_FAR;
        return true;
    }
    
    /* Determine swipe direction */
    if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 0) ) {
        gesture_motion_ = DIR_UP;
    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 0) ) {
        gesture_motion_ = DIR_DOWN;
    } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 1) ) {
        gesture_motion_ = DIR_RIGHT;
    } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == -1) ) {
        gesture_motion_ = DIR_LEFT;
    } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 1) ) {
        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
            gesture_motion_ = DIR_UP;
        } else {
            gesture_motion_ = DIR_RIGHT;
        }
    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == -1) ) {
        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
            gesture_motion_ = DIR_DOWN;
        } else {
            gesture_motion_ = DIR_LEFT;
        }
    } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == -1) ) {
        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
            gesture_motion_ = DIR_UP;
        } else {
            gesture_motion_ = DIR_LEFT;
        }
    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 1) ) {
        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
            gesture_motion_ = DIR_DOWN;
        } else {
            gesture_motion_ = DIR_RIGHT;
        }
    } else {
        return false;
    }
    
    return true;
}

/*******************************************************************************
 * Getters and setters for register values
 ******************************************************************************/

/**
 * @brief Returns the lower threshold for proximity detection
 *
 * @return lower threshold
 */
 
 uint8_t glibr::getProxIntLowThresh()
{
    uint8_t val;
    
    /* Read value from PILT register */
   /* if( !wireReadDataByte(APDS9960_PILT, val) ) {
        val = 0;
    }*/
    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PILT);
    if(val==ERROR){
        val=0;   
    }
    
    return val;
}
 
 bool glibr::setProxIntLowThresh(uint8_t threshold)
{
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PILT, threshold) ) {
        return false;
    }
    
    return true;
}

bool glibr::setProxIntHighThresh(uint8_t threshold)
{
   
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PIHT, threshold) ) {
        return false;
    }
    
    return true;
}

 
 
bool glibr::setLEDDrive(uint8_t drive)
{
    uint8_t val;
    
    /* Read value from CONTROL register */

    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
    if(val==ERROR){
        return false;   
    }
    /* Set bits in register to given value */
    drive &= 0x03;
    drive = drive << 6;
    //val &= 0b00111111;
    val &= 0x3F;
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
        return false;
    }
    
    return true;
}



bool glibr::setProximityGain(uint8_t drive)
{
    uint8_t val;
    
    /* Read value from CONTROL register */
   
    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
    
    if(val==ERROR){
        return false;   
    }
    /* Set bits in register to given value */
    //drive &= 0b00000011;
    drive &=0x03;
    drive = drive << 2;
    val &= 0xF3;
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
        return false;
    }
    return true;
}


bool glibr::setAmbientLightGain(uint8_t drive){
{
    uint8_t val;
    
    /* Read value from CONTROL register */
   
    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
    
    if(val==ERROR){
        return false;   
    }
    /* Set bits in register to given value */
    //drive &= 0b00000011;
    drive &=0x03;
    drive = drive << 2;
    val &= 0xF3;
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
        return false;
    }
    return true;
}


bool glibr::setLightIntLowThreshold(uint16_t threshold)
{
    uint8_t val_low;
    uint8_t val_high;
    
    /* Break 16-bit threshold into 2 8-bit values */
    val_low = threshold & 0x00FF;
    val_high = (threshold & 0xFF00) >> 8;
    
    /* Write low byte */
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AILTL, val_low) ) {
        return false;
    }
    
    /* Write high byte */
    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AILTH, val_high) ) {
        return false;
    }
    
    return true;
}





int glibr::I2CwriteByte(char address, char subAddress, char data)
{   
    int ret;
    char cmd[2] = {subAddress, data};
    ret=i2c.write(address<<1, cmd, 2);  //if ret is 1, then not acked.
    return ret;
}



uint8_t glibr::I2CreadByte(char address, char subAddress)
{
    char data; // store the register data
    
    if(i2c.write(address<<1, &subAddress, 1, true)){
        return ERROR;   //7 bit   //not acked
    } 
    i2c.read(address<<1, &data, 1);
    return data;

}

/**
 * @brief Reads a block (array) of bytes from the I2C device and register
 *
 * @param[in] reg the register to read from
 * @param[out] val pointer to the beginning of the data
 * @param[in] len number of bytes to read
 * @return Number of bytes read. -1 on read error.
 */
int glibr::I2CReadDataBlock(char address, char subAddress, uint8_t *val, unsigned int len)
{
  //  unsigned char i = 0;

    /* Indicate which register we want to read from */
   
      if(i2c.write(address<<1, subAddress, 1, true)){
        return -1;   //7 bit   //not acked
      } 
    
    /* Read block data */
     
    if(i2c.read(address<<1, &val, len)){
        return -1;
    }
    
    return 1;
    //Wire.requestFrom(APDS9960_I2C_ADDR, len);
    /*while (Wire.available()) {
        if (i >= len) {
            return -1;
        }
        val[i] = Wire.read();
        i++;
    }*/
}