#ifdef USE_LOGGING

#include <avr/io.h>
#include <avr/eeprom.h>

#include "lights.h"

#define LOG_BUFFER 64
static unsigned char log_buffer_ee[LOG_BUFFER] EEMEM;
static unsigned char log_buffer_count;
static unsigned char log_buffer[LOG_BUFFER];
static unsigned char log_state EEMEM;
/* Upper 4 bits are reset count, lower 4 bits are reset reason from MCUSR */
static unsigned char reboot_count EEMEM = 0;
static unsigned char can_write_eeprom = 0;
static uint16_t flushed_end;

void log_set_state(unsigned char val)
{
	if (can_write_eeprom)
		eeprom_write_byte(&log_state, val);
}

void init_log()
{
	unsigned char r_count;

	r_count = eeprom_read_byte(&reboot_count);
	r_count >>= 4;

	if (r_count < 5) {
		r_count++;
		eeprom_write_byte(&reboot_count,
			(r_count << 4) | (MCUSR & 0xF));
		MCUSR = 0;
		can_write_eeprom = 1;
	} else {
		//eeprom_write_byte(&log_state, 0xFF);
		can_write_eeprom = 0;
	}

	log_set_state(1);
	log_buffer_count = 0;
	flushed_end = 0;
}

void log_byte(unsigned char byte) {
	if (log_buffer_count >= LOG_BUFFER)
		return;
	
	// eeprom_write_word(&log_buffer[log_buffer_count], word);
	log_buffer[log_buffer_count++] = byte;

	if (log_buffer_count == LOG_BUFFER)
		log_flush();
}

void log_word(uint16_t word) {
	log_byte(word & 0xFF);
	log_byte(word >> 8);
}

void log_flush() {
	unsigned char i;

	if (!can_write_eeprom)
		return;

	for (i=flushed_end; i < log_buffer_count; i++) {
		eeprom_write_byte(&log_buffer_ee[i],
			log_buffer[i]);
	}

	flushed_end = i;

	if (flushed_end == LOG_BUFFER)
		log_set_state(0x42);
}

#endif

