#include "aw8646.h"
#include "config.h"
#include "esp_log.h"
#include "io_expansion_board.h"

// Define AW8646 specific pins for Board v1.1.16
// P0_0 -> DRV_T0
// P0_6 -> DRV_T1
// Only if NFC_RST_PIN is defined (implying it's moved to GPIO, freeing up P0_0)
#ifdef NFC_RST_PIN
#define AW8646_USE_CURRENT_CONTROL
#define AW8646_T0_PIN 0
#define AW8646_T0_PORT OUTPUT_PORT0
#define AW8646_T1_PIN 6
#define AW8646_T1_PORT OUTPUT_PORT0
#endif

Aw8646Driver::Aw8646Driver(IOExpansionBoard &ioExp) : io_expansion_(ioExp)
{
}

esp_err_t Aw8646Driver::Init()
{
    esp_err_t err = ESP_OK;

    // 1. 默认处于休眠和禁用状态，防止发热！
    err = io_expansion_.SetMotorSleepMode(
        STEPPER_MOTOR_SLEEP,
        STEPPER_MOTOR_SLEEP_PORT,
        STEPPER_MOTOR_SLEEP_PIN
    );
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set sleep mode: 0x%x", err);
        return err;
    }

    // 2. Disable Enable Pin (High = Disable for AW8646)
    err = io_expansion_.SetMotorEnableMode(
        true, // High -> Disable
        STEPPER_MOTOR_ENABLE_PORT,
        STEPPER_MOTOR_ENABLE_PIN
    );
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set enable mode: 0x%x", err);
        return err;
    }

#ifdef AW8646_USE_CURRENT_CONTROL
    // 3. Initialize T0 and T1 pins for Current Control
    // Default to Low (0, 0) for now.
    uint8_t output0_val = 0;
    err = io_expansion_.IoexpandReadReg(OUTPUT_PORT0, &output0_val);
    if (err == ESP_OK) {
        output0_val &= ~(1 << AW8646_T0_PIN); // Set T0 Low
        output0_val &= ~(1 << AW8646_T1_PIN); // Set T1 Low
        err = io_expansion_.IoexpandWriteReg(OUTPUT_PORT0, output0_val);
        if (err != ESP_OK) {
             ESP_LOGE(TAG, "Failed to set T0/T1: 0x%x", err);
        }
    } else {
        ESP_LOGE(TAG, "Failed to read port 0: 0x%x", err);
    }
#endif

    // 4. Set Microsteps (Default to 1/8 step)
    err = SetMicrosteps(8);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set microsteps: 0x%x", err);
        return err;
    }

    // 5. Set Default Direction
    err = SetDirection(true); 
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set direction: 0x%x", err);
        return err;
    }

    ESP_LOGI(TAG, "AW8646 Driver Initialized (EN Logic Inverted, T0/T1 Configured)");
    return ESP_OK;
}

esp_err_t Aw8646Driver::Enable(bool enable)
{
    esp_err_t err = ESP_OK;
    if (enable)
    {
        ESP_LOGD(TAG, "Enabling AW8646 (sleep + enable=Low)");
        
        // Ensure Sleep is High (Work)
        err = io_expansion_.SetMotorSleepMode(
            STEPPER_MOTOR_WORK,
            STEPPER_MOTOR_SLEEP_PORT,
            STEPPER_MOTOR_SLEEP_PIN
        );
        if (err != ESP_OK) return err;
        
        // Enable Pin: Low to Enable
        err = io_expansion_.SetMotorEnableMode(
            false, // Low -> Enable
            STEPPER_MOTOR_ENABLE_PORT,
            STEPPER_MOTOR_ENABLE_PIN
        );
    }
    else
    {
        ESP_LOGD(TAG, "Disabling AW8646 (disable=High + sleep)");
        
        // Enable Pin: High to Disable
        err = io_expansion_.SetMotorEnableMode(
            true, // High -> Disable
            STEPPER_MOTOR_ENABLE_PORT,
            STEPPER_MOTOR_ENABLE_PIN
        );
        if (err != ESP_OK) return err;
        
        // Sleep Pin: Low to Sleep
        err = io_expansion_.SetMotorSleepMode(
            STEPPER_MOTOR_SLEEP,
            STEPPER_MOTOR_SLEEP_PORT,
            STEPPER_MOTOR_SLEEP_PIN
        );
    }
    return err;
}

esp_err_t Aw8646Driver::SetDirection(bool dir)
{
    uint8_t mode = dir ? STEPPER_MOTOR_RIGHT : STEPPER_MOTOR_LEFT;
    
    return io_expansion_.StepperMotorSetDir(
        mode,
        STEPPER_MOTOR_DIR_PORT,
        STEPPER_MOTOR_DIR_PIN
    );
}

esp_err_t Aw8646Driver::Sleep(bool sleep)
{
    uint8_t mode = sleep ? STEPPER_MOTOR_SLEEP : STEPPER_MOTOR_WORK;
    return io_expansion_.SetMotorSleepMode(
        mode,
        STEPPER_MOTOR_SLEEP_PORT,
        STEPPER_MOTOR_SLEEP_PIN
    );
}

esp_err_t Aw8646Driver::SetTriLevelPin(uint8_t port_reg, uint8_t config_reg, uint8_t pin, int state)
{
    uint8_t val;
    esp_err_t err = ESP_OK;

    if (state == 2) { // Hi-Z -> Input
        // Set as Input
        err = io_expansion_.IoexpandReadReg(config_reg, &val);
        if (err != ESP_OK) return err;
        val |= (1 << pin); // Set Input
        err = io_expansion_.IoexpandWriteReg(config_reg, val);
    } else { // 0 or 1 -> Output
        // Set as Output
        err = io_expansion_.IoexpandReadReg(config_reg, &val);
        if (err != ESP_OK) return err;
        val &= ~(1 << pin); // Set Output
        err = io_expansion_.IoexpandWriteReg(config_reg, val);
        if (err != ESP_OK) return err;
        
        // Set Level
        err = io_expansion_.IoexpandReadReg(port_reg, &val);
        if (err != ESP_OK) return err;
        
        if (state == 1) val |= (1 << pin);
        else val &= ~(1 << pin);
        
        err = io_expansion_.IoexpandWriteReg(port_reg, val);
    }
    return err;
}

esp_err_t Aw8646Driver::SetMicrosteps(int microsteps)
{
    // AW8646 Microstep Table (Rising Edge Only)
    // 1 (Full): MODE1=0, MODE0=0
    // 2 (1/2):  MODE1=0, MODE0=Z (Hi-Z)
    // 4 (1/4):  MODE1=0, MODE0=1
    // 8 (1/8):  MODE1=Z, MODE0=0
    // 16 (1/16): MODE1=Z, MODE0=1
    // 32 (1/32): MODE1=1, MODE0=Z

    int m1 = 0;
    int m0 = 0;

    switch(microsteps) {
        case 1:  m1 = 0; m0 = 0; break;
        case 2:  m1 = 0; m0 = 2; break; // Z
        case 4:  m1 = 0; m0 = 1; break;
        case 8:  m1 = 2; m0 = 0; break; // Z
        case 16: m1 = 2; m0 = 1; break; // Z
        case 32: m1 = 1; m0 = 2; break; // Z
        default: 
            ESP_LOGW(TAG, "Unsupported microsteps: %d, defaulting to 1/8", microsteps);
            m1 = 2; m0 = 0; 
            break;
    }

    ESP_LOGI(TAG, "Setting Microsteps to 1/%d (M1=%d, M0=%d)", microsteps, m1, m0);

    esp_err_t err;
    err = SetTriLevelPin(STEPPER_MOTOR_M1_MADE_PORT, STEPPER_MOTOR_M1_CONFIG, STEPPER_MOTOR_M1_PIN, m1);
    if (err != ESP_OK) return err;
    
    err = SetTriLevelPin(STEPPER_MOTOR_M0_MADE_PORT, STEPPER_MOTOR_M0_CONFIG, STEPPER_MOTOR_M0_PIN, m0);
    return err;
}