#include "drivers/AccelerometerController.h"

ADXL345Controller::ADXL345Controller(uint8_t deviceAddress) :
    _deviceAddress(deviceAddress),
    _lastReadTime(0),
    _minReadInterval(DEFAULT_MIN_READ_INTERVAL),
    _lastX(0.0),
    _lastY(0.0),
    _lastZ(0.0),
    _lastReadSuccess(false) {
}

void ADXL345Controller::begin() {
    Wire.begin();
    
    uint8_t deviceID = readRegister(ADXL345_DEVID);
    if (deviceID != 0xE5) {
        _lastReadSuccess = false;
        return;
    }
    
    setRegister(ADXL345_POWER_CTL, 0x08);
    
    setRegister(ADXL345_DATA_FORMAT, 0x0B);
    
    setRegister(ADXL345_BW_RATE, 0x0A);
    
    setRegister(ADXL345_OFSX, 0x00);
    setRegister(ADXL345_OFSY, 0x00);
    setRegister(ADXL345_OFSZ, 0x00);
    
    _lastReadSuccess = true;
}

bool ADXL345Controller::read() {
    unsigned long currentTime = millis();
    
    if (currentTime - _lastReadTime >= _minReadInterval) {
        _lastReadTime = currentTime;
        
        uint8_t buffer[6];
        readRegisters(ADXL345_DATAX0, 6, buffer);
        
        int16_t x = ((int16_t)buffer[1] << 8) | buffer[0];
        int16_t y = ((int16_t)buffer[3] << 8) | buffer[2];
        int16_t z = ((int16_t)buffer[5] << 8) | buffer[4];
        
        _lastX = x / 256.0;
        _lastY = y / 256.0;
        _lastZ = z / 256.0;
        
        _lastReadSuccess = true;
        return true;
    }
    
    return _lastReadSuccess;
}

double ADXL345Controller::getX() {
    if (millis() - _lastReadTime >= _minReadInterval) read();
    return _lastX;
}

double ADXL345Controller::getY() {
    if (millis() - _lastReadTime >= _minReadInterval) read();
    return _lastY;
}

double ADXL345Controller::getZ() {
    if (millis() - _lastReadTime >= _minReadInterval) read();
    return _lastZ;
}

bool ADXL345Controller::isLastReadSuccessful() const {
    return _lastReadSuccess;
}

void ADXL345Controller::setMinReadInterval(unsigned long interval) {
    _minReadInterval = interval;
}

void ADXL345Controller::setRange(uint8_t range) {
    uint8_t format = readRegister(ADXL345_DATA_FORMAT);
    
    format &= ~0x03;
    
    if (range > 3) range = 3;
    format |= range;
    
    setRegister(ADXL345_DATA_FORMAT, format);
}

void ADXL345Controller::setDataRate(uint8_t rate) {
    if (rate > 15) rate = 15;
    
    setRegister(ADXL345_BW_RATE, rate);
}

void ADXL345Controller::setOffsets(int8_t xOffset, int8_t yOffset, int8_t zOffset) {
    setRegister(ADXL345_OFSX, xOffset);
    setRegister(ADXL345_OFSY, yOffset);
    setRegister(ADXL345_OFSZ, zOffset);
}

void ADXL345Controller::setRegister(uint8_t reg, uint8_t value) {
    Wire.beginTransmission(_deviceAddress);
    Wire.write(reg);
    Wire.write(value);
    Wire.endTransmission();
}

uint8_t ADXL345Controller::readRegister(uint8_t reg) {
    Wire.beginTransmission(_deviceAddress);
    Wire.write(reg);
    Wire.endTransmission();
    
    Wire.requestFrom(_deviceAddress, (uint8_t)1);
    return Wire.read();
}

void ADXL345Controller::readRegisters(uint8_t reg, uint8_t count, uint8_t *buffer) {
    Wire.beginTransmission(_deviceAddress);
    Wire.write(reg);
    Wire.endTransmission();
    
    Wire.requestFrom(_deviceAddress, count);
    for (uint8_t i = 0; i < count && Wire.available(); i++) {
        buffer[i] = Wire.read();
    }
}