import machine
import time

# Setup SPI interface and chip select (CS) pin
spi = machine.SPI(1, baudrate=100000, polarity=0, phase=0, sck=machine.Pin(10), mosi=machine.Pin(11), miso=machine.Pin(12))
cs = machine.Pin(5, machine.Pin.OUT)

# Select the SD card by pulling CS low
def select_card():
    cs.value(0)

# Deselect the SD card by pulling CS high
def deselect_card():
    cs.value(1)

# Send a command to the SD card
def send_command(command, arg, crc):
    packet = bytearray(6)
    packet[0] = 0x40 | command
    packet[1] = (arg >> 24) & 0xFF
    packet[2] = (arg >> 16) & 0xFF
    packet[3] = (arg >> 8) & 0xFF
    packet[4] = arg & 0xFF
    packet[5] = crc

    select_card()
    spi.write(packet)
    deselect_card()

# Send ACMD (Application-specific command, must be preceded by CMD55)
def send_acmd(command, arg, crc):
    send_command(55, 0, 0xFF)  # CMD55 must be sent before ACMD
    send_command(command, arg, crc)

# Read the response from the SD card
def read_response(timeout=1000):
    select_card()
    for _ in range(timeout):
        response = spi.read(1)[0]
        if response != 0xFF:  # 0xFF indicates no response, wait for actual response
            deselect_card()
            return response
    deselect_card()
    return 0xFF  # Timeout if no response

# Wait until the card is no longer busy (response != 0xFF)
def wait_for_card(timeout=1000):
    select_card()
    for _ in range(timeout):
        response = spi.read(1)[0]
        if response != 0xFF:  # Card is no longer busy
            deselect_card()
            return response
    deselect_card()
    return 0xFF  # Timeout

# Initialize SD card (CMD0 is GO_IDLE_STATE, which resets the card)
def init_card():
    deselect_card()
    time.sleep(0.1)

    # Send 80 clock pulses before sending the first command
    for _ in range(10):
        spi.write(b'\xFF')

    # Send CMD0 to reset the card
    send_command(0, 0, 0x95)
    response = read_response()

    # Wait until the SD card goes idle
    while response != 0x01:
        response = read_response()
        print("Waiting for card to go idle...")

    print("Card initialized")

# Check SD card version using CMD8
def check_sd_version():
    send_command(8, 0x1AA, 0x87)  # CMD8: Check voltage range
    response = read_response()
    if response == 0x01:
        print("SD card supports modern voltage levels.")
        return True
    else:
        print(f"Unexpected response for CMD8: {response}")
        return False

# Complete SD card initialization using ACMD41
def complete_initialization():
    # Send ACMD41 to complete initialization (repeatedly until the card exits idle state)
    while True:
        send_acmd(41, 0x40000000, 0xFF)  # ACMD41 with HCS (Host Capacity Support)
        response = read_response()
        if response == 0x00:
            print("SD card initialization complete.")
            break
        elif response == 0x01:
            print("Still in idle state, retrying ACMD41...")
        else:
            print(f"Unexpected response: {response}")
            break
        time.sleep(0.5)  # Delay between retries

# Read a single block (512 bytes) from the SD card
def read_block(block_number):
    block_address = block_number * 512  # Convert block number to byte address for older SD cards
    send_command(17, block_address, 0xFF)  # CMD17 is READ_SINGLE_BLOCK
    response = read_response()

    if response == 0x00:
        # Wait for the data token (0xFE)
        print("Waiting for data token (0xFE)...")
        for _ in range(10000):  # Add a timeout for waiting for the data token
            if spi.read(1)[0] == 0xFE:
                print("Data token (0xFE) received")
                break
            time.sleep(0.001)  # Small delay between checks
        else:
            print("Data token (0xFE) not received in time")
            return None

        # Read 512 bytes of data
        data = spi.read(512)

        # Read CRC (but ignore it for now)
        spi.read(2)

        print("Block read successful!")
        return data
    else:
        print(f"Failed to read block, response: {response}")
        return None

# Write a single block (512 bytes) to the SD card
def write_block(block_number, data):
    block_address = block_number * 512  # Convert block number to byte address
    send_command(24, block_address, 0xFF)  # CMD24 is WRITE_BLOCK
    response = wait_for_card()

    if response == 0x00:
        # Send data token (0xFE)
        print("Sending data token (0xFE) for write...")
        select_card()
        spi.write(b'\xFE')

        # Write 512 bytes of data
        spi.write(data)

        # Write dummy CRC
        spi.write(b'\xFF\xFF')

        # Read response token
        response = spi.read(1)[0]
        deselect_card()

        if (response & 0x1F) == 0x05:
            print("Block written successfully!")
        else:
            print(f"Failed to write block, response: {response}")
    else:
        print(f"Failed to initiate block write, response: {response}")

# Initialize SD card
init_card()

# Check SD card version
is_sdhc = check_sd_version()

# Complete SD card initialization using ACMD41
complete_initialization()

# Write data to the first block (block 0)
test_data = bytearray(512)
test_data[0:4] = b'Test'  # Example data

print("Writing to block 0...")
write_block(0, test_data)

# Read back data from the first block (block 0)
print("Reading from block 0...")
block_data = read_block(0)

if block_data:
    print("Data from block 0:", block_data[:4])  # Only print the first 4 bytes for testing