Diff: glibr.cpp

Revision:

0:1aac12a5f1e0

Child:

2:ba051af6731a

diff -r 000000000000 -r 1aac12a5f1e0 glibr.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/glibr.cpp	Thu Mar 12 20:39:10 2015 +0000
@@ -0,0 +1,2198 @@
+#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(){
+       
+} 
+
+ bool  glibr::ginit(){
+    uint8_t 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 true;
+    
+}
+
+//#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;
+    //char fifo_data[128];
+    char fifo_data[128];
+   char *fptr;
+   fptr= fifo_data;
+    
+    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, 
+                                                fptr, 
+                                                (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;
+        }
+    }
+   // delete fptr;
+}
+/**
+ * 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_ >= 5 ) {
+                gesture_ud_count_ = 0;
+                gesture_lr_count_ = 0;
+                gesture_ud_delta_ = 0;
+                gesture_lr_delta_ = 0;
+            }
+        }
+    }
+    
+// #if DEBUG
+  /*    printf("UD_CT: %d\n",gesture_ud_count_);
+      printf("LR_CT: %d\n",gesture_lr_count_);
+      printf("NEAR_CT: %d\n",gesture_near_count_);
+      printf(" FAR_CT: %d\n",gesture_far_count_);
+      printf("----------"); */
+//#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;
+}
+ 
+ /**
+ * @brief Sets the lower threshold for proximity detection
+ *
+ * @param[in] threshold the lower proximity threshold
+ * @return True if operation successful. False otherwise.
+ */
+ bool glibr::setProxIntLowThresh(uint8_t threshold)
+{
+    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PILT, threshold) ) {
+        return false;
+    }
+    
+    return true;
+}
+
+/**
+ * @brief Returns the high threshold for proximity detection
+ *
+ * @return high threshold
+ */
+uint8_t glibr::getProxIntHighThresh()
+{
+    uint8_t val;
+    
+    /* Read value from PIHT register */
+    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PILT);
+    if( val==ERROR ) {
+        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 glibr::setProxIntHighThresh(uint8_t threshold)
+{
+   
+    if( I2CwriteByte(APDS9960_I2C_ADDR,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 glibr::getLEDDrive()
+{
+    uint8_t val;
+    
+    /* Read value from CONTROL register */
+    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+    if(  val == ERROR ){//!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 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 &= 0b00000011
+    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;
+}
+
+/**
+ * @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 glibr::getProximityGain()
+{
+    uint8_t val;
+    
+    /* Read value from CONTROL register */
+    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+    if( val == ERROR){//!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 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 &= 0b11110011
+    val &= 0xF3;
+    val |= drive;
+    
+    /* Write register value back into CONTROL register */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,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 glibr::getAmbientLightGain()
+{
+    uint8_t val;
+    
+    /* Read value from CONTROL register */
+    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+    if( val == ERROR){//!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 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 &=0b11111100
+    val &= 0xF3;
+    val |= drive;
+    
+    /* Write register value back into CONTROL register */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,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 glibr::getLEDBoost() {
+    uint8_t val;
+    
+    /* Read value from CONFIG2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG2);
+    if( val == ERROR){//!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 glibr::setLEDBoost(uint8_t boost)
+{
+    uint8_t val;
+    
+    /* Read value from CONFIG2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG2);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG2, val)){//!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 glibr::getProxGainCompEnable()
+{
+    uint8_t val;
+    
+    /* Read value from CONFIG3 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3); 
+    if( val == ERROR){//!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 glibr::setProxGainCompEnable(uint8_t enable)
+{
+    uint8_t val;
+    
+    /* Read value from CONFIG3 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG3, val)){//!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 glibr::getProxPhotoMask()
+{
+    uint8_t val;
+    
+    /* Read value from CONFIG3 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+    if( val == ERROR){//!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 glibr::setProxPhotoMask(uint8_t mask)
+{
+    uint8_t val;
+    
+    /* Read value from CONFIG3 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+    if( val == ERROR){//!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 */
+    I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3, val);
+    if( val == ERROR){//!wireWriteDataByte(APDS9960_CONFIG3, val) ) {
+        return false;
+    }
+    
+    return true;
+}
+
+/**
+ * @brief Gets the entry proximity threshold for gesture sensing
+ *
+ * @return Current entry proximity threshold.
+ */
+uint8_t glibr::getGestureEnterThresh()
+{
+    uint8_t val;
+    
+    /* Read value from GPENTH register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GPENTH);
+    if( val == ERROR){//!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 glibr::setGestureEnterThresh(uint8_t threshold)
+{
+    
+    if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GPENTH, threshold)){;//!wireWriteDataByte(APDS9960_GPENTH, threshold) ) {
+        return false;
+    }
+    
+    return true;
+}
+
+/**
+ * @brief Gets the exit proximity threshold for gesture sensing
+ *
+ * @return Current exit proximity threshold.
+ */
+uint8_t glibr::getGestureExitThresh()
+{
+    uint8_t val;
+    
+    /* Read value from GEXTH register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GEXTH);
+    if( val == ERROR){//!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 glibr::setGestureExitThresh(uint8_t threshold)
+{
+    if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GEXTH, threshold)){//!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 glibr::getGestureGain()
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!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 glibr::setGestureGain(uint8_t gain)
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GCONF2, val)){//!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 glibr::getGestureLEDDrive()
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!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 glibr::setGestureLEDDrive(uint8_t drive)
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GCONF2, val)){//!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 glibr::getGestureWaitTime()
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+        return ERROR;
+    }
+    
+    /* Mask out GWTIME bits */
+    val &= 0x07;//0b00000111;
+    
+    return val;
+}
+
+/*
+*
+*
+*
+*LEFT OFF HERE AT 3:47PM ON 3/6/15
+*
+*
+*
+*
+*/
+
+
+/**
+ * @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 glibr::setGestureWaitTime(uint8_t time)
+{
+    uint8_t val;
+    
+    /* Read value from GCONF2 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+    if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+        return false;
+    }
+    /* 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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF2,val)){//!wireWriteDataByte(APDS9960_GCONF2, val) ) {
+        return false;
+    }
+    /*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 glibr::getLightIntLowThreshold(uint16_t &threshold)
+{
+    uint8_t val_byte;
+    threshold = 0;
+    
+    /* Read value from ambient light low threshold, low byte register */
+    val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AILTL);
+    if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AILTL, val_byte) ) {
+        return false;
+    }
+    threshold = val_byte;
+    
+    /* Read value from ambient light low threshold, high byte register */
+    val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AILTH);
+    if( val_byte == ERROR){//!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 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)){//!wireWriteDataByte(APDS9960_AILTL, val_low) ) {
+        return false;
+    }
+    
+    /* Write high byte */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AILTH,val_high)){//!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 glibr::getLightIntHighThreshold(uint16_t &threshold)
+{
+    uint8_t val_byte;
+    threshold = 0;
+    
+    /* Read value from ambient light high threshold, low byte register */
+    val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AIHTL);
+    if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AIHTL, val_byte) ) {
+        return false;
+    }
+    threshold = val_byte;
+    
+    /* Read value from ambient light high threshold, high byte register */
+    val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AIHTH);
+    if( val_byte == ERROR){//!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 glibr::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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AIHTL,val_low)){//!wireWriteDataByte(APDS9960_AIHTL, val_low) ) {
+        return false;
+    }
+    
+    /* Write high byte */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AIHTH,val_high)){//!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 glibr::getProximityIntLowThreshold(uint8_t &threshold)
+{
+    threshold = 0;
+    
+    /* Read value from proximity low threshold register */
+    threshold = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PILT);
+    if( threshold == ERROR){//!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 glibr::setProximityIntLowThreshold(uint8_t threshold)
+{
+    
+    /* Write threshold value to register */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PILT,threshold)){//!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 glibr::getProximityIntHighThreshold(uint8_t &threshold)
+{
+    threshold = 0;
+    
+    /* Read value from proximity low threshold register */
+    threshold = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PIHT);
+    if( threshold == ERROR){//!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 glibr::setProximityIntHighThreshold(uint8_t threshold)
+{
+    
+    /* Write threshold value to register */
+    if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PIHT,threshold)){//!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 glibr::getAmbientLightIntEnable()
+{
+    uint8_t val;
+    
+    /* Read value from ENABLE register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+    if( val == ERROR){//!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 glibr::setAmbientLightIntEnable(uint8_t enable)
+{
+    uint8_t val;
+    
+    /* Read value from ENABLE register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE,val)){//!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 glibr::getProximityIntEnable()
+{
+    uint8_t val;
+    
+    /* Read value from ENABLE register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+    if( val == ERROR){//!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 glibr::setProximityIntEnable(uint8_t enable)
+{
+    uint8_t val;
+    
+    /* Read value from ENABLE register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE,val)){//!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 glibr::getGestureIntEnable()
+{
+    uint8_t val;
+    
+    /* Read value from GCONF4 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+    if( val == ERROR){//!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 glibr::setGestureIntEnable(uint8_t enable)
+{
+    uint8_t val;
+    
+    /* Read value from GCONF4 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF4,val)){//!wireWriteDataByte(APDS9960_GCONF4, val) ) {
+        return false;
+    }
+    
+    return true;
+}
+
+/**
+ * @brief Clears the ambient light interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool glibr::clearAmbientLightInt()
+{
+    uint8_t throwaway;
+    throwaway = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AICLEAR);
+    if( throwaway == ERROR){//!wireReadDataByte(APDS9960_AICLEAR, throwaway) ) {
+        return false;
+    }
+    
+    return true;
+}
+
+/**
+ * @brief Clears the proximity interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool glibr::clearProximityInt()
+{
+    uint8_t throwaway;
+    throwaway = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PICLEAR);
+    if( throwaway == ERROR){//!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 glibr::getGestureMode()
+{
+    uint8_t val;
+    
+    /* Read value from GCONF4 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+    if( val == ERROR){//!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 glibr::setGestureMode(uint8_t mode)
+{
+    uint8_t val;
+    
+    /* Read value from GCONF4 register */
+    val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+    if( val == ERROR){//!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( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF4,val)){//!wireWriteDataByte(APDS9960_GCONF4, val) ) {
+        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
+    } 
+    if(i2c.read(address<<1, &data, 1)){    /////CHANGED THIS NEED TO TEST.
+        return ERROR;
+    }
+        
+    
+    //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, char *data, 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, data, len)){
+        return -1;
+    }
+    
+    return 1;
+    //Wire.requestFrom(APDS9960_I2C_ADDR, len);
+    /*while (Wire.available()) {
+        if (i >= len) {
+            return -1;
+        }
+        val[i] = Wire.read();
+        i++;
+    }*/
+}
\ No newline at end of file