// ----------------------------------------
// SONAR-APP ------------------------------
// ----------------------------------------

#include "Arduino.h"
#include "EEPROM.h"
#include "U8glib.h"

// ----------------------------------------

#define MIN3(a, b, c) ((a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c))
/*#define MIN(...) ({ \
	int values[] = { __VA_ARGS__ }; \
	int numValues = sizeof(values) / sizeof(values[0]); \
	int minValue = values[0]; \
	for (int i = 1; i < numValues; i++) { \
		if (values[i] < minValue) minValue = values[i]; \
	} \
	minValue; \
})*/

// ----------------------------------------

#define __SERIAL__ true

// ----------------------------------------

#if __SERIAL__
  #define LOG_B(...) Serial.begin(__VA_ARGS__)
  #define LOG_PR(...) Serial.print(__VA_ARGS__)
  #define LOG_PL(...) Serial.println(__VA_ARGS__)
  #define LOG_PR_(...) Serial.print(F(__VA_ARGS__))
  #define LOG_PL_(...) Serial.println(F(__VA_ARGS__))
#else
  #define LOG_B(...)
  #define LOG_PR(...)
  #define LOG_PL(...)
  #define LOG_PR_(...)
  #define LOG_PL_(...)
#endif

// ----------------------------------------

#define BAUD_RATE 9600

#define SONAR_NUM 3
#define SONAR_READINGS 3
#define SONAR_01_TRIG_PIN 9
#define SONAR_01_ECHO_PIN 8
#define SONAR_02_TRIG_PIN 7
#define SONAR_02_ECHO_PIN 6
#define SONAR_03_TRIG_PIN 5
#define SONAR_03_ECHO_PIN 4
#define MIN_DISTANCE 2
#define MAX_DISTANCE 400
#define DET_DISTANCE 100

#define BUTTON_PIN 3
#define BUTTON_DEBOUNCE 50

#define BUZZER_PIN 11
#define MUTE_ADDRESS 0

#define BIT8 8

// ----------------------------------------

struct Data_t {
  uint8_t EchoPin;
	uint8_t TriggerPin;
	uint16_t Distance;
	uint16_t LastDistance;
	int LabelXPos;
	int LabelYPos;
	int LabelWidth;
	int LabelStartPosX;
	int LabelStartPosY;
	int LabelEndPosX;
	int LabelEndPosY;
};

struct Data_t Data[SONAR_NUM];

// ----------------------------------------

uint8_t CurrSonar = 0;
int DistanceStep01;
int DistanceStep02;
int DistanceStep03;
int DistanceStep04;
char Buffer[10];

volatile bool ButtonInit = false;
volatile bool ButtonStatus = {
  !true
};

bool Mute = false;

// ----------------------------------------

const unsigned char BitmapTitle[140] PROGMEM = {
	0xFF, 0xCF, 0xFD, 0x07, 0x9F, 0xF9, 0xFF, 0xFF, 0xCF, 0xFD, 0x07, 0x9F, 0xF9, 0xFF, 0xF0, 0x08,
	0x3D, 0x07, 0x90, 0x79, 0xE1, 0xF0, 0x08, 0x3D, 0x07, 0x90, 0x79, 0xE1, 0xF0, 0x08, 0x3D, 0xC7,
	0x90, 0x79, 0xE1, 0xF0, 0x08, 0x3D, 0xC7, 0x90, 0x79, 0xE1, 0xFF, 0xC8, 0x3D, 0x27, 0x90, 0x79,
	0xFE, 0xFF, 0xC8, 0x3D, 0x27, 0x9F, 0xF9, 0xFE, 0x03, 0xC8, 0x3D, 0x1F, 0x9F, 0xF9, 0xE1, 0x03,
	0xC8, 0x3D, 0x1F, 0x90, 0x79, 0xE1, 0x03, 0xC8, 0x3D, 0x07, 0x90, 0x79, 0xE1, 0xFF, 0xCF, 0xFD,
	0x07, 0x90, 0x79, 0xE1, 0xFF, 0xCF, 0xFD, 0x07, 0x90, 0x79, 0xE1, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x0F,
	0xFF, 0xFF, 0xFF, 0xF0, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0x00, 0x00
};

const unsigned char BitmapSonar01AOff[56] PROGMEM = {
	0x00, 0x40, 0x00, 0x00, 0x02, 0xa8, 0x00, 0x00, 0x05, 0x55, 0x00, 0x00, 0x02, 0xaa, 0xa0, 0x00, 
	0x05, 0x55, 0x54, 0x00, 0x0a, 0xaa, 0xaa, 0xa0, 0x15, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa0, 
	0x01, 0x55, 0x55, 0x40, 0x00, 0x2a, 0xaa, 0xa0, 0x00, 0x15, 0x55, 0x40, 0x00, 0x02, 0xaa, 0xa0, 
	0x00, 0x00, 0x15, 0x40, 0x00, 0x00, 0x02, 0xa0
};

const unsigned char BitmapSonar01AOn[56] PROGMEM = {
	0x00, 0xc0, 0x00, 0x00, 0x03, 0xf8, 0x00, 0x00, 0x07, 0xff, 0x00, 0x00, 0x07, 0xff, 0xe0, 0x00, 
	0x0f, 0xff, 0xfe, 0x00, 0x0f, 0xff, 0xff, 0xf0, 0x1f, 0xff, 0xff, 0xf0, 0x0f, 0xff, 0xff, 0xe0, 
	0x03, 0xff, 0xff, 0xe0, 0x00, 0x7f, 0xff, 0xe0, 0x00, 0x1f, 0xff, 0xe0, 0x00, 0x03, 0xff, 0xe0, 
	0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x03, 0xe0
};

const unsigned char BitmapSonar01BOff[64] PROGMEM = {
	0x02, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x02, 0xa0, 0x00, 0x00, 0x05, 0x54, 0x00, 0x00, 
	0x0a, 0xaa, 0x80, 0x00, 0x15, 0x55, 0x50, 0x00, 0x0a, 0xaa, 0xaa, 0x80, 0x15, 0x55, 0x55, 0x50, 
	0x0a, 0xaa, 0xaa, 0xa8, 0x05, 0x55, 0x55, 0x50, 0x00, 0xaa, 0xaa, 0xa8, 0x00, 0x15, 0x55, 0x50, 
	0x00, 0x02, 0xaa, 0xa0, 0x00, 0x00, 0x55, 0x50, 0x00, 0x00, 0x0a, 0xa0, 0x00, 0x00, 0x00, 0x50
};

const unsigned char BitmapSonar01BOn[64] PROGMEM = {
	0x02, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x00, 0x07, 0xf0, 0x00, 0x00, 0x0f, 0xfc, 0x00, 0x00, 
	0x0f, 0xff, 0x80, 0x00, 0x1f, 0xff, 0xf8, 0x00, 0x1f, 0xff, 0xff, 0x80, 0x3f, 0xff, 0xff, 0xf8, 
	0x1f, 0xff, 0xff, 0xf8, 0x07, 0xff, 0xff, 0xf8, 0x01, 0xff, 0xff, 0xf8, 0x00, 0x3f, 0xff, 0xf8, 
	0x00, 0x07, 0xff, 0xf0, 0x00, 0x00, 0xff, 0xf0, 0x00, 0x00, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0xf0
};

const unsigned char BitmapSonar01COff[68] PROGMEM = {
	0x08, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x0a, 0x80, 0x00, 0x00, 0x15, 0x50, 0x00, 0x00, 
	0x2a, 0xaa, 0x00, 0x00, 0x15, 0x55, 0x40, 0x00, 0x2a, 0xaa, 0xaa, 0x00, 0x55, 0x55, 0x55, 0x40, 
	0x2a, 0xaa, 0xaa, 0xaa, 0x15, 0x55, 0x55, 0x54, 0x02, 0xaa, 0xaa, 0xa8, 0x00, 0x55, 0x55, 0x54, 
	0x00, 0x0a, 0xaa, 0xa8, 0x00, 0x01, 0x55, 0x54, 0x00, 0x00, 0x2a, 0xa8, 0x00, 0x00, 0x05, 0x54, 
	0x00, 0x00, 0x00, 0x28
};

const unsigned char BitmapSonar01COn[68] PROGMEM = {
	0x08, 0x00, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x00, 0x1f, 0xc0, 0x00, 0x00, 0x1f, 0xf8, 0x00, 0x00, 
	0x3f, 0xff, 0x00, 0x00, 0x3f, 0xff, 0xe0, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x7f, 0xff, 0xff, 0xe0, 
	0x7f, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xfe, 0x03, 0xff, 0xff, 0xfc, 0x00, 0xff, 0xff, 0xfc, 
	0x00, 0x1f, 0xff, 0xfc, 0x00, 0x03, 0xff, 0xfc, 0x00, 0x00, 0x3f, 0xfc, 0x00, 0x00, 0x07, 0xfc, 
	0x00, 0x00, 0x00, 0x3c
};

const unsigned char BitmapSonar01DOff[72] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x55, 0x00, 0x00, 0x00, 0x2a, 0x80, 0x00, 0x00, 
	0x55, 0x50, 0x00, 0x00, 0xaa, 0xaa, 0x00, 0x00, 0x55, 0x55, 0x50, 0x00, 0xaa, 0xaa, 0xaa, 0x00, 
	0x55, 0x55, 0x55, 0x50, 0x2a, 0xaa, 0xaa, 0xaa, 0x05, 0x55, 0x55, 0x54, 0x02, 0xaa, 0xaa, 0xaa, 
	0x00, 0x55, 0x55, 0x54, 0x00, 0x0a, 0xaa, 0xaa, 0x00, 0x01, 0x55, 0x54, 0x00, 0x00, 0x2a, 0xa8, 
	0x00, 0x00, 0x01, 0x54, 0x00, 0x00, 0x00, 0x08
};

const unsigned char BitmapSonar02AOff[36] PROGMEM = {
	0x05, 0x55, 0x55, 0x40, 0x0a, 0xaa, 0xaa, 0xa0, 0x05, 0x55, 0x55, 0x40, 0x0a, 0xaa, 0xaa, 0xa0, 
	0x05, 0x55, 0x55, 0x40, 0x0a, 0xaa, 0xaa, 0xa0, 0x05, 0x55, 0x55, 0x40, 0x0a, 0xaa, 0xaa, 0xa0, 
	0x15, 0x55, 0x55, 0x50
};

const unsigned char BitmapSonar01DOn[72] PROGMEM = {
	0x20, 0x00, 0x00, 0x00, 0x38, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x7f, 0xc0, 0x00, 0x00, 
	0xff, 0xf8, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0xff, 0xff, 0xf0, 0x00, 0xff, 0xff, 0xfe, 0x00, 
	0x7f, 0xff, 0xff, 0xf0, 0x3f, 0xff, 0xff, 0xfe, 0x0f, 0xff, 0xff, 0xfe, 0x03, 0xff, 0xff, 0xfe, 
	0x00, 0x7f, 0xff, 0xfe, 0x00, 0x1f, 0xff, 0xfe, 0x00, 0x01, 0xff, 0xfc, 0x00, 0x00, 0x3f, 0xfc, 
	0x00, 0x00, 0x03, 0xfc, 0x00, 0x00, 0x00, 0x1c
};

const unsigned char BitmapSonar02AOn[36] PROGMEM = {
	0x07, 0xff, 0xff, 0xc0, 0x0f, 0xff, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 
	0x0f, 0xff, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 
	0x1f, 0xff, 0xff, 0xf0
};

const unsigned char BitmapSonar02BOff[36] PROGMEM = {
	0x15, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa0, 0x15, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa0, 
	0x15, 0x55, 0x55, 0x50, 0x2a, 0xaa, 0xaa, 0xa8, 0x15, 0x55, 0x55, 0x50, 0x2a, 0xaa, 0xaa, 0xa8, 
	0x05, 0x55, 0x55, 0x40
};

const unsigned char BitmapSonar02BOn[36] PROGMEM = {
	0x1f, 0xff, 0xff, 0xf0, 0x1f, 0xff, 0xff, 0xf0, 0x1f, 0xff, 0xff, 0xf0, 0x1f, 0xff, 0xff, 0xf0, 
	0x3f, 0xff, 0xff, 0xf8, 0x3f, 0xff, 0xff, 0xf8, 0x3f, 0xff, 0xff, 0xf8, 0x3f, 0xff, 0xff, 0xf8, 
	0x0f, 0xff, 0xff, 0xe0
};

const unsigned char BitmapSonar02COff[40] PROGMEM = {
	0x10, 0x00, 0x00, 0x04, 0x0a, 0xaa, 0xaa, 0xa8, 0x15, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xaa, 
	0x15, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xaa, 0x15, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xaa, 
	0x15, 0x55, 0x55, 0x54, 0x0a, 0xaa, 0xaa, 0xa8
};

const unsigned char BitmapSonar02COn[40] PROGMEM = {
	0x18, 0x00, 0x00, 0x0c, 0x1f, 0xff, 0xff, 0xfc, 0x3f, 0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xfe, 
	0x3f, 0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xfe, 
	0x3f, 0xff, 0xff, 0xfe, 0x0f, 0xff, 0xff, 0xf8
};

const unsigned char BitmapSonar02DOff[50] PROGMEM = {
	0x02, 0x80, 0x00, 0x00, 0x28, 0x01, 0x55, 0x55, 0x55, 0x50, 0x02, 0xaa, 0xaa, 0xaa, 0xa8, 0x01, 
	0x55, 0x55, 0x55, 0x50, 0x02, 0xaa, 0xaa, 0xaa, 0xa8, 0x05, 0x55, 0x55, 0x55, 0x54, 0x02, 0xaa, 
	0xaa, 0xaa, 0xa8, 0x05, 0x55, 0x55, 0x55, 0x54, 0x02, 0xaa, 0xaa, 0xaa, 0xa8, 0x00, 0x55, 0x55, 
	0x55, 0x40
};

const unsigned char BitmapSonar02DOn[50] PROGMEM = {
	0x03, 0x80, 0x00, 0x00, 0x38, 0x03, 0xff, 0xff, 0xff, 0xf8, 0x03, 0xff, 0xff, 0xff, 0xf8, 0x03, 
	0xff, 0xff, 0xff, 0xf8, 0x03, 0xff, 0xff, 0xff, 0xf8, 0x07, 0xff, 0xff, 0xff, 0xfc, 0x07, 0xff, 
	0xff, 0xff, 0xfc, 0x07, 0xff, 0xff, 0xff, 0xfc, 0x07, 0xff, 0xff, 0xff, 0xfc, 0x00, 0xff, 0xff, 
	0xff, 0xe0
};

const unsigned char BitmapSonar03AOff[56] PROGMEM = {
	0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x2a, 0x80, 0x00, 0x01, 0x55, 0x40, 0x00, 0x0a, 0xaa, 0x80, 
	0x00, 0x55, 0x55, 0x40, 0x0a, 0xaa, 0xaa, 0xa0, 0x15, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa0, 
	0x05, 0x55, 0x55, 0x00, 0x0a, 0xaa, 0xa8, 0x00, 0x05, 0x55, 0x50, 0x00, 0x0a, 0xaa, 0x80, 0x00, 
	0x05, 0x50, 0x00, 0x00, 0x0a, 0x80, 0x00, 0x00
};

const unsigned char BitmapSonar03BOn[64] PROGMEM = {
	0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x01, 0xe0, 0x00, 0x00, 0x0f, 0xe0, 0x00, 0x00, 0x3f, 0xf0, 
	0x00, 0x01, 0xff, 0xf0, 0x00, 0x1f, 0xff, 0xf8, 0x01, 0xff, 0xff, 0xf8, 0x1f, 0xff, 0xff, 0xfc, 
	0x1f, 0xff, 0xff, 0xf8, 0x1f, 0xff, 0xff, 0xe0, 0x1f, 0xff, 0xff, 0x80, 0x1f, 0xff, 0xfc, 0x00, 
	0x0f, 0xff, 0xe0, 0x00, 0x0f, 0xff, 0x00, 0x00, 0x0f, 0xf0, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x00
};

const unsigned char BitmapSonar03AOn[56] PROGMEM = {
	0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x3f, 0x80, 0x00, 0x01, 0xff, 0xc0, 0x00, 0x0f, 0xff, 0xc0, 
	0x00, 0xff, 0xff, 0xe0, 0x1f, 0xff, 0xff, 0xe0, 0x1f, 0xff, 0xff, 0xf0, 0x0f, 0xff, 0xff, 0xe0, 
	0x0f, 0xff, 0xff, 0x80, 0x0f, 0xff, 0xfc, 0x00, 0x0f, 0xff, 0xf0, 0x00, 0x0f, 0xff, 0x80, 0x00, 
	0x0f, 0xf8, 0x00, 0x00, 0x0f, 0x80, 0x00, 0x00
};

const unsigned char BitmapSonar03BOff[64] PROGMEM = {
	0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0xa0, 0x00, 0x00, 0x05, 0x40, 0x00, 0x00, 0x2a, 0xa0, 
	0x00, 0x01, 0x55, 0x50, 0x00, 0x0a, 0xaa, 0xa8, 0x01, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa8, 
	0x15, 0x55, 0x55, 0x50, 0x0a, 0xaa, 0xaa, 0xa0, 0x15, 0x55, 0x55, 0x00, 0x0a, 0xaa, 0xa8, 0x00, 
	0x05, 0x55, 0x40, 0x00, 0x0a, 0xaa, 0x00, 0x00, 0x05, 0x50, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00
};

const unsigned char BitmapSonar03COff[68] PROGMEM = {
	0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0xa0, 0x00, 0x00, 0x01, 0x50, 0x00, 0x00, 0x0a, 0xa8, 
	0x00, 0x00, 0x55, 0x54, 0x00, 0x02, 0xaa, 0xa8, 0x00, 0x55, 0x55, 0x54, 0x02, 0xaa, 0xaa, 0xaa, 
	0x55, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xa8, 0x15, 0x55, 0x55, 0x40, 0x2a, 0xaa, 0xaa, 0x00, 
	0x15, 0x55, 0x50, 0x00, 0x2a, 0xaa, 0x80, 0x00, 0x15, 0x54, 0x00, 0x00, 0x2a, 0xa0, 0x00, 0x00, 
	0x14, 0x00, 0x00, 0x00
};

const unsigned char BitmapSonar03DOn[72] PROGMEM = {
	0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x38, 0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x07, 0xfc, 
	0x00, 0x00, 0x3f, 0xfe, 0x00, 0x01, 0xff, 0xfe, 0x00, 0x1f, 0xff, 0xfe, 0x00, 0xff, 0xff, 0xfe, 
	0x1f, 0xff, 0xff, 0xfc, 0xff, 0xff, 0xff, 0xf8, 0xff, 0xff, 0xff, 0xe0, 0xff, 0xff, 0xff, 0x80, 
	0xff, 0xff, 0xfc, 0x00, 0xff, 0xff, 0xf0, 0x00, 0x7f, 0xff, 0x00, 0x00, 0x7f, 0xf8, 0x00, 0x00, 
	0x7f, 0x80, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00
};

const unsigned char BitmapSonar03DOff[72] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x01, 0x54, 0x00, 0x00, 0x02, 0xa8, 
	0x00, 0x00, 0x15, 0x54, 0x00, 0x00, 0xaa, 0xaa, 0x00, 0x15, 0x55, 0x54, 0x00, 0xaa, 0xaa, 0xaa, 
	0x15, 0x55, 0x55, 0x54, 0xaa, 0xaa, 0xaa, 0xa8, 0x55, 0x55, 0x55, 0x40, 0xaa, 0xaa, 0xaa, 0x80, 
	0x55, 0x55, 0x54, 0x00, 0xaa, 0xaa, 0xa0, 0x00, 0x55, 0x55, 0x00, 0x00, 0x2a, 0xa8, 0x00, 0x00, 
	0x55, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00
};

const unsigned char BitmapSonar03COn[68] PROGMEM = {
	0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x03, 0xf8, 0x00, 0x00, 0x1f, 0xf8, 
	0x00, 0x00, 0xff, 0xfc, 0x00, 0x07, 0xff, 0xfc, 0x00, 0x7f, 0xff, 0xfe, 0x07, 0xff, 0xff, 0xfe, 
	0x7f, 0xff, 0xff, 0xfe, 0x7f, 0xff, 0xff, 0xf8, 0x3f, 0xff, 0xff, 0xc0, 0x3f, 0xff, 0xff, 0x00, 
	0x3f, 0xff, 0xf8, 0x00, 0x3f, 0xff, 0xc0, 0x00, 0x3f, 0xfc, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 
	0x3c, 0x00, 0x00, 0x00
};

const unsigned char BitmapSoundOn[30] PROGMEM = {
	0x03, 0xc0, 0x0f, 0x02, 0x0c, 0x01, 0x02, 0x1c, 0x11, 0x02, 0x3c, 0x49, 0x00, 0x7d, 0x28, 0x00, 
	0x7d, 0x28, 0x02, 0x3c, 0x49, 0x02, 0x1c, 0x11, 0x02, 0x0c, 0x01, 0x03, 0xc0, 0x0f
};

const unsigned char BitmapSoundOff[30] PROGMEM = {
	0x03, 0xC0, 0x0F, 0x02, 0x0C, 0x01, 0x02, 0x1C, 0x01, 0x02, 0x3C, 0x01, 0x00, 0x7C, 0x00, 0x00,
	0x7C, 0x00, 0x02, 0x3C, 0x01, 0x02, 0x1C, 0x01, 0x02, 0x0C, 0x01, 0x03, 0xC0, 0x0F
};

const unsigned char BitmapBattery[30] PROGMEM = {
	0xF0, 0x03, 0xC0, 0x80, 0x00, 0x40, 0xBF, 0xFC, 0x40, 0xA0, 0x07, 0x40, 0x20, 0x07, 0x00, 0x20,
  0x07, 0x00, 0xA0, 0x07, 0x40, 0xBF, 0xFC, 0x40, 0x80, 0x00, 0x40, 0xF0, 0x03, 0xC0
};

const unsigned char BitmapEmptyBattery[288] PROGMEM = {
	0x1F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xE0, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xF0, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x30,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x31, 0xFF, 0xB0, 0xF7, 0xFE, 0xFF, 0xEF, 0x06,
	0x3F, 0x31, 0xFF, 0xB0, 0xF7, 0xFE, 0xFF, 0xEF, 0x06, 0x3F, 0x31, 0xE0, 0x30, 0xF7, 0x86, 0x1F,
	0x0F, 0x06, 0x3F, 0x31, 0xE0, 0x30, 0xF7, 0x86, 0x1F, 0x0F, 0x06, 0x3F, 0x31, 0xE0, 0x3B, 0xF7,
	0x86, 0x1F, 0x0F, 0x06, 0x3F, 0x31, 0xE0, 0x3B, 0xF7, 0x86, 0x1F, 0x0F, 0x06, 0x3F, 0x31, 0xE0,
	0x3B, 0xF7, 0x86, 0x1F, 0x0F, 0x06, 0x3F, 0x31, 0xFF, 0xB6, 0xF7, 0xFE, 0x1F, 0x07, 0xF8, 0x3F,
	0x31, 0xFF, 0xB6, 0xF7, 0xFE, 0x1F, 0x07, 0xF8, 0x3F, 0x31, 0xE0, 0x36, 0xF7, 0x80, 0x1F, 0x01,
	0xF0, 0x3F, 0x31, 0xE0, 0x36, 0xF7, 0x80, 0x1F, 0x01, 0xF0, 0x3F, 0x31, 0xE0, 0x30, 0xF7, 0x80,
	0x1F, 0x01, 0xF0, 0x3F, 0x31, 0xE0, 0x30, 0xF7, 0x80, 0x1F, 0x01, 0xF0, 0x3F, 0x31, 0xE0, 0x30,
	0xF7, 0x80, 0x1F, 0x01, 0xF0, 0x3F, 0x31, 0xFF, 0xB0, 0xF7, 0x80, 0x1F, 0x01, 0xF0, 0x3F, 0x31,
	0xFF, 0xB0, 0xF7, 0x80, 0x1F, 0x01, 0xF0, 0x3F, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x3F, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x3F, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0x1F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xE0
};

// ----------------------------------------

const u8g_fntpgm_uint8_t Font[137] U8G_FONT_SECTION("Font") = {
	0, 9, 5, 0, 250, 6, 0, 0, 0, 0, 48, 57, 0, 0, 250, 6, 
	0, 5, 6, 6, 6, 0, 250, 248, 216, 216, 216, 216, 248, 4, 6, 6, 
	6, 1, 250, 224, 96, 96, 96, 96, 240, 5, 6, 6, 6, 0, 250, 248, 
	24, 120, 192, 192, 248, 5, 6, 6, 6, 0, 250, 248, 24, 248, 24, 216, 
	248, 5, 6, 6, 6, 0, 250, 56, 120, 216, 248, 24, 24, 5, 6, 6, 
	6, 0, 250, 248, 192, 248, 24, 24, 248, 5, 6, 6, 6, 0, 250, 248, 
	192, 248, 216, 216, 248, 5, 6, 6, 6, 0, 250, 248, 24, 24, 24, 24, 
	24, 5, 6, 6, 6, 0, 250, 248, 216, 248, 216, 216, 248, 5, 6, 6, 
	6, 0, 250, 248, 216, 216, 248, 24, 248
};

// ----------------------------------------

U8GLIB_SSD1306_128X64 *Display;

// ----------------------------------------

class SonarApp {
  public:
    void Init() {
      LOG_B(BAUD_RATE);
			pinMode(BUZZER_PIN, OUTPUT);
			pinMode(BUTTON_PIN, INPUT_PULLUP);
			attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), ButtonISR, CHANGE);
			ButtonInit = true;
      Mute = (bool)EEPROM.read(MUTE_ADDRESS);
      Display = new U8GLIB_SSD1306_128X64(U8G_I2C_OPT_NONE);
      Display->setFont(Font);
      Display->setColorIndex(1);
      DistanceStep01 = MIN_DISTANCE + round((MAX_DISTANCE - MIN_DISTANCE) / 4.0 * 1.0);
      DistanceStep02 = MIN_DISTANCE + round((MAX_DISTANCE - MIN_DISTANCE) / 4.0 * 2.0);
      DistanceStep03 = MIN_DISTANCE + round((MAX_DISTANCE - MIN_DISTANCE) / 4.0 * 3.0);
      DistanceStep04 = MIN_DISTANCE + round((MAX_DISTANCE - MIN_DISTANCE) / 4.0 * 4.0);			
      Data[0].EchoPin = SONAR_01_ECHO_PIN;
      Data[0].TriggerPin = SONAR_01_TRIG_PIN;	
      Data[1].EchoPin = SONAR_02_ECHO_PIN;
      Data[1].TriggerPin = SONAR_02_TRIG_PIN;	
      Data[2].EchoPin = SONAR_03_ECHO_PIN;
      Data[2].TriggerPin = SONAR_03_TRIG_PIN;
      pinMode(Data[0].TriggerPin, OUTPUT);
      pinMode(Data[0].EchoPin, INPUT);		
      pinMode(Data[1].TriggerPin, OUTPUT);
      pinMode(Data[1].EchoPin, INPUT);		
      pinMode(Data[2].TriggerPin, OUTPUT);
      pinMode(Data[2].EchoPin, INPUT);	
      Data[0].LabelStartPosX = 41;		
      Data[0].LabelStartPosY = 20;	
      Data[0].LabelEndPosX = 30;		
      Data[0].LabelEndPosY = 58;		
      Data[1].LabelStartPosX = 63;		
      Data[1].LabelStartPosY = 23;	
      Data[1].LabelEndPosX = 63;		
      Data[1].LabelEndPosY = 60;			
      Data[2].LabelStartPosX = 85;		
      Data[2].LabelStartPosY = 20;	
      Data[2].LabelEndPosX = 97;		
      Data[2].LabelEndPosY = 57;
			LOG_PL_("Sonar device initialized");
    };
    
    void Run() {
      ButtonPressed();
			if (SonarScan() == SONAR_NUM) {
        SoundPlay();
        DisplayDraw();
      };
		};

  private:
    uint16_t SonarEcho(uint8_t echoPin, uint8_t triggerPin) {
      digitalWrite(triggerPin, LOW);
      delayMicroseconds(2);
      digitalWrite(triggerPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(triggerPin, LOW);
      uint16_t duration = pulseIn(echoPin, HIGH);
      return round(duration * 0.0343 / 2.0);
    };

		uint8_t SonarScan() {
      uint16_t totalDistance = 0;
			for (int i = 0; i < SONAR_READINGS; i++) {
        totalDistance += SonarEcho(Data[CurrSonar].EchoPin, Data[CurrSonar].TriggerPin);
        ButtonPressed();
      };
      Data[CurrSonar].Distance = round(totalDistance / SONAR_READINGS);
      if (Data[CurrSonar].LastDistance != Data[CurrSonar].Distance) {
				LOG_PR_("Sonar [");
				LOG_PR(CurrSonar);
				LOG_PR_("] = ");
				LOG_PR(Data[CurrSonar].Distance);
				LOG_PL_("cm");
        itoa(Data[CurrSonar].Distance, Buffer, 10);
        Data[CurrSonar].LabelWidth = Display->getStrWidth(Buffer);
        Data[CurrSonar].LabelXPos = map(constrain(Data[CurrSonar].Distance, MIN_DISTANCE, MAX_DISTANCE), MIN_DISTANCE, MAX_DISTANCE, Data[CurrSonar].LabelStartPosX, Data[CurrSonar].LabelEndPosX);
        Data[CurrSonar].LabelYPos = map(constrain(Data[CurrSonar].Distance, MIN_DISTANCE, MAX_DISTANCE), MIN_DISTANCE, MAX_DISTANCE, Data[CurrSonar].LabelStartPosY, Data[CurrSonar].LabelEndPosY);		
        Data[CurrSonar].LastDistance = Data[CurrSonar].Distance;
      };
      CurrSonar++;
      if (CurrSonar >= SONAR_NUM) CurrSonar = 0;
      return CurrSonar + 1;
		};
		
		void DisplayDraw() {
      uint16_t batteryStatus = BatteryStatus();
			Display->firstPage();
      do {
        if (batteryStatus >= 3610) {
          uint8_t batteryPercentage = map(batteryStatus, 3600, 4200, 0, 10);
          batteryPercentage = batteryPercentage > 10 ? 10 : batteryPercentage;
          Display->drawBitmapP(36, 0, (56 / BIT8), 20, BitmapTitle);
          Display->drawBitmapP(104, 0, (24 / BIT8), 10, Mute ? BitmapSoundOff : BitmapSoundOn);
          Display->drawBitmapP(0, 0, (24 / BIT8), 10, BitmapBattery);
          Display->drawBox(3, 3, batteryPercentage, 4);
          Display->drawBitmapP(24, 17, (32 / BIT8), 14, Data[0].Distance > DistanceStep01 ? BitmapSonar01AOn : BitmapSonar01AOff);
          Display->drawBitmapP(21, 25, (32 / BIT8), 16, Data[0].Distance > DistanceStep02 ? BitmapSonar01BOn : BitmapSonar01BOff);
          Display->drawBitmapP(18, 34, (32 / BIT8), 17, Data[0].Distance > DistanceStep03 ? BitmapSonar01COn : BitmapSonar01COff);
          Display->drawBitmapP(16, 43, (32 / BIT8), 18, Data[0].Distance > DistanceStep04 ? BitmapSonar01DOn : BitmapSonar01DOff);
          Display->drawBitmapP(48, 23, (32 / BIT8), 9, Data[1].Distance > DistanceStep01 ? BitmapSonar02AOn : BitmapSonar02AOff);
          Display->drawBitmapP(48, 33, (32 / BIT8), 9, Data[1].Distance > DistanceStep02 ? BitmapSonar02BOn : BitmapSonar02BOff);
          Display->drawBitmapP(47, 42, (32 / BIT8), 10, Data[1].Distance > DistanceStep03 ? BitmapSonar02COn : BitmapSonar02COff);
          Display->drawBitmapP(42, 52, (40 / BIT8), 10, Data[1].Distance > DistanceStep04 ? BitmapSonar02DOn : BitmapSonar02DOff);
          Display->drawBitmapP(72, 17, (32 / BIT8), 14, Data[2].Distance > DistanceStep01 ? BitmapSonar03AOn : BitmapSonar03AOff);
          Display->drawBitmapP(74, 25, (32 / BIT8), 16, Data[2].Distance > DistanceStep02 ? BitmapSonar03BOn : BitmapSonar03BOff);
          Display->drawBitmapP(77, 34, (32 / BIT8), 17, Data[2].Distance > DistanceStep03 ? BitmapSonar03COn : BitmapSonar03COff);
          Display->drawBitmapP(80, 43, (32 / BIT8), 18, Data[2].Distance > DistanceStep04 ? BitmapSonar03DOn : BitmapSonar03DOff);
          for (int i = 0; i < SONAR_NUM; i++) {
            Display->setColorIndex(0);
            Display->drawBox((Data[i].LabelXPos - Data[i].LabelWidth / 2) - 1 , Data[i].LabelYPos - 4, Data[i].LabelWidth + 2, 8);
            itoa(Data[i].Distance, Buffer, 10);
            Display->setColorIndex(1);
            Display->drawStr(Data[i].LabelXPos - Data[i].LabelWidth / 2, Data[i].LabelYPos - 3, Buffer);
          };
        } else Display->drawBitmapP(28, 16, (72 / BIT8), 32, BitmapEmptyBattery);
      } while (Display->nextPage());
		};

    static void ButtonISR() {
			if (ButtonInit == false) return;
			if (ButtonStatus == true) return;
			if (digitalRead(BUTTON_PIN) == HIGH) ButtonStatus = true;
    };

    static bool ButtonRead() {
			bool result = false;
			int buttonRead;
			static bool buttonPending = false;
  		static long int elapseTimer;
			if (ButtonStatus == false) return result;
			buttonRead = digitalRead(BUTTON_PIN);
			if (buttonRead == HIGH) {
				buttonPending = true;
				elapseTimer = millis();
			};
			if (buttonPending && buttonRead == LOW) {
				if (millis() - elapseTimer >= BUTTON_DEBOUNCE) {
					buttonPending = false;
					ButtonStatus = false;
					result = true;
				};
			};
			return result;
		};

		static void ButtonPressed() {
			if (ButtonRead() == false) return;
      Mute = !Mute;
      EEPROM.write(MUTE_ADDRESS, Mute ? 1 : 0);
			LOG_PR_("Buzzer ");
			LOG_PL(Mute ? "disabled" : "enabled");
    };

    void SoundPlay() {
      if (Mute == true) {
        noTone(BUZZER_PIN);
        return;
      };
			uint16_t distance = MIN3(Data[0].Distance, Data[1].Distance, Data[2].Distance);
			if (distance <= DET_DISTANCE) {
				uint16_t frequency = map(distance, MIN_DISTANCE, DET_DISTANCE, 2000, 300);
				tone(BUZZER_PIN, frequency);
			} else noTone(BUZZER_PIN);
		};

    uint16_t BatteryStatus() {
      uint16_t voltage = 0;
      ADCSRA = (1 << ADEN);
      ADCSRA |= (1 << ADPS0) | (1 << ADPS1) | (1 << ADPS2);
      ADMUX = (1 << REFS0) | (1 << MUX3) | (1 << MUX2) | (1 << MUX1);
      delay(1);
      ADCSRA |= (1 << ADSC);
      while (bit_is_set(ADCSRA, ADSC));
      voltage = ADC;
      ADCSRA |= (1 << ADSC);
      while (bit_is_set(ADCSRA, ADSC));
      voltage = ADC;
      voltage = 1148566UL / (unsigned long)voltage;
      return voltage;
    };

} Sonar;

// ----------------------------------------

void setup() {
  Sonar.Init();
};

// ----------------------------------------

void loop() {
  Sonar.Run();
};