#include "charge_registers.h"

#include <stddef.h>
#include <string.h>

#define REG_BQ_CHARGE_CURRENT 0x03u
#define REG_BQ_INPUT_CURRENT 0x06u
#define REG_BQ_TERMINATION 0x09u
#define REG_BQ_CONTROL_1 0x10u
#define REG_BQ_CONTROL_5 0x14u
#define REG_BQ_NTC_CONTROL_1 0x18u
#define REG_BQ_STATUS_0 0x1Bu
#define REG_BQ_STATUS_COUNT 7u
#define REG_BQ_ADC_CONTROL 0x2Eu
#define REG_BQ_VBUS_ADC 0x35u

#define MASK_STOP_WD_CHG 0x20u
#define MASK_WD_RST 0x08u
#define MASK_WATCHDOG 0x07u
#define MASK_EN_EXTILIM 0x02u
#define MASK_ADC_EN 0x80u
#define MASK_ADC_RATE 0x40u
#define MASK_ADC_SAMPLE 0x30u
#define MASK_TS_IGNORE 0x01u

static bool io_valid(const charge_register_io *io) {
    return io != NULL && io->read != NULL && io->write != NULL &&
           io->set_ce_enabled != NULL && io->millis != NULL;
}

static bool rd(charge_registers *d, uint8_t address, uint8_t reg,
               uint8_t *data, uint8_t length) {
    return d != NULL && d->io.read(d->io.ctx, address, reg, data, length);
}

static bool wr(charge_registers *d, uint8_t address, uint8_t reg,
               const uint8_t *data, uint8_t length) {
    return d != NULL && d->io.write(d->io.ctx, address, reg, data, length);
}

static bool read8(charge_registers *d, uint8_t reg, uint8_t *value) {
    return rd(d, CHARGE_BQ25798_ADDR, reg, value, 1u);
}

static bool write8_verify(charge_registers *d, uint8_t reg, uint8_t value,
                          uint8_t mask) {
    uint8_t readback = 0u;
    return wr(d, CHARGE_BQ25798_ADDR, reg, &value, 1u) &&
           read8(d, reg, &readback) && ((readback ^ value) & mask) == 0u;
}

static bool read16(charge_registers *d, uint8_t reg, uint16_t *value) {
    uint8_t bytes[2];
    if (value == NULL || !rd(d, CHARGE_BQ25798_ADDR, reg, bytes, 2u))
        return false;
    *value = (uint16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
    return true;
}

static bool write16_verify(charge_registers *d, uint8_t reg, uint16_t value,
                           uint16_t mask) {
    uint8_t bytes[2] = {(uint8_t)(value >> 8), (uint8_t)value};
    uint16_t readback = 0u;
    return wr(d, CHARGE_BQ25798_ADDR, reg, bytes, 2u) &&
           read16(d, reg, &readback) && ((readback ^ value) & mask) == 0u;
}

bool charge_registers_init(charge_registers *device,
                           const charge_register_io *io) {
    if (device == NULL || !io_valid(io)) return false;
    memset(device, 0, sizeof(*device));
    device->io = *io;

    if (!device->io.set_ce_enabled(device->io.ctx, false)) return false;

    /* A BQ watchdog timeout must disable charge, then kick the watchdog from
     * the main loop. Preserve all unrelated termination-control bits. */
    uint8_t term = 0u;
    if (!read8(device, REG_BQ_TERMINATION, &term)) return false;
    term |= MASK_STOP_WD_CHG;
    if (!write8_verify(device, REG_BQ_TERMINATION, term, MASK_STOP_WD_CHG))
        return false;

    /* Keep TI's 40 s default watchdog and reset its timer during bring-up. */
    uint8_t ctrl1 = 0u;
    if (!read8(device, REG_BQ_CONTROL_1, &ctrl1)) return false;
    ctrl1 = (uint8_t)((ctrl1 & (uint8_t)~(MASK_WATCHDOG | MASK_WD_RST)) |
                      5u | MASK_WD_RST);
    if (!wr(device, CHARGE_BQ25798_ADDR, REG_BQ_CONTROL_1, &ctrl1, 1u))
        return false;
    if (!read8(device, REG_BQ_CONTROL_1, &ctrl1) ||
        (ctrl1 & MASK_WATCHDOG) != 5u || (ctrl1 & MASK_WD_RST) != 0u)
        return false;

    /* 15-bit continuous ADC. Preserve the low averaging/reserved controls. */
    uint8_t adc = 0u;
    if (!read8(device, REG_BQ_ADC_CONTROL, &adc)) return false;
    adc = (uint8_t)((adc & (uint8_t)~(MASK_ADC_EN | MASK_ADC_RATE |
                                     MASK_ADC_SAMPLE)) | MASK_ADC_EN);
    if (!write8_verify(device, REG_BQ_ADC_CONTROL, adc,
                       MASK_ADC_EN | MASK_ADC_RATE | MASK_ADC_SAMPLE))
        return false;
    return true;
}

bool charge_registers_kick_watchdog(charge_registers *device) {
    uint8_t ctrl1 = 0u;
    if (!read8(device, REG_BQ_CONTROL_1, &ctrl1)) return false;
    ctrl1 = (uint8_t)((ctrl1 & (uint8_t)~MASK_WD_RST) | MASK_WD_RST);
    if (!wr(device, CHARGE_BQ25798_ADDR, REG_BQ_CONTROL_1, &ctrl1, 1u))
        return false;
    if (!read8(device, REG_BQ_CONTROL_1, &ctrl1)) return false;
    return (ctrl1 & MASK_WATCHDOG) == 5u && (ctrl1 & MASK_WD_RST) == 0u;
}

bool charge_registers_observe(charge_registers *device,
                              charge_observation *observation) {
    if (device == NULL || observation == NULL || !io_valid(&device->io))
        return false;
    memset(observation, 0, sizeof(*observation));
    const uint32_t start = device->io.millis(device->io.ctx);

    uint8_t usb[3] = {0};
    observation->source_read_ok = rd(device, CHARGE_HUSB238_ADDR, 0x00u,
                                     usb, 3u);
    if (observation->source_read_ok) {
        observation->pd_status0 = usb[0];
        observation->pd_status1 = usb[1];
        observation->src_pdo_5v = usb[2];
    }

    uint8_t status[REG_BQ_STATUS_COUNT] = {0};
    uint8_t ntc_control = 0u;
    uint8_t adc_control = 0u;
    uint8_t vbus_bytes[2] = {0};
    bool charger_ok = rd(device, CHARGE_BQ25798_ADDR, REG_BQ_STATUS_0,
                         status, REG_BQ_STATUS_COUNT);
    charger_ok = rd(device, CHARGE_BQ25798_ADDR, REG_BQ_NTC_CONTROL_1,
                    &ntc_control, 1u) && charger_ok;
    charger_ok = read8(device, REG_BQ_ADC_CONTROL, &adc_control) && charger_ok;
    charger_ok = rd(device, CHARGE_BQ25798_ADDR, REG_BQ_VBUS_ADC,
                    vbus_bytes, 2u) && charger_ok;
    observation->charger_read_ok = charger_ok;

    if (charger_ok) {
        const uint8_t status0 = status[0];       /* REG1B */
        const uint8_t battery_status = status[2];/* REG1D */
        const uint8_t timer_status = status[3];  /* REG1E */
        const uint8_t ts_status = status[4];     /* REG1F */
        const uint8_t fault0 = status[5];        /* REG20 */
        const uint8_t fault1 = status[6];        /* REG21 */
        observation->vbus_mv = (uint16_t)(((uint16_t)vbus_bytes[0] << 8) |
                                          vbus_bytes[1]);
        observation->charger_fault_free =
            (status0 & 0x20u) == 0u && /* watchdog has not expired */
            (timer_status & 0x0Eu) == 0u &&
            (fault0 & 0x7Fu) == 0u &&
            (fault1 & 0xF4u) == 0u;
        /* Fail closed unless the pack is present, the NTC is enabled, and TS
         * is in its nominal window. Cool/warm JEITA operation needs a product
         * approved derating curve before this policy can permit charging. */
        observation->pack_and_ntc_valid =
            (battery_status & 0x01u) != 0u &&
            (ts_status & 0x0Fu) == 0u &&
            (ntc_control & MASK_TS_IGNORE) == 0u &&
            (adc_control & MASK_ADC_EN) != 0u &&
            (status0 & 0x01u) != 0u;
    }

    const uint32_t elapsed = device->io.millis(device->io.ctx) - start;
    observation->snapshot_age_ms = elapsed > UINT16_MAX ? UINT16_MAX :
                                    (uint16_t)elapsed;
    return observation->source_read_ok && observation->charger_read_ok;
}

static bool ce(void *ctx, bool enabled) {
    charge_registers *d = ctx;
    return d != NULL && d->io.set_ce_enabled(d->io.ctx, enabled);
}

static bool input_limit(void *ctx, uint16_t ma) {
    charge_registers *d = ctx;
    if (d == NULL || ma < 100u || ma > 3300u || ma % 10u != 0u) return false;
    return write16_verify(d, REG_BQ_INPUT_CURRENT, (uint16_t)(ma / 10u),
                          0x01FFu);
}

static bool charge_limit(void *ctx, uint16_t ma) {
    charge_registers *d = ctx;
    if (d == NULL || ma < 50u || ma > 5000u || ma % 10u != 0u) return false;
    return write16_verify(d, REG_BQ_CHARGE_CURRENT, (uint16_t)(ma / 10u),
                          0x01FFu);
}

static bool ext_ilim(void *ctx, bool enabled) {
    charge_registers *d = ctx;
    uint8_t value = 0u;
    if (d == NULL || !read8(d, REG_BQ_CONTROL_5, &value)) return false;
    if (enabled) value |= MASK_EN_EXTILIM;
    else value &= (uint8_t)~MASK_EN_EXTILIM;
    return write8_verify(d, REG_BQ_CONTROL_5, value, MASK_EN_EXTILIM);
}

charge_hal charge_registers_make_hal(charge_registers *device) {
    return (charge_hal){device, ce, input_limit, charge_limit, ext_ilim};
}
