#include "U8glib.h"
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

// 'car_image', 56x15px
const unsigned char bitmap_car_image [] PROGMEM = {
	0xc0, 0x40, 0x00, 0x00, 0x00, 0x02, 0x03, 0xc0, 0x8f, 0xff, 0xff, 0xff, 0xf1, 0x03, 0xe0, 0x70, 
	0x00, 0x00, 0x00, 0x0e, 0x07, 0xff, 0x00, 0xff, 0xff, 0xff, 0x00, 0xff, 0xc0, 0xff, 0x00, 0x00, 
	0x00, 0xff, 0x03, 0xc0, 0x00, 0x05, 0x75, 0xc0, 0x00, 0x03, 0x47, 0xf8, 0x05, 0x51, 0x40, 0x1f, 
	0xe2, 0x47, 0xfc, 0x05, 0x65, 0x80, 0x3f, 0xe2, 0x40, 0x78, 0x06, 0x45, 0x40, 0x1e, 0x02, 0x30, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x1e, 0x01, 0xff, 0xff, 0xff, 0x80, 0x78, 0x07, 0xe2, 0x00, 
	0x00, 0x00, 0x47, 0xe0, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0xbf, 0xff, 0xff, 0xff, 
	0xfd, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x06, 0x00
};
// 'sound_on', 24x10px
const unsigned char bitmap_sound_on [] 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
};
// 'unit_cm', 24x10px
const unsigned char bitmap_unit_cm [] PROGMEM = {
	0xf0, 0x03, 0xc0, 0x80, 0x00, 0x40, 0xbe, 0xff, 0x40, 0xb6, 0xdb, 0x40, 0x30, 0xdb, 0x00, 0x30, 
	0xdb, 0x00, 0xb6, 0xdb, 0x40, 0xbe, 0xdb, 0x40, 0x80, 0x00, 0x40, 0xf0, 0x03, 0xc0
};
/*
  Fontname: gamer_digits
  Copyright: Created with Fony 1.4.7
  Capital A Height: 0, '1' Height: 6
  Calculated Max Values w= 5 h= 6 x= 1 y= 0 dx= 6 dy= 0 ascent= 0 len= 6
  Font Bounding box     w= 9 h= 5 x= 0 y=-6
  Calculated Min Values           x= 0 y=-6 dx= 0 dy= 0
  Pure Font   ascent = 6 descent= 0
  X Font      ascent = 6 descent= 0
  Max Font    ascent = 0 descent=-6
*/
const u8g_fntpgm_uint8_t gamer_digits[137] U8G_FONT_SECTION("gamer_digits") = {
  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};

#define sensor_01_ECHO_PIN 2
#define sensor_01_TRIG_PIN 3

#define sensor_02_ECHO_PIN 4
#define sensor_02_TRIG_PIN 5

#define sensor_03_ECHO_PIN 6
#define sensor_03_TRIG_PIN 7

#define sensor_04_ECHO_PIN 8
#define sensor_04_TRIG_PIN 9


#define owner_name "cfs jan"


#define NUMBER_OF_SENSORS 4 //max is 6
#define bars_count  10 //max is 20

struct sensor_data { // structure for the sensor data
  int echo_pin; // ECHO pin for the ultrasonic distance sensor
	int trig_pin; // TRIG pin for the ultrasonic distance sensor
	int measured_distance_cm; // measured distance in CM
	int label_xpos; // x position of the distance label 
	int label_ypos; // y position of the distance label
	int label_width; // calculated width of the distance string
	int label_startpos_x; // start X position for the label
	int label_startpos_y; // start Y position for the label
	int label_endpos_x; // end X position for the label
	int label_endpos_y; // end Y position for the label
};
char buffer[10]; // helper C-style array of characters to store integers converted to strings

struct sensor_data sensor[NUMBER_OF_SENSORS]; // we have 3 sensors for now

int min_dist = 10;  // minimum distance to display in cm, this is also the minimum measurable distance for the used sensor
int max_dist = 200; // maximum distance to display in cm with the tile images, maximum measurable distance is 400cm = 4m

int bar_width = 128 / NUMBER_OF_SENSORS;
int bar_height = (42 - bars_count) / bars_count;


int dist_step[bars_count]; //

void setup() {
  // calculate individual steps for the 4 tiles
	for (int i=0; i < bars_count; i++) {
		dist_step[i] = min_dist + round((max_dist - min_dist) / bars_count * i);
	}
	bar_width = round(bar_width);
	bar_height = round(bar_height);

  sensor[0].echo_pin = sensor_01_ECHO_PIN;
  sensor[0].trig_pin = sensor_01_TRIG_PIN;

	sensor[1].echo_pin = sensor_02_ECHO_PIN;
  sensor[1].trig_pin = sensor_02_TRIG_PIN;

	sensor[2].echo_pin = sensor_03_ECHO_PIN;
  sensor[2].trig_pin = sensor_03_TRIG_PIN;

	sensor[3].echo_pin = sensor_04_ECHO_PIN;
  sensor[3].trig_pin = sensor_04_TRIG_PIN;

  // set pins to outputs and inputs  - trigger is output, echo is input
	 for (int i=0; i<NUMBER_OF_SENSORS; i++) { // for each sensor
	 	pinMode(sensor[i].trig_pin, OUTPUT);
  	pinMode(sensor[i].echo_pin, INPUT);
	 }

  u8g.setFont(gamer_digits); // gamer digits font - manually generated, only includes digits
  u8g.setColorIndex(1); // white color

	Serial.begin(115200); // debug only
}

void loop() {
    for (int i=0; i<NUMBER_OF_SENSORS; i++) { // for each sensor
	  // MEASURE DISTANCES

		// trigger the trigger pin for at least 10 microseconds
		digitalWrite(sensor[i].trig_pin, HIGH);
		delayMicroseconds(10);
		digitalWrite(sensor[i].trig_pin, LOW);

		// Read the result:
		sensor[i].measured_distance_cm = pulseIn(sensor[i].echo_pin, HIGH); // pulseIn command will measure the time for the pulse on a defined pin
		sensor[i].measured_distance_cm = round(sensor[i].measured_distance_cm * 0.0343 / 2.0); // speed of sound = 343m/s, has to travel back and forth, that´s why we divide it by 2

		//Serial.print("Sensor ");
		//Serial.print(i);		
		//Serial.print("  ");				
		//Serial.println(sensor[i].measured_distance_cm);		

		// calculate string width			
		itoa(sensor[i].measured_distance_cm, buffer, 10); // convert integer to C-style string
		sensor[i].label_width = u8g.getStrWidth(buffer);

    // calculate label positions
		sensor[i].label_xpos = map(constrain(sensor[i].measured_distance_cm, min_dist, max_dist), min_dist, max_dist, sensor[i].label_startpos_x, sensor[i].label_endpos_x);
		sensor[i].label_ypos = map(constrain(sensor[i].measured_distance_cm, min_dist, max_dist), min_dist, max_dist, sensor[i].label_startpos_y, sensor[i].label_endpos_y);
	}
  u8g.firstPage();
	 do {
		itoa(sensor[0].label_width, buffer, 10);
		u8g.drawStr(20, 58, buffer); // draw the label

    //u8g.drawBitmapP( 36, 0, 56/8, 15, bitmap_car_image); // car image on top of the display
		
		u8g.setFont(u8g_font_7x14B);
		int ownerNameWidth  = u8g.getStrWidth(owner_name);
		u8g.drawStr((128 - ownerNameWidth) / 2, 10, owner_name);

		u8g.setFont(gamer_digits);
		// u8g.drawStr(20, 0, "123"); // draw the Owner label
    u8g.drawBitmapP( 104, 0, 24/8, 10, bitmap_sound_on); // sound on indicator - not doing anything for now
    u8g.drawBitmapP( 0, 0, 24/8, 10, bitmap_unit_cm);    // cm unit indicator - not doing anything for now    

		for (int i=0; i < NUMBER_OF_SENSORS; i++) { // for all the sensors
			u8g.setColorIndex(1); // black color
			// draw box below the label
			for (int j=0; j < bars_count; j++) {
				if (sensor[i].measured_distance_cm > dist_step[j])
					u8g.drawBox(bar_width * i, 15 + ((j * (bar_height + 1))), bar_width - 1, bar_height);
			}
			//draw distance
			u8g.setColorIndex(0); // black color
			// draw box below the label
			u8g.drawBox(bar_width * i, 57, bar_width, 8);
			itoa(sensor[i].measured_distance_cm, buffer, 10); // convert integer to C-style string
			u8g.setColorIndex(1); // white color
			// draw the distance label
			u8g.drawStr(bar_width * i + ((bar_width - sensor[0].label_width) / 2), 57, buffer); // draw the label
		}


  } while ( u8g.nextPage() );
}