#include "drv8428.h"
#include "config.h"

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

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

    // 1. Set Sleep Mode to WORK (wake up initially to configure?)
    // Actually BoardSweekar::StepperMotorInit sets it to WORK then ENABLE_DISABLE.
    // Let's follow that.
    err = io_expansion_.SetMotorSleepMode(
        STEPPER_MOTOR_WORK,
        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
    err = io_expansion_.SetMotorEnableMode(
        STEPPER_MOTOR_ENABLE_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;
    }

    // 3. Set Microsteps (Defaulting to 1/8 as per original code which passed 4? need to verify)
    // BoardSweekar passed 4.
    // "io_expansion_.SetStepperMicrostepMode(4, ...)"
    // Let's assume 4 maps to 1/8 or whatever.
    err = SetMicrosteps(4);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set microsteps: 0x%x", err);
        return err;
    }

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

    ESP_LOGI(TAG, "DRV8428 Driver Initialized");
    return ESP_OK;
}

esp_err_t Drv8428Driver::Enable(bool enable)
{
    esp_err_t err = ESP_OK;
    if (enable)
    {
        ESP_LOGD(TAG, "Enabling stepper motor (sleep + enable)");
        
        // 1. Set sleep mode to WORK
        err = io_expansion_.SetMotorSleepMode(
            STEPPER_MOTOR_WORK,
            STEPPER_MOTOR_SLEEP_PORT,
            STEPPER_MOTOR_SLEEP_PIN
        );
        if (err != ESP_OK) return err;
        
        // 2. Set enable mode to ENABLE
        err = io_expansion_.SetMotorEnableMode(
            STEPPER_MOTOR_ENABLE_ENABLE,
            STEPPER_MOTOR_ENABLE_PORT,
            STEPPER_MOTOR_ENABLE_PIN
        );
    }
    else
    {
        ESP_LOGD(TAG, "Disabling stepper motor (disable + sleep)");
        
        // 1. Disable enable pin
        err = io_expansion_.SetMotorEnableMode(
            STEPPER_MOTOR_ENABLE_DISABLE,
            STEPPER_MOTOR_ENABLE_PORT,
            STEPPER_MOTOR_ENABLE_PIN
        );
        if (err != ESP_OK) return err;
        
        // 2. Set sleep mode to SLEEP
        err = io_expansion_.SetMotorSleepMode(
            STEPPER_MOTOR_SLEEP,
            STEPPER_MOTOR_SLEEP_PORT,
            STEPPER_MOTOR_SLEEP_PIN
        );
    }
    return err;
}

esp_err_t Drv8428Driver::SetDirection(bool dir)
{
    // dir: true=UP, false=DOWN (based on original StepperMotor::SetDir logic)
    // Original: dir ? STEPPER_MOTOR_RIGHT : STEPPER_MOTOR_LEFT
    // Wait, StepperMotor::SetDir used STEPPER_MOTOR_RIGHT for true.
    // STEPPER_MOTOR_RIGHT is a macro, likely 1 or 0.
    
    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 Drv8428Driver::Sleep(bool sleep)
{
    // This is explicitly setting sleep mode, might be redundant with Enable(false)
    // but useful if we want to sleep without disabling or vice versa if supported.
    // For DRV8428, Enable(false) sets Sleep to 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 Drv8428Driver::SetMicrosteps(int microsteps)
{
    // Original code used hardcoded ports for microsteps.
    // io_expansion_.SetStepperMicrostepMode(4, STEPPER_MOTOR_M0_MADE_PORT, STEPPER_MOTOR_M0_PIN, 
    //                                     STEPPER_MOTOR_M1_MADE_PORT, STEPPER_MOTOR_M1_PIN);
    
    // We can just pass the value 'microsteps' if it corresponds to the index expected by SetStepperMicrostepMode.
    // Or we might need a mapping.
    // Assuming 'microsteps' passed here is the raw value/index for now.
    
    io_expansion_.SetStepperMicrostepMode(
        (uint8_t)microsteps, 
        STEPPER_MOTOR_M0_MADE_PORT, STEPPER_MOTOR_M0_PIN, 
        STEPPER_MOTOR_M1_MADE_PORT, STEPPER_MOTOR_M1_PIN
    );
    return ESP_OK;
}