#include "adxl345.h"

#include <stddef.h>

#define ADXL345_REG_DEVID       0x00
#define ADXL345_REG_BW_RATE     0x2C
#define ADXL345_REG_POWER_CTL   0x2D
#define ADXL345_REG_DATA_FORMAT 0x31
#define ADXL345_REG_DATAX0      0x32

#define ADXL345_DEVID_VALUE     0xE5
#define ADXL345_BW_RATE_100_HZ  0x0A
#define ADXL345_POWER_MEASURE   0x08
#define ADXL345_FORMAT_FULL_8G  0x0A
#define ADXL345_TIMEOUT_MS      100

static esp_err_t read_registers(i2c_master_dev_handle_t device, uint8_t start_register,
                                uint8_t *data, size_t length)
{
    return i2c_master_transmit_receive(device, &start_register, 1, data, length,
                                       ADXL345_TIMEOUT_MS);
}

static esp_err_t write_register(i2c_master_dev_handle_t device, uint8_t reg, uint8_t value)
{
    const uint8_t bytes[] = {reg, value};
    return i2c_master_transmit(device, bytes, sizeof(bytes), ADXL345_TIMEOUT_MS);
}

static esp_err_t verify_register(i2c_master_dev_handle_t device, uint8_t reg, uint8_t expected)
{
    uint8_t actual = 0;
    esp_err_t err = read_registers(device, reg, &actual, 1);
    if (err != ESP_OK) {
        return err;
    }
    return actual == expected ? ESP_OK : ESP_ERR_INVALID_RESPONSE;
}

esp_err_t adxl345_init(adxl345_t *sensor, i2c_master_bus_handle_t bus, uint32_t bus_speed_hz)
{
    if (sensor == NULL || bus == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    *sensor = (adxl345_t){0};
    static const uint8_t candidate_addresses[] = {0x53, 0x1D};

    for (size_t i = 0; i < sizeof(candidate_addresses); ++i) {
        const i2c_device_config_t config = {
            .dev_addr_length = I2C_ADDR_BIT_LEN_7,
            .device_address = candidate_addresses[i],
            .scl_speed_hz = bus_speed_hz,
        };

        i2c_master_dev_handle_t device = NULL;
        esp_err_t err = i2c_master_bus_add_device(bus, &config, &device);
        if (err != ESP_OK) {
            return err;
        }

        uint8_t device_id = 0;
        err = read_registers(device, ADXL345_REG_DEVID, &device_id, 1);
        if (err == ESP_OK && device_id == ADXL345_DEVID_VALUE) {
            sensor->device = device;
            sensor->address = candidate_addresses[i];
            break;
        }

        i2c_master_bus_rm_device(device);
    }

    if (sensor->device == NULL) {
        return ESP_ERR_NOT_FOUND;
    }

    // Configure while in standby, then enter measurement mode.
    esp_err_t err = write_register(sensor->device, ADXL345_REG_POWER_CTL, 0x00);
    if (err == ESP_OK) {
        err = write_register(sensor->device, ADXL345_REG_DATA_FORMAT, ADXL345_FORMAT_FULL_8G);
    }
    if (err == ESP_OK) {
        err = write_register(sensor->device, ADXL345_REG_BW_RATE, ADXL345_BW_RATE_100_HZ);
    }
    if (err == ESP_OK) {
        err = write_register(sensor->device, ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
    }
    if (err == ESP_OK) {
        err = verify_register(sensor->device, ADXL345_REG_DATA_FORMAT, ADXL345_FORMAT_FULL_8G);
    }
    if (err == ESP_OK) {
        err = verify_register(sensor->device, ADXL345_REG_BW_RATE, ADXL345_BW_RATE_100_HZ);
    }
    if (err == ESP_OK) {
        err = verify_register(sensor->device, ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
    }

    return err;
}

esp_err_t adxl345_read_raw(const adxl345_t *sensor, adxl345_sample_t *sample)
{
    if (sensor == NULL || sensor->device == NULL || sample == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    uint8_t data[6] = {0};
    esp_err_t err = read_registers(sensor->device, ADXL345_REG_DATAX0, data, sizeof(data));
    if (err != ESP_OK) {
        return err;
    }

    sample->x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
    sample->y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8));
    sample->z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
    return ESP_OK;
}

uint8_t adxl345_address(const adxl345_t *sensor)
{
    return sensor != NULL ? sensor->address : 0;
}