#include "ITG3200.h"
#include "board.h"

ITG3200::ITG3200()
{
    devAddr = ITG3200_DEFAULT_ADDRESS;
}

ITG3200::ITG3200(unsigned char address)
{
    devAddr = address;
}

void ITG3200::initialize()
{
    setFullScaleRange(ITG3200_FULLSCALE_2000);
    setClockSource(ITG3200_CLOCK_PLL_XGYRO);
}

bool ITG3200::testConnection()
{
    return getDeviceID() == 0x34;
}

// WHO_AM_I register
unsigned char ITG3200::getDeviceID()
{
    Board::get()->i2c_read_bits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, buffer);
    return buffer[0];
}

// SMPLRT_DIV register
/** Set sample rate.
 * @param rate New sample rate
 * @see getRate()
 * @see setDLPFBandwidth()
 * @see ITG3200_RA_SMPLRT_DIV
 */
void ITG3200::setRate(unsigned char rate)
{
    Board::get()->i2c_write_byte(devAddr, ITG3200_RA_SMPLRT_DIV, rate);
}

/** Set full-scale range setting.
 * @param range New full-scale range value
 * @see getFullScaleRange()
 * @see ITG3200_FULLSCALE_2000
 * @see ITG3200_RA_DLPF_FS
 * @see ITG3200_DF_FS_SEL_BIT
 * @see ITG3200_DF_FS_SEL_LENGTH
 */
void ITG3200::setFullScaleRange(unsigned char range)
{
    Board::get()->i2c_write_bits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT, ITG3200_DF_FS_SEL_LENGTH, range);
}

/** Set digital low-pass filter bandwidth.
 * @param bandwidth New DLFP bandwidth setting
 * @see getDLPFBandwidth()
 * @see ITG3200_DLPF_BW_256
 * @see ITG3200_RA_DLPF_FS
 * @see ITG3200_DF_DLPF_CFG_BIT
 * @see ITG3200_DF_DLPF_CFG_LENGTH
 */
void ITG3200::setDLPFBandwidth(unsigned char bandwidth)
{
    Board::get()->i2c_write_bits(devAddr, ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT, ITG3200_DF_DLPF_CFG_LENGTH, bandwidth);
}

// GYRO_*OUT_* registers

/** Get 3-axis gyroscope readings.
 * @param x 16-bit signed integer container for X-axis rotation
 * @param y 16-bit signed integer container for Y-axis rotation
 * @param z 16-bit signed integer container for Z-axis rotation
 * @see ITG3200_RA_GYRO_XOUT_H
 */
void ITG3200::getRotation(short* x, short* y, short* z)
{
    Board::get()->i2c_read_buf(devAddr, ITG3200_RA_GYRO_XOUT_H, buffer, 6);
    *x = (((short)buffer[0]) << 8) | buffer[1];
    *y = (((short)buffer[2]) << 8) | buffer[3];
    *z = (((short)buffer[4]) << 8) | buffer[5];
}

// PWR_MGM register

/** Trigger a full device reset.
 * A small delay of ~50ms may be desirable after triggering a reset.
 * @see ITG3200_RA_PWR_MGM
 * @see ITG3200_PWR_H_RESET_BIT
 */
void ITG3200::reset()
{
    Board::get()->i2c_write_bit(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true);
}

/** Set clock source setting.
 * On power up, the ITG-3200 defaults to the internal oscillator. It is highly recommended that the device is configured to use one of the gyros (or an external clock) as the clock reference, due to the improved stability.
 *
 * The CLK_SEL setting determines the device clock source as follows:
 *
 * CLK_SEL | Clock Source
 * --------+--------------------------------------
 * 0       | Internal oscillator
 * 1       | PLL with X Gyro reference
 * 2       | PLL with Y Gyro reference
 * 3       | PLL with Z Gyro reference
 * 4       | PLL with external 32.768kHz reference
 * 5       | PLL with external 19.2MHz reference
 * 6       | Reserved
 * 7       | Reserved
 *
 * @param source New clock source setting
 * @see getClockSource()
 * @see ITG3200_RA_PWR_MGM
 * @see ITG3200_PWR_CLK_SEL_BIT
 * @see ITG3200_PWR_CLK_SEL_LENGTH
 */
void ITG3200::setClockSource(unsigned char source)
{
    Board::get()->i2c_write_bits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT, ITG3200_PWR_CLK_SEL_LENGTH, source);
}