/********************************************************
Description:PAJ7620 wake up command
Input:none
Return: uint8_t
********************************************************/
bool GroveGesture::pajWakeUp()
{
    uint8_t data;
    uint8_t rtn = suli_i2c_read(this->i2c, PAJ7620_ID, &data, 1);
    if (rtn == 1 && data == 0x20) return true;
    else return false;
}
static void grove_compass_getxyz_raw(I2C_T *i2c, int16_t *x, int16_t *y, int16_t *z)
{
	cmdbuf[0] = DATA_REGISTER_BEGIN;
	suli_i2c_write(i2c, HMC5883L_ADDRESS, &cmdbuf[0], 1);
	suli_i2c_read(i2c, HMC5883L_ADDRESS, databuf, 6);
	*x = (databuf[0] << 8) | databuf[1];
	*y = (databuf[2] << 8) | databuf[3];
	*z = (databuf[4] << 8) | databuf[5];
}
// Reads num bytes starting from address register on device in to _buff array
void readFrom(uint8 address, uint8 num, uint8 buff[])
{
    //grove_hal_i2c_read(ADXL345_DEVICE, address, buff, num);

    uint8 dta_send[] = {address};

    suli_i2c_write(__I2C_Device, ADXL345_DEVICE, dta_send, 1);
    suli_i2c_read(__I2C_Device, ADXL345_DEVICE, buff, num);

}
/*Function: The clock timing will start */
void rtc_ds1307_start(void)                                 // set the ClockHalt bit low to start the rtc
{
    uint8 dta[2] = {0x00};
    suli_i2c_write(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 1);
    
    suli_i2c_read(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 1);
    
    dta[1] = dta[0] & 0x7f;
    dta[0] = 0x00;
    suli_i2c_write(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 2);
}
/*Function: The clock timing will stop */
void rtc_ds1307_stop(void)         // set the ClockHalt bit high to stop the rtc
{
    uint8 dta[2] = {0x00};
    suli_i2c_write(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 1);
    
    suli_i2c_read(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 1);
    
    dta[1] = dta[0] | 0x80;
    dta[0] = 0x00;
    
    suli_i2c_write(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 2);
}
/*Function: Read time and date from RTC    */
void rtc_ds1307_get_time(struct __time * time_get)
{
    uint8 dta[1] = {0x00};
    suli_i2c_write(__I2C_Device_RTC, DS1307_I2C_ADDRESS, dta, 1);
    suli_i2c_read(__I2C_Device_RTC, DS1307_I2C_ADDRESS, time_get->data, 7);
    
    time_get->data[0] &= 0x7f;
    time_get->data[2] &= 0x3f;
    for(int i=0; i<7; i++)
    {
        time_get->data[i] = bcdToDec(time_get->data[i]);
    }
}
bool GroveCompass::read_compass_heading(float *heading_deg)
{
    int16_t x,y,z;
    float cx, cy, cz;

    suli_i2c_read(i2c, HMC5883L_ADDRESS, databuf, 6);
	x = (databuf[0] << 8) | databuf[1];
	z = (databuf[2] << 8) | databuf[3];
	y = (databuf[4] << 8) | databuf[5];

    //Serial1.println(x);
    //Serial1.println(y);
    //Serial1.println(z);

	cx = static_cast<float>(x);
	cy = static_cast<float>(y);
	cz = static_cast<float>(z);

	float head = atan2f(cy, cx) - 0.0457;

	// Correct for when signs are reversed.
	if(head < 0)
	head += 2*PI;

	// Check for wrap due to addition of declination.
	if(head > 2*PI)
	head -= 2*PI;

	// Convert radians to degrees for readability.
	*heading_deg = head * 180 / PI;

    cmdbuf[0] = DATA_REGISTER_BEGIN;
	suli_i2c_write(i2c, HMC5883L_ADDRESS, cmdbuf, 1);

	return true;
}