/*
 * SD Card Driver Implementation
 */

#include "sd_card.h"
#include "hardware/gpio.h"
#include "board_config.h"
#include "ff.h"  // FatFS
#include <string.h>
#include <stdio.h>

static const sd_card_config_t *g_config = NULL;
static sd_card_info_t g_card_info = {0};

// Helper: Select SD card (CS low)
static inline void sd_card_select(void) {
    gpio_put(g_config->gpio_cs, 0);
    sleep_us(1);
}

// Helper: Deselect SD card (CS high)
static inline void sd_card_deselect(void) {
    sleep_us(1);
    gpio_put(g_config->gpio_cs, 1);
    sleep_us(1);
}

// Helper: Send a byte and receive response
static uint8_t sd_card_transfer(uint8_t data) {
    uint8_t rx;
    spi_write_read_blocking(g_config->spi, &data, &rx, 1);
    return rx;
}

// Helper: Send dummy clocks
static void sd_card_send_dummy_clocks(int count) {
    for (int i = 0; i < count; i++) {
        sd_card_transfer(0xFF);
    }
}

// Helper: Wait for card ready
static bool sd_card_wait_ready(uint32_t timeout_ms) {
    uint32_t start = to_ms_since_boot(get_absolute_time());
    while ((to_ms_since_boot(get_absolute_time()) - start) < timeout_ms) {
        if (sd_card_transfer(0xFF) == 0xFF) {
            return true;
        }
        sleep_us(100);
    }
    return false;
}

// Helper: Send command to SD card
static uint8_t sd_card_send_command(uint8_t cmd, uint32_t arg) {
    uint8_t crc = 0xFF;
    
    // Special case for CMD0 and CMD8
    if (cmd == SD_CMD0) crc = 0x95;
    if (cmd == SD_CMD8) crc = 0x87;
    
    // Wait for card ready
    sd_card_wait_ready(500);
    
    // Send command packet
    sd_card_transfer(0x40 | cmd);
    sd_card_transfer((arg >> 24) & 0xFF);
    sd_card_transfer((arg >> 16) & 0xFF);
    sd_card_transfer((arg >> 8) & 0xFF);
    sd_card_transfer(arg & 0xFF);
    sd_card_transfer(crc);
    
    // Wait for response (up to 10 bytes)
    uint8_t response;
    for (int i = 0; i < 10; i++) {
        response = sd_card_transfer(0xFF);
        if ((response & 0x80) == 0) {
            return response;
        }
    }
    
    return 0xFF;  // Timeout
}

// Helper: Send application-specific command
static uint8_t sd_card_send_acmd(uint8_t acmd, uint32_t arg) {
    sd_card_send_command(SD_CMD55, 0);
    return sd_card_send_command(acmd, arg);
}

bool sd_card_init(const sd_card_config_t *config) {
    if (config == NULL) return false;
    
    g_config = config;
    memset(&g_card_info, 0, sizeof(g_card_info));
    
    // Initialize CS pin (active low)
    gpio_init(config->gpio_cs);
    gpio_set_dir(config->gpio_cs, GPIO_OUT);
    gpio_put(config->gpio_cs, 1);  // Deselect initially
    
    // Configure MISO pin for SPI (MUST be done for SD card reads to work)
    gpio_set_function(config->gpio_miso, GPIO_FUNC_SPI);
    
    // Save current SPI speed
    uint baudrate = spi_get_baudrate(config->spi);
    
    // Slow down SPI for SD card initialization (100-400 kHz recommended)
    spi_set_baudrate(config->spi, 400 * 1000);  // 400 kHz
    
    printf("[SD] Init: CS=%d, MISO=%d, MOSI=%d, SCK=%d\n", 
           config->gpio_cs, config->gpio_miso, config->gpio_mosi, config->gpio_sck);
    printf("[SD] SPI speed set to 400 kHz for initialization\n");
    
    // Send at least 74 dummy clocks with CS high to initialize card
    sd_card_deselect();
    sd_card_send_dummy_clocks(10);
    
    // Enter SPI mode (CMD0)
    sd_card_select();
    uint8_t r1 = sd_card_send_command(SD_CMD0, 0);
    sd_card_deselect();
    
    printf("[SD] CMD0 response: 0x%02X (expected 0x01)\n", r1);
    
    if (r1 != SD_R1_IDLE_STATE) {
        printf("[SD] Card not responding to CMD0\n");
        return false;  // Card not responding
    }
    
    // Check card version (CMD8)
    sd_card_select();
    r1 = sd_card_send_command(SD_CMD8, 0x1AA);
    
    printf("[SD] CMD8 response: 0x%02X\n", r1);
    
    if (r1 == SD_R1_IDLE_STATE) {
        // SD v2.0 or later
        printf("[SD] Detected SD v2.0 or later\n");
        uint8_t ocr[4];
        for (int i = 0; i < 4; i++) {
            ocr[i] = sd_card_transfer(0xFF);
        }
        sd_card_deselect();
        
        printf("[SD] OCR response: %02X %02X %02X %02X\n", ocr[0], ocr[1], ocr[2], ocr[3]);
        
        // Check if voltage range is supported
        if (ocr[2] != 0x01 || ocr[3] != 0xAA) {
            printf("[SD] Voltage range not supported\n");
            return false;
        }
        
        // Initialize card (ACMD41 with HCS bit)
        printf("[SD] Initializing with ACMD41...\n");
        uint32_t timeout = 1000;  // 1 second timeout
        uint32_t start = to_ms_since_boot(get_absolute_time());
        
        do {
            sd_card_select();
            r1 = sd_card_send_acmd(SD_ACMD41, 0x40000000);
            sd_card_deselect();
            
            if (r1 == SD_R1_READY) break;
            
            sleep_ms(10);
        } while ((to_ms_since_boot(get_absolute_time()) - start) < timeout);
        
        printf("[SD] ACMD41 final response: 0x%02X (expected 0x00)\n", r1);
        
        if (r1 != SD_R1_READY) {
            printf("[SD] ACMD41 initialization timeout\n");
            return false;  // Initialization failed
        }
        
        // Read OCR to check card type
        sd_card_select();
        r1 = sd_card_send_command(SD_CMD58, 0);
        if (r1 == SD_R1_READY) {
            uint8_t ocr_resp[4];
            for (int i = 0; i < 4; i++) {
                ocr_resp[i] = sd_card_transfer(0xFF);
            }
            
            // Check CCS bit (bit 30)
            if (ocr_resp[0] & 0x40) {
                g_card_info.type = SD_CARD_TYPE_SDHC;
                printf("[SD] Card type: SDHC\n");
            } else {
                g_card_info.type = SD_CARD_TYPE_SD2;
                printf("[SD] Card type: SD v2\n");
            }
        }
        sd_card_deselect();
        
    } else {
        // SD v1.x or MMC
        printf("[SD] Trying SD v1.x initialization\n");
        sd_card_deselect();
        
        // Try ACMD41
        sd_card_select();
        r1 = sd_card_send_acmd(SD_ACMD41, 0);
        sd_card_deselect();
        
        uint32_t timeout = 1000;
        uint32_t start = to_ms_since_boot(get_absolute_time());
        
        if (r1 <= 1) {
            // SD v1.x
            do {
                sd_card_select();
                r1 = sd_card_send_acmd(SD_ACMD41, 0);
                sd_card_deselect();
                
                if (r1 == SD_R1_READY) break;
                sleep_ms(10);
            } while ((to_ms_since_boot(get_absolute_time()) - start) < timeout);
            
            g_card_info.type = SD_CARD_TYPE_SD1;
        } else {
            return false;  // Not supported
        }
        
        if (r1 != SD_R1_READY) {
            return false;
        }
    }
    
    // Set block length to 512 bytes (for non-SDHC cards)
    if (g_card_info.type != SD_CARD_TYPE_SDHC) {
        sd_card_select();
        r1 = sd_card_send_command(SD_CMD16, SD_BLOCK_SIZE);
        sd_card_deselect();
        
        if (r1 != SD_R1_READY) {
            return false;
        }
    }
    
    g_card_info.initialized = true;
    
    // Increase SPI speed for data transfer (up to 25 MHz for SD cards)
    // Be conservative to avoid reliability issues
    spi_set_baudrate(config->spi, 12500 * 1000);  // 12.5 MHz (safe speed)
    
    return true;
}

bool sd_card_get_info(sd_card_info_t *info) {
    if (info == NULL || !g_card_info.initialized) return false;
    
    memcpy(info, &g_card_info, sizeof(sd_card_info_t));
    return true;
}

bool sd_card_read_block(uint32_t block_addr, uint8_t *buffer) {
    if (!g_card_info.initialized || buffer == NULL) return false;
    
    // For non-SDHC cards, convert block address to byte address
    if (g_card_info.type != SD_CARD_TYPE_SDHC) {
        block_addr *= SD_BLOCK_SIZE;
    }
    
    sd_card_select();
    
    // Send read command
    uint8_t r1 = sd_card_send_command(SD_CMD17, block_addr);
    if (r1 != SD_R1_READY) {
        sd_card_deselect();
        return false;
    }
    
    // Wait for start token
    uint32_t timeout = 200;  // 200ms timeout
    uint32_t start = to_ms_since_boot(get_absolute_time());
    uint8_t token;
    
    do {
        token = sd_card_transfer(0xFF);
        if (token == SD_START_TOKEN) break;
    } while ((to_ms_since_boot(get_absolute_time()) - start) < timeout);
    
    if (token != SD_START_TOKEN) {
        sd_card_deselect();
        return false;
    }
    
    // Read data block
    for (int i = 0; i < SD_BLOCK_SIZE; i++) {
        buffer[i] = sd_card_transfer(0xFF);
    }
    
    // Read CRC (2 bytes, but we ignore them)
    sd_card_transfer(0xFF);
    sd_card_transfer(0xFF);
    
    sd_card_deselect();
    return true;
}

bool sd_card_read_blocks(uint32_t block_addr, uint32_t num_blocks, uint8_t *buffer) {
    if (!g_card_info.initialized || buffer == NULL || num_blocks == 0) return false;
    
    // Simple implementation: read one block at a time
    // Can be optimized with CMD18 (READ_MULTIPLE_BLOCK)
    for (uint32_t i = 0; i < num_blocks; i++) {
        if (!sd_card_read_block(block_addr + i, buffer + (i * SD_BLOCK_SIZE))) {
            return false;
        }
    }
    
    return true;
}

bool sd_card_write_block(uint32_t block_addr, const uint8_t *buffer) {
    if (!g_card_info.initialized || buffer == NULL) return false;
    
    // For non-SDHC cards, convert block address to byte address
    if (g_card_info.type != SD_CARD_TYPE_SDHC) {
        block_addr *= SD_BLOCK_SIZE;
    }
    
    sd_card_select();
    
    // Send write command
    uint8_t r1 = sd_card_send_command(SD_CMD24, block_addr);
    if (r1 != SD_R1_READY) {
        sd_card_deselect();
        return false;
    }
    
    // Send start token
    sd_card_transfer(SD_START_TOKEN);
    
    // Write data block
    for (int i = 0; i < SD_BLOCK_SIZE; i++) {
        sd_card_transfer(buffer[i]);
    }
    
    // Send dummy CRC (2 bytes)
    sd_card_transfer(0xFF);
    sd_card_transfer(0xFF);
    
    // Check data response
    uint8_t response = sd_card_transfer(0xFF);
    if ((response & 0x1F) != SD_DATA_ACCEPTED) {
        sd_card_deselect();
        return false;
    }
    
    // Wait for card to finish writing
    if (!sd_card_wait_ready(500)) {
        sd_card_deselect();
        return false;
    }
    
    sd_card_deselect();
    return true;
}

bool sd_card_write_blocks(uint32_t block_addr, uint32_t num_blocks, const uint8_t *buffer) {
    if (!g_card_info.initialized || buffer == NULL || num_blocks == 0) return false;
    
    // Simple implementation: write one block at a time
    // Can be optimized with CMD25 (WRITE_MULTIPLE_BLOCK)
    for (uint32_t i = 0; i < num_blocks; i++) {
        if (!sd_card_write_block(block_addr + i, buffer + (i * SD_BLOCK_SIZE))) {
            return false;
        }
    }
    
    return true;
}

bool sd_card_erase_blocks(uint32_t start_block, uint32_t end_block) {
    // Erase functionality - implementation depends on specific requirements
    // Would use CMD32, CMD33, and CMD38
    // Not implemented in this basic version
    return false;
}

bool sd_card_is_ready(void) {
    return g_card_info.initialized;
}

bool sd_card_init_with_board_config(void) {
    // Build configuration from board_config.h
    static const sd_card_config_t config = {
        .spi = SD_SPI_PORT,
        .gpio_cs = SD_CS_PIN,
        .gpio_miso = DISPLAY_MISO_PIN,
        .gpio_mosi = DISPLAY_MOSI_PIN,
        .gpio_sck = DISPLAY_SCK_PIN
    };
    
    return sd_card_init(&config);
}

bool sd_card_test_fatfs(void) {
    if (!g_card_info.initialized) {
        printf("SD Card not initialized\n");
        return false;
    }
    
    printf("SD Card initialized: Type=%d\n", g_card_info.type);
    
    // Try to read first block
    uint8_t buffer[512];
    if (sd_card_read_block(0, buffer)) {
        printf("Read block 0: ");
        for (int i = 0; i < 16; i++) {
            printf("%02X ", buffer[i]);
        }
        printf("\n");
    }
    
    // Test FatFS filesystem
    printf("\n=== FatFS Test ===\n");
    FATFS fs;
    FRESULT res = f_mount(&fs, "0:", 1);  // Mount drive 0
    
    if (res != FR_OK) {
        printf("✗ FatFS mount failed (error: %d)\n", res);
        printf("  Make sure SD card is formatted as FAT/FAT32\n");
        printf("==================\n\n");
        return false;
    }
    
    printf("✓ FatFS mounted successfully\n");
    
    // Get volume information
    DWORD fre_clust, fre_sect, tot_sect;
    FATFS *fs_ptr;
    res = f_getfree("0:", &fre_clust, &fs_ptr);
    if (res == FR_OK) {
        tot_sect = (fs_ptr->n_fatent - 2) * fs_ptr->csize;
        fre_sect = fre_clust * fs_ptr->csize;
        printf("  Total: %lu KB, Free: %lu KB\n", 
               tot_sect / 2, fre_sect / 2);
    }
    
    // List root directory
    printf("\nRoot directory contents:\n");
    DIR dir;
    FILINFO fno;
    res = f_opendir(&dir, "/");
    if (res == FR_OK) {
        int file_count = 0;
        while (1) {
            res = f_readdir(&dir, &fno);
            if (res != FR_OK || fno.fname[0] == 0) break;
            
            printf("  %s %s (%lu bytes)\n", 
                   (fno.fattrib & AM_DIR) ? "[DIR]" : "[FILE]",
                   fno.fname, fno.fsize);
            file_count++;
            
            if (file_count >= 10) {
                printf("  ... (showing first 10 entries)\n");
                break;
            }
        }
        f_closedir(&dir);
        
        if (file_count == 0) {
            printf("  (empty)\n");
        }
    }
    
    // Test file write
    printf("\nTesting file write...\n");
    FIL fil;
    res = f_open(&fil, "test.txt", FA_CREATE_ALWAYS | FA_WRITE);
    bool write_success = false;
    if (res == FR_OK) {
        const char *test_str = "Hello from RP2350 with FatFS!\n";
        UINT bytes_written;
        res = f_write(&fil, test_str, strlen(test_str), &bytes_written);
        f_close(&fil);
        
        if (res == FR_OK) {
            printf("✓ Wrote %u bytes to test.txt\n", bytes_written);
            
            // Read it back
            res = f_open(&fil, "test.txt", FA_READ);
            if (res == FR_OK) {
                char read_buffer[64];
                UINT bytes_read;
                res = f_read(&fil, read_buffer, sizeof(read_buffer)-1, &bytes_read);
                f_close(&fil);
                
                if (res == FR_OK) {
                    read_buffer[bytes_read] = '\0';
                    printf("✓ Read back: %s", read_buffer);
                    write_success = true;
                }
            }
        }
    } else {
        printf("✗ Failed to create test.txt (error: %d)\n", res);
    }
    
    // Safely unmount filesystem
    printf("\nUnmounting filesystem...\n");
    res = f_unmount("0:");
    if (res == FR_OK) {
        printf("✓ Filesystem unmounted successfully\n");
    } else {
        printf("✗ Unmount failed (error: %d)\n", res);
    }
    
    printf("==================\n\n");
    
    return write_success;
}