/*
 * SD Card Driver Implementation
 */

#include "sd_card.h"
#include "hardware/gpio.h"
#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;
}