#include <ADuCM360.h>
#include <cstring>
#include <cassert>
#include <errno.h>
#include <hal/drivers/flash.h>

static int position = 0;

//flash_disk exclusive in his own flash sector
static const struct flash_file flash_disk __attribute__ ((aligned (512), section (".flash_disk")));

static const char * const p_flash_disk = (const char *) (&flash_disk);

ssize_t flash_write(const void *buf, size_t count)
{
	if (position >= sizeof(flash_file))
	{
		errno = ENOSPC;
		return -1;
	}

	int start_page = position >> 9;

	int end_page = ((position + count) < sizeof(flash_file) ? (position + count - 1) : (sizeof(flash_file) - 1)) >> 9;

	int remain_bytes = count;
	const char *remain_buf;
	remain_buf = (const char *) buf;

	for (int i = start_page; i <= end_page; ++i)
	{
		char buffer[512] __attribute__ ((aligned (sizeof(unsigned long))));

		memcpy(buffer, &p_flash_disk[i << 9], 512);

		pADI_FEE->FEEADR0H = (((int) (&p_flash_disk[i << 9]) >> 16) & 0x1);
		pADI_FEE->FEEADR0L = ((int) (&p_flash_disk[i << 9]) & 0xfe00);

		pADI_FEE->FEEKEY = FEEKEY_VALUE_USERKEY1;
		pADI_FEE->FEEKEY = FEEKEY_VALUE_USERKEY2;

		pADI_FEE->FEECMD = FEECMD_CMD_ERASEPAGE; // erase
		while (FEESTA_CMDBUSY_BBA)
			;

		int write_count = (position & 0x1ff) + remain_bytes > 512 ? 512 - (position & 0x1ff) : remain_bytes;

		memcpy(&buffer[position & 0x1ff], remain_buf, write_count);

		FEECON0_WREN_BBA = true;

		unsigned long *p_src, *p_dst;
		p_src = (unsigned long *) buffer;
		p_dst = (unsigned long *) (&p_flash_disk[i << 9]);

		for (int i = 0; i < 128; ++i)
		{
			while (FEESTA_WRBUSY_BBA)
				;
			p_dst[i] = p_src[i];
		}

		FEECON0_WREN_BBA = false;

		position += write_count;
		remain_bytes -= write_count;
		remain_buf += write_count;
	}

	return (count - remain_bytes);
}

ssize_t flash_read(void *buf, size_t count)
{
	if ((position + count) > sizeof(flash_file))
	{
		count = sizeof(flash_file) - position;
	}

	memcpy(buf, &p_flash_disk[position], count);

	position += count;

	return count;
}

off_t flash_lseek(off_t offset, int whence)
{
	switch (whence)
	{
	case SEEK_SET:
		if (offset > sizeof(flash_file) || offset < 0)
		{
			errno = EINVAL;
			return -1;
		}
		else
		{
			position = offset;
		}

		break;
	case SEEK_CUR:
		if ((position + offset) > sizeof(flash_file) || (position + offset) < 0)
		{
			errno = EINVAL;
			return -1;
		}
		else
		{
			position += offset;
		}
		break;
	case SEEK_END:
		if (offset > 0 || sizeof(flash_file) + offset < 0)
		{
			errno = EINVAL;
			return -1;
		}
		else
		{
			position = sizeof(flash_file) + offset;
		}
		break;
	default:
		errno = ENXIO;
		return -1;
		break;
	}

	return position;
}

void flash_open(void)
{

}
