#include "U8glib.h"

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

// 'Lewy1', 40x14px
// 'lewyon', 30x10px
// 'off', 32x10px
const unsigned char epd_bitmap_lewyoff [] PROGMEM = {
	0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 
	0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 
	0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa
};
// 'on', 32x10px
const unsigned char epd_bitmap_lewyon [] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

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};




// set the correct pins for the distance sensors, we can (probably) use any digital pins
#define sensor_01_ECHO_PIN 6
#define sensor_01_TRIG_PIN 7
#define sensor_02_ECHO_PIN 4
#define sensor_02_TRIG_PIN 5
#define sensor_03_ECHO_PIN 2
#define sensor_03_TRIG_PIN 3

// number of sensors - note that adding a new sensor is not as simple as changing the number, as the image drawing is hardcoded
#define NUMBER_OF_SENSORS 3

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 = 2;  // minimum distance to display in cm, this is also the minimum measurable distance for the used sensor
int max_dist = 100; // maximum distance to display in cm with the tile images, maximum measurable distance is 400cm = 4m

int dist_step_01; // distance for the first tile (there are 4 tiles for each sensor)
int dist_step_02; // distance for the 2nd tile
int dist_step_03; // distance for the 3rd tile
int dist_step_04; // distance for the 4th tile



void setup() {
  // calculate individual steps for the 4 tiles
	dist_step_01 = min_dist + round((max_dist - min_dist) / 4.0 * 1.0);
	dist_step_02 = min_dist + round((max_dist - min_dist) / 4.0 * 2.0);
	dist_step_03 = min_dist + round((max_dist - min_dist) / 4.0 * 3.0);
	dist_step_04 = min_dist + round((max_dist - min_dist) / 4.0 * 4.0);			

	// initialize pins for sensors
  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;	

  // set pins to outputs and inputs  - trigger is output, echo is input
  pinMode(sensor[0].trig_pin, OUTPUT);
  pinMode(sensor[0].echo_pin, INPUT);		
  pinMode(sensor[1].trig_pin, OUTPUT);
  pinMode(sensor[1].echo_pin, INPUT);		
  pinMode(sensor[2].trig_pin, OUTPUT);
  pinMode(sensor[2].echo_pin, INPUT);	

  // initialize label position data
  // those are manualy defined in the Photoshop/Photopea file, since it´s just easier to do it that way instead of guessing the numbers
	sensor[0].label_startpos_x = 41;		
	sensor[0].label_startpos_y = 20;	
	sensor[0].label_endpos_x = 30;		
	sensor[0].label_endpos_y = 58;		
	sensor[1].label_startpos_x = 63;		
	sensor[1].label_startpos_y = 23;	
	sensor[1].label_endpos_x = 63;		
	sensor[1].label_endpos_y = 60;			
	sensor[2].label_startpos_x = 85;		
	sensor[2].label_startpos_y = 20;	
	sensor[2].label_endpos_x = 97;		
	sensor[2].label_endpos_y = 57;		

  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);		
	}

	//delay(500);

  u8g.firstPage();
  do {
    //u8g.drawBitmapP( 36, 0, 56/8, 15, bitmap_car_image); // car image on top of the display
    //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    

    // individual pieces of the distance measurement in the 3*4 grid
    u8g.drawBitmapP( 17, 17, 32/8, 10, sensor[0].measured_distance_cm > dist_step_01 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
		u8g.drawBitmapP( 17, 28, 32/8, 10, sensor[0].measured_distance_cm > dist_step_02 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);     
		u8g.drawBitmapP( 17, 39, 32/8, 10, sensor[0].measured_distance_cm > dist_step_03 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 17, 50, 32/8, 10, sensor[0].measured_distance_cm > dist_step_04 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);                    

    u8g.drawBitmapP( 50, 17, 32/8, 10, sensor[1].measured_distance_cm > dist_step_01 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 50, 28, 32/8, 10, sensor[1].measured_distance_cm > dist_step_02 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 50, 39, 32/8, 10, sensor[1].measured_distance_cm > dist_step_03 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 50, 50, 32/8, 10, sensor[1].measured_distance_cm > dist_step_04 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);                        

    u8g.drawBitmapP( 83, 17, 32/8, 10, sensor[2].measured_distance_cm > dist_step_01 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 83, 28, 32/8, 10, sensor[2].measured_distance_cm > dist_step_02 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 83, 39, 32/8, 10, sensor[2].measured_distance_cm > dist_step_03 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);        
    u8g.drawBitmapP( 83, 50, 32/8, 10, sensor[2].measured_distance_cm > dist_step_04 ? epd_bitmap_lewyon : epd_bitmap_lewyoff);                        

		for (int i=0; i<NUMBER_OF_SENSORS; i++) { // for all the sensors
		  	u8g.setColorIndex(0); // black color

				// draw box below the label
				u8g.drawBox((sensor[i].label_xpos - sensor[i].label_width / 2) - 1 ,sensor[i].label_ypos - 4,sensor[i].label_width + 2, 8);

				itoa(sensor[i].measured_distance_cm, buffer, 10); // convert integer to C-style string
				u8g.setColorIndex(1); // color

				// draw the distance label/ white
				u8g.drawStr(sensor[i].label_xpos - sensor[i].label_width / 2, sensor[i].label_ypos - 3, buffer); // draw the label
		}


  } while ( u8g.nextPage() );

}