#include "sd_mmc.h"
#include "sd_mmc_intf.h"
#include <assert.h>


// Functions used to interact with the SD card
static sd_mmc_err_t sd_mmc_power_up_sequence();
static sd_mmc_err_t sd_mmc_idle_sequence();
static sd_mmc_err_t sd_mmc_if_sequence();
static sd_mmc_err_t sd_mmc_read_ocr(uint8_t* res);
static sd_mmc_err_t sd_mmc_read_res1(uint8_t* res1);
static sd_mmc_err_t sd_mmc_read_res7(uint8_t* res7);
static sd_mmc_err_t sd_mmc_read_res3(uint8_t* res3);
static bool sd_mmc_check_cd(void);
static void sd_mmc_send_clock(void);

// Functions used for debugging
static void sd_mmc_print_err(sd_mmc_err_t err);
static void sd_mmc_print_res1(uint8_t res);
static void sd_mmc_print_res7(const uint8_t* const res);
static void sd_mmc_print_res3(const uint8_t* const res);

sd_mmc_err_t sd_mmc_init(void)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t ocr[5];

	do
	{
		// Hardware initialize
		if(0 != SD_MMC_INTF_INIT())
		{
			ret = SD_MMC_INIT_FAILURE;
			break;
		}

		// Power up sd card sequence
		if((ret = sd_mmc_power_up_sequence()) != SD_MMC_SUCCESS)
		{
			break;
		}

		// go idle
		if((ret = sd_mmc_idle_sequence()) != SD_MMC_SUCCESS)
		{
			break;
		}

		// interface sequence
		if((ret = sd_mmc_if_sequence()) != SD_MMC_SUCCESS)
		{
			break;
		}

		// read operation conditions register
		if((ret = sd_mmc_read_ocr(ocr)) != SD_MMC_SUCCESS)
		{
			break;
		}

	}while(0);

	if(ret != SD_MMC_SUCCESS)
	{
		printf("Error initializing SD Card: ");
		sd_mmc_print_err(ret);
	}

	return ret;
}

sd_mmc_err_t sd_mmc_deselect(uint8_t slot)
{
	UNUSED(slot);
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t dummy = 0xFF;
	SD_MMC_INTF_WRITE(&dummy, 1);
	SD_MMC_INTF_DESELECT_DEVICE();
	SD_MMC_INTF_WRITE(&dummy, 1);
	return ret;
}
sd_mmc_err_t sd_mmc_select(uint8_t slot, uint32_t clock, uint8_t bus_width, bool high_speed)
{
	UNUSED(slot);
	UNUSED(clock);
	UNUSED(bus_width);
	UNUSED(high_speed);
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t dummy = 0xFF;
	SD_MMC_INTF_WRITE(&dummy, 1);
	SD_MMC_INTF_SELECT_DEVICE();
	SD_MMC_INTF_WRITE(&dummy, 1);
	return ret;
}

sd_mmc_err_t sd_mmc_send_command(uint8_t cmd, uint32_t arg, uint8_t crc)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t bytes[6];
	bytes[0] = cmd | 0x40;
	bytes[1] = ((uint8_t)(arg >> 24));
	bytes[2] = ((uint8_t)(arg >> 16));
	bytes[3] = ((uint8_t)(arg >> 8));
	bytes[4] = ((uint8_t)(arg));
	bytes[5] = crc | 0x01;
	if(6 != SD_MMC_INTF_WRITE(bytes, 6))
	{
		ret = SD_MMC_ERR_COMM;
	}
	return ret;
}


sd_mmc_err_t sd_mmc_read_res1(uint8_t* byte)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;

	uint8_t i = 0;
	do
	{
		i++;
		SD_MMC_INTF_READ(byte, 1);
	}while(i <= 8 && *byte == 0xFF);
	if(*byte == 0xFF)
	{
		return SD_MMC_TIMEOUT;
	}

	return ret;
}

sd_mmc_err_t sd_mmc_read_res7(uint8_t* res7)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	do
	{
		if(5 != SD_MMC_INTF_READ(res7, 5))
		{
			ret = SD_MMC_ERR_COMM;
			break;
		}
		printf("hi");
		if(res7[0] > 1)
		{
			// handle this error better later
			printf("res7: 0x%02x\n", res7[0]);
			perr("res7 > 1...");
			ret = SD_MMC_ERR_COMM;
			break;
		}
	}while(0);
	return ret;
}

sd_mmc_err_t sd_mmc_read_res3(uint8_t* res3)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	do
	{
		if(5 != SD_MMC_INTF_READ(res3, 5))
		{
			ret = SD_MMC_ERR_COMM;
			break;
		}

		if(res3[0] > 1)
		{
			// handle this error better later
			ret = SD_MMC_ERR_COMM;
			break;
		}
	}while(0);
	return ret;

}
sd_mmc_err_t sd_mmc_power_up_sequence()
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	// power up sequence
	delay_ms(1);
	sd_mmc_send_clock();
	return ret;
}

sd_mmc_err_t sd_mmc_idle_sequence()
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t res1 = 0xFF;

	sd_mmc_select(0, 0, 0, false);
	do
	{
		sd_mmc_select(0, 0, 0, true);
		if((ret = sd_mmc_send_command(CMD0, CMD0_ARG, CMD0_CRC)) != SD_MMC_SUCCESS)
		{
			break;
		}

		if((ret = sd_mmc_read_res1(&res1)) != SD_MMC_SUCCESS)
		{
			break;
		}

		sd_mmc_print_res1(res1);
	}while(0);
	sd_mmc_deselect(0);
	return ret;
}
sd_mmc_err_t sd_mmc_if_sequence()
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	uint8_t res7[5];

	sd_mmc_select(0, 0, 0, false);
	do
	{

		if((ret = sd_mmc_send_command(CMD8, CMD8_ARG, CMD8_CRC)) != SD_MMC_SUCCESS)
		{
			break;
		}

		if((ret = sd_mmc_read_res7(res7)) != SD_MMC_SUCCESS)
		{
			break;
		}

		sd_mmc_print_res7(res7);
	}while(0);
	sd_mmc_deselect(0);
	return ret;
}

void sd_mmc_print_err(sd_mmc_err_t err)
{
	switch(err)
	{
		case SD_MMC_SUCCESS:
		{
			printf("%s", "SD_MMC_SUCCESS\n");
		}break;
		case SD_MMC_FAILURE:
		{
			printf("%s", "SD_MMC_FAILURE\n");
		}break;
		case SD_MMC_INIT_ONGOING:
		{
			printf("%s", "SD_MMC_INIT_ONGOING\n");
		}break;
		case SD_MMC_ERR_NO_CARD:
		{
			printf("%s", "SD_MMC_ERR_NO_CARD\n");
		}
		case SD_MMC_ERR_UNUSABLE:
		{
			printf("%s", "SD_MMC_UNUSABLE\n");
		}break;
		case SD_MMC_ERR_SLOT:
		{
			printf("%s", "SD_MMC_ERR_SLOT\n");
		}break;
		case SD_MMC_ERR_COMM:
		{
			printf("%s", "SD_MMC_ERR_COMM\n");
		}break;
		case SD_MMC_ERR_PARAM:
		{
			printf("%s", "SD_MMC_ERR_PARAM\n");
		}break;
		case SD_MMC_ERR_WP:
		{
			printf("%s", "SD_MMC_ERR_WP\n");
		}break;
		case SD_MMC_INIT_FAILURE:
		{
			printf("%s", "SD_MMC_INIT_FAILURE\n");
		}break;
		case SD_MMC_TIMEOUT:
		{
			printf("%s", "SD_MMC_TIMEOUT\n");
		}break;
		default:
		{
			printf("%s", "UNKNOWN ERROR");
		}
	}
}

void sd_mmc_print_res1(uint8_t res)
{
	printf("RES1: 0x%02x\n", res);
    if(res & 0b10000000)
	{ printf("\tError: MSB = 1\r\n"); return; }
    if(res == 0)
	{ printf("\tCard Ready\r\n"); return; }
    if(PARAM_ERROR(res))
        printf("\tParameter Error\r\n");
    if(ADDR_ERROR(res))
        printf("\tAddress Error\r\n");
    if(ERASE_SEQ_ERROR(res))
        printf("\tErase Sequence Error\r\n");
    if(CRC_ERROR(res))
        printf("\tCRC Error\r\n");
    if(ILLEGAL_CMD(res))
        printf("\tIllegal Command\r\n");
    if(ERASE_RESET(res))
        printf("\tErase Reset Error\r\n");
    if(IN_IDLE(res))
        printf("\tIn Idle State\r\n");
}

void sd_mmc_print_res7(const uint8_t* const res)
{
	sd_mmc_print_res1(res[0]);
	if(res[0] > 1) return;

	printf("\tCommand Version: 0x%02x\n", CMD_VER(res[1]));

	printf("\tVoltage Accepted: %s\n",
		   (res[3] == VOLTAGE_ACC_27_33) ? "2.7-3.6V"
		   : (res[3] == VOLTAGE_ACC_LOW) ? "LOW VOLTAGE"
		   : (res[3] == VOLTAGE_ACC_RES1 || res[3] ==  VOLTAGE_ACC_RES2) ? "RESERVED"
		   : "NOT DEFINED");
	printf("\tEcho: 0x%02x\n", res[4]);
}

void sd_mmc_print_res3(const uint8_t* const res)
{
	sd_mmc_print_res1(res[0]);
	if(res[0] > 1) return;

	printf("\tCard Power Up Status: 0x%02x\n", res[1]);
	if(POWER_UP_STATUS(res[1]))
	{
		printf("READY\n");
		printf("\tCCS Status: %d\n", CCS_VAL(res[1]) ? 1 : 0);
	}
	else
	{
		printf("BUSY\n");
	}

	printf("\tVDD Window: ");
	if(VDD_2728(res[3])) printf("2.7-2.8, ");
    if(VDD_2829(res[2])) printf("2.8-2.9, ");
    if(VDD_2930(res[2])) printf("2.9-3.0, ");
    if(VDD_3031(res[2])) printf("3.0-3.1, ");
    if(VDD_3132(res[2])) printf("3.1-3.2, ");
    if(VDD_3233(res[2])) printf("3.2-3.3, ");
    if(VDD_3334(res[2])) printf("3.3-3.4, ");
    if(VDD_3435(res[2])) printf("3.4-3.5, ");
    if(VDD_3536(res[2])) printf("3.5-3.6");
    printf("\n");
}


bool sd_mmc_check_cd(void)
{
	switch(SD_MMC_CD_MODE)
	{
		case SD_MMC_CD_MODE_UNUSED:
		{
			// Always assume the card is detected since we aren't using this function
			return true;
		}break;
		case SD_MMC_CD_MODE_SYNC:
		{
			return SD_MMC_INTF_CHECK_CD();
		}break;
		case SD_MMC_CD_MODE_ASYNC:
		{
			// Unimplemented
			return false;
		}break;
		default:
		{
			return false;
		}
	}
	return false;
}

void sd_mmc_send_clock()
{
	uint8_t i;
	uint8_t dummy = 0xFF;
	for(i = 0; i < 10; i++)
	{
		SD_MMC_INTF_WRITE(&dummy, 1);
	}
}

sd_mmc_err_t sd_mmc_read_ocr(uint8_t* res)
{
	sd_mmc_err_t ret = SD_MMC_SUCCESS;
	sd_mmc_select(0, 0, 0, false);
	do
	{
		if((ret = sd_mmc_send_command(CMD58, CMD58_ARG, CMD58_CRC)) != SD_MMC_SUCCESS)
		{
			break;
		}

		if((ret = sd_mmc_read_res3(res)) != SD_MMC_SUCCESS)
		{
			break;
		}
		sd_mmc_print_res3(res);
	}while(0);
	sd_mmc_deselect(0);
	return ret;
}