#include "common.h"

#include <util/atomic.h>
#include <string.h>
#include <stdio.h>

#define MEM_SIZE        1024
#define MEM_START       0x00
#define MEM_BLOCK_SIZE  ((int) sizeof(mem_block_t))
#define MEM_MAX_BLOCKS  (MEM_SIZE / MEM_BLOCK_SIZE)

static byte  GetSentinel(void);
static int   GetFreeBlock(void);
static int   GetUsedBlock(void);
static void  WriteBlock(int n, mem_block_t *in);
static void  ReadBlock(int n, mem_block_t *out);
static void  WriteRaw(int addr, byte data);
static byte  ReadRaw(int addr);

// The memory manager divides the memory space into blocks
// with each block starting with a sentinel byte. This byte
// indicates if the block was already used in this cycle.

//  /--- used
// 80 50 DD EE FF 60 AA BB CC
// 00 00 00 00 00 00 00 00 00
//  \--- free

// The first block is always considered used and therefore
// its sentinel value can act as a reference for the "used"
// flag. This means a block is considered free, only if its
// sentinel value differs from the first block.

void MEM_Write(mem_block_t *in)
{
	int head;
	byte flag;

	// As we add entries we check the flags for each
	// block in sequence until we find a free one and
	// go back one step.

	head = GetFreeBlock();
	flag = GetSentinel();

	// When we have to write new data but there are no
	// free entries, we change the sentinel bit value
	// for the reference block and all other blocks are
	// considered free again without erasing any flags.

	//  /--- used
	// 00 50 DD EE FF 60 AA BB CC
	// 80 20 FF CC 25 00 EE FF CC
	//  \--- free

	if (head == 0) {
		// Clear all "used" flags
		flag = flag ? 0x00 : 0x80;
	}

	in->flag = flag;
	WriteBlock(head, in);
}

int MEM_Read(mem_block_t *out)
{
	int head;

	head = GetUsedBlock();

	if (head < 0) { // Empty?
		return -1;
	}

	ReadBlock(head, out);

	return 0;
}

void MEM_Free(void)
{
	int head;

	for (head = 0; head < MEM_MAX_BLOCKS; head++) {
		// Set sentinel values to unitialized
		WriteRaw(head * MEM_BLOCK_SIZE, 0xFF);

		if ((head % 64) == 0) {
			WDT_Reset();
		}
	}
}

void MEM_Dump(void)
{
	int n, m;
	char buf[MEM_BLOCK_SIZE*3+1];
	char *ptr = buf;

	// Used for debugging purposes
	for (n = 0, m = 0; n < MEM_SIZE; n++) {
		ptr += sprintf(ptr, "%02X ", ReadRaw(n));
		if (++m == MEM_BLOCK_SIZE || n == (MEM_SIZE - 1)) {
			Print("%s\r\n", buf);
			WDT_Reset();
			ptr = buf;
			m = 0;
		}
	}
}

static byte GetSentinel(void)
{
	return ReadRaw(MEM_START);
}

static int GetFreeBlock(void)
{
	int head;
	byte ref, flag;

	if ((ref = GetSentinel()) == 0xFF) {
		return 0; // Fresh memory
	}

	for (head = 0; head < MEM_MAX_BLOCKS; head++) {
		flag = ReadRaw(head * MEM_BLOCK_SIZE);

		if (flag != ref) {
			break;
		}
	}

	if (head == MEM_MAX_BLOCKS) {
		return 0; // Rollover
	}

	return head;
}

static int GetUsedBlock(void)
{
	int next, last;
	byte ref;

	if ((ref = GetSentinel()) == 0xFF) {
		return -1; // Fresh memory
	}

	if ((next = GetFreeBlock()) == 0) {
		// Important edge case: Rollover or empty?
		last = (MEM_MAX_BLOCKS - 1) * MEM_BLOCK_SIZE;

		if (ReadRaw(last) == GetSentinel()) {
			return MEM_MAX_BLOCKS - 1;
		} else {
			return -1; // None
		}
	}

	return next - 1;
}

static void WriteBlock(int n, mem_block_t *in)
{
	int addr;
	byte *raw;

	raw = (byte *) in;

	addr = n * MEM_BLOCK_SIZE;
	for (int i = 0; i < MEM_BLOCK_SIZE; i++) {
		WriteRaw(addr + i, raw[i]);
	}
}

static void ReadBlock(int n, mem_block_t *out)
{
	int addr;
	byte block[MEM_BLOCK_SIZE];

	addr = n * MEM_BLOCK_SIZE;
	for (int i = 0; i < MEM_BLOCK_SIZE; i++) {
		block[i] = ReadRaw(addr + i);
	}

	memcpy(out, block, sizeof(mem_block_t));
}

static void WriteRaw(int addr, byte data)
{
	// The EEMWE bit determines whether setting EEWE to
	// one causes the EEPROM to be written. When EEMWE
	// is set, setting EEWE within four clock cycles
	// will write data to the EEPROM at the selected
	// address.

	// If EEMWE is zero, setting EEWE will have no
	// effect. When EEMWE has been written to one by
	// software, hardware clears the bit to zero after
	// four clock cycles.

	// Caution: An interrupt between the last two steps
	// will make the write cycle fail, since the EEPROM
	// Master Write Enable will time-out.

	// If an interrupt routine accessing the EEPROM is
	// interrupting another EEPROM Access, the EEAR or
	// EEDR reGister will be modified, causing the
	// interrupted EEPROM Access to fail.

	// It is recommended to have the Global Interrupt
	// Flag cleared during all the steps to avoid these
	// problems.

	// No interrupts during EEPROM write
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {

		// Wait until ready
		while (EECR & BIT(EEWE));

		// The EEPROM Address Registers – EEARH and
		// EEARL – specify the EEPROM address in the
		// 1024bytes EEPROM space. The EEPROM data
		// bytes are addressed linearly between 0
		// and 1023. The initial value of EEAR is
		// undefined. A proper value must be written
		// before the EEPROM may be accessed.

		EEAR = addr;
		EEDR = data;

		// Write to address
		EECR |= BIT(EEMWE);
		EECR |= BIT(EEWE);
	}
}

static byte ReadRaw(int addr)
{
	byte data;

	// The EEPROM Read Enable Signal EERE is the read
	// strobe to the EEPROM. When the correct address
	// is set up in the EEAR Register, the EERE bit
	// must be written to a logic one to trigger the
	// EEPROM read.

	// The EEPROM read access takes one instruction,
	// and the requested data is available immediately.
	// When the EEPROM is read, the CPU is halted for
	// four cycles before the next instruction is
	// executed.

	// No interrupts during EEPROM read
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {

		// Wait until ready
		while (EECR & BIT(EEWE));

		EEAR = addr;

		// Read from address
		EECR |= BIT(EERE);
		data = EEDR;
	}

	return data;
}