/**
 * @file CVManager.cpp
 * @brief Configuration Variable Manager Implementation
 */

#include "CVManager.h"

CVManager::CVManager() {}

bool CVManager::begin() {
    if (!preferences.begin("dcc-decoder", false)) {
        return false;
    }
    
    // Check if this is first boot
    if (preferences.getUChar("initialized", 0) == 0) {
        setDefaultCVs();
        preferences.putUChar("initialized", 1);
    }
    
    return true;
}

uint8_t CVManager::readCV(uint16_t cvNumber, uint8_t defaultValue) {
    if (cvNumber < 1 || cvNumber > MAX_CV_NUMBER) {
        return defaultValue;
    }
    
    return preferences.getUChar(getCVKey(cvNumber).c_str(), defaultValue);
}

bool CVManager::writeCV(uint16_t cvNumber, uint8_t value) {
    if (cvNumber < 1 || cvNumber > MAX_CV_NUMBER) {
        return false;
    }
    
    return preferences.putUChar(getCVKey(cvNumber).c_str(), value);
}

void CVManager::resetToDefaults() {
    preferences.clear();
    setDefaultCVs();
    preferences.putUChar("initialized", 1);
}

uint16_t CVManager::getLocoAddress() {
    // Check CV29 bit 5 to determine address mode
    uint8_t cv29 = readCV(CV_CONFIG_DATA_1, 0x06);
    
    if (cv29 & 0x20) {
        // Long address mode
        uint8_t highByte = readCV(CV_EXTENDED_ADDRESS_HIGH, 0xC0);
        uint8_t lowByte = readCV(CV_EXTENDED_ADDRESS_LOW, 0x03);
        return ((highByte & 0x3F) << 8) | lowByte;
    } else {
        // Short address mode
        return readCV(CV_PRIMARY_ADDRESS, 3);
    }
}

void CVManager::setLocoAddress(uint16_t address) {
    if (address >= 1 && address <= 127) {
        // Short address
        writeCV(CV_PRIMARY_ADDRESS, address);
        
        // Clear long address bit in CV29
        uint8_t cv29 = readCV(CV_CONFIG_DATA_1, 0x06);
        cv29 &= ~0x20;
        writeCV(CV_CONFIG_DATA_1, cv29);
    } else if (address >= 128 && address <= 10239) {
        // Long address
        uint8_t highByte = 0xC0 | ((address >> 8) & 0x3F);
        uint8_t lowByte = address & 0xFF;
        
        writeCV(CV_EXTENDED_ADDRESS_HIGH, highByte);
        writeCV(CV_EXTENDED_ADDRESS_LOW, lowByte);
        
        // Set long address bit in CV29
        uint8_t cv29 = readCV(CV_CONFIG_DATA_1, 0x06);
        cv29 |= 0x20;
        writeCV(CV_CONFIG_DATA_1, cv29);
    }
}

bool CVManager::isLongAddress() {
    uint8_t cv29 = readCV(CV_CONFIG_DATA_1, 0x06);
    return (cv29 & 0x20) != 0;
}

void CVManager::setDefaultCVs() {
    // Standard CVs
    writeCV(CV_PRIMARY_ADDRESS, 3);           // Default address 3
    writeCV(CV_VSTART, 1);                    // Start voltage
    writeCV(CV_ACCEL_RATE, 10);               // Acceleration rate
    writeCV(CV_DECEL_RATE, 10);               // Deceleration rate
    writeCV(CV_VHIGH, 255);                   // Max voltage
    writeCV(CV_VMID, 128);                    // Mid voltage
    writeCV(CV_VERSION_ID, 1);                // Version 1
    writeCV(CV_MANUFACTURER_ID, 13);          // DIY decoder
    writeCV(CV_TOTAL_PWM_PERIOD, 20);         // 20ms PWM period
    writeCV(CV_CONFIG_DATA_1, 0x06);          // 128 speed steps, short address
    
    // Custom CVs
    writeCV(CV_MOTOR_KP, 50);                 // PID Kp = 5.0
    writeCV(CV_MOTOR_KI, 5);                  // PID Ki = 0.5
    writeCV(CV_MOTOR_KD, 10);                 // PID Kd = 1.0
    writeCV(CV_RAILCOM_ENABLE, 1);            // RailCom enabled
    writeCV(CV_LOAD_COMP_ENABLE, 1);          // Load compensation enabled
    writeCV(CV_LED_BRIGHTNESS, 128);          // 50% brightness
    writeCV(CV_ACCESSORY_1_MODE, ACC_FUNCTION); // Function controlled
    writeCV(CV_ACCESSORY_2_MODE, ACC_FUNCTION); // Function controlled
}

String CVManager::getCVKey(uint16_t cvNumber) {
    return "cv" + String(cvNumber);
}