#include "attenuator.h"
#include <soc/gpio_struct.h>

Attenuator::Attenuator() : _step(0), _mutex(xSemaphoreCreateMutex()) {}

void Attenuator::begin() {
    // Configure all 6 pins as outputs
    for (uint8_t i = 0; i < 6; i++) {
        pinMode(ATTEN_PINS[i], OUTPUT);
    }

    // Restore last setting from NVS
    _step = loadFromNVS();
    applyToGPIO();

    Serial0.printf("[Attenuator] Initialized, restored step=%u (%.1f dB)\n", _step, getDB());
}

float Attenuator::setDB(float db) {
    // Clamp to valid range
    if (db < DB_MIN) db = DB_MIN;
    if (db > DB_MAX) db = DB_MAX;

    // Convert to step: each step is 0.5 dB
    // Round to nearest step
    uint8_t step = static_cast<uint8_t>(db / DB_STEP + 0.5f);
    return setStep(step) * DB_STEP;
}

uint8_t Attenuator::setStep(uint8_t step, bool persist) {
    if (step > STEP_MAX) step = STEP_MAX;

    xSemaphoreTake(_mutex, portMAX_DELAY);
    _step = step;
    applyToGPIO();
    if (persist) saveToNVS();
    xSemaphoreGive(_mutex);

    Serial0.printf("[Attenuator] Set step=%u (%.1f dB)%s\n",
                   _step, getDB(), persist ? "" : " [no-persist]");
    return _step;
}

uint8_t Attenuator::setBits(const uint8_t bits[6]) {
    // Convert bit array to step value
    // bits[0] = V1 (16 dB, weight 32), bits[5] = V6 (0.5 dB, weight 1)
    uint8_t step = 0;
    for (uint8_t i = 0; i < 6; i++) {
        if (bits[i]) {
            step |= (1 << (5 - i));
        }
    }
    return setStep(step);
}

float Attenuator::getDB() const {
    return _step * DB_STEP;
}

uint8_t Attenuator::getStep() const {
    return _step;
}

uint8_t Attenuator::getBit(uint8_t index) const {
    if (index >= 6) return 0;
    // Bit order: index 0 = V1 (MSB, weight 32), index 5 = V6 (LSB, weight 1)
    return (_step >> (5 - index)) & 0x01;
}

void Attenuator::getBits(uint8_t bits[6]) const {
    for (uint8_t i = 0; i < 6; i++) {
        bits[i] = getBit(i);
    }
}

bool Attenuator::getGPIOState(uint8_t index) const {
    if (index >= 6) return HIGH;
    // Active-low: bit=1 → GPIO LOW, bit=0 → GPIO HIGH
    return getBit(index) ? LOW : HIGH;
}

void Attenuator::applyToGPIO() {
    // Optimized bitwise GPIO update — no loop needed!
    //
    // Pin mapping: GPIO(n) = step bit (n-1), so step << 1 aligns with GPIOs 1-6
    // Active-low: step bit 1 → GPIO LOW, step bit 0 → GPIO HIGH
    //
    // Example: step=5 (0b000101 = 2.5dB) → GPIO1,3 LOW, GPIO2,4,5,6 HIGH

    uint32_t step_bits = (_step & 0x3F) << 1;  // Step value shifted to GPIO positions

    // Atomic register writes for glitch-free update
    GPIO.out_w1tc = step_bits;                           // Set LOW where step bit = 1
    GPIO.out_w1ts = (~step_bits) & ATTEN_PIN_MASK;       // Set HIGH where step bit = 0
}

void Attenuator::saveToNVS() {
    _prefs.begin(NVS_NAMESPACE, false);  // false = read-write
    _prefs.putUChar(NVS_KEY_STEP, _step);
    _prefs.end();
}

uint8_t Attenuator::loadFromNVS() {
    _prefs.begin(NVS_NAMESPACE, true);  // true = read-only
    uint8_t step = _prefs.getUChar(NVS_KEY_STEP, 0);  // default 0 = no attenuation
    _prefs.end();

    // Validate loaded value
    if (step > STEP_MAX) step = 0;
    return step;
}