/**
 * @file sdcard.cpp
 * @brief SD Card driver implementation for ESP32
 * @details Ported from Arduino SD library to use ESP-IDF SPI driver
 * 
 * This implementation ports the Arduino SD library's low-level SD card
 * communication and FAT filesystem logic to ESP-IDF.
 * 
 * @author Mahmoud Elmohtady
 * @company Nabd solutions - ASF
 * @copyright Copyright (c) 2025
 */

#include "sdcard.hpp"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <string.h>
#include <stdlib.h>

static const char* TAG = "SDCard";

// SD card commands (from Arduino SdInfo.h)
#define CMD0    0x00  // GO_IDLE_STATE
#define CMD8    0x08  // SEND_IF_COND
#define CMD9    0x09  // SEND_CSD
#define CMD10   0x0A  // SEND_CID
#define CMD13   0x0D  // SEND_STATUS
#define CMD17   0x11  // READ_BLOCK
#define CMD24   0x18  // WRITE_BLOCK
#define CMD25   0x19  // WRITE_MULTIPLE_BLOCK
#define CMD32   0x20  // ERASE_WR_BLK_START
#define CMD33   0x21  // ERASE_WR_BLK_END
#define CMD38   0x26  // ERASE
#define CMD55   0x37  // APP_CMD
#define CMD58   0x3A  // READ_OCR
#define ACMD23  0x17  // SET_WR_BLK_ERASE_COUNT
#define ACMD41  0x29  // SD_SEND_OP_COND

// SD card responses
#define R1_READY_STATE      0x00
#define R1_IDLE_STATE       0x01
#define R1_ILLEGAL_COMMAND  0x04

// Data tokens
#define DATA_START_BLOCK     0xFE
#define STOP_TRAN_TOKEN      0xFD
#define WRITE_MULTIPLE_TOKEN 0xFC
#define DATA_RES_MASK        0x1F
#define DATA_RES_ACCEPTED    0x05

// Timeouts (milliseconds)
#define SD_INIT_TIMEOUT   2000
#define SD_READ_TIMEOUT   300
#define SD_WRITE_TIMEOUT  600
#define SD_ERASE_TIMEOUT  10000

// Error codes
#define SD_CARD_ERROR_CMD0              0x01
#define SD_CARD_ERROR_CMD8              0x02
#define SD_CARD_ERROR_CMD17             0x03
#define SD_CARD_ERROR_CMD24             0x04
#define SD_CARD_ERROR_CMD25             0x05
#define SD_CARD_ERROR_CMD58             0x06
#define SD_CARD_ERROR_ACMD23            0x07
#define SD_CARD_ERROR_ACMD41            0x08
#define SD_CARD_ERROR_BAD_CSD           0x09
#define SD_CARD_ERROR_ERASE             0x0A
#define SD_CARD_ERROR_ERASE_SINGLE_BLOCK 0x0B
#define SD_CARD_ERROR_ERASE_TIMEOUT     0x0C
#define SD_CARD_ERROR_READ              0x0D
#define SD_CARD_ERROR_READ_REG          0x0E
#define SD_CARD_ERROR_READ_TIMEOUT      0x0F
#define SD_CARD_ERROR_STOP_TRAN         0x10
#define SD_CARD_ERROR_WRITE             0x11
#define SD_CARD_ERROR_WRITE_BLOCK_ZERO  0x12
#define SD_CARD_ERROR_WRITE_MULTIPLE    0x13
#define SD_CARD_ERROR_WRITE_PROGRAMMING 0x14
#define SD_CARD_ERROR_WRITE_TIMEOUT     0x15
#define SD_CARD_ERROR_SCK_RATE          0x16

// FAT filesystem structures (from Arduino SdFat)
#define FAT16_BOOT_SIGNATURE  0x29
#define FAT32_BOOT_SIGNATURE  0x29

// Cache structure for 512-byte blocks
typedef struct {
    uint32_t blockNumber;
    uint8_t data[512];
} cache_t;

// CSD structure (Card-Specific Data)
typedef struct {
    union {
        struct {  // CSD Version 1.0
            unsigned reserved1 : 6;
            unsigned csd_ver : 2;
            uint8_t taac;
            uint8_t nsac;
            uint8_t tran_speed;
            uint8_t ccc_high;
            unsigned read_bl_len : 4;
            unsigned ccc_low : 4;
            unsigned c_size_high : 2;
            unsigned reserved2 : 2;
            unsigned dsr_imp : 1;
            unsigned read_blk_misalign : 1;
            unsigned write_blk_misalign : 1;
            unsigned read_bl_partial : 1;
            uint8_t c_size_mid;
            unsigned vdd_r_curr_max : 3;
            unsigned vdd_r_curr_min : 3;
            unsigned c_size_low : 2;
            unsigned c_size_mult_high : 2;
            unsigned vdd_w_cur_max : 3;
            unsigned vdd_w_curr_min : 3;
            unsigned sector_size_high : 6;
            unsigned erase_blk_en : 1;
            unsigned c_size_mult_low : 1;
            unsigned wp_grp_size : 7;
            unsigned sector_size_low : 1;
            unsigned write_bl_len_high : 2;
            unsigned r2w_factor : 3;
            unsigned reserved3 : 2;
            unsigned wp_grp_enable : 1;
            unsigned reserved4 : 5;
            unsigned write_partial : 1;
            unsigned write_bl_len_low : 2;
            unsigned reserved5: 2;
            unsigned file_format : 2;
            unsigned tmp_write_protect : 1;
            unsigned perm_write_protect : 1;
            unsigned copy : 1;
            unsigned file_format_grp : 1;
            unsigned always1 : 1;
            unsigned crc : 7;
        } v1;
        struct {  // CSD Version 2.0
            unsigned reserved1 : 6;
            unsigned csd_ver : 2;
            uint8_t taac;
            uint8_t nsac;
            uint8_t tran_speed;
            uint8_t ccc_high;
            unsigned read_bl_len : 4;
            unsigned ccc_low : 4;
            unsigned reserved2 : 4;
            unsigned dsr_imp : 1;
            unsigned read_blk_misalign : 1;
            unsigned write_blk_misalign : 1;
            unsigned read_bl_partial : 1;
            unsigned reserved3 : 2;
            unsigned c_size_high : 6;
            uint8_t c_size_mid;
            uint8_t c_size_low;
            unsigned sector_size_high : 6;
            unsigned erase_blk_en : 1;
            unsigned reserved4 : 1;
            unsigned wp_grp_size : 7;
            unsigned sector_size_low : 1;
            unsigned write_bl_len_high : 2;
            unsigned r2w_factor : 3;
            unsigned reserved5 : 2;
            unsigned wp_grp_enable : 1;
            unsigned reserved6 : 5;
            unsigned write_partial : 1;
            unsigned write_bl_len_low : 2;
            unsigned reserved7: 2;
            unsigned file_format : 2;
            unsigned tmp_write_protect : 1;
            unsigned perm_write_protect : 1;
            unsigned copy : 1;
            unsigned file_format_grp : 1;
            unsigned always1 : 1;
            unsigned crc : 7;
        } v2;
    };
} csd_t;

// FAT Boot Sector structure
typedef struct {
    uint8_t jump[3];
    char oemName[8];
    uint16_t bytesPerSector;
    uint8_t sectorsPerCluster;
    uint16_t reservedSectorCount;
    uint8_t fatCount;
    uint16_t rootDirEntryCount;
    uint16_t totalSectors16;
    uint8_t mediaType;
    uint16_t sectorsPerFat16;
    uint16_t sectorsPerTrack;
    uint16_t headCount;
    uint32_t hiddenSectors;
    uint32_t totalSectors32;
    union {
        struct {  // FAT16
            uint8_t driveNumber;
            uint8_t reserved1;
            uint8_t bootSignature;
            uint32_t volumeSerialNumber;
            char volumeLabel[11];
            char fileSystemType[8];
        } fat16;
        struct {  // FAT32
            uint32_t sectorsPerFat32;
            uint16_t fatFlags;
            uint16_t version;
            uint32_t rootCluster;
            uint16_t fsInfo;
            uint16_t backupBootSector;
            uint8_t reserved[12];
            uint8_t driveNumber;
            uint8_t reserved1;
            uint8_t bootSignature;
            uint32_t volumeSerialNumber;
            char volumeLabel[11];
            char fileSystemType[8];
        } fat32;
    };
} __attribute__((packed)) fat_boot_t;

// Directory entry structure
typedef struct {
    uint8_t name[11];         // 8.3 filename
    uint8_t attributes;        // File attributes
    uint8_t reserved;
    uint8_t creationTimeTenths;
    uint16_t creationTime;
    uint16_t creationDate;
    uint16_t lastAccessDate;
    uint16_t firstClusterHigh;  // High word of first cluster (FAT32)
    uint16_t writeTime;
    uint16_t writeDate;
    uint16_t firstClusterLow;   // Low word of first cluster
    uint32_t fileSize;
} __attribute__((packed)) dir_t;

// File attributes
#define DIR_ATT_READ_ONLY   0x01
#define DIR_ATT_HIDDEN      0x02
#define DIR_ATT_SYSTEM      0x04
#define DIR_ATT_VOLUME_ID   0x08
#define DIR_ATT_DIRECTORY   0x10
#define DIR_ATT_ARCHIVE     0x20
#define DIR_ATT_LONG_NAME   0x0F

// Special cluster values
#define CLUSTER_FREE        0x0000
#define CLUSTER_MIN_DATA    0x0002
#define FAT16_EOC_MIN       0xFFF8
#define FAT32_EOC_MIN       0x0FFFFFF8

//==============================================================================
// Sd2Card class - Low-level SD card SPI communication
//==============================================================================
class SdCardDriver::Sd2Card {
public:
    Sd2Card();
    
    bool init(spi_device_handle_t spi, uint8_t csPin);
    uint8_t type() const { return m_type; }
    uint8_t errorCode() const { return m_errorCode; }
    uint8_t errorData() const { return m_status; }
    uint32_t cardSize();
    
    bool readBlock(uint32_t block, uint8_t* dst);
    bool writeBlock(uint32_t blockNumber, const uint8_t* src);
    bool readData(uint32_t block, uint16_t offset, uint16_t count, uint8_t* dst);
    bool readCSD(csd_t* csd);
    
private:
    spi_device_handle_t m_spi;
    uint8_t m_csPin;
    uint8_t m_type;
    uint8_t m_errorCode;
    uint8_t m_status;
    uint8_t m_inBlock;
    uint16_t m_offset;
    uint32_t m_block;
    
    // Helper methods (ported from Arduino)
    void chipSelectLow();
    void chipSelectHigh();
    uint8_t spiSend(uint8_t data);
    uint8_t spiRec();
    uint8_t cardCommand(uint8_t cmd, uint32_t arg);
    uint8_t cardAcmd(uint8_t cmd, uint32_t arg);
    bool waitNotBusy(unsigned int timeoutMillis);
    bool waitStartBlock();
    void readEnd();
    uint8_t readRegister(uint8_t cmd, void* buf);
    void error(uint8_t code) { m_errorCode = code; }
    uint32_t millis() { return (uint32_t)(esp_timer_get_time() / 1000); }
};

//==============================================================================
// SdVolume class - FAT filesystem volume management
//==============================================================================
class SdCardDriver::SdVolume {
public:
    SdVolume();
    
    bool init(Sd2Card* dev);
    uint8_t fatType() const { return m_fatType; }
    uint32_t blocksPerCluster() const { return m_blocksPerCluster; }
    uint32_t blocksPerFat() const { return m_blocksPerFat; }
    uint32_t clusterCount() const { return m_clusterCount; }
    uint32_t dataStartBlock() const { return m_dataStartBlock; }
    uint32_t fatStartBlock() const { return m_fatStartBlock; }
    uint32_t rootDirStart() const { return m_rootDirStart; }
    
    bool cacheRawBlock(uint32_t blockNumber, uint8_t action);
    cache_t* cacheBuffer() { return &m_cacheBuffer; }
    void cacheDirty() { m_cacheDirty = true; }
    bool cacheFlush();
    
    bool fatGet(uint32_t cluster, uint32_t* value);
    bool fatPut(uint32_t cluster, uint32_t value);
    bool freeChain(uint32_t cluster);
    bool allocContiguous(uint32_t count, uint32_t* curCluster);
    
private:
    static Sd2Card* m_sdCard;
    static cache_t m_cacheBuffer;
    static bool m_cacheDirty;
    static uint32_t m_cacheBlockNumber;
    
    uint8_t m_fatType;
    uint8_t m_blocksPerCluster;
    uint16_t m_rootDirEntryCount;
    uint32_t m_fatCount;
    uint32_t m_blocksPerFat;
    uint32_t m_clusterCount;
    uint32_t m_dataStartBlock;
    uint32_t m_fatStartBlock;
    uint32_t m_rootDirStart;
};

//==============================================================================
// SdFile class - File operations
//==============================================================================
class SdCardDriver::SdFile {
public:
    // File open flags
    static const uint8_t O_READ = 0x01;
    static const uint8_t O_RDWR = 0x02;
    static const uint8_t O_WRITE = 0x02;
    static const uint8_t O_CREAT = 0x04;
    static const uint8_t O_APPEND = 0x08;
    static const uint8_t O_TRUNC = 0x10;
    static const uint8_t O_DIR = 0x20;
    
    SdFile();
    
    bool open(SdVolume* vol, const char* fileName, uint8_t oflag);
    bool isOpen() const { return m_vol != nullptr; }
    bool isDir() const { return (m_flags & O_DIR) != 0; }
    void close();
    
    int read();
    int read(void* buf, uint16_t nbyte);
    int write(const void* buf, size_t size);
    bool sync();
    
    uint32_t fileSize() const { return m_fileSize; }
    uint32_t curPosition() const { return m_curPosition; }
    bool seekSet(uint32_t pos);
    
private:
    
    SdVolume* m_vol;
    uint8_t m_flags;
    uint32_t m_fileSize;
    uint32_t m_firstCluster;
    uint32_t m_curCluster;
    uint32_t m_curPosition;
    uint16_t m_dirIndex;
    uint32_t m_dirBlock;
    
    bool addCluster();
    bool addDirCluster();
    dir_t* cacheDirEntry(uint8_t action);
};

//==============================================================================
// Sd2Card Implementation
//==============================================================================
SdCardDriver::Sd2Card::Sd2Card() 
    : m_spi(nullptr)
    , m_csPin(0)
    , m_type(0)
    , m_errorCode(0)
    , m_status(0)
    , m_inBlock(0)
    , m_offset(0)
    , m_block(0)
{
}

void SdCardDriver::Sd2Card::chipSelectLow() {
    gpio_set_level((gpio_num_t)m_csPin, 0);
}

void SdCardDriver::Sd2Card::chipSelectHigh() {
    gpio_set_level((gpio_num_t)m_csPin, 1);
}

uint8_t SdCardDriver::Sd2Card::spiSend(uint8_t data) {
    spi_transaction_t trans = {};
    uint8_t rx_data;
    
    trans.length = 8;  // 8 bits
    trans.tx_buffer = &data;
    trans.rx_buffer = &rx_data;
    
    esp_err_t ret = spi_device_polling_transmit(m_spi, &trans);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "SPI transmit failed: %s", esp_err_to_name(ret));
        return 0xFF;
    }
    
    return rx_data;
}

uint8_t SdCardDriver::Sd2Card::spiRec() {
    return spiSend(0xFF);
}

uint8_t SdCardDriver::Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
    // End read if in partial block read mode
    readEnd();
    
    // Select card
    chipSelectLow();
    
    // Wait up to 300 ms if busy
    waitNotBusy(300);
    
    // Send command
    spiSend(cmd | 0x40);
    
    // Send argument
    for (int8_t s = 24; s >= 0; s -= 8) {
        spiSend(arg >> s);
    }
    
    // Send CRC
    uint8_t crc = 0xFF;
    if (cmd == CMD0) {
        crc = 0x95;  // Correct CRC for CMD0 with arg 0
    }
    if (cmd == CMD8) {
        crc = 0x87;  // Correct CRC for CMD8 with arg 0x1AA
    }
    spiSend(crc);
    
    // Wait for response
    for (uint8_t i = 0; ((m_status = spiRec()) & 0x80) && i != 0xFF; i++);
    
    return m_status;
}

uint8_t SdCardDriver::Sd2Card::cardAcmd(uint8_t cmd, uint32_t arg) {
    cardCommand(CMD55, 0);
    return cardCommand(cmd, arg);
}

bool SdCardDriver::Sd2Card::waitNotBusy(unsigned int timeoutMillis) {
    unsigned int t0 = millis();
    unsigned int d;
    
    do {
        if (spiRec() == 0xFF) {
            return true;
        }
        d = millis() - t0;
    } while (d < timeoutMillis);
    
    return false;
}

bool SdCardDriver::Sd2Card::waitStartBlock() {
    unsigned int t0 = millis();
    
    while ((m_status = spiRec()) == 0xFF) {
        unsigned int d = millis() - t0;
        if (d > SD_READ_TIMEOUT) {
            error(SD_CARD_ERROR_READ_TIMEOUT);
            goto fail;
        }
    }
    
    if (m_status != DATA_START_BLOCK) {
        error(SD_CARD_ERROR_READ);
        goto fail;
    }
    return true;
    
fail:
    chipSelectHigh();
    return false;
}

void SdCardDriver::Sd2Card::readEnd() {
    if (m_inBlock) {
        // Skip data and CRC
        while (m_offset++ < 514) {
            spiRec();
        }
        chipSelectHigh();
        m_inBlock = 0;
    }
}

uint8_t SdCardDriver::Sd2Card::readRegister(uint8_t cmd, void* buf) {
    uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
    
    if (cardCommand(cmd, 0)) {
        error(SD_CARD_ERROR_READ_REG);
        goto fail;
    }
    
    if (!waitStartBlock()) {
        goto fail;
    }
    
    // Transfer data
    for (uint16_t i = 0; i < 16; i++) {
        dst[i] = spiRec();
    }
    
    spiRec();  // Get first CRC byte
    spiRec();  // Get second CRC byte
    chipSelectHigh();
    return true;
    
fail:
    chipSelectHigh();
    return false;
}

bool SdCardDriver::Sd2Card::readCSD(csd_t* csd) {
    return readRegister(CMD9, csd);
}

uint32_t SdCardDriver::Sd2Card::cardSize() {
    csd_t csd;
    if (!readCSD(&csd)) {
        return 0;
    }
    
    if (csd.v1.csd_ver == 0) {
        // CSD Version 1.0
        uint8_t read_bl_len = csd.v1.read_bl_len;
        uint16_t c_size = (csd.v1.c_size_high << 10)
                        | (csd.v1.c_size_mid << 2)
                        | csd.v1.c_size_low;
        uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1)
                             | csd.v1.c_size_mult_low;
        return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7);
    } else if (csd.v2.csd_ver == 1) {
        // CSD Version 2.0
        uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16)
                        | (csd.v2.c_size_mid << 8)
                        | csd.v2.c_size_low;
        return (c_size + 1) << 10;
    } else {
        error(SD_CARD_ERROR_BAD_CSD);
        return 0;
    }
}

bool SdCardDriver::Sd2Card::init(spi_device_handle_t spi, uint8_t csPin) {
    m_spi = spi;
    m_csPin = csPin;
    m_errorCode = m_inBlock = m_type = 0;
    
    unsigned int t0 = millis();
    uint32_t arg;
    
    // Configure CS pin as output with proper GPIO config
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << csPin);
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    gpio_config(&io_conf);
    chipSelectHigh();
    
    ESP_LOGI(TAG, "CS pin GPIO%d configured as output", csPin);
    
    // Must supply min of 74 clock cycles with CS high
    vTaskDelay(pdMS_TO_TICKS(10));  // Small delay
    for (uint8_t i = 0; i < 10; i++) {
        spiSend(0xFF);
    }
    
    chipSelectLow();
    
    // Command to go idle in SPI mode
    while ((m_status = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
        unsigned int d = millis() - t0;
        if (d > SD_INIT_TIMEOUT) {
            error(SD_CARD_ERROR_CMD0);
            goto fail;
        }
    }
    
    // Check SD version
    if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) {
        m_type = SD_CARD_TYPE_SD1;
    } else {
        // Only need last byte of r7 response
        for (uint8_t i = 0; i < 4; i++) {
            m_status = spiRec();
        }
        if (m_status != 0xAA) {
            error(SD_CARD_ERROR_CMD8);
            goto fail;
        }
        m_type = SD_CARD_TYPE_SD2;
    }
    
    // Initialize card and send host supports SDHC if SD2
    arg = (m_type == SD_CARD_TYPE_SD2) ? 0x40000000 : 0;
    
    while ((m_status = cardAcmd(ACMD41, arg)) != R1_READY_STATE) {
        // Check for timeout
        unsigned int d = millis() - t0;
        if (d > SD_INIT_TIMEOUT) {
            error(SD_CARD_ERROR_ACMD41);
            goto fail;
        }
    }
    
    // If SD2 read OCR register to check for SDHC card
    if (m_type == SD_CARD_TYPE_SD2) {
        if (cardCommand(CMD58, 0)) {
            error(SD_CARD_ERROR_CMD58);
            goto fail;
        }
        if ((spiRec() & 0xC0) == 0xC0) {
            m_type = SD_CARD_TYPE_SDHC;
        }
        // Discard rest of OCR - contains allowed voltage range
        for (uint8_t i = 0; i < 3; i++) {
            spiRec();
        }
    }
    
    chipSelectHigh();
    ESP_LOGI(TAG, "SD card initialized successfully. Type: %d", m_type);
    return true;
    
fail:
    chipSelectHigh();
    ESP_LOGE(TAG, "SD card initialization failed. Error: 0x%02X", m_errorCode);
    return false;
}

bool SdCardDriver::Sd2Card::readBlock(uint32_t block, uint8_t* dst) {
    return readData(block, 0, 512, dst);
}

bool SdCardDriver::Sd2Card::readData(uint32_t block, uint16_t offset, 
                                     uint16_t count, uint8_t* dst) {
    if (count == 0) {
        return true;
    }
    if ((count + offset) > 512) {
        goto fail;
    }
    
    if (!m_inBlock || block != m_block || offset < m_offset) {
        m_block = block;
        // Use address if not SDHC card
        if (m_type != SD_CARD_TYPE_SDHC) {
            block <<= 9;
        }
        if (cardCommand(CMD17, block)) {
            error(SD_CARD_ERROR_CMD17);
            goto fail;
        }
        if (!waitStartBlock()) {
            goto fail;
        }
        m_offset = 0;
        m_inBlock = 1;
    }
    
    // Skip data before offset
    for (; m_offset < offset; m_offset++) {
        spiRec();
    }
    
    // Transfer data
    for (uint16_t i = 0; i < count; i++) {
        dst[i] = spiRec();
    }
    
    m_offset += count;
    if (m_offset >= 512) {
        // Read rest of data, checksum and set chip select high
        readEnd();
    }
    return true;
    
fail:
    chipSelectHigh();
    return false;
}

bool SdCardDriver::Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) {
    // Use address if not SDHC card
    if (m_type != SD_CARD_TYPE_SDHC) {
        blockNumber <<= 9;
    }
    
    if (cardCommand(CMD24, blockNumber)) {
        error(SD_CARD_ERROR_CMD24);
        goto fail;
    }
    
    // Send data token
    spiSend(DATA_START_BLOCK);
    
    // Send data
    for (uint16_t i = 0; i < 512; i++) {
        spiSend(src[i]);
    }
    
    // Dummy CRC
    spiSend(0xFF);
    spiSend(0xFF);
    
    m_status = spiRec();
    if ((m_status & DATA_RES_MASK) != DATA_RES_ACCEPTED) {
        error(SD_CARD_ERROR_WRITE);
        goto fail;
    }
    
    // Wait for flash programming to complete
    if (!waitNotBusy(SD_WRITE_TIMEOUT)) {
        error(SD_CARD_ERROR_WRITE_TIMEOUT);
        goto fail;
    }
    
    // Response is r2 so get and check two bytes for nonzero
    if (cardCommand(CMD13, 0) || spiRec()) {
        error(SD_CARD_ERROR_WRITE_PROGRAMMING);
        goto fail;
    }
    
    chipSelectHigh();
    return true;
    
fail:
    chipSelectHigh();
    return false;
}

//==============================================================================
// SdVolume Implementation
//==============================================================================
SdCardDriver::Sd2Card* SdCardDriver::SdVolume::m_sdCard = nullptr;
cache_t SdCardDriver::SdVolume::m_cacheBuffer;
bool SdCardDriver::SdVolume::m_cacheDirty = false;
uint32_t SdCardDriver::SdVolume::m_cacheBlockNumber = 0xFFFFFFFF;

SdCardDriver::SdVolume::SdVolume()
    : m_fatType(0)
    , m_blocksPerCluster(0)
    , m_rootDirEntryCount(0)
    , m_fatCount(0)
    , m_blocksPerFat(0)
    , m_clusterCount(0)
    , m_dataStartBlock(0)
    , m_fatStartBlock(0)
    , m_rootDirStart(0)
{
}

bool SdCardDriver::SdVolume::cacheFlush() {
    if (m_cacheDirty) {
        if (!m_sdCard->writeBlock(m_cacheBlockNumber, m_cacheBuffer.data)) {
            return false;
        }
        m_cacheDirty = false;
    }
    return true;
}

bool SdCardDriver::SdVolume::cacheRawBlock(uint32_t blockNumber, uint8_t action) {
    if (m_cacheBlockNumber != blockNumber) {
        if (!cacheFlush()) {
            return false;
        }
        if (!m_sdCard->readBlock(blockNumber, m_cacheBuffer.data)) {
            return false;
        }
        m_cacheBlockNumber = blockNumber;
    }
    m_cacheDirty |= (action == 1);
    return true;
}

bool SdCardDriver::SdVolume::init(Sd2Card* dev) {
    m_sdCard = dev;
    
    // Read MBR
    cache_t* pc = &m_cacheBuffer;
    if (!cacheRawBlock(0, 0)) {
        ESP_LOGE(TAG, "Failed to read MBR");
        return false;
    }
    
    // Find FAT partition
    uint32_t volumeStartBlock = 0;
    uint8_t* p = pc->data + 0x1BE;  // First partition entry
    
    for (uint8_t i = 0; i < 4; i++, p += 16) {
        uint8_t partType = p[4];
        if (partType == 0x04 || partType == 0x06 || partType == 0x0B || 
            partType == 0x0C || partType == 0x0E) {
            // Found FAT partition
            volumeStartBlock = *((uint32_t*)(p + 8));
            break;
        }
    }
    
    // If no partition found, try whole device as FAT
    if (volumeStartBlock == 0) {
        ESP_LOGW(TAG, "No MBR partition found, trying whole device as FAT");
    }
    
    // Read volume boot record
    if (!cacheRawBlock(volumeStartBlock, 0)) {
        ESP_LOGE(TAG, "Failed to read volume boot record");
        return false;
    }
    
    fat_boot_t* boot = (fat_boot_t*)pc->data;
    
    // Validate boot record
    if (boot->bytesPerSector != 512) {
        ESP_LOGE(TAG, "Invalid sector size: %d", boot->bytesPerSector);
        return false;
    }
    
    m_blocksPerCluster = boot->sectorsPerCluster;
    m_fatCount = boot->fatCount;
    m_blocksPerFat = (boot->sectorsPerFat16 != 0) ? boot->sectorsPerFat16 : 
                                                     boot->fat32.sectorsPerFat32;
    m_rootDirEntryCount = boot->rootDirEntryCount;
    
    uint32_t totalBlocks = (boot->totalSectors16 != 0) ? boot->totalSectors16 : 
                                                          boot->totalSectors32;
    
    m_fatStartBlock = volumeStartBlock + boot->reservedSectorCount;
    uint32_t rootDirBlocks = ((m_rootDirEntryCount * 32) + 511) / 512;
    m_dataStartBlock = m_fatStartBlock + m_fatCount * m_blocksPerFat + rootDirBlocks;
    m_clusterCount = (totalBlocks - (m_dataStartBlock - volumeStartBlock)) / m_blocksPerCluster;
    
    // Determine FAT type
    if (m_clusterCount < 4085) {
        m_fatType = 12;  // FAT12 (not supported)
        ESP_LOGE(TAG, "FAT12 not supported");
        return false;
    } else if (m_clusterCount < 65525) {
        m_fatType = 16;
        m_rootDirStart = m_fatStartBlock + m_fatCount * m_blocksPerFat;
    } else {
        m_fatType = 32;
        m_rootDirStart = m_dataStartBlock + (boot->fat32.rootCluster - 2) * m_blocksPerCluster;
    }
    
    ESP_LOGI(TAG, "FAT%d volume initialized. Clusters: %lu", m_fatType, m_clusterCount);
    return true;
}

bool SdCardDriver::SdVolume::fatGet(uint32_t cluster, uint32_t* value) {
    if (cluster > (m_clusterCount + 1)) {
        return false;
    }
    
    uint32_t lba;
    if (m_fatType == 16) {
        lba = m_fatStartBlock + (cluster >> 8);
    } else {
        lba = m_fatStartBlock + (cluster >> 7);
    }
    
    if (lba != m_cacheBlockNumber) {
        if (!cacheRawBlock(lba, 0)) {
            return false;
        }
    }
    
    if (m_fatType == 16) {
        *value = *((uint16_t*)(m_cacheBuffer.data + ((cluster & 0xFF) << 1)));
    } else {
        *value = *((uint32_t*)(m_cacheBuffer.data + ((cluster & 0x7F) << 2))) & 0x0FFFFFFF;
    }
    
    return true;
}

bool SdCardDriver::SdVolume::fatPut(uint32_t cluster, uint32_t value) {
    if (cluster < 2 || cluster > (m_clusterCount + 1)) {
        return false;
    }
    
    uint32_t lba;
    if (m_fatType == 16) {
        lba = m_fatStartBlock + (cluster >> 8);
    } else {
        lba = m_fatStartBlock + (cluster >> 7);
    }
    
    if (lba != m_cacheBlockNumber) {
        if (!cacheRawBlock(lba, 0)) {
            return false;
        }
    }
    
    if (m_fatType == 16) {
        *((uint16_t*)(m_cacheBuffer.data + ((cluster & 0xFF) << 1))) = value;
    } else {
        *((uint32_t*)(m_cacheBuffer.data + ((cluster & 0x7F) << 2))) = value;
    }
    
    m_cacheDirty = true;
    return true;
}

bool SdCardDriver::SdVolume::freeChain(uint32_t cluster) {
    uint32_t next;
    
    while (cluster != 0) {
        if (!fatGet(cluster, &next)) {
            return false;
        }
        if (!fatPut(cluster, 0)) {
            return false;
        }
        cluster = next;
    }
    
    return true;
}

bool SdCardDriver::SdVolume::allocContiguous(uint32_t count, uint32_t* curCluster) {
    // Simplified version - find contiguous free clusters
    uint32_t start = 0;
    uint32_t found = 0;
    
    for (uint32_t cluster = 2; cluster < m_clusterCount + 2; cluster++) {
        uint32_t value;
        if (!fatGet(cluster, &value)) {
            return false;
        }
        
        if (value == 0) {
            if (start == 0) {
                start = cluster;
            }
            found++;
            if (found >= count) {
                // Allocate clusters
                for (uint32_t c = start; c < start + count - 1; c++) {
                    if (!fatPut(c, c + 1)) {
                        return false;
                    }
                }
                // Mark last cluster as EOC
                uint32_t eoc = (m_fatType == 16) ? 0xFFFF : 0x0FFFFFFF;
                if (!fatPut(start + count - 1, eoc)) {
                    return false;
                }
                *curCluster = start;
                return true;
            }
        } else {
            start = 0;
            found = 0;
        }
    }
    
    return false;
}

//==============================================================================
// SdFile Implementation
//==============================================================================
SdCardDriver::SdFile::SdFile()
    : m_vol(nullptr)
    , m_flags(0)
    , m_fileSize(0)
    , m_firstCluster(0)
    , m_curCluster(0)
    , m_curPosition(0)
    , m_dirIndex(0)
    , m_dirBlock(0)
{
}

void SdCardDriver::SdFile::close() {
    if (m_vol) {
        sync();
        m_vol = nullptr;
    }
}

bool SdCardDriver::SdFile::sync() {
    if (m_flags & (O_WRITE | O_CREAT)) {
        dir_t* d = cacheDirEntry(1);  // Write action
        if (!d) {
            return false;
        }
        
        d->fileSize = m_fileSize;
        
        // Update timestamps (simplified - use current time)
        d->writeTime = 0;
        d->writeDate = 0;
        
        return m_vol->cacheFlush();
    }
    return true;
}

dir_t* SdCardDriver::SdFile::cacheDirEntry(uint8_t action) {
    if (!m_vol->cacheRawBlock(m_dirBlock, action)) {
        return nullptr;
    }
    return (dir_t*)(buffer + m_dirIndex * 32);
    //return (dir_t*)(m_vol->cacheBuffer()->data + (m_dirIndex & 0x0F) * 32);
}

bool SdCardDriver::SdFile::seekSet(uint32_t pos) {
    if (pos > m_fileSize) {
        return false;
    }
    
    m_curPosition = pos;
    
    // Calculate cluster from position
    uint32_t clusterOffset = pos / (m_vol->blocksPerCluster() * 512);
    m_curCluster = m_firstCluster;
    
    for (uint32_t i = 0; i < clusterOffset && m_curCluster != 0; i++) {
        uint32_t next;
        if (!m_vol->fatGet(m_curCluster, &next)) {
            return false;
        }
        m_curCluster = next;
    }
    
    return true;
}

int SdCardDriver::SdFile::read() {
    uint8_t b;
    if (read(&b, 1) == 1) {
        return b;
    }
    return -1;
}

int SdCardDriver::SdFile::read(void* buf, uint16_t nbyte) {
    uint8_t* dst = (uint8_t*)buf;
    uint16_t count = 0;
    
    while (count < nbyte && m_curPosition < m_fileSize) {
        uint32_t blockOffset = m_curPosition % 512;
        uint32_t toRead = 512 - blockOffset;
        if (toRead > (nbyte - count)) {
            toRead = nbyte - count;
        }
        if (toRead > (m_fileSize - m_curPosition)) {
            toRead = m_fileSize - m_curPosition;
        }
        
        // Calculate block number
        uint32_t clusterOffset = (m_curPosition / 512) % m_vol->blocksPerCluster();
        uint32_t blockNumber = m_vol->dataStartBlock() + 
                              (m_curCluster - 2) * m_vol->blocksPerCluster() + 
                              clusterOffset;
        
        if (!m_vol->cacheRawBlock(blockNumber, 0)) {
            return -1;
        }
        
        memcpy(dst + count, m_vol->cacheBuffer()->data + blockOffset, toRead);
        count += toRead;
        m_curPosition += toRead;
        
        // Move to next cluster if needed
        if ((m_curPosition % (m_vol->blocksPerCluster() * 512)) == 0) {
            uint32_t next;
            if (!m_vol->fatGet(m_curCluster, &next)) {
                return -1;
            }
            m_curCluster = next;
        }
    }
    
    return count;
}

int SdCardDriver::SdFile::write(const void* buf, size_t size) {
    const uint8_t* src = (const uint8_t*)buf;
    size_t count = 0;
    
    while (count < size) {
        uint32_t blockOffset = m_curPosition % 512;
        uint32_t toWrite = 512 - blockOffset;
        if (toWrite > (size - count)) {
            toWrite = size - count;
        }
        
        // Calculate block number
        uint32_t clusterOffset = (m_curPosition / 512) % m_vol->blocksPerCluster();
        uint32_t blockNumber = m_vol->dataStartBlock() + 
                              (m_curCluster - 2) * m_vol->blocksPerCluster() + 
                              clusterOffset;
        
        // Read block first if partial write
        if (blockOffset != 0 || toWrite != 512) {
            if (!m_vol->cacheRawBlock(blockNumber, 0)) {
                return -1;
            }
        }
        
        // Copy data to cache
        memcpy(m_vol->cacheBuffer()->data + blockOffset, src + count, toWrite);
        m_vol->cacheDirty();
        
        count += toWrite;
        m_curPosition += toWrite;
        if (m_curPosition > m_fileSize) {
            m_fileSize = m_curPosition;
        }
        
        // Move to next cluster if needed
        if ((m_curPosition % (m_vol->blocksPerCluster() * 512)) == 0) {
            uint32_t next;
            if (!m_vol->fatGet(m_curCluster, &next)) {
                // Allocate new cluster
                if (!addCluster()) {
                    return -1;
                }
            } else {
                m_curCluster = next;
            }
        }
    }
    
    return count;
}

bool SdCardDriver::SdFile::addCluster() {
    uint32_t newCluster;
    
    // Find free cluster (simplified - just find next free)
    for (newCluster = 2; newCluster < m_vol->clusterCount() + 2; newCluster++) {
        uint32_t value;
        if (!m_vol->fatGet(newCluster, &value)) {
            return false;
        }
        if (value == 0) {
            break;
        }
    }
    
    if (newCluster >= m_vol->clusterCount() + 2) {
        return false;  // Disk full
    }
    
    // Link current cluster to new cluster
    if (!m_vol->fatPut(m_curCluster, newCluster)) {
        return false;
    }
    
    // Mark new cluster as EOC
    uint32_t eoc = (m_vol->fatType() == 16) ? 0xFFFF : 0x0FFFFFFF;
    if (!m_vol->fatPut(newCluster, eoc)) {
        return false;
    }
    
    m_curCluster = newCluster;
    return true;
}

bool SdCardDriver::SdFile::open(SdVolume* vol, const char* fileName, uint8_t oflag) {
    m_vol = vol;
    m_flags = oflag;
    
    // Simplified: only support root directory and 8.3 filenames
    // Open root directory
    uint32_t cluster = vol->rootDirStartCluster(); // store this
    
    // Search for file
    bool found = false;
    uint32_t dirIndex = 0;
    
    // Parse filename to 8.3 format
    char name83[11];
    memset(name83, ' ', 11);
    
    const char* dot = strchr(fileName, '.');
    size_t nameLen = dot ? (dot - fileName) : strlen(fileName);
    if (nameLen > 8) nameLen = 8;
    memcpy(name83, fileName, nameLen);
    
    if (dot && strlen(dot + 1) > 0) {
        size_t extLen = strlen(dot + 1);
        if (extLen > 3) extLen = 3;
        memcpy(name83 + 8, dot + 1, extLen);
    }
    
    // Convert to uppercase
    for (int i = 0; i < 11; i++) {
        if (name83[i] >= 'a' && name83[i] <= 'z') {
            name83[i] -= 32;
        }
    }
    
    // Search directory
    while (cluster >= CLUSTER_MIN_DATA) {

            for (uint32_t block = 0; block < vol->blocksPerCluster(); block++) {
                uint32_t lba = vol->dataStartBlock() +
                            (cluster - 2) * vol->blocksPerCluster() +
                            block;

                if (!vol->cacheRawBlock(lba, 0)) return false;

                // scan 16 directory entries here
            }

            uint32_t next;
            if (!vol->fatGet(cluster, &next)) return false;
            if (next >= FAT32_EOC_MIN) break;

            cluster = next;
        }
        
        dir_t* dir = (dir_t*)vol->cacheBuffer()->data;
        for (uint8_t i = 0; i < 16; i++, dirIndex++) {
            if (dir[i].name[0] == 0x00) {
                // End of directory
                break;
            }
            if (dir[i].name[0] == 0xE5) {
                // Deleted entry
                continue;
            }
            if (dir[i].attributes & (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY)) {
                // Skip volumes and directories
                continue;
            }
            
            if (memcmp(dir[i].name, name83, 11) == 0) {
                // Found file
                found = true;
                m_dirBlock = dirBlock + block;
                m_dirIndex = i;
                m_fileSize = dir[i].fileSize;
                m_firstCluster = ((uint32_t)dir[i].firstClusterHigh << 16) | 
                                dir[i].firstClusterLow;
                m_curCluster = m_firstCluster;
                m_curPosition = 0;
                
                if (oflag & O_TRUNC) {
                    m_fileSize = 0;
                    if (m_firstCluster != 0) {
                        vol->freeChain(m_firstCluster);
                        m_firstCluster = 0;
                        m_curCluster = 0;
                    }
                }
                
                if (oflag & O_APPEND) {
                    seekSet(m_fileSize);
                }
                
                return true;
            }
        }
    }
    
    // File not found - create if O_CREAT
    if (!found && (oflag & O_CREAT)) {
        // Find free directory entry
        dirIndex = 0;
        for (uint32_t block = 0; block < 32; block++) {
            if (!vol->cacheRawBlock(dirBlock + block, 0)) {
                return false;
            }
            
            dir_t* dir = (dir_t*)vol->cacheBuffer()->data;
            for (uint8_t i = 0; i < 16; i++, dirIndex++) {
                if (dir[i].name[0] == 0x00 || dir[i].name[0] == 0xE5) {
                    // Found free entry
                    memset(&dir[i], 0, sizeof(dir_t));
                    memcpy(dir[i].name, name83, 11);
                    dir[i].attributes = DIR_ATT_ARCHIVE;
                    
                    // Allocate first cluster
                    if (!vol->allocContiguous(1, &m_firstCluster)) {
                        return false;
                    }
                    
                    dir[i].firstClusterHigh = m_firstCluster >> 16;
                    dir[i].firstClusterLow = m_firstCluster & 0xFFFF;
                    dir[i].fileSize = 0;
                    
                    m_dirBlock = dirBlock + block;
                    m_dirIndex = i;
                    m_fileSize = 0;
                    m_curCluster = m_firstCluster;
                    m_curPosition = 0;
                    
                    vol->cacheDirty();
                    vol->cacheFlush();
                    
                    ESP_LOGI(TAG, "Created file: %s", fileName);
                    return true;
                }
            }
        }
        
        ESP_LOGE(TAG, "Directory full");
        return false;
    }
    
    ESP_LOGE(TAG, "File not found: %s", fileName);
    return false;
}

//==============================================================================
// SdCardDriver Implementation
//==============================================================================
SdCardDriver::SdCardDriver()
    : m_mounted(false)
    , m_spiHost(SPI3_HOST)
    , m_csPin(0)
    , m_spiDevice(nullptr)
    , m_card(nullptr)
    , m_volume(nullptr)
{
    memset(m_mountPoint, 0, sizeof(m_mountPoint));
}

SdCardDriver::~SdCardDriver() {
    unmount();
}

bool SdCardDriver::initializeSpi() {
    // SPI device configuration for SD card
    // Start with slow clock (400kHz) for initialization
    spi_device_interface_config_t dev_cfg = {};
    dev_cfg.command_bits = 0;
    dev_cfg.address_bits = 0;
    dev_cfg.dummy_bits = 0;
    dev_cfg.mode = 0;  // SPI_MODE0
    dev_cfg.clock_speed_hz = 400000;  // 400 kHz for initialization
    dev_cfg.spics_io_num = -1;  // Manual CS control
    dev_cfg.queue_size = 1;
    dev_cfg.flags = 0;
    
    esp_err_t ret = spi_bus_add_device(m_spiHost, &dev_cfg, &m_spiDevice);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to add SPI device: %s", esp_err_to_name(ret));
        return false;
    }
    
    return true;
}

void SdCardDriver::deinitializeSpi() {
    if (m_spiDevice) {
        spi_bus_remove_device(m_spiDevice);
        m_spiDevice = nullptr;
    }
}

bool SdCardDriver::mount(const char* mount_point, spi_host_device_t spi_host, uint8_t cs_pin) {
    if (m_mounted) {
        ESP_LOGW(TAG, "SD card already mounted");
        return true;
    }
    
    m_spiHost = spi_host;
    m_csPin = cs_pin;
    strncpy(m_mountPoint, mount_point, sizeof(m_mountPoint) - 1);
    
    ESP_LOGI(TAG, "Mounting SD card on %s (CS: GPIO%d)", mount_point, cs_pin);
    
    // Initialize SPI device
    if (!initializeSpi()) {
        return false;
    }
    
    // Create and initialize SD card
    m_card = new Sd2Card();
    if (!m_card->init(m_spiDevice, cs_pin)) {
        ESP_LOGE(TAG, "SD card initialization failed");
        delete m_card;
        m_card = nullptr;
        deinitializeSpi();
        return false;
    }
    
    // Create and initialize volume
    m_volume = new SdVolume();
    if (!m_volume->init(m_card)) {
        ESP_LOGE(TAG, "Failed to initialize FAT volume");
        delete m_volume;
        delete m_card;
        m_volume = nullptr;
        m_card = nullptr;
        deinitializeSpi();
        return false;
    }
    
    m_mounted = true;
    ESP_LOGI(TAG, "SD card mounted successfully");
    return true;
}

void SdCardDriver::unmount() {
    if (m_mounted) {
        if (m_volume) {
            m_volume->cacheFlush();
            delete m_volume;
            m_volume = nullptr;
        }
        
        if (m_card) {
            delete m_card;
            m_card = nullptr;
        }
        
        deinitializeSpi();
        m_mounted = false;
        ESP_LOGI(TAG, "SD card unmounted");
    }
}

uint8_t SdCardDriver::getCardType() const {
    return m_card ? m_card->type() : 0;
}

uint64_t SdCardDriver::getCardSize() const {
    if (!m_card) {
        return 0;
    }
    return (uint64_t)m_card->cardSize() * 512;
}

int SdCardDriver::open(const char* filepath, uint8_t mode) {
    if (!m_mounted) {
        ESP_LOGE(TAG, "SD card not mounted");
        return -1;
    }
    
    // Create file object
    SdFile* file = new SdFile();
    
    // Convert mode
    uint8_t oflag = 0;
    if (mode & SD_FILE_READ) {
        oflag |= SdFile::O_READ;
    }
    if (mode & SD_FILE_WRITE) {
        oflag |= SdFile::O_WRITE | SdFile::O_CREAT | SdFile::O_APPEND;
    }
    
    // Skip mount point from filepath
    const char* filename = filepath;
    if (strncmp(filepath, m_mountPoint, strlen(m_mountPoint)) == 0) {
        filename = filepath + strlen(m_mountPoint);
        if (filename[0] == '/') {
            filename++;
        }
    }
    
    if (!file->open(m_volume, filename, oflag)) {
        delete file;
        return -1;
    }
    
    // Return file pointer as descriptor (simplified)
    return (int)file;
}

bool SdCardDriver::close(int fd) {
    if (fd < 0) {
        return false;
    }
    
    SdFile* file = (SdFile*)fd;
    file->close();
    
    // Flush volume cache to ensure all data is written  to SD card
    if (m_volume) {
        m_volume->cacheFlush();
    }
    
    delete file;
    return true;
}

int SdCardDriver::write(int fd, const void* data, size_t size) {
    if (fd < 0) {
        return -1;
    }
    
    SdFile* file = (SdFile*)fd;
    return file->write(data, size);
}

int SdCardDriver::read(int fd, void* buffer, size_t size) {
    if (fd < 0) {
        return -1;
    }
    
    SdFile* file = (SdFile*)fd;
    return file->read(buffer, (uint16_t)size);
}

bool SdCardDriver::exists(const char* filepath) {
    // Simplified: try to open file for reading
    int fd = open(filepath, SD_FILE_READ);
    if (fd >= 0) {
        close(fd);
        return true;
    }
    return false;
}

bool SdCardDriver::mkdir(const char* dirpath) {
    // Not implemented in this simplified version
    ESP_LOGW(TAG, "mkdir not implemented");
    return false;
}

bool SdCardDriver::remove(const char* filepath) {
    // Not implemented in this simplified version
    ESP_LOGW(TAG, "remove not implemented");
    return false;
}

bool SdCardDriver::rmdir(const char* dirpath) {
    // Not implemented in this simplified version
    ESP_LOGW(TAG, "rmdir not implemented");
    return false;
}
