#include "charge_manager.h"

#include <stddef.h>

#define PRODUCT_MAX_INPUT_MA 2000u
#define CHARGE_LIMIT_MA 1000u
#define SOURCE_MARGIN_PERCENT 90u
#define MAX_SNAPSHOT_AGE_MS 500u
#define HUSB_ATTACH_MASK 0x40u
#define HUSB_5V_CONTRACT_MASK 0x04u

static uint16_t source_current_ma(uint8_t code) {
    static const uint16_t table[16] = {
        500u, 700u, 1000u, 1250u, 1500u, 1750u, 2000u, 2250u,
        2500u, 2750u, 3000u, 3250u, 3500u, 4000u, 4500u, 5000u
    };
    return code < 16u ? table[code] : 0u;
}

bool charge_decode_source(uint8_t pd_status0, uint8_t pd_status1,
                          uint8_t src_pdo_5v, uint16_t *voltage_mv,
                          uint16_t *current_limit_ma) {
    if (voltage_mv == NULL || current_limit_ma == NULL) return false;
    *voltage_mv = 0u;
    *current_limit_ma = 0u;
    if ((pd_status1 & HUSB_ATTACH_MASK) == 0u) return false;

    const uint8_t pd_voltage = (uint8_t)(pd_status0 >> 4);
    uint16_t advertised_ma = 0u;
    if (pd_voltage == 1u || pd_voltage == 2u) {
        /* Explicit PD contract: 5V or 9V, source current in PD_STATUS0[3:0]. */
        *voltage_mv = pd_voltage == 1u ? 5000u : 9000u;
        advertised_ma = source_current_ma((uint8_t)(pd_status0 & 0x0Fu));
    } else if (pd_voltage != 0u) {
        /* Product input is specified only up to 9V. */
        return false;
    } else if ((pd_status1 & HUSB_5V_CONTRACT_MASK) != 0u) {
        /* Type-C 5V contract current: default/1.5A/2.4A/3A. */
        static const uint16_t typec_ma[4] = {500u, 1500u, 2400u, 3000u};
        *voltage_mv = 5000u;
        advertised_ma = typec_ma[pd_status1 & 0x03u];
    } else if ((src_pdo_5v & 0x80u) != 0u) {
        /* BC1.2 / legacy 5V detector current code. */
        *voltage_mv = 5000u;
        advertised_ma = source_current_ma((uint8_t)(src_pdo_5v & 0x0Fu));
    } else {
        return false;
    }

    if (advertised_ma == 0u) return false;
    if (advertised_ma > PRODUCT_MAX_INPUT_MA) advertised_ma = PRODUCT_MAX_INPUT_MA;
    /* Leave system-load headroom so input limit is not the same as a charge
     * current promise. Floor to the BQ 10mA step when programming. */
    *current_limit_ma = (uint16_t)((advertised_ma * SOURCE_MARGIN_PERCENT) / 100u);
    if (*current_limit_ma < 100u) return false;
    return true;
}

bool bq25798_encode_iindpm(uint16_t requested_ma, uint16_t *register_value,
                           uint16_t *applied_ma) {
    if (register_value == NULL || applied_ma == NULL || requested_ma < 100u ||
        requested_ma > 3300u) return false;
    uint16_t quantized_ma = (uint16_t)((requested_ma / 10u) * 10u);
    *register_value = (uint16_t)(quantized_ma / 10u);
    *applied_ma = quantized_ma;
    return true;
}

static bool hal_valid(const charge_hal *hal) {
    return hal != NULL && hal->set_charge_enabled != NULL &&
           hal->set_input_limit_ma != NULL && hal->set_charge_limit_ma != NULL &&
           hal->set_external_ilim_enabled != NULL;
}

static charge_state safe_off(const charge_hal *hal, charge_report *report,
                             charge_state reason) {
    bool off_ok = false;
    bool clamp_ok = false;
    if (hal != NULL && hal->set_charge_enabled != NULL)
        off_ok = hal->set_charge_enabled(hal->ctx, false);
    if (hal != NULL && hal->set_external_ilim_enabled != NULL)
        clamp_ok = hal->set_external_ilim_enabled(hal->ctx, true);
    if (report != NULL) {
        report->active = false;
        report->state = (off_ok && clamp_ok) ? reason : CHARGE_IO_ERROR;
        report->programmed_input_limit_ma = 0u;
        report->programmed_charge_limit_ma = 0u;
        report->source_limit_ma = 0u;
    }
    return (off_ok && clamp_ok) ? reason : CHARGE_IO_ERROR;
}

charge_state charge_manager_step(const charge_hal *hal,
                                 const charge_observation *observation,
                                 charge_report *report) {
    if (report == NULL) {
        if (hal != NULL && hal->set_charge_enabled != NULL)
            (void)hal->set_charge_enabled(hal->ctx, false);
        if (hal != NULL && hal->set_external_ilim_enabled != NULL)
            (void)hal->set_external_ilim_enabled(hal->ctx, true);
        return CHARGE_INVALID_ARGUMENT;
    }
    if (!hal_valid(hal) || observation == NULL)
        return safe_off(hal, report, CHARGE_INVALID_ARGUMENT);

    if (!observation->source_read_ok || !observation->charger_read_ok ||
        observation->snapshot_age_ms > MAX_SNAPSHOT_AGE_MS)
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if ((observation->pd_status1 & HUSB_ATTACH_MASK) == 0u)
        return safe_off(hal, report, CHARGE_DETACHED);
    if (!observation->pack_and_ntc_valid)
        return safe_off(hal, report, CHARGE_PACK_OR_TEMPERATURE_FAULT);
    if (!observation->charger_fault_free)
        return safe_off(hal, report, CHARGE_CHARGER_FAULT);

    uint16_t source_mv = 0u, source_ma = 0u;
    if (!charge_decode_source(observation->pd_status0, observation->pd_status1,
                              observation->src_pdo_5v, &source_mv, &source_ma)) {
        charge_state reason = (observation->pd_status0 >> 4) > 2u ?
                              CHARGE_UNSUPPORTED_VOLTAGE : CHARGE_SOURCE_UNKNOWN;
        return safe_off(hal, report, reason);
    }
    const uint16_t low_mv = source_mv == 9000u ? 8100u : 4400u;
    const uint16_t high_mv = source_mv == 9000u ? 9900u : 5500u;
    if (observation->vbus_mv < low_mv || observation->vbus_mv > high_mv)
        return safe_off(hal, report, CHARGE_VBUS_MISMATCH);

    uint16_t reg = 0u, applied_ma = 0u;
    if (!bq25798_encode_iindpm(source_ma, &reg, &applied_ma))
        return safe_off(hal, report, CHARGE_SOURCE_UNKNOWN);

    if (report->active && report->programmed_input_limit_ma == applied_ma &&
        report->programmed_charge_limit_ma == CHARGE_LIMIT_MA) {
        report->state = CHARGE_ACTIVE;
        report->source_limit_ma = source_ma;
        return CHARGE_ACTIVE;
    }

    /* Fail-closed sequence: CE high, restore the resistor clamp, program the
     * source-derived limits, then remove the clamp and assert CE last. */
    if (!hal->set_charge_enabled(hal->ctx, false))
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if (!hal->set_external_ilim_enabled(hal->ctx, true))
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if (!hal->set_input_limit_ma(hal->ctx, applied_ma))
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if (!hal->set_charge_limit_ma(hal->ctx, CHARGE_LIMIT_MA))
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if (!hal->set_external_ilim_enabled(hal->ctx, false))
        return safe_off(hal, report, CHARGE_IO_ERROR);
    if (!hal->set_charge_enabled(hal->ctx, true))
        return safe_off(hal, report, CHARGE_IO_ERROR);

    report->state = CHARGE_ACTIVE;
    report->active = true;
    report->source_limit_ma = source_ma;
    report->programmed_input_limit_ma = applied_ma;
    report->programmed_charge_limit_ma = CHARGE_LIMIT_MA;
    return CHARGE_ACTIVE;
}
