#define VCNL4000_ADDRESS 0x13  //I2C Address of the board

// VCNL4000 Register Map
#define COMMAND_0 0x80  // starts measurments, relays data ready info
#define PRODUCT_ID 0x81  // product ID/revision ID, should read 0x11
#define IR_CURRENT 0x83  // sets IR current in steps of 10mA 0-200mA
#define AMBIENT_PARAMETER 0x84  // Configures ambient light measures
#define AMBIENT_RESULT_MSB 0x85  // high byte of ambient light measure
#define AMBIENT_RESULT_LSB 0x86  // low byte of ambient light measure
#define PROXIMITY_RESULT_MSB 0x87  // High byte of proximity measure
#define PROXIMITY_RESULT_LSB 0x88  // low byte of proximity measure
#define PROXIMITY_FREQ 0x89  // Proximity IR test signal freq, 0-3
#define PROXIMITY_MOD 0x8A  // proximity modulator timing

void setup() {
    Serial.begin(9600);  // Serial's used to debug and print data

    RGB.control(true);
    RGB.color(20, 0, 0);

    delay(4000);

    RGB.color(0, 20, 0);

    Wire.begin();  // initialize I2C stuff
    Serial.println("starting ");

    initVCNL4000();
}

void loop(){
    Serial.println(Wire.available());
    delay(1000);
    return;

    // unsigned int ambientValue = readAmbient(); //can a tiny bit slow
    unsigned int proximityValue = readProximity();

    // Serial.print(ambientValue);
    // Serial.print(" | ");
    Serial.println(proximityValue);

    delay(1000);  //Just here to slow down the printing
    //note that the readings take about 100ms to execute
}

void initVCNL4000(){
    byte temp = readVCNLByte(PRODUCT_ID);
    if (temp != 0x11){  // Product ID Should be 0x11
        Serial.print("initVCNL4000 failed to initialize");
        Serial.println(temp, HEX);
    }else{
        Serial.println("VNCL4000 Online...");
    }

    /*VNCL400 init params
    Feel free to play with any of these values, but check the datasheet first!*/
    // writeVCNLByte(0x84, 0x0F);  // Configures ambient light measures - Single conversion mode, 128 averages
    // writeVCNLByte(0x83, 15);  // sets IR current in steps of 10mA 0-200mA --> 200mA
    // writeVCNLByte(0x89, 2);  // Proximity IR test signal freq, 0-3 - 781.25 kHz
    // writeVCNLByte(0x8A, 0x81);  // proximity modulator timing - 129, recommended by Vishay
}


unsigned int readProximity(){
    // readProximity() returns a 16-bit value from the VCNL4000's proximity data registers
    byte temp = readVCNLByte(0x80);
    writeVCNLByte(0x80, temp | 0x08);  // command the sensor to perform a proximity measure

    while(!(readVCNLByte(0x80)&0x20));  // Wait for the proximity data ready bit to be set
    unsigned int data = readVCNLByte(0x87) << 8;
    data |= readVCNLByte(0x88);

    return data;
}


unsigned int readAmbient(){
    // readAmbient() returns a 16-bit value from the VCNL4000's ambient light data registers
    byte temp = readVCNLByte(0x80);
    writeVCNLByte(0x80, temp | 0x10);  // command the sensor to perform ambient measure

    while(!(readVCNLByte(0x80)&0x40));  // wait for the proximity data ready bit to be set
    unsigned int data = readVCNLByte(0x85) << 8;
    data |= readVCNLByte(0x86);

    return data;
}


void writeVCNLByte(byte address, byte data){
    // writeVCNLByte(address, data) writes a single byte of data to address
    Wire.beginTransmission(VCNL4000_ADDRESS);
    Wire.write(address);
    Wire.write(data);
    Wire.endTransmission();
}


byte readVCNLByte(byte address){
    // readByte(address) reads a single byte of data from address
    Wire.beginTransmission(VCNL4000_ADDRESS);
    Wire.write(address);
    Wire.endTransmission();
    Wire.requestFrom(VCNL4000_ADDRESS, 1);

    // while(!Wire.available());

    byte data = Wire.read();

    return data;
}