// Ruud Boer
// Original version August 2019
// Revised version September 2021
// Two optical sensors are used, spaced SENSOR_DISTANCE [um] apart
// [us] timer starts when one of the sensor inputs goes LOW
// [us] timer stops when the other sensor goes LOW
// m_per_s = distance / (stop_time - start_time)
// km_per_hr = 3.6 * m_per_s, mph = 2.23694 * m_per_s;
// REMARK: connect OLED display VDD to Arduino 3.3V (may depend on OLED type)
//
// SCALE: HO 1/87. To change scale connect pin to GND:
//        A0=O 1/45, A1=OO 1/76, A2=TT 1/120, A3=N 1/160
//
// UNITS: km/hr. To change units to MPH connect pin 8 to GND

#define SENSOR_DISTANCE  2000000 // [um] measured distance between the two IR beams
#define INIT_TRAIN_LENGTH    20 // [cm] New measurement only starts when train fully passed
                                // Loco Length can be changed via keyboard input
#define SENSOR_L_PIN 6
#define SENSOR_R_PIN 7

#include <Wire.h> //I2C library
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>


#define SCREEN_WIDTH        128 // OLED display width, in pixels
#define SCREEN_HEIGHT        64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET            4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


byte l_r, state;
//byte scale, scale_old;
byte units=1;

int train_length, serialread;
unsigned long start_us, stop_us, measured_us, waittime;
float m_per_s;
float km_per_hr;
float mi_per_hr;

//	Define	the	*minimum*	length	of	time,	in	milli-seconds,	that	the	button	must	be	pressed	for a	particular	option	to	occur
int	optionOne_milliSeconds	=	100;
int	optionTwo_milliSeconds	=	1000;	
int	optionThree_milliSeconds	=	2000;	
int pressLength_milliSeconds;		
//int buttonClick = 0;
int previous_button_state = 0;
int button_state = 0;
int button_pressed_count = 0;
int scale[] = {76,45,87,120,160};  //OO, O, HO, TT, N
String gauge = "OO";
int scaleGauge = 76;
int scaleCalc = 76; //default

bool programmeMode;

//The	Pin	your	button	is	attached	to
int	buttonPin	=	2;

void write_to_display() {
  display.clearDisplay();
	display.setTextColor(WHITE,BLACK);
	if (units) { // units = 0: MPH
	  if ((mi_per_hr)<10) display.setCursor(35,0); else if ((mi_per_hr)<100) display.setCursor(8,0); else  display.setCursor(0,0);//change the start point of the text, depending on the number of digits
		display.setTextSize(3);
		display.print(mi_per_hr);
		display.setCursor(107,14);
		display.setTextSize(1);
		display.print("MPH");
		if ((km_per_hr)<10) display.setCursor(6,48); else display.setCursor(0,48);
		display.setTextSize(1);
		display.print(km_per_hr);
		display.setCursor(42,48);
		display.print("KPH");
	}
	else { // units = 1: km/hr
		if ((km_per_hr)<10) display.setCursor(35,0); else if ((km_per_hr)<100) display.setCursor(8,0); else  display.setCursor(0,0);
		display.setTextSize(3);
		display.print(km_per_hr);
		display.setCursor(107,14);
		display.setTextSize(1);
		display.print("KPH");
		if ((mi_per_hr)<10) display.setCursor(6,48); else display.setCursor(0,48);
		display.setTextSize(1);
		display.print(mi_per_hr);
		display.setCursor(36,48);
		display.print(" MPH");
	}
	if ((m_per_s)<10) display.setCursor(6,57); else display.setCursor(0,57);
  display.print(m_per_s);
	display.setCursor(36,57);
  display.print(" m/s");
	display.setCursor(79,48);
  display.print(int((measured_us+500)/1000));
	display.setCursor(115,48);
  display.print("ms");
	display.setCursor(79,57);
  display.print("1/");

	switch (scaleGauge) {
		case 45: 
			scaleCalc = 45;
			gauge = " O";
			
		break;
		case 76: 
			scaleCalc = 76;
			gauge = "OO";
			// display.print(gauge);
		break;
		case 87: 
			scaleCalc = 87;
			gauge = "HO";
			// display.print(gauge);
		break;
		case 120: 
			scaleCalc = 120;
			gauge = "TT";
			// display.print(gauge);
		break;
		case 160: 
			scaleCalc = 160;
			gauge = " N";
			// display.print(gauge);
		break;
	}
	display.print(scaleCalc);
  display.setCursor(115,57);
	display.print(gauge);

	//display.drawRect(13,36,102,5,WHITE);
	//display.fillRect(14,37,100,3,BLACK);
  //if(units) display.fillRect(14,37,int(km_per_hr+0.5),3,WHITE);  
	//else      display.fillRect(14,37,int(mi_per_hr+0.5),3,WHITE);  
	//if (programmeMode) display.fillRect(0,0,5,5,WHITE);
	//else  display.fillRect(0,0,5,5,BLACK);
	display.display();
}

void read_write_scale() {
  if (Serial.available()) {
    serialread = Serial.parseInt();
    if(serialread) train_length = serialread;
    Serial.print(F("Train length set to: "));
    Serial.print(train_length);
    Serial.println(F(" cm"));
    Serial.println(F("Waiting for train ..."));
    Serial.println();
  }
    write_to_display();
  // }

}

void setup() {
  pinMode(13, OUTPUT); // on board LED
	pinMode(SENSOR_L_PIN,INPUT_PULLUP);
	pinMode(SENSOR_R_PIN,INPUT_PULLUP);
  pinMode(buttonPin,	INPUT_PULLUP);		
	// pinMode( 8,INPUT_PULLUP); 
	// pinMode(14,INPUT_PULLUP);
	// pinMode(15,INPUT_PULLUP);
	// pinMode(16,INPUT_PULLUP);
	// pinMode(17,INPUT_PULLUP);
	// pinMode(18,INPUT_PULLUP);
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize OLED display
  train_length = INIT_TRAIN_LENGTH; // [cm]
  Serial.println();
  Serial.print(F("Train length is set to "));
  Serial.print(train_length);
  Serial.println(F(" cm "));
  Serial.println(F("For longer trains change this by typing"));
  Serial.println(F("in the input field above and hit ENTER"));
  Serial.println();
  read_write_scale();
  write_to_display();
  //display.fillRect(0,36,7,5,WHITE);
  //display.fillRect(120,36,7,5,WHITE);
  display.display();
	digitalWrite(13, HIGH); // LED on means ready for measurement
}

void loop() {
  read_write_scale();

		//Record	*roughly*	the	of	tenths	of	seconds	the	button	in	being	held	down
		while	(digitalRead(buttonPin)	==	LOW	){	
				delay(100);		//if	you	want	more	resolution,	lower	this	number	
        pressLength_milliSeconds	=	pressLength_milliSeconds	+	100;			
				//display	how	long	button	is	has	been	held
				Serial.print("ms	=	");
				Serial.println(pressLength_milliSeconds);
				if	(pressLength_milliSeconds	>=	optionTwo_milliSeconds) {
					break;
					}
		}//close	while

	//Different	if-else	conditions	are	triggered	based	on	the	length	of	the	button	press
	
		//Option	2	- Execute	the	second	option	if	the	button	is	held	for	the	correct	amount	of	time
		if	(pressLength_milliSeconds	>=	optionTwo_milliSeconds) {
				//Serial.print("Long Press");
				if(units == 1 ){
    			units = 0;
				}
				else{units=1;}
    		Serial.print(F("Units set to: "));
				if(!units) Serial.println(F("KPH"));
    		else      Serial.println(F("MPH"));
				programmeMode = 0;
    		write_to_display();
    	}					
    //option	1	- Execute	the	first	option	if	the	button	is	held	for	the	correct	amount	of	time
		else	if((pressLength_milliSeconds	>=	optionOne_milliSeconds) && (pressLength_milliSeconds	<	150)) {
				button_state=digitalRead(buttonPin);
				if (button_state == HIGH){
						button_pressed_count++;
					if (button_pressed_count>4){
						button_pressed_count=0;
						}
					scaleGauge = scale[button_pressed_count];
					programmeMode = 0;
				}


		}//close	if	options
		//every	time	through	the	loop,	we	need	to	reset	the	pressLength_Seconds	counter
		pressLength_milliSeconds	=	0;


  switch (state) {
		case 0: // initial state, ready to start measuring
			if (!digitalRead(SENSOR_L_PIN)) {start_us = micros(); l_r = 0; state = 1;}		//For some reason, the hardware requires these lines to use "!"
			if (!digitalRead(SENSOR_R_PIN)) {start_us = micros(); l_r = 1; state = 1;}
		break;

		case 1: // wait for the other sensor to be triggered
			digitalWrite(13, LOW);
			display.fillRect(0,48,127,15,BLACK);
			display.setCursor(3,48);
			display.setTextSize(2);
			if (!l_r) {
        Serial.println(F("L >>> R"));
				display.print("L >>>>>> R");
        display.fillRect(0,36,7,5,BLACK);
				display.display();
				while (digitalRead(SENSOR_R_PIN)) {} // loop here until sensor R is triggered			//and these lines do not use "!"
			}
			else {
        Serial.println(F("L <<< R"));
				display.print("L <<<<<< R");
        display.fillRect(120,36,7,5,BLACK);
				display.display();
				while (digitalRead(SENSOR_L_PIN)) {} // loop here until sensor L is triggered
			}
			stop_us = micros();
			state = 2;
		break;

		case 2: // calculate and show speed values
			measured_us = stop_us - start_us - 10UL; // -10 to compensate for code delay
			m_per_s = float(scaleCalc) * float(SENSOR_DISTANCE) / float(measured_us);
			km_per_hr = 3.6 * m_per_s;
			mi_per_hr = 2.23694 * m_per_s;
			write_to_display();
      if (units) {Serial.print(mi_per_hr); Serial.println(F(" MPH"));}
      else        {Serial.print(km_per_hr); Serial.println(F(" KPH"));}
      Serial.println();
      waittime = 2000;
			// measured_us is needed for a move of SENSOR_DISTANCE um
			// measured_us * train_length / SENSOR_DISTANCE) time is needed for train_length to pass
      Serial.print(F("Wait "));
      Serial.print(waittime);
      Serial.println(F(" ms"));
      delay(waittime);
      digitalWrite(13, HIGH); //Next measurement can start when LED is on
      //display.fillRect(0,36,7,5,WHITE);
      //display.fillRect(120,36,7,5,WHITE);
      display.display();
      Serial.println(F("Waiting for train..."));
      state = 0;
		break;
	}
}