#include "common.h"

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

#define MEM_SIZE        1024
#define MEM_NUM_CELLS   2
#define MEM_CELL_SIZE   ((int) sizeof(word))
#define MEM_BLOCK_SIZE  (MEM_NUM_CELLS * MEM_CELL_SIZE + 1)
#define MEM_MAX_BLOCKS  (MEM_SIZE / MEM_BLOCK_SIZE)
#define MEM_IS_DIRTY    0x80 // Dirty block marker

// TODO: Automated testing /w multiple write cycles
// TODO: Accept structure containing variable data

static int   GetUsedBlock(void);
static int   GetFreeBlock(void);
static byte  ReadRaw(int addr);
static void  WriteRaw(int addr, byte data);

void MEM_Init(void)
{
	// Valid first sentinel?
	if (ReadRaw(0) == 0xFF) {
		Info("Initializing fresh memory...");
		MEM_Free(); // Set dirty block markers
	}
}

void MEM_Read(word *temp, word *dewp)
{
	int bl;
	word addr;
	byte msb[2], lsb[2];

	bl = GetUsedBlock();

	if (bl < 0) {
		*temp = 0;
		*dewp = 0;
		return; // Empty
	}

	addr = bl * MEM_BLOCK_SIZE;

	msb[0] = ReadRaw(addr + 1);
	lsb[0] = ReadRaw(addr + 2);
	msb[1] = ReadRaw(addr + MEM_CELL_SIZE + 1);
	lsb[1] = ReadRaw(addr + MEM_CELL_SIZE + 2);

	*temp = ((msb[0] & 0xFF) << 8) | (lsb[0] & 0xFF);
	*dewp = ((msb[1] & 0xFF) << 8) | (lsb[1] & 0xFF);
}

void MEM_Write(word temp, word dewp)
{
	int bl;
	word addr;
	byte msb[2], lsb[2];

	bl = GetFreeBlock();

	if (bl < 0) {
		// Full
		MEM_Free();
		bl = 0;
	}

	addr = bl * MEM_BLOCK_SIZE;

	msb[0] = (temp >> 8) & 0xff;
	lsb[0] = temp & 0xff;
	msb[1] = (dewp >> 8) & 0xff;
	lsb[1] = dewp & 0xff;

	WriteRaw(addr, MEM_IS_DIRTY);
	WriteRaw(addr + 1, msb[0]);
	WriteRaw(addr + 2, lsb[0]);
	WriteRaw(addr + MEM_CELL_SIZE + 1, msb[1]);
	WriteRaw(addr + MEM_CELL_SIZE + 2, lsb[1]);
}

void MEM_Free(void)
{
	int bl;

	// Clear all dirty block markers
	for (bl = 0; bl < MEM_MAX_BLOCKS; bl++) {
		WriteRaw(bl * MEM_BLOCK_SIZE, 0x0);
		if ((bl % 64) == 0) WDT_Reset();
	}
}

void MEM_Dump(void)
{
	byte raw;
	char buf[3*16+1];

	Info("Dumping EEPROM memory:");
	for (int i = 0; i < 64; i++) {
		for (int j = 0; j < 16; j++) {
			raw = ReadRaw(i * 16 + j);
			sprintf(buf + j * 3, "%02X ", raw);
		}

		WDT_Reset();
		Info(buf);
	}
}

static int GetUsedBlock(void)
{
	int bl;

	bl = GetFreeBlock();

	if (bl == 0)
		return -1;
	else if (bl < 0)
		return MEM_MAX_BLOCKS;
	else
		return bl - 1;
}

static int GetFreeBlock(void)
{
	int bl;
	byte flag;
	bool is_free;

	for (bl = 0; bl < MEM_MAX_BLOCKS; bl++) {
		// Check dirty flag for current block
		flag = ReadRaw(bl * MEM_BLOCK_SIZE);
		if (flag != MEM_IS_DIRTY) {
			is_free = true;
			break;
		}
	}

	// Is there any space left?
	return (is_free) ? bl : -1;
}

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)
{
	// 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);
	}

	return EEDR;
}