#include <stddef.h>

/* Private */
#include "magnetic_bmm150.h"

static ims_ret_t ims_bmm150_read_register(ims_bmm150_t *bmm, uint8_t addr, uint8_t *val, uint8_t len) {
    ims_i2c_xfer_desc_t desc = {
        .tx_data = &addr,
        .tx_size = 1U,
        .rx_data = val,
        .rx_size = len,
    };

    return bmm->cb.i2c_xfer(bmm->pdev, &desc);
}

static ims_ret_t ims_bmm150_write_register(ims_bmm150_t *bmm, uint8_t addr, uint8_t val) {
    uint8_t             tx_buf[2] = {addr, val};
    ims_i2c_xfer_desc_t desc      = {
             .tx_data = tx_buf,
             .tx_size = 2U,
             .rx_data = NULL,
             .rx_size = 0U,
    };

    return bmm->cb.i2c_xfer(bmm->pdev, &desc);
}

static ims_ret_t ims_bmm150_set_mode(ims_bmm150_t *bmm, ims_bmm150_mode_t mode) {
    uint8_t b_mode;
    if (ims_bmm150_read_register(bmm, 0x4C, &b_mode, 0x01) != IMS_SUCCESS) {
        return IMS_FAIL;
    }
    switch (mode) {
        case IMS_BMM150_MODE_SLEEP:
            b_mode |= (0x03U << 1U);
            break;
        case IMS_BMM150_MODE_NORMAL:
            b_mode &= (0x03U << 1U);
            break;
        case IMS_BMM150_MODE_FORCED:
            b_mode &= (0x03U << 1U);
            b_mode |= (0x01U << 1U);
            break;
        default:
            break;
    }

    if (ims_bmm150_write_register(bmm, 0x4C, b_mode) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

static ims_ret_t ims_bmm150_enable_drdy(ims_bmm150_t *bmm) {
    uint8_t reg;
    if (ims_bmm150_read_register(bmm, 0x4E, &reg, 0x01) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    reg |= (1U << 7);

    if (ims_bmm150_write_register(bmm, 0x4E, reg) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

static ims_ret_t ims_bmm150_set_thresholds(ims_bmm150_t *bmm, uint8_t high, uint8_t low) {
    if (ims_bmm150_write_register(bmm, 0x4F, low) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    if (ims_bmm150_write_register(bmm, 0x50, high) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

static ims_ret_t ims_bmm150_set_repetitions(ims_bmm150_t *bmm, uint8_t xy, uint8_t z) {
    if (ims_bmm150_write_register(bmm, 0x51, xy) != IMS_SUCCESS) {
        return IMS_FAIL;
    }
    if (ims_bmm150_write_register(bmm, 0x52, z) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

static ims_ret_t ims_bmm150_reset(ims_bmm150_t *bmm) {
    if (ims_bmm150_write_register(bmm, 0x4B, 0x00) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    if (ims_bmm150_write_register(bmm, 0x4B, 0x01) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

ims_ret_t ims_bmm150_init(ims_bmm150_t *bmm) {
    if (!bmm) return IMS_FAIL;

    if (ims_bmm150_reset(bmm) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

ims_ret_t ims_bmm150_config(ims_bmm150_t *bmm, ims_bmm150_config_t *cfg) {
    if (ims_bmm150_set_repetitions(bmm, cfg->rep_xy, cfg->rep_z) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    uint8_t b_mode;

    if (ims_bmm150_read_register(bmm, 0x4C, &b_mode, 0x01) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    b_mode &= (7U << 3);
    b_mode |= (cfg->odr << 3);

    if (ims_bmm150_write_register(bmm, 0x4C, b_mode) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}

ims_ret_t ims_bmm150_watch(ims_bmm150_t *bmm) {
    /* Enable interrupts and ... */
    if (ims_bmm150_set_thresholds(bmm, 0, 0) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    return IMS_SUCCESS;
}