//
#include <ADXL345.h>
//
//-------------------------------------
// CADXL345 - Constructor
//-------------------------------------
CADXL345::CADXL345(TwoWire* pwire, byte i2caddress)
{
    FI2CAddress = i2caddress;
    FPWire = pwire;
}
//-------------------------------------
// CADXL345 - Helper
//-------------------------------------
void CADXL345::WriteRegister(byte index, byte value)
{
   FPWire->beginTransmission(FI2CAddress);
   FPWire->write(index);
   FPWire->write(value);
   FPWire->endTransmission();
}
byte CADXL345::ReadRegister(byte index)
{
   FPWire->beginTransmission(FI2CAddress);
   FPWire->write(index);
   FPWire->endTransmission(false);
   FPWire->requestFrom(FI2CAddress, 1);
   if (Wire.available())
   {
       return FPWire->read();
   }
   return 0;
}
void CADXL345::ReadRegisters(byte index, byte count, byte* data)
{
   FPWire->beginTransmission(FI2CAddress);
   FPWire->write(index);
   FPWire->endTransmission(false);
   FPWire->requestFrom(FI2CAddress, count);
   byte i = 0;
   while (Wire.available() && i < count)
   {
      data[i++] =FPWire->read();
   }
}
//-------------------------------------
// CADXL345 - Read/Write
//-------------------------------------
bool CADXL345::Open()
{
    byte DID = ReadRegister(REG_DEVID);
    if (0xE5 != DID)
    {
        return false;
    }
    // Measurement Mode on
    WriteRegister(REG_POWER_CTL, 0x08);
    // Standards
    SetRange(AR_2G);
    SetFullRange(true);
    SetDataRate(ADR_100HZ);
    return true;
}
bool CADXL345::Close()
{
    return true;
}
//
void CADXL345::SetDataRate(EAdxlDataRate datarate)
{
    WriteRegister(REG_BW_RATE, datarate);
}
void CADXL345::SetRange(EAdxlRange range)
{
    byte Format = ReadRegister(REG_DATA_FORMAT);
    Format &= ~0x03;
    Format |= range;
    WriteRegister(REG_DATA_FORMAT, Format);
    // Full-Resolution erneut setzen
    SetFullRange(FFullResolution);
}


void CADXL345::SetFullRange(bool enable)
{
    FFullResolution = enable;
    byte Format = ReadRegister(REG_DATA_FORMAT);
    if (enable)
    {
        Format |= 0x08;
    }
    else
    {
        Format &= ~0x08;
    }
    WriteRegister(REG_DATA_FORMAT, Format);
}
void CADXL345::ReadRawData(int16_t &ax, int16_t &ay, int16_t &az)
{
    byte Data[6];
    ReadRegisters(REG_DATAX0, 6, Data);
    ax = (int16_t)((Data[1] << 8) | Data[0]);
    ay = (int16_t)((Data[3] << 8) | Data[2]);
    az = (int16_t)((Data[5] << 8) | Data[4]);
}

//
void CADXL345::ReadAcceleration(float &xg, float &yg, float &zg)
{
    int16_t Ax;
    int16_t Ay;
    int16_t Az;
    ReadRawData(Ax, Ay, Az);
    // Full-Resolution:
    // 3.9 mg/LSB
    const float Scale = 0.0039;
    xg = Scale * Ax;
    yg = Scale * Ay;
    zg = Scale * Az;
}
void CADXL345::ReadAngles(float &xdeg, float &ydeg, float &zdeg)
{
    float Ax;
    float Ay;
    float Az;
    ReadAcceleration(Ax, Ay, Az);
    xdeg = 180.0 / PI * atan2(Ax, sqrt(Ay * Ay + Az * Az));
    ydeg = 180.0 / PI * atan2(Ay, sqrt(Ax * Ax + Az * Az));
    zdeg = 180.0 / PI * atan2(Az, sqrt(Ax * Ax + Ay * Ay));
}
