File content as of revision 0:b8221deeaa87:

/**
 * @author Abraham Marsen & Allison Ashlock
 * Georgia Institute of Technology
 * ECE 4180 Embedded Systems Design
 * Professor Hamblen
 * Spring 2015
 *
 * @section LICENSE
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <amarsen3@gmail.com> wrote this file. As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.
 * ----------------------------------------------------------------------------
 *
 *
 * @section DESCRIPTION
 *
 * Avago Digital Proximity, Ambient Light, RGB and Gesture Sensor APDS-9960
 *
 * Datasheet, specs, and information:
 *
 * https://www.sparkfun.com/products/12787
 */

#include "APDS9960.h"

APDS9960::APDS9960(PinName sda, PinName scl): i2c_(sda, scl)
{
    i2c_.frequency(400000);
    //i2c_ = I2C(sda, scl);
    //400KHz, as specified by the datasheet.

     gest_ud_delta_ =    0;
     gest_lr_delta_ =    0;

     gest_ud_count_ =    0;
     gest_lr_count_ =    0;

     gest_near_count_ =  0;
     gest_far_count_ =   0;

     gest_state_ =       0;
     gest_motion_ =      DIR_NA;
}
APDS9960::~APDS9960()
{
}
bool APDS9960::init()
{



    // Initialize I2C
    // Read ID register and check against known values for APDS-9960
    uint8_t id = ReadDataByte(APDS9960_ID);
    //printf("id:%u\n\r",id);

    if( !(id == APDS9960_ID_1 || id == APDS9960_ID_2) ) {

        return false;
    }
    //printf("APDS9960_ID_1:%u\n\rAPDS9960_ID_2:%u\n\r",APDS9960_ID_1,APDS9960_ID_2);
    /* Set ENABLE register to 0 (disable all features) */
    if( !setMode(ALL, OFF) ) {
        return false;
    }
    //printf("Set Mode: All off.\n\r");

    /* Set default values for ambient light and proximity registers */
    if( WriteDataByte(APDS9960_ATIME, DEFAULT_ATIME) ) {
        return false;
    }
    //printf("DEFAULTT_ATIME:%u\n\r",DEFAULT_ATIME);
    if( WriteDataByte(APDS9960_WTIME, DEFAULT_WTIME) ) {
        return false;
    }
    //printf("DEFAULTT_WTIME:%u\n\r",DEFAULT_WTIME);

    if( WriteDataByte(APDS9960_PPULSE, DEFAULT_PROX_PPULSE) ) {
        return false;
    }
    //printf("DEFAULT_PROX_PPULSE:%u\n\r",DEFAULT_PROX_PPULSE);


    if( WriteDataByte(APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR) ) {
        return false;
    }
    //printf("DEFAULT_POFFSET_UR:%u\n\r",DEFAULT_POFFSET_UR);

    if( WriteDataByte(APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL) ) {
        return false;
    }
    //printf("DEFAULT_POFFSET_DL:%u\n\r",DEFAULT_POFFSET_DL);

    if( WriteDataByte(APDS9960_CONFIG1, DEFAULT_CONFIG1) ) {
        return false;
    }
     //printf("DEFAULT_CONFIG1:%u\n\r",DEFAULT_CONFIG1);
if( !setLEDDrive(DEFAULT_LDRIVE) ) {
return false;
}
//printf("led drive set\n\r");
if( !setProxGain(DEFAULT_PGAIN) ) {
return false;
}
//printf("proxgain set\n\r");
if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
return false;
}
//printf("ambient light gain set\r\n");
if( !setProxIntLowThresh(DEFAULT_PILT) ) {
return false;
}
//printf("proxintlowthresh set\n\r");
if( !setProxIntHighThresh(DEFAULT_PIHT) ) {
return false;
}
//printf("proxinthigh thresh set\r\n");
if( !setLightIntLowThreshold(DEFAULT_AILT) ) {
return false;
}
//printf("light int low thresh set\r\n");
if( !setLightIntHighThreshold(DEFAULT_AIHT) ) {
return false;
}
//printf("light int hight thresh set\n\r");
//printf("All set\n\r");

    if( WriteDataByte(APDS9960_CONFIG2, DEFAULT_CONFIG2) ) {
        return false;
    }

//printf("DEFAULT_CONFIG2:%u\n\r",DEFAULT_CONFIG2);
    if( WriteDataByte(APDS9960_CONFIG3, DEFAULT_CONFIG3) ) {
        return false;
    }
//printf("DEFAULT_CONFIG3:%u\n\r",DEFAULT_CONFIG3);
    /* Set default values for gesture sense registers */
    if( !setGestEnterThresh(DEFAULT_GPENTH) ) {
        return false;
    }

//printf("Set Gesture Enter Thershold\n\r");
    if( !setGestExitThresh(DEFAULT_GEXTH) ) {
        return false;
    }

//printf("Set Gesture Exit Thershold\n\r");
    if( WriteDataByte(APDS9960_GCONF1, DEFAULT_GCONF1) ) {
        return false;
    }

//printf("DEFAULT_CONFIG1:%u\n\r",DEFAULT_CONFIG1);
    if( !setGestGain(DEFAULT_GGAIN) ) {
        return false;
    }
//printf("Set Gest Gain\n\r");

    if( !setGestLEDDrive(DEFAULT_GLDRIVE) ) {
        return false;
    }

//printf("Set Gest LED Drive\n\r");
    if( !setGestWaitTime(DEFAULT_GWTIME) ) {
        return false;
    }
//printf("set gest wait time\n\r");

    if( WriteDataByte(APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) {
        return false;
    }


    if( WriteDataByte(APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) {
        return false;
    }


    if( WriteDataByte(APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) {
        return false;
    }


    if( WriteDataByte(APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) {
        return false;
    }


    if( WriteDataByte(APDS9960_GPULSE, DEFAULT_GPULSE) ) {
        return false;
    }


    if( WriteDataByte(APDS9960_GCONF3, DEFAULT_GCONF3) ) {
        return false;
    }


    if( !setGestIntEnable(DEFAULT_GIEN) ) {
        return false;
    }

    return true;
};



uint8_t APDS9960::getMode()
{
    uint8_t enable_value= ReadDataByte(APDS9960_ENABLE);

    /* Read current ENABLE register */
    if( enable_value == ERROR) {
        return ERROR;
    }
   // //printf("enable_value:%u\r\n", enable_value);

    return enable_value;
}

bool APDS9960::setMode(uint8_t mode, uint8_t enable)
{
    uint8_t reg_val;
    /* Read current ENABLE register */
    reg_val = getMode();
    //printf("reg_val:%u\r\n", reg_val);
    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( WriteDataByte(APDS9960_ENABLE, reg_val) ) {
        return false;
    }
    return true;
}

bool APDS9960::enPower()
{
    if( !setMode(POWER, 1) ) {
        return false;
    }
    return true;
}

bool APDS9960::disPower()
{
    if( !setMode(POWER, 0) ) {
        return false;
    }
    return true;
}

bool APDS9960::enLightSens(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( !enPower() ){
        return false;
    }
    if( !setMode(AMBIENT_LIGHT, 1) ) {
        return false;
    }
    
    return true;

}

bool APDS9960::disLightSens()
{
    if( !setAmbientLightIntEnable(0) ) {
        return false;
    }
    if( !setMode(AMBIENT_LIGHT, 0) ) {
        return false;
    }
    
    return true;
}

bool APDS9960::enProxSens(bool interrupts )
{
     /* Set default gain, LED, interrupts, enable power, and enable sensor */
    if( !setProxGain(DEFAULT_PGAIN) ) {
        return false;
    }
    if( !setLEDDrive(DEFAULT_LDRIVE) ) {
        return false;
    }
    if( interrupts ) {
        if( !setProxIntEnable(1) ) {
            return false;
        }
    } else {
        if( !setProxIntEnable(0) ) {
            return false;
        }
    }
    if( !enPower() ){
        return false;
    }
    if( !setMode(PROXIMITY, 1) ) {
        return false;
    }
    
    return true;
}

bool APDS9960::disProxSens(){
    if( !setProxIntEnable(0) ) {
        return false;
    }
    if( !setMode(PROXIMITY, 0) ) {
        return false;
    }
 
    return true;
}

bool APDS9960::enGestSens(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
    */
    rstGestParameters();
    if( WriteDataByte(APDS9960_WTIME, 0xFF) ) {
        return false;
    }
    if( WriteDataByte(APDS9960_PPULSE, DEFAULT_GEST_PPULSE) ) {
        return false;
    }
    if( !setLEDBoost(LED_BOOST_300) ) {
        return false;
    }
    if( interrupts ) {
        if( !setGestIntEnable(1) ) {
            return false;
        }
    } else {
        if( !setGestIntEnable(0) ) {
            return false;
        }
    }
    if( !setGestMode(1) ) {
        return false;
    }
    if( !enPower() ) {
        return false;
    }
    if( !setMode(WAIT, 1) ) {
        return false;
    }
    if( !setMode(PROXIMITY, 1) ) {
        return false;
    }
    if( !setMode(GESTURE, 1) ) {
        return false;
    }
    //printf("gestures enabled");
    return true;
}

bool APDS9960::disGestSens()
{
    rstGestParameters();
    if( !setGestIntEnable(0) ) {
        return false;
    }
    if( !setGestMode(0) ) {
        return false;
    }
    if( !setMode(GESTURE, 0) ) {
        return false;
    }
    return true;
}

uint8_t APDS9960::getLEDDrive()
{
     uint8_t val = ReadDataByte(APDS9960_CONTROL);
    
    /* Read value from CONTROL register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out LED drive bits */
    val = (val >> 6) & 0x03;
    
    return val;    
}

bool APDS9960::setLEDDrive(uint8_t drive)
{
     uint8_t val = ReadDataByte(APDS9960_CONTROL);
    
    /* Read value from CONTROL register */
    if( val == ERROR) {
        return false;
    }
    
    /* Set bits in register to given value */
    drive &= 0x03;
    drive = drive << 6;
    val &= 0x3F;
    val |= drive;
    
    /* Write register value back into CONTROL register */
    if( WriteDataByte(APDS9960_CONTROL, val) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getGestLEDDrive()
{
    uint8_t val=ReadDataByte(APDS9960_GCONF2);
    /* Read valeue from GCONF2 register */
    if( val == ERROR) {
        return ERROR;
    }
    /* Shift and mask out GLDRIVE bits */
    val = (val >> 3) & 0x03;
    return val;
}

bool APDS9960::setGestLEDDrive(uint8_t drive)
{
    uint8_t val =ReadDataByte(APDS9960_GCONF2);
    
    /* Read value from GCONF2 register */
    
    if( val == ERROR){
        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( WriteDataByte(APDS9960_GCONF2, val)){
        return false;
    }
    
    return true;
}

/* Gain control */
uint8_t APDS9960::getAmbientLightGain()
{   
     uint8_t val = ReadDataByte(APDS9960_CONTROL);
    
    /* Read value from CONTROL register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out ADRIVE bits */
    val &= 0x03;
    
    return val;
}

bool APDS9960::setAmbientLightGain(uint8_t gain)
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    
    /* Read value from CONTROL register */
    
    if( val == ERROR){//!wireReadDataByte(APDS9960_CONTROL, val) ) {
        return ERROR;
    }
    
    /* Set bits in register to given value */
    gain &= 0x03;
    gain = gain << 2;
    val &= 0xF3;
    val |= gain;
    
    /*Write register value back into CONTROL register*/
    if(WriteDataByte(APDS9960_CONTROL, val)){
        return false;
        }
    return true;
}

uint8_t APDS9960::getProxGain()
{
     uint8_t val = ReadDataByte(APDS9960_CONTROL);
    
    /* Read value from CONTROL register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out PDRIVE bits */
    val = (val >> 2) & 0x03;
    
    return val;
}

bool APDS9960::setProxGain(uint8_t gain)
{
    uint8_t val = ReadDataByte(APDS9960_CONTROL);
    
    /* Read value from CONTROL register */
    if( val == ERROR ) {
        return false;
    }
    
    /* Set bits in register to given value */
    gain &= 0x03;
    gain = gain << 2;
    val &= 0xF3;
    val |= gain;
    
    /* Write register value back into CONTROL register */
    if( WriteDataByte(APDS9960_CONTROL, val) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getGestGain()
{
    uint8_t val = ReadDataByte(APDS9960_GCONF2);
    /* Read value from GCONF2 register */
    if( val == ERROR ) {
        return ERROR;
    }
    /* Shift and mask out GGAIN bits */
    val = (val >> 5) & 0x03;
    return val;
}

bool APDS9960::setGestGain(uint8_t gain)
{
    uint8_t val = ReadDataByte(APDS9960_GCONF2);
    /* Read value from GCONF2 register */
    if( val == ERROR ) {
        //printf("I'm over Here\r\n");
        return false;
    }
    /* Set bits in register to given value */
    gain &= 0x03;
    gain = gain << 5;
    val &= 0x9F;
    val |= gain;
    /* Write register value back into GCONF2 register */
    if( WriteDataByte(APDS9960_GCONF2, val) ) {
        //printf("Nope I'm over here!/r/n");
        return false;
        
    }
    return true;
}

/* Get and set light interrupt thresholds */
bool APDS9960::getLightIntLowThreshold(uint16_t &threshold)
{
    uint8_t val_byte = ReadDataByte(APDS9960_AILTL);
    threshold = 0;
    
    /* Read value from ambient light low threshold, low byte register */
    if( val_byte == ERROR) {
        return false;
    }
    threshold = val_byte;
    
    /* Read value from ambient light low threshold, high byte register */
    val_byte = ReadDataByte(APDS9960_AILTH);
    if( val_byte == ERROR ) {
        return false;
    }
    threshold = threshold + ((uint16_t)val_byte << 8);
    
    return true;    
}

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( WriteDataByte(APDS9960_AILTL, val_low) ) {
        return false;
    }
    
    /* Write high byte */
    if( WriteDataByte(APDS9960_AILTH, val_high) ) {
        return false;
    }
    
    return true;
}

bool APDS9960::getLightIntHighThreshold(uint16_t &threshold)
{
     uint8_t val_byte = ReadDataByte(APDS9960_AIHTL);
    threshold = 0;
    
    /* Read value from ambient light high threshold, low byte register */
    if( val_byte == ERROR ) {
        return false;
    }
    threshold = val_byte;
    
    /* Read value from ambient light high threshold, high byte register */
    val_byte = ReadDataByte(APDS9960_AIHTH);
    if( val_byte == ERROR ) {
        return false;
    }
    threshold = threshold + ((uint16_t)val_byte << 8);
    
    return true;
}

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( WriteDataByte(APDS9960_AIHTL, val_low) ) {
        //printf("fail here\r\n");
        return false;
    }
    
    /* Write high byte */
    if( WriteDataByte(APDS9960_AIHTH, val_high) ) {
        //printf("fail over here\r\n");
        return false;
    }
    
    return true;
}

/* Get and set proximity interrupt thresholds */
bool APDS9960::getProxIntLowThreshold(uint8_t &threshold)
{
    
    
    /* Read value from proximity low threshold register */
    threshold = ReadDataByte(APDS9960_PILT);
    if( threshold == ERROR ) {
        threshold = 0;
        return false;
    }
    
    return true;
}

bool APDS9960::setProxIntLowThreshold(uint8_t threshold)
{
    /* Write threshold value to register */
    if( WriteDataByte(APDS9960_PILT, threshold) ) {
        return false;
    }
    
    return true;
}

bool APDS9960::getProxIntHighThreshold(uint8_t &threshold)
{
    threshold = 0;
    
    /* Read value from proximity low threshold register */
    threshold = ReadDataByte(APDS9960_PIHT);
    if( threshold == ERROR ) {
        threshold = 0;
        return false;
    }
    
    return true;
}

bool APDS9960::setProxIntHighThreshold(uint8_t threshold)
{
     /* Write threshold value to register */
    if( WriteDataByte(APDS9960_PIHT, threshold) ) {
        return false;
    }
    
    return true;
}

/* Get and set interrupt enables */
uint8_t APDS9960::getAmbientLightIntEnable()
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    
    /* Read value from ENABLE register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out AIEN bit */
    val = (val >> 4) & 0x01;
    
    return val;
}

bool APDS9960::setAmbientLightIntEnable(uint8_t enable)
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    
    /* Read value from ENABLE register */
    if( val == ERROR ) {
        return false;
    }
    
    /* Set bits in register to given value */
    enable &= 0x01;
    enable = enable << 4;
    val &= 0xEF;
    val |= enable;
    
    /* Write register value back into ENABLE register */
    if( WriteDataByte(APDS9960_ENABLE, val) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getProxIntEnable()
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    
    /* Read value from ENABLE register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out PIEN bit */
    val = (val >> 5) & 0x01;
    
    return val;
}

bool APDS9960::setProxIntEnable(uint8_t enable)
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    
    /* Read value from ENABLE register */
    if( val == ERROR ) {
        return false;
    }
    
    /* Set bits in register to given value */
    enable &= 0x01;
    enable = enable << 5;
    val &= 0xDF;
    val |= enable;
    
    /* Write register value back into ENABLE register */
    if( WriteDataByte(APDS9960_ENABLE, val) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getGestIntEnable()
{
    uint8_t val = ReadDataByte(APDS9960_ENABLE);
    /* Read value from GCONF4 register */
    if( val ==ERROR ) {
        return ERROR;
    }
    /* Shift and mask out GIEN bit */
    val = (val >> 1) & 0x01;
    return val;
}

bool APDS9960::setGestIntEnable(uint8_t enable)
{
    uint8_t val = ReadDataByte(APDS9960_GCONF4);
    /* Read value from GCONF4 register */
    if( val == ERROR ) {
        return false;
    }
    /* Set bits in register to given value */
    enable &= 0x01;
    enable = enable << 1;
    val &= 0xFD;
    val |= enable;
    /* Write register value back into GCONF4 register */
    if( WriteDataByte(APDS9960_GCONF4, val) ) {
        return false;
    }
    return true;
}

/* Clear interrupts */
bool APDS9960::clrAmbientLightInt()
{
    uint8_t throwaway = ReadDataByte(APDS9960_AICLEAR);
    if( throwaway == ERROR ) {
        return false;
    }
    
    return true;
}

bool APDS9960::clrProxInt()
{
     uint8_t throwaway = ReadDataByte(APDS9960_PICLEAR);
    if( throwaway == ERROR ) {
        return false;
    }
    
    return true;
}

// Ambient light methods
bool APDS9960::readAmbientLight(uint16_t &val)
{
    uint8_t val_byte = ReadDataByte(APDS9960_CDATAL);
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( val_byte == ERROR ) {
        return false;
    }
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    val_byte = ReadDataByte(APDS9960_CDATAH);
    if( val_byte == ERROR ) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

bool APDS9960::readRedLight(uint16_t &val)
{
    uint8_t val_byte = ReadDataByte(APDS9960_RDATAL);
    val = 0;
    
    /* Read value from clear channel, low byte register */
    if( val_byte == ERROR ) {
        return false;
    }
    val = val_byte;
    
    /* Read value from clear channel, high byte register */
    val_byte = ReadDataByte(APDS9960_RDATAH);
    if( val_byte == ERROR ) {
        return false;
    }
    val = val + ((uint16_t)val_byte << 8);
    
    return true;
}

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

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

// Proximity methods
bool APDS9960::readProx(uint8_t &val)
{
    val = ReadDataByte(APDS9960_PDATA);
    
    /* Read value from proximity data register */
    if( val == ERROR) {
        return false;
    }
    
    return true;
}

// Gesture methods
bool APDS9960::isGestAvailable()
{
    uint8_t val = ReadDataByte(APDS9960_GSTATUS);

    /* Read value from GSTATUS register */
    if( val == ERROR ) {
        return ERROR;
    }
    //printf("val is %u\r\n", val);
    /* Shift and mask out GVALID bit */
    val &= APDS9960_GVALID;
    
    //printf("val & with APDS9960_GVAILD=%u\r\n",val);
    /* Return true/false based on GVALID bit */
    if( val == 1) {
        return true;
    } else {
        return false;
    }
}

int APDS9960::readGest()
{
    uint8_t fifo_level = 0;
    int bytes_read = 0;
    uint8_t fifo_data[128];
    uint8_t gstatus;
    int motion;
    int i;

    /* Make sure that power and gesture is on and data is valid */
    if( !isGestAvailable() || !(getMode() & 0x41) ) {
        return DIR_NA;
    }

    /* Keep looping as long as gesture data is valid */
    while(1) {

        /* Wait some time to collect next batch of FIFO data */
        wait(FIFO_TIME);

        /* Get the contents of the STATUS register. Is data still valid? */
        gstatus = ReadDataByte(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 = ReadDataByte(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 */
            if( fifo_level > 0) {
                bytes_read = ReadDataBlock(  APDS9960_GFIFO_U,
                                             fifo_data,
                                             (fifo_level * 4) );
                if( bytes_read == -1 ) {
                    return ERROR;
                }
#if DEBUG
                Serial.print("FIFO Dump: ");
                for ( i = 0; i < bytes_read; 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( bytes_read >= 4 ) {
                    for( i = 0; i < bytes_read; i += 4 ) {
                        gest_data_.u_data[gest_data_.index_gest] = \
                                fifo_data[i + 0];
                        gest_data_.d_data[gest_data_.index_gest] = \
                                fifo_data[i + 1];
                        gest_data_.l_data[gest_data_.index_gest] = \
                                fifo_data[i + 2];
                        gest_data_.r_data[gest_data_.index_gest] = \
                                fifo_data[i + 3];
                        gest_data_.index_gest++;
                        gest_data_.total_gests++;
                    }

#if DEBUG
                    Serial.print("Up Data: ");
                    for ( i = 0; i < gest_data_.total_gests; i++ ) {
                        Serial.print(gest_data_.u_data[i]);
                        Serial.print(" ");
                    }
                    Serial.println();
#endif

                    /* Filter and process gest data. Decode near/far state */
                    if( procGestData() ) {
                        if( decodeGest() ) {
                            //***TODO: U-Turn Gestures
#if DEBUG
                            //Serial.println(gest_motion_);
#endif
                        }
                    }

                    /* Reset data */
                    gest_data_.index_gest = 0;
                    gest_data_.total_gests = 0;
                }
            }
        } else {

            /* Determine best guessed gest and clean up */
            wait(FIFO_TIME);
            decodeGest();
            motion = gest_motion_;
#if DEBUG
            Serial.print("END: ");
            Serial.println(gest_motion_);
#endif
            rstGestParameters();
            return motion;
        }
    }
}

// Gesture processing
void APDS9960::rstGestParameters()
{
    gest_data_.index_gest = 0;
    gest_data_.total_gests = 0;

    gest_ud_delta_ = 0;
    gest_lr_delta_ = 0;

    gest_ud_count_ = 0;
    gest_lr_count_ = 0;

    gest_near_count_ = 0;
    gest_far_count_ = 0;

    gest_state_ = 0;
    gest_motion_ = DIR_NA;
}

bool APDS9960::procGestData()
{
    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( gest_data_.total_gests <= 4 ) {
        return false;
    }

    /* Check to make sure our data isn't out of bounds */
    if( (gest_data_.total_gests <= 32) && \
            (gest_data_.total_gests > 0) ) {

        /* Find the first value in U/D/L/R above the threshold */
        for( i = 0; i < gest_data_.total_gests; i++ ) {
            if( (gest_data_.u_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.d_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.l_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.r_data[i] > GEST_THRESHOLD_OUT) ) {

                u_first = gest_data_.u_data[i];
                d_first = gest_data_.d_data[i];
                l_first = gest_data_.l_data[i];
                r_first = gest_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 = gest_data_.total_gests - 1; i >= 0; i-- ) {
#if DEBUG
            Serial.print(F("Finding last: "));
            Serial.print(F("U:"));
            Serial.print(gest_data_.u_data[i]);
            Serial.print(F(" D:"));
            Serial.print(gest_data_.d_data[i]);
            Serial.print(F(" L:"));
            Serial.print(gest_data_.l_data[i]);
            Serial.print(F(" R:"));
            Serial.println(gest_data_.r_data[i]);
#endif
            if( (gest_data_.u_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.d_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.l_data[i] > GEST_THRESHOLD_OUT) &&
                    (gest_data_.r_data[i] > GEST_THRESHOLD_OUT) ) {

                u_last = gest_data_.u_data[i];
                d_last = gest_data_.d_data[i];
                l_last = gest_data_.l_data[i];
                r_last = gest_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 */
    gest_ud_delta_ += ud_delta;
    gest_lr_delta_ += lr_delta;

#if DEBUG
    Serial.print("Accumulations: ");
    Serial.print("UD: ");
    Serial.print(gest_ud_delta_);
    Serial.print(" LR: ");
    Serial.println(gest_lr_delta_);
#endif

    /* Determine U/D gesture */
    if( gest_ud_delta_ >= GEST_SENSITIVITY_1 ) {
        gest_ud_count_ = 1;
    } else if( gest_ud_delta_ <= -GEST_SENSITIVITY_1 ) {
        gest_ud_count_ = -1;
    } else {
        gest_ud_count_ = 0;
    }

    /* Determine L/R gesture */
    if( gest_lr_delta_ >= GEST_SENSITIVITY_1 ) {
        gest_lr_count_ = 1;
    } else if( gest_lr_delta_ <= -GEST_SENSITIVITY_1 ) {
        gest_lr_count_ = -1;
    } else {
        gest_lr_count_ = 0;
    }

    /* Determine Near/Far gesture */
    if( (gest_ud_count_ == 0) && (gest_lr_count_ == 0) ) {
        if( (abs(ud_delta) < GEST_SENSITIVITY_2) && \
                (abs(lr_delta) < GEST_SENSITIVITY_2) ) {

            if( (ud_delta == 0) && (lr_delta == 0) ) {
               // printf("increase near count\n\r");
                gest_near_count_++;
            } else if( (ud_delta != 0) || (lr_delta != 0) ) {
               // printf("increase far count\r\n");
                gest_far_count_++;
            }

            if( (gest_near_count_ >= 3) && (gest_far_count_ >= 2) ) {
                if( (ud_delta == 0) && (lr_delta == 0) ) {
                    gest_state_ = NEAR_STATE;
                } else if( (ud_delta != 0) && (lr_delta != 0) ) {
                    gest_state_ = FAR_STATE;
                }
                return true;
            }
        }
    } else {
        if( (abs(ud_delta) < GEST_SENSITIVITY_2) && \
                (abs(lr_delta) < GEST_SENSITIVITY_2) ) {

            if( (ud_delta == 0) && (lr_delta == 0) ) {
                gest_near_count_++;
            }

            if( gest_near_count_ >= 5 ) {
                gest_ud_count_ = 0;
                gest_lr_count_ = 0;
                gest_ud_delta_ = 0;
                gest_lr_delta_ = 0;
            }
        }
    }

#if DEBUG
    Serial.print("UD_CT: ");
    Serial.print(gest_ud_count_);
    Serial.print(" LR_CT: ");
    Serial.print(gest_lr_count_);
    Serial.print(" NEAR_CT: ");
    Serial.print(gest_near_count_);
    Serial.print(" FAR_CT: ");
    Serial.println(gest_far_count_);
    Serial.println("----------");
#endif

    return false;
}

bool APDS9960::decodeGest()
{
    /* Return if near or far event is detected */
    if( gest_state_ == NEAR_STATE ) {
        gest_motion_ = DIR_I;
        return true;
    } else if ( gest_state_ == FAR_STATE ) {
        gest_motion_ = DIR_O;
        return true;
    }

    /* Determine swipe direction */
    if( (gest_ud_count_ == -1) && (gest_lr_count_ == 0) ) {
        gest_motion_ = DIR_N;
    } else if( (gest_ud_count_ == 1) && (gest_lr_count_ == 0) ) {
        gest_motion_ = DIR_S;
    } else if( (gest_ud_count_ == 0) && (gest_lr_count_ == 1) ) {
        gest_motion_ = DIR_E;
    } else if( (gest_ud_count_ == 0) && (gest_lr_count_ == -1) ) {
        gest_motion_ = DIR_W;
    } else if( (gest_ud_count_ == -1) && (gest_lr_count_ == 1) ) {
        gest_motion_ = DIR_NE;
        /*if( abs(gest_ud_delta_) > abs(gest_lr_delta_) ) {
            gest_motion_ = DIR_N;
        } else {
            gest_motion_ = DIR_E;
        }*/
    } else if( (gest_ud_count_ == 1) && (gest_lr_count_ == -1) ) {
        if( abs(gest_ud_delta_) > abs(gest_lr_delta_) ) {
            gest_motion_ = DIR_S;
        } else {
            gest_motion_ = DIR_W;
        }
    } else if( (gest_ud_count_ == -1) && (gest_lr_count_ == -1) ) {
        if( abs(gest_ud_delta_) > abs(gest_lr_delta_) ) {
            gest_motion_ = DIR_N;
        } else {
            gest_motion_ = DIR_W;
        }
    } else if( (gest_ud_count_ == 1) && (gest_lr_count_ == 1) ) {
        if( abs(gest_ud_delta_) > abs(gest_lr_delta_) ) {
            gest_motion_ = DIR_S;
        } else {
            gest_motion_ = DIR_E;
        }
    } else {
        return false;
    }

    return true;
}
// Proximity Interrupt Threshold
uint8_t APDS9960::getProxIntLowThresh()
{
    uint8_t val = ReadDataByte(APDS9960_PILT);
    
    /* Read value from PILT register */
    if( val == ERROR ) {
        val = 0;
    }
    
    return val;
}

bool APDS9960::setProxIntLowThresh(uint8_t threshold)
{
    if( WriteDataByte(APDS9960_PILT, threshold) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getProxIntHighThresh()
{
    uint8_t val = ReadDataByte(APDS9960_PIHT);
    
    /* Read value from PIHT register */
    if( val == ERROR ) {
        val = 0;
    }
    
    return val;
}

bool APDS9960::setProxIntHighThresh(uint8_t threshold)
{
    if( WriteDataByte(APDS9960_PIHT, threshold) ) {
        return false;
    }
    
    return true;
}

// LED Boost Control
uint8_t APDS9960::getLEDBoost()
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG2);
    
    /* Read value from CONFIG2 register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out LED_BOOST bits */
    val = (val >> 4) & 0x03;
    
    return val;
}

bool APDS9960::setLEDBoost(uint8_t boost)
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG2);

    /* Read value from CONFIG2 register */
    if( val == ERROR ) {
        return false;
    }

    /* Set bits in register to given value */
    boost &= 0x03;
    boost = boost << 4;
    val &= 0xCF;
    val |= boost;

    /* Write register value back into CONFIG2 register */
    if( WriteDataByte(APDS9960_CONFIG2, val) ) {
        return false;
    }

    return true;
}

// Proximity photodiode select
uint8_t APDS9960::getProxGainCompEnable()
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG3);
    
    /* Read value from CONFIG3 register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Shift and mask out PCMP bits */
    val = (val >> 5) & 0x01;
    
    return val;
}

bool APDS9960::setProxGainCompEnable(uint8_t enable)
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG3);
    
    /* Read value from CONFIG3 register */
    if( val == ERROR ) {
        return false;
    }
    
    /* Set bits in register to given value */
    enable &= 0x01;
    enable = enable << 5;
    val &= 0xDF;
    val |= enable;
    
    /* Write register value back into CONFIG3 register */
    if( WriteDataByte(APDS9960_CONFIG3, val) ) {
        return false;
    }
    
    return true;
}

uint8_t APDS9960::getProxPhotoMask()
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG3);
    
    /* Read value from CONFIG3 register */
    if( val == ERROR ) {
        return ERROR;
    }
    
    /* Mask out photodiode enable mask bits */
    val &= 0x0F;
    
    return val;
}

bool APDS9960::setProxPhotoMask(uint8_t mask)
{
    uint8_t val = ReadDataByte(APDS9960_CONFIG3);
    
    /* Read value from CONFIG3 register */
    if( val == ERROR ) {
        return false;
    }
    
    /* Set bits in register to given value */
    mask &= 0x0F;
    val &= 0xF0;
    val |= mask;
    
    /* Write register value back into CONFIG3 register */
    if( WriteDataByte(APDS9960_CONFIG3, val) ) {
        return false;
    }
    
    return true;
}

// Gesture threshold control
uint8_t APDS9960::getGestEnterThresh()
{
    uint8_t val = ReadDataByte(APDS9960_GPENTH);
    /* Read value from GPENTH register */
    if( val == ERROR ) {
        val = 0;
    }
    return val;
}

bool APDS9960::setGestEnterThresh(uint8_t threshold)
{
    if( WriteDataByte(APDS9960_GPENTH, threshold) ) {
        return false;
    }
    return true;
}

uint8_t APDS9960::getGestExitThresh()
{
    uint8_t val = ReadDataByte(APDS9960_GEXTH);
    /* Read value from GEXTH register */
    if( val == ERROR ) {
        val = 0;
    }
    return val;
}

bool APDS9960::setGestExitThresh(uint8_t threshold)
{
    if( WriteDataByte(APDS9960_GEXTH, threshold) ) {
        return false;
    }
    return true;
}

// Gesture LED, gain, and time control
uint8_t APDS9960::getGestWaitTime()
{
    uint8_t val = ReadDataByte(APDS9960_GCONF2);
    /* Read value from GCONF2 register */
    if( val == ERROR ) {
        return ERROR;
    }
    /* Mask out GWTIME bits */
    val &= 0x07;
    return val;
}

bool APDS9960::setGestWaitTime(uint8_t time)
{
    uint8_t val = ReadDataByte(APDS9960_GCONF2);
    /* Read value from GCONF2 register */
    if( val == ERROR ) {
        return false;
    }
    /* Set bits in register to given value */
    time &= 0x07;
    val &= 0xF8;
    val |= time;
    /* Write register value back into GCONF2 register */
    if( WriteDataByte(APDS9960_GCONF2, val) ) {
        return false;
    }
    return true;
}

// Gesture mode
uint8_t APDS9960::getGestMode()
{
    uint8_t val = ReadDataByte(APDS9960_GCONF4);
    /* Read value from GCONF4 register */
    if( val == ERROR ) {
        return ERROR;
    }
    /* Mask out GMODE bit */
    val &= 0x01;
    return val;
}

bool APDS9960::setGestMode(uint8_t mode)
{
    uint8_t val = ReadDataByte(APDS9960_GCONF4);
    /* Read value from GCONF4 register */
    if( val == ERROR ) {
        return false;
    }
    /* Set bits in register to given value */
    mode &= 0x01;
    val &= 0xFE;
    val |= mode;
    /* Write register value back into GCONF4 register */
    if( WriteDataByte(APDS9960_GCONF4, val) ) {
        return false;
    }
    return true;
}

// Raw I2C Commands
bool APDS9960::WriteByte(uint8_t val)
{
    if(i2c_.write(val)) {
        return false;
    }

    return true;
}

int APDS9960::WriteDataByte(char reg, char val)
{
    /*Wire.beginTransmission(APDS9960_I2C_ADDR);
    Wire.write(reg);
    Wire.write(val);
    if( Wire.endTransmission() != 0 ) {
        return false;
    }*/
    /*char regdata[1];
    regdata[0] = reg;*/
   // printf("reg is %u, val is %u\r\n", reg, val);
    
    char cmd[2] = {reg, val};
    uint8_t data = i2c_.write(APDS9960_I2C_ADDR, cmd, 2,true); 
  /*  if(i2c_.write(APDS9960_I2C_ADDR, (char*) &val, 1) !=0) {
        return false;
    }*/

   // uint8_t temp=i2c_.read(APDS9960_I2C_ADDR);
    //printf("temp:%u\r\n");

    return data;

}

/*bool APDS9960::WriteDataBlock(uint8_t reg, uint8_t *val, unsigned int len){
        unsigned int i;
        char regdata[len];

    //Wire.beginTransmission(APDS9960_I2C_ADDR);
    //Wire.write(reg);
    for(i = 0; i < len; i++) {
        //Wire.beginTransmission(val[i]);
        regdata[i]= reg;

    }
    if( Wire.endTransmission() != 0 ) {
        return false;
    }

    return true;
}*/

uint8_t APDS9960::ReadDataByte(char reg)
{
    char val;
    /* Indicate which register we want to read from */
    if (i2c_.write(APDS9960_I2C_ADDR, &reg, 1, true)) {
        //printf("write failed\n\r");
        return false;
    }
    /* Read from register */
    if(i2c_.read(APDS9960_I2C_ADDR, &val, 1)) {
        //printf("read failed\r\n");
        return false;
    }

    /*while (Wire.available()) {
        val = Wire.read();
    }*/
    // printf("The register we are reading from is %u\r\n",reg);
   // printf("The read value is %u\r\n", *val);

    return val;
}

int APDS9960::ReadDataBlock(uint8_t reg, uint8_t *val, unsigned int len)
{

    /*Indicate which register we want to read from */
    if (i2c_.write(APDS9960_I2C_ADDR,(char*) &reg,1)) {
        return -1;
    }
    if ( len == 0) {
        return -1;
    }
    /*Read block data */
    //Wire.requestFrom(APDS9960_I2C_ADDR, len);
    // while (i < len) {
    if (i2c_.read(APDS9960_I2C_ADDR,(char*) val, len) != 0) {
        return -1;
    }

    // i++;
    //}
    /* if (i > len) {
         return -1;
     }*/

    return len;
}