File content as of revision 0:830ac1db5bf7:

/**
 * @file        apds9960.cpp
 * @brief       Library for the SparkFun APDS-9960 breakout board
 * @author      devsar
 * @origin      The origin of this file was from Arduino's library
 *              by Shawn Hymel (SparkFun Electronics).
 * @copyright   Public domain (Beerware license).
 *
 * This library interfaces the Avago APDS-9960 to Arduino over I2C. The library
 * relies on the Arduino Wire (I2C) library. to use the library, instantiate an
 * APDS9960 object, call init(), and call the appropriate functions.
 *
 * APDS-9960 current draw tests (default parameters):
 *   Off:                   1mA
 *   Waiting for gesture:   14mA
 *   Gesture in progress:   35mA
 */
#include "mbed.h"
#include "apds9960.h"

/**
 * @brief Constructor - Instantiates SparkFun_APDS9960 object
 */
 apds9960::apds9960(PinName sda, PinName scl):i2c(sda, scl){   
    resetGestureParameters();
}

/**
 * @brief Destructor
 */
apds9960::~apds9960(){
       
} 

/**
 * @brief Configures I2C communications and initializes registers to defaults
 *
 * @return True if initialized successfully. False otherwise.
 */
#ifdef DEBUG
bool  apds9960::ginit(Serial &par){
    pc = ∥
#else
bool  apds9960::ginit(){
#endif
    uint8_t id;
    
    /* Read ID register and check against known values for APDS-9960 */
    if( !wireReadDataByte(APDS9960_ID, id) ) { return false; }
    
    if( (!(id == APDS9960_ID_1 || id == APDS9960_ID_2))||id==ERROR) { return false; }
    
    /* Set ENABLE register to 0 (disable all features) */
    if(!setMode(ALL, OFF)) { return false; }

    /* Set default values for ambient light and proximity registers */
    if( !wireWriteDataByte(APDS9960_ATIME, DEFAULT_ATIME) ) { return false; }
    if( !wireWriteDataByte(APDS9960_WTIME, DEFAULT_WTIME) ) { return false; }
    if( !wireWriteDataByte(APDS9960_PPULSE, DEFAULT_PROX_PPULSE) ) { return false; }
    if( !wireWriteDataByte(APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR) ) { return false; }
    if( !wireWriteDataByte(APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL) ) { return false; }
    if( !wireWriteDataByte(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( !wireWriteDataByte(APDS9960_CONFIG2, DEFAULT_CONFIG2) ) { return false; }
    if( !wireWriteDataByte(APDS9960_CONFIG3, DEFAULT_CONFIG3) ) { return false; }

    /* Set default values for gesture sense registers */    
    if( !setGestureEnterThresh(DEFAULT_GPENTH) ) { return false; }
    if( !setGestureExitThresh(DEFAULT_GEXTH) ) { return false; }
    if( !wireWriteDataByte(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( !wireWriteDataByte(APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) { return false; }
    if( !wireWriteDataByte(APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) { return false; }
    if( !wireWriteDataByte(APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) { return false; }
    if( !wireWriteDataByte(APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) { return false; }
    if( !wireWriteDataByte(APDS9960_GPULSE, DEFAULT_GPULSE) ) { return false; }
    if( !wireWriteDataByte(APDS9960_GCONF3, DEFAULT_GCONF3) ) { return false; }
    if( !setGestureIntEnable(DEFAULT_GIEN) ) { return false; }
    
    return true;
}

/*******************************************************************************
 * Public methods for controlling the APDS-9960
 ******************************************************************************/

/**
 * @brief Reads and returns the contents of the ENABLE register
 *
 * @return Contents of the ENABLE register. 0xFF if error.
 */
uint8_t apds9960::getMode()
{
    uint8_t enable_value;
    if( !wireReadDataByte(APDS9960_ENABLE, enable_value) ) { return ERROR; }
    return enable_value;
}

/**
 * @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 apds9960::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 <= 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( !wireWriteDataByte(APDS9960_ENABLE, reg_val) ) { return false; }
   
    return true;
}

/**
 * @brief Starts the light (R/G/B/Ambient) sensor on the APDS-9960
 *
 * @param[in] interrupts true to enable hardware interrupt on high or low light
 * @return True if sensor enabled correctly. False on error.
 */
bool apds9960::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 apds9960::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 apds9960::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 apds9960::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 apds9960::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( !wireWriteDataByte(APDS9960_WTIME, 0xFF) ) { return false; }
    if( !wireWriteDataByte(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 apds9960::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 apds9960::isGestureAvailable()
{
    uint8_t val;
    
    /* Read value from GSTATUS register */
    if( !wireReadDataByte(APDS9960_GSTATUS, val) ) { 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; }
}

/**
 * @brief Processes a gesture event and returns best guessed gesture
 *
 * @return Number corresponding to gesture. -1 on error.
 */
int apds9960::readGesture()
{
    uint8_t fifo_level = 0;
    uint8_t bytes_read = 0;
    char 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_ms(FIFO_PAUSE_TIME);
        
        /* Get the contents of the STATUS register. Is data still valid? */
          
        if( !wireReadDataByte(APDS9960_GSTATUS, gstatus ) ) { return ERROR; }

        /* If we have valid data, read in FIFO */
        if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
        
            /* Read the current FIFO level */
            if( !wireReadDataByte(APDS9960_GFLVL, fifo_level) ) { return ERROR; }
            
#if DEBUG
            pc->.printf("FIFO Level: &d\n\r", fifo_level);
#endif

            /* If there's stuff in the FIFO, read it into our data block */ 
            if( fifo_level > 0) {
                bytes_read = wireReadDataBlock(APDS9960_GFIFO_U, (uint8_t*)fifo_data, (fifo_level * 4) );
                
                if( bytes_read == -1 ) { return ERROR; }
                
#if DEBUG
                pc->printf("FIFO Dump: ");
                for ( i = 0; i < (fifo_level * 4); i++ ) {
                   pc->printf("%d ", fifo_data[i]);
                }
                pc->printf("\n\r");
#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
                pc->printf("Up Data: ");
                for ( i = 0; i < gesture_data_.total_gestures; i++ ) {
                    pc->printf("%d ", gesture_data_.u_data[i]);
                }
                pc->printf("\n\r");
#endif

                    /* Filter and process gesture data. Decode near/far state */
                    if( processGestureData() ) {
                        if( decodeGesture() ) {
                            //***TODO: U-Turn Gestures
#if DEBUG
                            pc->printf("%s",gesture_motion_);
#endif
                        }
                    }
                    
                    /* Reset data */
                    gesture_data_.sindex = 0;
                    gesture_data_.total_gestures = 0;
                }
            }
        } else {
    
            /* Determine best guessed gesture and clean up */
            wait_ms(FIFO_PAUSE_TIME);
            decodeGesture();
            motion = gesture_motion_;
#if DEBUG
        pc->printf("END: %s",gesture_motion_);
#endif
            resetGestureParameters();
            return motion;
        }
    }
   // delete fptr;
}

/**
 * Turn the APDS-9960 on
 *
 * @return True if operation successful. False otherwise.
 */
bool apds9960::enablePower()
{
    if( !setMode(POWER, 1) ) { return false; }
    
    return true;
}

/**
 * Turn the APDS-9960 off
 *
 * @return True if operation successful. False otherwise.
 */
bool apds9960::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 apds9960::readAmbientLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( !wireReadDataByte(APDS9960_CDATAL, val_byte) ) { return false; }
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
   
    if( !wireReadDataByte(APDS9960_CDATAH, val_byte) ) { 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 apds9960::readRedLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( !wireReadDataByte(APDS9960_RDATAL, val_byte) ) { return false; }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    if( !wireReadDataByte(APDS9960_RDATAH, val_byte) ) { 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 apds9960::readGreenLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( !wireReadDataByte(APDS9960_GDATAL, val_byte) ) { return false; }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    if( !wireReadDataByte(APDS9960_GDATAH, val_byte) ) { 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 apds9960::readBlueLight(uint16_t &val)
{
    uint8_t val_byte;
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( !wireReadDataByte(APDS9960_BDATAL, val_byte) ) { return false; }
    
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    if( !wireReadDataByte(APDS9960_BDATAH, val_byte) ) { 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 apds9960::readProximity(uint8_t &val)
{
    val = 0;
    
    /* Read value from proximity data register */
    if( !wireReadDataByte(APDS9960_PDATA, val) ) { return false; }
        
    return true;
}

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

/**
 * @brief Resets all the parameters in the gesture data member
 */
void apds9960::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;
}

/**
 * @brief Processes the raw gesture data to determine swipe direction
 *
 * @return True if near or far state seen. False otherwise.
 */
bool apds9960::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
            pc->printf(F("Finding last: "));
            pc->printf(F("U:"));
            pc->printf(gesture_data_.u_data[i]);
            pc->printf(F(" D:"));
            pc->printf(gesture_data_.d_data[i]);
            pc->printf(F(" L:"));
            pc->printf(gesture_data_.l_data[i]);
            pc->printf(F(" R:"));
            pc->printfln(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
    pc->printf(F("Last Values: "));
    pc->printf(F("U:"));
    pc->printf(u_last);
    pc->printf(F(" D:"));
    pc->printf(d_last);
    pc->printf(F(" L:"));
    pc->printf(l_last);
    pc->printf(F(" R:"));
    pc->printf(r_last);
    pc->printf("\n\r");

    pc->printf(F("Ratios: "));
    pc->printf(F("UD Fi: "));
    pc->printf(ud_ratio_first);
    pc->printf(F(" UD La: "));
    pc->printf(ud_ratio_last);
    pc->printf(F(" LR Fi: "));
    pc->printf(lr_ratio_first);
    pc->printf(F(" LR La: "));
    pc->printf(lr_ratio_last);
    pc->printf("\n\r");
#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
    pc->printf("Deltas: ");
    pc->printf("UD: ");
    pc->printf(ud_delta);
    pc->printf(" LR: ");
    pc->printf(lr_delta);
    pc->printf("\n\r");
#endif */

    /* Accumulate the UD and LR delta values */
    gesture_ud_delta_ += ud_delta;
    gesture_lr_delta_ += lr_delta;
    
/* #if DEBUG
    pc->printf("Accumulations: ");
    pc->printf("UD: ");
    pc->printf(gesture_ud_delta_);
    pc->printf(" LR: ");
    pc->printf(gesture_lr_delta_);
    pc->printf("\n\r");
#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_ >= 5 ) {
                gesture_ud_count_ = 0;
                gesture_lr_count_ = 0;
                gesture_ud_delta_ = 0;
                gesture_lr_delta_ = 0;
            }
        }
    }
    
/*
#if DEBUG
      pc->printf("UD_CT: %d\n",gesture_ud_count_);
      pc->printf("LR_CT: %d\n",gesture_lr_count_);
      pc->printf("NEAR_CT: %d\n",gesture_near_count_);
      pc->printf(" FAR_CT: %d\n",gesture_far_count_);
      pc->printf("----------");
#endif
*/
    
    return false;
}

/**
 * @brief Determines swipe direction or near/far state
 *
 * @return True if near/far event. False otherwise.
 */
bool apds9960::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 apds9960::getProxIntLowThresh()
{
    uint8_t val;
    
    /* Read value from PILT register */
    if( !wireReadDataByte(APDS9960_PILT, val) ) { val = 0; }
    
    return val;
}
 
 /**
 * @brief Sets the lower threshold for proximity detection
 *
 * @param[in] threshold the lower proximity threshold
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProxIntLowThresh(uint8_t threshold)
{
    if( !wireWriteDataByte(APDS9960_PILT, threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Returns the high threshold for proximity detection
 *
 * @return high threshold
 */
uint8_t apds9960::getProxIntHighThresh()
{
    uint8_t val;
    
    /* Read value from PIHT register */
    if( !wireReadDataByte(APDS9960_PILT, val) ) { val = 0; }
    
    return val;
}

/**
 * @brief Sets the high threshold for proximity detection
 *
 * @param[in] threshold the high proximity threshold
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProxIntHighThresh(uint8_t threshold)
{
   
    if( !wireWriteDataByte(APDS9960_PIHT, threshold) ) { return false; }
    
    return true;
}

 /**
 * @brief Returns LED drive strength for proximity and ALS
 *
 * Value    LED Current
 *   0        100 mA
 *   1         50 mA
 *   2         25 mA
 *   3         12.5 mA
 *
 * @return the value of the LED drive strength. 0xFF on failure.
 */
uint8_t apds9960::getLEDDrive()
{
    uint8_t val;
    
    /* Read value from CONTROL register */
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return ERROR; }
    
    /* Shift and mask out LED drive bits */
    val = (val >> 6) & 0x03; //0b00000011;
    
    return val;
}
 
 /**
 * @brief Sets the LED drive strength for proximity and ALS
 *
 * Value    LED Current
 *   0        100 mA
 *   1         50 mA
 *   2         25 mA
 *   3         12.5 mA
 *
 * @param[in] drive the value (0-3) for the LED drive strength
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setLEDDrive(uint8_t drive)
{
    uint8_t val;
    
    /* Read value from CONTROL register */
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return false; }

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

/**
 * @brief Returns receiver gain for proximity detection
 *
 * Value    Gain
 *   0       1x
 *   1       2x
 *   2       4x
 *   3       8x
 *
 * @return the value of the proximity gain. 0xFF on failure.
 */
uint8_t apds9960::getProximityGain()
{
    uint8_t val;
    
    /* Read value from CONTROL register */
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return ERROR; }
    
    /* Shift and mask out PDRIVE bits */
    val = (val >> 2) & 0x03; //0b00000011;
    
    return val;
}

/**
 * @brief Sets the receiver gain for proximity detection
 *
 * Value    Gain
 *   0       1x
 *   1       2x
 *   2       4x
 *   3       8x
 *
 * @param[in] drive the value (0-3) for the gain
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProximityGain(uint8_t drive)
{
    uint8_t val;
    
    /* Read value from CONTROL register */
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return false; }
    
    /* Set bits in register to given value */
    drive &=0x03; // 0b00000011
    drive = drive << 2;
    val &= 0xF3; // 0b11110011
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( !wireWriteDataByte(APDS9960_CONTROL, val) ) { return false; }
    return true;
}

/**
 * @brief Returns receiver gain for the ambient light sensor (ALS)
 *
 * Value    Gain
 *   0        1x
 *   1        4x
 *   2       16x
 *   3       64x
 *
 * @return the value of the ALS gain. 0xFF on failure.
 */
uint8_t apds9960::getAmbientLightGain()
{
    uint8_t val;
    
    /* Read value from CONTROL register */
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return ERROR; }
    
    /* Shift and mask out ADRIVE bits */
    val &= 0x03; //0b00000011;
    
    return val;
}

/**
 * @brief Sets the receiver gain for the ambient light sensor (ALS)
 *
 * Value    Gain
 *   0        1x
 *   1        4x
 *   2       16x
 *   3       64x
 *
 * @param[in] drive the value (0-3) for the gain
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setAmbientLightGain(uint8_t drive){

    uint8_t val;
    
    /* Read value from CONTROL register */
   
    if( !wireReadDataByte(APDS9960_CONTROL, val) ) { return false; }

    /* Set bits in register to given value */
    drive &=0x03; // 0b00000011
    drive = drive << 2;
    val &= 0xF3; // 0b11111100
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( !wireWriteDataByte(APDS9960_CONTROL, val) ) { return false; }
    return true;
}

/**
 * @brief Get the current LED boost value
 * 
 * Value  Boost Current
 *   0        100%
 *   1        150%
 *   2        200%
 *   3        300%
 *
 * @return The LED boost value. 0xFF on failure.
 */
uint8_t apds9960::getLEDBoost() {
    uint8_t val;
    
    /* Read value from CONFIG2 register */
    if( !wireReadDataByte(APDS9960_CONFIG2, val) ) { return ERROR; }
    
    /* Shift and mask out LED_BOOST bits */
    val = (val >> 4) & 0x03; //0b00000011;
    
    return val;
}

/**
 * @brief Sets the LED current boost value
 *
 * Value  Boost Current
 *   0        100%
 *   1        150%
 *   2        200%
 *   3        300%
 *
 * @param[in] drive the value (0-3) for current boost (100-300%)
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setLEDBoost(uint8_t boost)
{
    uint8_t val;
    
    /* Read value from CONFIG2 register */
    if( !wireReadDataByte(APDS9960_CONFIG2, val) ) { return false; }
    
    /* Set bits in register to given value */
    boost &= 0x03; //0b00000011;
    boost = boost << 4;
    val &= 0xCF; //0b11001111;
    val |= boost;
    
    /* Write register value back into CONFIG2 register */
    
    if( !wireWriteDataByte(APDS9960_CONFIG2, val) ) { return false; }
    
    return true;
}    

/**
 * @brief Gets proximity gain compensation enable
 *
 * @return 1 if compensation is enabled. 0 if not. 0xFF on error.
 */
uint8_t apds9960::getProxGainCompEnable()
{
    uint8_t val;
    
    /* Read value from CONFIG3 register */
    if( !wireReadDataByte(APDS9960_CONFIG3, val) ) { return ERROR; }
    
    /* Shift and mask out PCMP bits */
    val = (val >> 5) & 0x01; //0b00000001;
    
    return val;
}

/**
 * @brief Sets the proximity gain compensation enable
 *
 * @param[in] enable 1 to enable compensation. 0 to disable compensation.
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProxGainCompEnable(uint8_t enable)
{
    uint8_t val;
    
    /* Read value from CONFIG3 register */
    if( !wireReadDataByte(APDS9960_CONFIG3, val) ) { return false; }
    
    /* Set bits in register to given value */
    enable &= 0x01; //0b00000001;
    enable = enable << 5;
    val &= 0xCF; //0b11011111;
    val |= enable;
    
    /* Write register value back into CONFIG3 register */
    
    if( !wireWriteDataByte(APDS9960_CONFIG3, val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the current mask for enabled/disabled proximity photodiodes
 *
 * 1 = disabled, 0 = enabled
 * Bit    Photodiode
 *  3       UP
 *  2       DOWN
 *  1       LEFT
 *  0       RIGHT
 *
 * @return Current proximity mask for photodiodes. 0xFF on error.
 */
uint8_t apds9960::getProxPhotoMask()
{
    uint8_t val;
    
    /* Read value from CONFIG3 register */
    if( !wireReadDataByte(APDS9960_CONFIG3, val) ) { return ERROR; }
    
    /* Mask out photodiode enable mask bits */
    val &= 0x0F; //0b00001111;
    
    return val;
}

/**
 * @brief Sets the mask for enabling/disabling proximity photodiodes
 *
 * 1 = disabled, 0 = enabled
 * Bit    Photodiode
 *  3       UP
 *  2       DOWN
 *  1       LEFT
 *  0       RIGHT
 *
 * @param[in] mask 4-bit mask value
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProxPhotoMask(uint8_t mask)
{
    uint8_t val;
    
    /* Read value from CONFIG3 register */
    if( !wireReadDataByte(APDS9960_CONFIG3, val) ) { return false; }
    
    /* Set bits in register to given value */
    mask &= 0x0F; //0b00001111;
    val &= 0xF0; //0b11110000;
    val |= mask;
    
    /* Write register value back into CONFIG3 register */
    if( !wireWriteDataByte(APDS9960_CONFIG3, val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the entry proximity threshold for gesture sensing
 *
 * @return Current entry proximity threshold.
 */
uint8_t apds9960::getGestureEnterThresh()
{
    uint8_t val;
    
    /* Read value from GPENTH register */
    if( !wireReadDataByte(APDS9960_GPENTH, val) ) { val = 0; }
    
    return val;
}

/**
 * @brief Sets the entry proximity threshold for gesture sensing
 *
 * @param[in] threshold proximity value needed to start gesture mode
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureEnterThresh(uint8_t threshold)
{
    
    if( !wireWriteDataByte(APDS9960_GPENTH, threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the exit proximity threshold for gesture sensing
 *
 * @return Current exit proximity threshold.
 */
uint8_t apds9960::getGestureExitThresh()
{
    uint8_t val;
    
    /* Read value from GEXTH register */
    if( !wireReadDataByte(APDS9960_GEXTH, val) ) { val = 0; }
    
    return val;
}

/**
 * @brief Sets the exit proximity threshold for gesture sensing
 *
 * @param[in] threshold proximity value needed to end gesture mode
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureExitThresh(uint8_t threshold)
{
    if( !wireWriteDataByte( APDS9960_GEXTH, threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the gain of the photodiode during gesture mode
 *
 * Value    Gain
 *   0       1x
 *   1       2x
 *   2       4x
 *   3       8x
 *
 * @return the current photodiode gain. 0xFF on error.
 */
uint8_t apds9960::getGestureGain()
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return ERROR; }
    
    /* Shift and mask out GGAIN bits */
    val = (val >> 5) & 0x03; //0b00000011;
    
    return val;
}

/**
 * @brief Sets the gain of the photodiode during gesture mode
 *
 * Value    Gain
 *   0       1x
 *   1       2x
 *   2       4x
 *   3       8x
 *
 * @param[in] gain the value for the photodiode gain
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureGain(uint8_t gain)
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return false; }
    
    /* Set bits in register to given value */
    gain &= 0x03; //0b00000011;
    gain = gain << 5;
    val &= 0x9F; //0b10011111;
    val |= gain;
    
    /* Write register value back into GCONF2 register */
    if( !wireWriteDataByte( APDS9960_GCONF2, val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the drive current of the LED during gesture mode
 *
 * Value    LED Current
 *   0        100 mA
 *   1         50 mA
 *   2         25 mA
 *   3         12.5 mA
 *
 * @return the LED drive current value. 0xFF on error.
 */
uint8_t apds9960::getGestureLEDDrive()
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return ERROR; }
    
    /* Shift and mask out GLDRIVE bits */
    val = (val >> 3) & 0x03; //0b00000011;
    
    return val;
}

/**
 * @brief Sets the LED drive current during gesture mode
 *
 * Value    LED Current
 *   0        100 mA
 *   1         50 mA
 *   2         25 mA
 *   3         12.5 mA
 *
 * @param[in] drive the value for the LED drive current
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureLEDDrive(uint8_t drive)
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return false; }
    
    /* Set bits in register to given value */
    drive &= 0x03; //0b00000011;
    drive = drive << 3;
    val &= 0xE7; //0b11100111;
    val |= drive;
    
    /* Write register value back into GCONF2 register */
    if( !wireWriteDataByte( APDS9960_GCONF2, val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the time in low power mode between gesture detections
 *
 * Value    Wait time
 *   0          0 ms
 *   1          2.8 ms
 *   2          5.6 ms
 *   3          8.4 ms
 *   4         14.0 ms
 *   5         22.4 ms
 *   6         30.8 ms
 *   7         39.2 ms
 *
 * @return the current wait time between gestures. 0xFF on error.
 */
uint8_t apds9960::getGestureWaitTime()
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return ERROR; }
    
    /* Mask out GWTIME bits */
    val &= 0x07; //0b00000111;
    
    return val;
}

/**
 * @brief Sets the time in low power mode between gesture detections
 *
 * Value    Wait time
 *   0          0 ms
 *   1          2.8 ms
 *   2          5.6 ms
 *   3          8.4 ms
 *   4         14.0 ms
 *   5         22.4 ms
 *   6         30.8 ms
 *   7         39.2 ms
 *
 * @param[in] the value for the wait time
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureWaitTime(uint8_t time)
{
    uint8_t val;
    
    /* Read value from GCONF2 register */
    if( !wireReadDataByte(APDS9960_GCONF2, val) ) { return false; }
    
    /* Set bits in register to given value */
    time &= 0x07; //0b00000111;
    val &= 0xF8; //0b11111000;
    val |= time;
    
    /* Write register value back into GCONF2 register */
    if( !wireWriteDataByte(APDS9960_GCONF2,val) ) { return false; }

    return true;
}

/**
 * @brief Gets the low threshold for ambient light interrupts
 *
 * @param[out] threshold current low threshold stored on the APDS-9960
 * @return True if operation successful. False otherwise.
 */
bool apds9960::getLightIntLowThreshold(uint16_t &threshold)
{
    uint8_t val_byte;
    threshold = 0;
    
    /* Read value from ambient light low threshold, low byte register */
    if( !wireReadDataByte(APDS9960_AILTL, val_byte) ) { return false; }
    threshold = val_byte;
    
    /* Read value from ambient light low threshold, high byte register */
    if( !wireReadDataByte(APDS9960_AILTH, val_byte) ) { return false; }
    threshold = threshold + ((uint16_t)val_byte << 8);
    
    return true;
}

/**
 * @brief Sets the low threshold for ambient light interrupts
 *
 * @param[in] threshold low threshold value for interrupt to trigger
 * @return True if operation successful. False otherwise.
 */
bool apds9960::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( !wireWriteDataByte(APDS9960_AILTL,val_low) ) { return false; }
    
    /* Write high byte */
    if( !wireWriteDataByte(APDS9960_AILTH,val_high) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the high threshold for ambient light interrupts
 *
 * @param[out] threshold current low threshold stored on the APDS-9960
 * @return True if operation successful. False otherwise.
 */
bool apds9960::getLightIntHighThreshold(uint16_t &threshold)
{
    uint8_t val_byte;
    threshold = 0;
    
    /* Read value from ambient light high threshold, low byte register */
    if( !wireReadDataByte(APDS9960_AIHTL, val_byte) ) { return false; }
    threshold = val_byte;
    
    /* Read value from ambient light high threshold, high byte register */
    if( !wireReadDataByte(APDS9960_AIHTH, val_byte) ) { return false; }
    threshold = threshold + ((uint16_t)val_byte << 8);
    
    return true;
}

/**
 * @brief Sets the high threshold for ambient light interrupts
 *
 * @param[in] threshold high threshold value for interrupt to trigger
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setLightIntHighThreshold(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( !wireWriteDataByte(APDS9960_AIHTL,val_low) ) { return false; }
    
    /* Write high byte */
    if( !wireWriteDataByte(APDS9960_AIHTH,val_high) ) { return false; }
    
    return true;
}

/**
 * @brief Gets the low threshold for proximity interrupts
 *
 * @param[out] threshold current low threshold stored on the APDS-9960
 * @return True if operation successful. False otherwise.
 */
bool apds9960::getProximityIntLowThreshold(uint8_t &threshold)
{
    threshold = 0;
    
    /* Read value from proximity low threshold register */
    if( !wireReadDataByte(APDS9960_PILT, threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Sets the low threshold for proximity interrupts
 *
 * @param[in] threshold low threshold value for interrupt to trigger
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProximityIntLowThreshold(uint8_t threshold)
{
    
    /* Write threshold value to register */
    if( !wireWriteDataByte(APDS9960_PILT,threshold) ) { return false; }
    
    return true;
}
    
/**
 * @brief Gets the high threshold for proximity interrupts
 *
 * @param[out] threshold current low threshold stored on the APDS-9960
 * @return True if operation successful. False otherwise.
 */
bool apds9960::getProximityIntHighThreshold(uint8_t &threshold)
{
    threshold = 0;
    
    /* Read value from proximity low threshold register */
    if( !wireReadDataByte(APDS9960_PIHT, threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Sets the high threshold for proximity interrupts
 *
 * @param[in] threshold high threshold value for interrupt to trigger
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProximityIntHighThreshold(uint8_t threshold)
{
    
    /* Write threshold value to register */
    if( !wireWriteDataByte(APDS9960_PIHT,threshold) ) { return false; }
    
    return true;
}

/**
 * @brief Gets if ambient light interrupts are enabled or not
 *
 * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
 */
uint8_t apds9960::getAmbientLightIntEnable()
{
    uint8_t val;
    
    /* Read value from ENABLE register */
    if( !wireReadDataByte(APDS9960_ENABLE, val) ) { return ERROR; }
    
    /* Shift and mask out AIEN bit */
    val = (val >> 4) & 0x01; //0b00000001;
    
    return val;
}

/**
 * @brief Turns ambient light interrupts on or off
 *
 * @param[in] enable 1 to enable interrupts, 0 to turn them off
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setAmbientLightIntEnable(uint8_t enable)
{
    uint8_t val;
    
    /* Read value from ENABLE register */
    if( !wireReadDataByte(APDS9960_ENABLE, val) ) { return false; }
    
    /* Set bits in register to given value */
    enable &= 0x01; //0b00000001;
    enable = enable << 4;
    val &= 0xEF; //0b11101111;
    val |= enable;
    
    /* Write register value back into ENABLE register */
    if( !wireWriteDataByte(APDS9960_ENABLE,val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets if proximity interrupts are enabled or not
 *
 * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
 */
uint8_t apds9960::getProximityIntEnable()
{
    uint8_t val;
    
    /* Read value from ENABLE register */
    if( !wireReadDataByte(APDS9960_ENABLE, val) ) { return ERROR; }
    
    /* Shift and mask out PIEN bit */
    val = (val >> 5) & 0x01; //0b00000001;
    
    return val;
}

/**
 * @brief Turns proximity interrupts on or off
 *
 * @param[in] enable 1 to enable interrupts, 0 to turn them off
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setProximityIntEnable(uint8_t enable)
{
    uint8_t val;
    
    /* Read value from ENABLE register */
    if( !wireReadDataByte(APDS9960_ENABLE, val) ) { return false; }
    
    /* Set bits in register to given value */
    enable &= 0x01;//0b00000001;
    enable = enable << 5;
    val &= 0xDF;//0b11011111;
    val |= enable;
    
    /* Write register value back into ENABLE register */
    if( !wireWriteDataByte(APDS9960_ENABLE,val) ) { return false; }
    
    return true;
}

/**
 * @brief Gets if gesture interrupts are enabled or not
 *
 * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
 */
uint8_t apds9960::getGestureIntEnable()
{
    uint8_t val;
    
    /* Read value from GCONF4 register */
    if( !wireReadDataByte(APDS9960_GCONF4, val) ) { return ERROR; }
    
    /* Shift and mask out GIEN bit */
    val = (val >> 1) & 0x01; //0b00000001;
    
    return val;
}

/**
 * @brief Turns gesture-related interrupts on or off
 *
 * @param[in] enable 1 to enable interrupts, 0 to turn them off
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureIntEnable(uint8_t enable)
{
    uint8_t val;
    
    /* Read value from GCONF4 register */
    if( !wireReadDataByte(APDS9960_GCONF4, val) ) { return false; }
    
    /* Set bits in register to given value */
    enable &= 0x01; //0b00000001;
    enable = enable << 1;
    val &= 0xFD; //0b11111101;
    val |= enable;
    
    /* Write register value back into GCONF4 register */
    if( !wireWriteDataByte(APDS9960_GCONF4,val) ) { return false; }
    
    return true;
}

/**
 * @brief Clears the ambient light interrupt
 *
 * @return True if operation completed successfully. False otherwise.
 */
bool apds9960::clearAmbientLightInt()
{
    uint8_t throwaway;
    if( !wireReadDataByte(APDS9960_AICLEAR, throwaway) ) { return false; }
    
    return true;
}

/**
 * @brief Clears the proximity interrupt
 *
 * @return True if operation completed successfully. False otherwise.
 */
bool apds9960::clearProximityInt()
{
    uint8_t throwaway;
    if( !wireReadDataByte(APDS9960_PICLEAR, throwaway) ) { return false; }
    
    return true;
}

/**
 * @brief Tells if the gesture state machine is currently running
 *
 * @return 1 if gesture state machine is running, 0 if not. 0xFF on error.
 */
uint8_t apds9960::getGestureMode()
{
    uint8_t val;
    
    /* Read value from GCONF4 register */
    if( !wireReadDataByte(APDS9960_GCONF4, val) ) { return ERROR; }
    
    /* Mask out GMODE bit */
    val &= 0x01; //0b00000001;
    
    return val;
}

/**
 * @brief Tells the state machine to either enter or exit gesture state machine
 *
 * @param[in] mode 1 to enter gesture state machine, 0 to exit.
 * @return True if operation successful. False otherwise.
 */
bool apds9960::setGestureMode(uint8_t mode)
{
    uint8_t val;
    
    /* Read value from GCONF4 register */
    if( !wireReadDataByte(APDS9960_GCONF4, val) ) { return false; }
    
    /* Set bits in register to given value */
    mode &= 0x01; //0b00000001;
    val &= 0xFE; //0b11111110;
    val |= mode;
    
    /* Write register value back into GCONF4 register */
    if( !wireWriteDataByte(APDS9960_GCONF4,val) ) { return false; }
    
    return true;
}

/*******************************************************************************
 * Raw I2C Reads and Writes
 ******************************************************************************/

bool apds9960::wireWriteDataByte(char reg, uint8_t val)
{   
    int ret;
    char cmd[2] = {reg, val};
    ret=i2c.write(APDS9960_I2C_ADDR<<1, cmd, 2);  
    if(ret==1) return false;
    else return true;
}

bool apds9960::wireReadDataByte(char reg, uint8_t &val)
{   
    if(i2c.write(APDS9960_I2C_ADDR<<1, &reg, 1, true)){ return false; }
    if(i2c.read(APDS9960_I2C_ADDR<<1, (char*)&val, 1)){ return ERROR; }
    
    return true;
}

int apds9960::wireReadDataBlock(char reg, uint8_t *data, unsigned int len)
{
    /* Indicate which register we want to read from */
    if(i2c.write(APDS9960_I2C_ADDR<<1, &reg, 1, true)){ return -1; }
    
    /* Read block data */     
    if(i2c.read(APDS9960_I2C_ADDR<<1, (char*)data, len)){ return -1; }
    
    return 1;
}