+#ifdef USE_LOGGING
+
+#include <avr/io.h>
+#include <avr/eeprom.h>
+
+#include "lights.h"
+
+#define LOG_BUFFER 128
+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
+