#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/sleep.h>

#include <stdint.h>

#include "i2c_master.h"

#define EEPROM_SIZE		256
#define EEPROM_IDX_SIZE 4

#define BATTERY_REG_MODE		0x00
#define BATTERY_REG_CTRL		0x01
#define BATTERY_READ_ADDR		0x02
#define BATTERY_READ_ADDR_END	0x0B
#define BATTERY_BUFF_SIZE		(BATTERY_READ_ADDR_END - BATTERY_READ_ADDR + 1)

#define BATTERY_CHARGE			0
#define BATTERY_VOLT			6
#define BATTERY_TEMP			8

#define BATTERY_MILLIOHM		30

#define BATTERY_CHARGE_MULT		67
#define BATTERY_CHARGE_DIV		(10 * BATTERY_MILLIOHM)
#define BATTERY_VOLT_MULT		244
#define BATTERY_VOLT_DIV		100
#define BATTERY_TEMP_MULT		125
#define BATTERY_TEMP_DIV		1000

#define CHAR_SIZE		5

#define OLED_ADDRESS	(0x3C << 1)
// #define OLED_ADDRESS	(0x3D << 1)
#define EEPROM_ADDRESS	(0x50 << 1)
#define BATTERY_ADDRESS	(0x70 << 1)

#define OLED_X_SIZE		128
#define OLED_Y_SIZE		64

// Time(ms) to keep the display on before sleep
#define DISPLAY_DELAY	3000

// Time(ms) to assume long button press
#define LONG_PRESS		500

// Rotary Encoder Parameters
#define ROT_PULSE_COUNT	12
#define ROT_DETENTS		24
#define ROT_WHEEL_RAD	13.875
//#define ROT_REVERSE

// Event queue macros
#define MAX_EVENT_COUNT 64
#define PTR_INC(x) ((x) = events + ((((x) - events) + 1) % MAX_EVENT_COUNT))

enum event_e
{
	EVENT_NONE,
	EVENT_ROT_CW,
	EVENT_ROT_CCW,
	EVENT_SEL_UP,
	EVENT_SEL_DOWN,
	EVENT_BTN_UP,
	EVENT_BTN_DOWN
};

enum btn_state_e
{
	BTN_NONE = 0,
	BTN_PRESS,
	BTN_UP5,
	BTN_UP14,
	BTN_DOWN5,
	BTN_DOWN14
};

// These are set on hardware init
uint8_t old_btn_pin_state;
uint8_t old_rot_pin_state;

uint8_t btn_state_stable = BTN_NONE;
uint8_t btn_state_current = BTN_NONE;

uint8_t events[MAX_EVENT_COUNT];
uint8_t event_count  = 0;
uint8_t *event_read  = events;
uint8_t *event_write = events;

uint32_t ms = 0;

#ifdef ROT_REVERSE
const uint8_t PROGMEM rot_table[16] =
{
	EVENT_NONE, EVENT_ROT_CCW, EVENT_ROT_CW, EVENT_NONE,
	EVENT_ROT_CW, EVENT_NONE, EVENT_NONE, EVENT_ROT_CCW,
	EVENT_ROT_CCW, EVENT_NONE, EVENT_NONE, EVENT_ROT_CW,
	EVENT_NONE, EVENT_ROT_CW, EVENT_ROT_CCW, EVENT_NONE
};
#else
const uint8_t PROGMEM rot_table[16] =
{
	EVENT_NONE, EVENT_ROT_CW, EVENT_ROT_CCW, EVENT_NONE,
	EVENT_ROT_CCW, EVENT_NONE, EVENT_NONE, EVENT_ROT_CW,
	EVENT_ROT_CW, EVENT_NONE, EVENT_NONE, EVENT_ROT_CCW,
	EVENT_NONE, EVENT_ROT_CCW, EVENT_ROT_CW, EVENT_NONE
};
#endif

const uint8_t PROGMEM btn_table[36] =
{
	// Old BTN_NONE
	EVENT_NONE, EVENT_BTN_DOWN, EVENT_NONE, EVENT_SEL_UP, EVENT_NONE, EVENT_SEL_DOWN,
	// Old BTN_PRESS
	EVENT_BTN_UP, EVENT_NONE, EVENT_BTN_UP, EVENT_SEL_UP, EVENT_BTN_UP, EVENT_SEL_DOWN,
	// Old BTN_UP5
	EVENT_NONE, EVENT_BTN_DOWN, EVENT_NONE, 0xFF, EVENT_NONE, EVENT_SEL_DOWN,
	// Old BTN_UP14
	EVENT_NONE, EVENT_BTN_DOWN, EVENT_NONE, EVENT_NONE, EVENT_NONE, EVENT_SEL_DOWN,
	// Old BTN_DOWN5
	EVENT_NONE, EVENT_BTN_DOWN, EVENT_NONE, EVENT_SEL_UP, EVENT_NONE, 0xFF,
	// Old BTN_DOWN14
	EVENT_NONE, EVENT_BTN_DOWN, EVENT_NONE, EVENT_SEL_UP, EVENT_NONE, EVENT_NONE
};

const uint8_t PROGMEM unfold_table[16] =
{
	0x00,
	0x02,
	0x08,
	0x0A,
	0x20,
	0x22,
	0x28,
	0x2A,
	0x80,
	0x82,
	0x88,
	0x8A,
	0xA0,
	0xA2,
	0xA8,
	0xAA
};

// Bottom -> Top (In Byte); Left -> Right (In Row)
const uint8_t PROGMEM symbols[90] =
{
	0x3E, 0x51, 0x49, 0x45, 0x3E, // 0
	0x00, 0x42, 0x7F, 0x40, 0x00, // 1
	0x42, 0x61, 0x51, 0x49, 0x46, // 2
	0x21, 0x41, 0x45, 0x4B, 0x31, // 3
	0x18, 0x14, 0x12, 0x7F, 0x10, // 4
	0x27, 0x45, 0x45, 0x45, 0x39, // 5
	0x3C, 0x4A, 0x49, 0x49, 0x30, // 6
	0x03, 0x71, 0x09, 0x05, 0x03, // 7
	0x36, 0x49, 0x49, 0x49, 0x36, // 8
	0x06, 0x49, 0x49, 0x29, 0x1E, // 9
	0x7E, 0x11, 0x11, 0x11, 0x7E, // A
	0x7F, 0x49, 0x49, 0x49, 0x36, // B
	0x3E, 0x41, 0x41, 0x41, 0x22, // C
	0x1F, 0x20, 0x40, 0x20, 0x1F, // V
	0x7F, 0x08, 0x04, 0x04, 0x78, // h
	0x7C, 0x04, 0x78, 0x04, 0x78, // m
	0x00, 0x60, 0x60, 0x00, 0x00, // .
	0x41, 0x22, 0x14, 0x08, 0x00, // >
	0x0E, 0x11, 0x11, 0x0E, 0x00  // deg
};

// Init direct hardware
void simple_init();

// OLED send cmd helper
void display_send_cmd(uint8_t cmd);

// OLED send data helper
void display_send_data(const uint8_t *data, uint16_t buflen);

// OLED init
void display_init();

// OLED On/Off
void display_enable(uint8_t en);

// Symbol printing helper
void get_symbol16(uint8_t index, uint8_t *out);

// Print symbol on screen
// Symbol idx in table,
// Start X pixel - 0 -- 117
// Y row - 0 -- 3
// Char size - 10x16
void print_symbol(uint8_t symbol_idx, uint8_t x, uint8_t y, uint8_t invert);

// Print the length left with mm and highlight the required digit
// 0 - no highlight, 0xFF - clears everything
void print_mm(uint32_t value, uint8_t highlight);

// Print the direction of the filament
// 0xFF - clears everything
void print_direction(uint8_t is_A, uint8_t highlight);

// Find the current eeprom data idx of the attached spool
int16_t find_eeprom_idx();

// Read value and direction of current spool
int8_t read_eeprom_val(int8_t idx, uint32_t *value, uint8_t *direction);

// Write value and direction of current spool
int8_t write_eeprom_val(int8_t idx, uint32_t value, uint8_t direction);

// Reset battery counter stats
int8_t reset_battery();

// Read battery stats
// temp is in celsius
int8_t read_battery(int16_t *mAh, int16_t *mV, int16_t *temp);

// I2C write helper
int8_t i2c_write_buff(uint8_t i2c_addr, uint8_t data_addr, uint8_t *buf, uint8_t len);

// I2C read helper
int8_t i2c_read_buff(uint8_t i2c_addr, uint8_t data_addr, uint8_t *buf, uint8_t len);

// Do the big sleep
void do_sleep();

// Extracts digit from value
// 1 - ones, 2 - tens, 3 - hundreds ...
uint8_t extract_digit(uint32_t value, uint8_t digit_num);

// Updates the ms counter
ISR(TIMER1_OVF_vect)
{
	cli();
	ms += 10;
	sei();
}

// Changes on button
ISR(PCINT0_vect)
{
	// Check button inputs
	uint8_t btn_pin_state = PINA & 0x2F;
	uint8_t btn_event = EVENT_NONE;
	uint8_t btn_state;
	uint8_t btn_idx;

	cli();

	// Check if button has changed
	if ((btn_pin_state ^ old_btn_pin_state) && (event_count != MAX_EVENT_COUNT))
	{
		// Pins to state
		switch (btn_pin_state)
		{
		// None
		case 0x2F:
			btn_state = BTN_NONE;
			break;
		
		// Press
		case 0x0F:
			btn_state = BTN_PRESS;
			break;

		// Up 5
		case 0x2D:
			btn_state = BTN_UP5;
			break;
		
		// Up 14
		case 0x2C:
			btn_state = BTN_UP14;
			break;
		
		// Down 5
		case 0x27:
			btn_state = BTN_DOWN5;
			break;
		
		// Down 14
		case 0x23:
			btn_state = BTN_DOWN14;
			break;

		// This should never happen -> Button is broken
		default:
			btn_state = btn_state_current;
			break;
		}

		btn_idx = btn_state_current * 6 + btn_state;
		btn_event = pgm_read_byte(&(btn_table[btn_idx]));
		if (0xFF == btn_event)
		{
			switch (btn_state)
			{
			case BTN_UP14:
				if (BTN_UP14 != btn_state_stable)
				{
					btn_event = EVENT_SEL_UP;
				}
				else
				{
					btn_event = EVENT_NONE;
				}
				break;
			
			case BTN_DOWN14:
				if (BTN_DOWN14 != btn_state_stable)
				{
					btn_event = EVENT_SEL_DOWN;
				}
				else
				{
					btn_event = EVENT_NONE;
				}
				break;
			
			default:
				btn_event = EVENT_NONE;
				break;
			}
		}
	}

	old_btn_pin_state = btn_pin_state;
	btn_state_current = btn_state;
	if ((btn_state != BTN_UP5) && (btn_state != BTN_DOWN5))
	{
		btn_state_stable = btn_state;
	}

	sei();
}

// Changes on rotary encoder
ISR(PCINT1_vect)
{
	uint8_t rot_pin_state = PINB & 0x03;
	uint8_t rot_event = EVENT_NONE;
	uint8_t rot_idx;

	cli();
	if ((rot_pin_state ^ old_rot_pin_state) && (event_count != MAX_EVENT_COUNT))
	{
		rot_idx = (old_rot_pin_state << 2) | rot_pin_state;
		rot_event = pgm_read_byte(&(rot_table[rot_idx]));
		if (EVENT_NONE != rot_event)
		{
			*event_write = rot_event;
			++event_count;
			PTR_INC(event_write);
		}
	}
	old_rot_pin_state = rot_pin_state;

	sei();
}

int main()
{
	// 1 / (4 * ROT_PULSE_COUNT) * (2 * pi * ROT_WHEEL_RAD)
	const float rot_coeff = 1.57f * ROT_WHEEL_RAD / ROT_PULSE_COUNT;
	uint32_t sleep_when_ms = 0;
	uint32_t long_press_when_ms = 0;
	uint32_t count_value = 0;
	float count_value_fine = 0;
	uint32_t eeprom_value = 0;
	uint16_t eeprom_idx = 0;
	uint8_t move_dir = 0;
	int8_t rot_value = 0;
	uint8_t dir_highlight = 0;
	uint8_t highlight = 0;
	uint8_t needs_update = 0;

	uint8_t curr_event;
	uint8_t state;

	// Init Direct Hardware
	simple_init();

	i2c_init();
	display_init();

	state = STATE_COUNTING;

	eeprom_idx = find_eeprom_idx();
	if (EEPROM_SIZE == eeprom_idx)
	{
		eeprom_idx = 0;
		state = STATE_SETTING;
		highlight = 6;
		needs_update = 1;
	}
	else
	{
		read_eeprom_val(eeprom_idx, &eeprom_value, &move_dir);
		if (0 == eeprom_value)
		{
			state = STATE_SETTING;
			highlight = 6;
			needs_update = 1;
		}
		else
		{
			count_value = eeprom_value;
			count_value_fine = eeprom_value;
		}
	}

	if (STATE_COUNTING == state)
	{
		sleep_when_ms = 1;
		display_enable(0);
	}

	sei();
	while(1)
	{
		switch (state)
		{
		case STATE_COUNTING:
			while (event_count > 0)
			{
				// Consume Event
				cli();
				curr_event = *event_read;
				PTR_INC(event_read);
				--event_count;
				sei();

				// Process Event
				switch(curr_event)
				{
				case EVENT_BUTTON_DOWN:
					long_press_when_ms = ms + LONG_PRESS;
					sleep_when_ms = ms + DISPLAY_DELAY;
					display_enable(1);
					needs_update = 1;
					break;

				case EVENT_BUTTON_UP:
					long_press_when_ms = 0;
					break;

				case EVENT_ROT_CW:
					if (0 != move_dir)
					{
						--rot_value;
					}
					else
					{
						++rot_value;
					}
					break;

				case EVENT_ROT_CCW:
					if (0 != move_dir)
					{
						++rot_value;
					}
					else
					{
						--rot_value;
					}
					break;
				}
			}

			if (rot_value / (ROT_PULSE_COUNT * 4 / ROT_DETENTS) != 0)
			{
				count_value_fine += rot_coeff * rot_value;
				rot_value = 0;

				if (count_value_fine < 0)
				{
					// Write the zero to EEPROM
					++eeprom_idx;
					eeprom_idx %= EEPROM_SIZE;
					write_eeprom_val(eeprom_idx, 0, move_dir);

					count_value_fine = 0;
					display_enable(1);

					// Switch State
					sleep_when_ms = 0;
					long_press_when_ms = 0;
					highlight = 6;
					needs_update = 1;
					state = STATE_SETTING;
				}

				// Update EEPROM When Meter Value Changes
				if ((uint32_t)count_value_fine / 1000 != count_value / 1000)
				{
					++eeprom_idx;
					eeprom_idx %= EEPROM_SIZE;
					write_eeprom_val(eeprom_idx, (uint32_t) count_value_fine, move_dir);
				}
				count_value = (uint32_t) count_value_fine;

				if ((0 != sleep_when_ms) && (sleep_when_ms > ms))
				{
					needs_update = 1;
				}
			}

			if ((0 != long_press_when_ms) && (long_press_when_ms < ms))
			{
				// Switch State
				sleep_when_ms = 0;
				long_press_when_ms = 0;
				highlight = 6;
				needs_update = 1;
				state = STATE_SETTING;
			}

			if ((0 != sleep_when_ms) && (sleep_when_ms < ms))
			{
				display_enable(0);
				do_sleep();
				sleep_when_ms = 1;
			}

			// STATE_COUNTING
			break;
		
		case STATE_SETTING:
			while (event_count > 0)
			{
				// Consume Event
				cli();
				curr_event = *event_read;
				PTR_INC(event_read);
				--event_count;
				sei();

				// Process Event
				switch(curr_event)
				{
				case EVENT_BUTTON_DOWN:
					long_press_when_ms = ms + LONG_PRESS;
					break;

				case EVENT_BUTTON_UP:
					if (long_press_when_ms > ms)
					{
						// Short Press

						if (0 != dir_highlight)
						{
							if (0 == move_dir)
							{
								move_dir = 1;
							}
							else
							{
								move_dir = 0;
							}
						}

						if (0 != highlight)
						{
							uint32_t temp_count = count_value;
							uint32_t div = 1;
							uint8_t i;

							for (i = 1; i < highlight; ++i)
							{
								div *= 10;
							}

							temp_count = count_value % (div * 10);
							count_value -= temp_count;
							temp_count += div;
							temp_count %= div * 10;
							count_value += temp_count;
							count_value_fine = count_value;
						}

						needs_update = 1;
					}
					long_press_when_ms = 0;

					// EVENT_BUTTON_UP
					break;
				}
			}

			if ((0 != long_press_when_ms) && (long_press_when_ms < ms))
			{
				// Long press

				if (0 != highlight)
				{
					--highlight;
					if (0 == highlight)
					{
						dir_highlight = 1;
					}
				}
				else if (0 != dir_highlight)
				{
					// Switch state
					sleep_when_ms = ms + DISPLAY_DELAY;
					dir_highlight = 0;
					state = STATE_COUNTING;

					// Write starting value to EEPROM
					++eeprom_idx;
					eeprom_idx %= EEPROM_SIZE;
					write_eeprom_val(eeprom_idx, count_value, move_dir);
				}

				needs_update = 1;
				long_press_when_ms = 0;
			}

			// STATE_SETTING
			break;
		}

		if (needs_update)
		{
			update_display(count_value, highlight, move_dir, dir_highlight);
			needs_update = 0;
		}
	}
}

void simple_init()
{
	// No Prescaler - 1 MHz
	// Set Mode to CTC with ICR1
	TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 < CS10)

	// 1 000 000 / 10 000 = 100 (Hz)
	// Interrupt every 10ms
	ICR1 = 10000;

	// Enable Interrupt on overflow
	TIMSK = (1 << TOIE1);

	// Interrupt on Up/Down/Click pins
	PCMSK0 = (1 << PCINT0) | (1 << PCINT1) | (1 << PCINT2) | (1 << PCINT3) | (1 << PCINT5);

	// Interrupt on rotary encoder pins
	PCMSK1 = (1 << PCINT8) | (1 << PCINT9);

	// Enable Interrupt on pin-changes
	GIMSK = (1 << PCIE0) | (1 << PCIE1);

	// Sleep mode - power down
	MCUCR = (1 << SM1);

	// Power reduction - Disable timer 0 and ADC
	PRR = (1 << PRTIM0) | (1 << PRADC);

	old_btn_pin_state = 0x2F;
	old_rot_pin_state = (PINB & 0x03);
}

void display_send_cmd(uint8_t cmd)
{
	i2c_start(OLED_ADDRESS | I2C_WRITE);
	i2c_write(0x00); // Command Indicator
	i2c_write(cmd);
	i2c_stop();
}

void display_send_data(const uint8_t *data, uint16_t buflen)
{
	uint16_t i;

	for (i = 0; i < buflen; ++i)
	{
		if (0 == (i % 0x0F))
		{
			i2c_start(OLED_ADDRESS | I2C_WRITE);
			i2c_write(0x40); // Data Indicator
		}

		i2c_write(data[i]);

		if (0x0F == (i % 0x0F))
		{
			i2c_stop();
		}
	}

	if (0 != (i % 0x0F))
	{
		i2c_stop();
	}
}

void display_init()
{
	uint16_t i;
	uint8_t buf[16];

	const uint8_t cmd_list[] =
	{
		0xAE,        // Display OFF
		0xD5,        // Set Clock Divider / Oscillator Frequency
		0x80,        // Default OSC / No Div
		0xA8,        // Set Multiplex Ratio
		OLED_Y_SIZE - 1,
		0xD3,        // Set Display Offset
		0x00,        // 0 Offset
		0x40 | 0x00, // Set Start Line To 0
		0x8D,        // Set Charge Pump
		0x14,        // Enable Charge Pump When Display Is On
		0x20,        // Set Memory Mode
		0x00,        // Horizontal Mode
		0xA0 | 0x01, // Set Segment Remap - column 127 is seg 0
		0xC8,        // Set COM Scan Direction To Decrement
		0xDA,        // Set COM Pin Configuration
		0x02 | 0x10, // Alternating COM Pin Configuration (COM63 = 0, COM31 = 1, ...)
		0x81,        // Set Contrast
		0xFF,        // Contrast Value
		0xD9,        // Set Precharge Period
		0x01 | 0xF0, // Phase 1 - 1 CLK, Phase 2 - 15 CLK
		0xDB,        // Set VCOM Deselect Level
#if 1
		0x30,        // 0.83 x Vcc
#else
		0x40,        // UNKNOWN (0.90 x Vcc Assumed)
#endif
		0xA4,        // Display RAM Contents
		0xA6,        // Set Normal Display (1 is white)
		0x2E,        // Deactivate Scroll
		0xAF,        // Display ON

		// Commands to send to RAM
		0x21,        // Set Column Address
		0x00,
		OLED_X_SIZE - 1,
		0x22,        // Set Page Address
		0x00,
		(OLED_Y_SIZE / 8) - 1
	};

	// Send Commands
	for (i = 0; i < sizeof(cmd_list); ++i)
	{
		display_send_cmd(cmd_list[i]);
	}

	// Fill Buffer With Black
	for (i = 0; i < sizeof(buf); ++i)
	{
		buf[i] = 0;
	}

	// Black Entire Screen
	for (i = 0; i < OLED_X_SIZE * OLED_Y_SIZE / 8 / sizeof(buf); ++i)
	{
		display_send_data(buf, sizeof(buf));
	}
}

void display_enable(uint8_t en)
{
	static uint8_t old_state = 1;

	if (en != 0)
	{
		en = 1;
	}

	if (en != old_state)
	{
		display_send_cmd(0xAE | (en & 0x01));
	}

	old_state = en;
}

void get_symbol16(uint8_t index, uint8_t *out)
{
	uint8_t i;
	uint8_t data_byte;

	for (i = 0; i < CHAR_SIZE * 4; ++i)
	{
		out[i] = 0;
	}

	if (index > sizeof(symbols) / 5)
	{
		return;
	}

	for (i = 0; i < CHAR_SIZE; ++i)
	{
		data_byte = pgm_read_byte(&(symbols[index * CHAR_SIZE + i]));
		out[i * 2 + 1] = pgm_read_byte(&(unfold_table[data_byte & 0x0F]));
		out[CHAR_SIZE * 2 + i * 2 + 1] = pgm_read_byte(&(unfold_table[(data_byte >> 4) & 0x0F]));
	}
}

// Symbol idx in table,
// Start x pixel - 0 -- 117
// Y row - 0 -- 3
// Char Size - 10x16
void print_symbol(uint8_t symbol_idx, uint8_t x, uint8_t y, uint8_t invert)
{
	uint8_t unfolded_symbol[CHAR_SIZE * 4];
	uint8_t cmd_list[6] = 
	{
		0x21, // Set Column Address
		0x00, // Start Address
		0x00, // End Address
		0x22, // Set Page Address
		0x00, // Start Page 0
		0x00 // End Page
	};
	uint8_t i;

	get_symbol16(symbol_idx, unfolded_symbol);
	if (invert)
	{
		for (i = 0; i < sizeof(unfolded_symbol); ++i)
		{
			unfolded_symbol[i] = ~(unfolded_symbol[i]);
		}
	}

	x = x % (OLED_X_SIZE);
	y = y % (OLED_Y_SIZE / (8 * 2));

	if (x > (OLED_X_SIZE - CHAR_SIZE * 2))
	{
		x = OLED_X_SIZE - CHAR_SIZE * 2;
	}

	cmd_list[1] = x;
	cmd_list[2] = x + CHAR_SIZE * 2 - 1;
	cmd_list[4] = y * 2;
	cmd_list[5] = y * 2 + 1;

	for (i = 0; i < sizeof(cmd_list); ++i)
	{
		display_send_cmd(cmd_list[i]);
	}

	display_send_data(unfolded_symbol, sizeof(unfolded_symbol));
}

void print_mm(uint32_t value, uint8_t highlight)
{
	static_uint8_t old_symbols[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
	static uint8_t old_highlight = 0xFF;
	uint8_t x;
	uint8_t y;
	uint8_t symbol;
	uint8_t invert;
	uint8_t is_first;
	uint8_t i;

	x = 0;
	y = 0;
	is_first = 1;

	// Print 6 digits (only needed ones)
	for (i = 6; i > 0; --i)
	{
		symbol = extract_digit(value, i);
		if (highlight == i)
		{
			invert = 1;
		}
		else
		{
			invert = 0;
		}

		// Leading zero removal
		// There's no highlight, this is the first digit to write, it's 0
		// and it's not the last digit
		// or everything is removed
		if ((is_first && (0 == highlight) && (0 == symbol) && (1 != i)) ||
			(0xFF == highlight))
		{
			symbol = 0xFF;
		}
		else
		{
			is_first = 0;
		}

		// Update only necessary digits
		// On first run, on highlight change or on symbol change
		if ((0xFF == old_highlight) || (old_highlight != highlight) ||
			(symbol != old_symbols[6 - i]))
		{
			print_symbol(symbol, x, y, invert);
			old_symbols[6 - i] = symbol;
		}
		x += (CHAR_SIZE + 1) * 2;
	}

	// Print 'mm' (only on first run)
	if (0xFF == old_highlight)
	{
		print_symbol(15, x, y, 0);
		x += (CHAR_SIZE + 1) * 2;
		print_symbol(15, x, y, 0);
	}

	if (0xFF == highlight)
	{
		print_symbol(0xFF, x, y, 0);
		x += (CHAR_SIZE + 1) * 2;
		print_symbol(0xFF, x, y, 0);
	}
	old_highlight = highlight;
}

void print_direction(uint8_t is_A, uint8_t highlight)
{
	uint8_t symbol;
	static uint8_t old_is_A = 0;
	static uint8_t old_highlight = 0xFF;

	if (is_A)
	{
		symbol = 10;
	}
	else
	{
		symbol = 11;
	}

	// Clear it all
	if ((0xFF == highlight) && (old_highlight != highlight))
	{
		print_symbol(0xFF, OLED_X_SIZE - (CHAR_SIZE + 1) * 2 * 2, 0, 0);
		print_symbol(0xFF, OLED_X_SIZE - (CHAR_SIZE + 1) * 2, 0, 0);
	}
	else
	{
		// Print '>' only when display is comming from clear
		if (0xFF == old_highlight)
		{
			print_symbol(symbol_index[0], OLED_X_SIZE - (CHAR_SIZE + 1) * 2 * 2, 0, 0);
		}

		// Print symbol on change or highlight change
		if ((old_is_A != is_A) || (old_highlight != highlight))
		{
			print_symbol(symbol, OLED_X_SIZE - (CHAR_SIZE + 1) * 2, 0, highlight);
		}
	}
	old_is_A = is_A;
	old_highlight = highlight;
}

int16_t find_eeprom_idx()
{
	uint8_t err;
	int16_t idx = 0;
	uint8_t found = 0;
	uint32_t value = 0xFFFFFFFF;

	// FF is the erased byte value
	for (idx = 0; idx < EEPROM_SIZE / EEPROM_IDX_SIZE; ++idx)
	{
		err = read_eeprom_val(idx, &value);
		if (0 != err)
		{
			return -1;
		}

		if (value != 0xFFFFFFFF)
		{
			found = 1;
			break;
		}
	}

	// Not found = new EEPROM
	if (!found)
	{
		return 0;
	}

	return idx;
}

int8_t read_eeprom_val(int8_t idx, uint32_t *value, uint8_t *direction)
{
	int8_t err;
	uint8_t buf[4];

	if (idx < 0)
	{
		return -1;
	}
	idx %= (EEPROM_SIZE / EEPROM_IDX_SIZE);

	err = i2c_read_buff(EEPROM_ADDRESS, idx * EEPROM_IDX_SIZE, buf, 4);
	if (4 != err)
	{
		return -1;
	}

	(*direction) = buf[0];
	buf[0] = 0;
	(*value) = 0;

	for (uint8_t i = 0; i < 4; ++i)
	{
		(*value) <<= 8;
		(*value) |= buf[i];
	}

	return 0;
}

int8_t write_eeprom_val(int8_t idx, uint32_t value, uint8_t direction)
{
	int8_t err;
	uint8_t buf[4];

	if (idx < 0)
	{
		return -1;
	}
	idx %= (EEPROM_SIZE / EEPROM_IDX_SIZE);

	for (uint8_t i = 0; i < 4; ++i)
	{
		buf[i] = ((value >> ((3 - i) * 8)) & 0xFF);
	}

	buf[0] = direction;

	err = i2c_write_buff(EEPROM_ADDRESS, idx * EEPROM_IDX_SIZE, buf, 4)
	if (4 != err)
	{
		return -1;
	}

	return 0;
}

int8_t reset_battery()
{
	int8_t err;
	uint8_t val;

	// Reset All Battery Counters
	val = 0x02;
	err = i2c_write_buff(BATTERY_ADDRESS, BATTERY_REG_CTRL, &val, 1);
	if (1 != err)
	{
		return -1;
	}

	// Set Running Mode
	val = 0x10;
	err = i2c_write_buff(BATTERY_ADDRESS, BATTERY_REG_MODE, &val, 1);
	if (1 != err)
	{
		return -1;
	}

	return 0;
}

int8_t read_battery(int16_t *mAh, int16_t *mV, int16_t *temp)
{
	// Reading from 0x02 to 0x0B inclusive
	uint8_t buf[BATTERY_BUFF_SIZE];
	int8_t err;
	int32_t val;

	err = i2c_read_buff(BATTERY_ADDRESS, BATTERY_READ_ADDR, buf, BATTERY_BUFF_SIZE);
	if (err != BATTERY_BUFF_SIZE)
	{
		return -1;
	}

	// Calc mAh
	(*mAh) = buf[BATTERY_CHARGE + 1];
	(*mAh) <<= 8;
	(*mAh) |= buf[BATTERY_CHARGE];

	val = (*mAh);
	val *= BATTERY_CHARGE_MULT;
	val /= BATTERY_CHARGE_DIV;
	(*mAh) = val;

	// Calc mV
	(*mV) = buf[BATTERY_VOLT + 1];
	(*mV) <<= 8;
	(*mV) |= buf[BATTERY_VOLT];

	val = (*mV);
	val *= BATTERY_VOLT_MULT;
	val /= BATTERY_VOLT_DIV;
	(*mV) = val;

	// Calc temp
	(*temp) = buf[BATTERY_TEMP + 1];
	(*temp) <<= 8;
	(*temp) |= buf[BATTERY_TEMP];

	val = (*temp);
	val *= BATTERY_TEMP_MULT;
	val /= BATTERY_TEMP_DIV;
	(*temp) = val;

	return 0;
}

int8_t i2c_write_buff(uint8_t i2c_addr, uint8_t data_addr, uint8_t *buf, uint8_t len)
{
	uint8_t err;

	err = i2c_start(i2c_addr | I2C_WRITE);
	if (0 != err)
	{
		i2c_stop();
		return -1;
	}

	err = i2c_write(data_addr);
	if (0 != err)
	{
		i2c_stop();
		return -1;
	}

	for (uint8_t i = 0; i < len; ++i)
	{
		err = i2c_write(buf[i]);
		if (0 != err)
		{
			i2c_stop();
			return i;
		}
	}

	i2c_stop();
	return i;
}

int8_t i2c_read_buff(uint8_t i2c_addr, uint8_t data_addr, uint8_t *buf, uint8_t len)
{
	uint8_t err;

	err = i2c_start(i2c_addr | I2C_WRITE);
	if (0 != err)
	{
		i2c_stop();
		return -1;
	}

	err = i2c_write(data_addr);
	if (0 != err)
	{
		i2c_stop();
		return -1;
	}

	err = i2c_start(i2c_addr | I2C_READ);
	if (0 != err)
	{
		i2c_stop();
		return -1;
	}

	for (uint8_t i = 0; i < len; ++i)
	{
		buf[i] = i2c_read(i == len - 1);
	}

	i2c_stop();
	return i;
}

void do_sleep()
{
	// Disable clock to USI
	PRR |= (1 << PRUSI);

	// Enable sleep
	MCUCR |= (1 << SE);

	// Sleep
	sleep_cpu();

	// Reset ms counter
	cli();
	ms = 2;
	sei();

	// Disable sleep
	MCUCR &= ~(1 << SE);

	// Enable clock to USI
	PRR &= ~(1 << PRUSI);
}

uint8_t extract_digit(uint32_t value, uint8_t digit_num)
{
	while (1 != digit_num)
	{
		value /= 10;
		--digit_num;
	}
	return value % 10;
}
