#define SENSOR_DISTANCE  72000 // [um] measured distance between the two IR beams

#define PIN_CLK 2
#define PIN_DT 3
#define PIN_SW 9
#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 units=1;

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

long int modeLastChanged = 0;
int scale[] = {76,45,87,120,160,220,450};  //OO, O, HO, TT, N, Z, T
String gauge = "OO";
int scaleGauge = 76;
int scaleCalc = 76; //default
 //Rotary encoder pins

int	buttonPin	=	5;

// A turn counter for the rotary encoder (negative = anti-clockwise)
int rotationCounter = 0;

// Flag from interrupt routine (moved=true)
volatile bool rotaryEncoder = false;

 //Interrupt routine just sets a flag when rotation is detected
void rotary(){
    rotaryEncoder = true;
}



int8_t checkRotaryEncoder()
{
    // Reset the flag that brought us here (from ISR)
    rotaryEncoder = false;

    static uint8_t lrmem = 3;
    static int lrsum = 0;
    static int8_t TRANS[] = {0, -1, 1, 14, 1, 0, 14, -1, -1, 14, 0, 1, 14, 1, -1, 0};

    // Read BOTH pin states to deterimine validity of rotation (ie not just switch bounce)
    int8_t l = digitalRead(PIN_CLK);
    int8_t r = digitalRead(PIN_DT);

    // Move previous value 2 bits to the left and add in our new values
    lrmem = ((lrmem & 0x03) << 2) + 2 * l + r;

    // Convert the bit pattern to a movement indicator (14 = impossible, ie switch bounce)
    lrsum += TRANS[lrmem];

    /* encoder not in the neutral (detent) state */
    if (lrsum % 4 != 0)
    {
        return 0;
    }

    /* encoder in the neutral state - clockwise rotation*/
    if (lrsum == 4)
    {
        lrsum = 0;
        return 1;
    }

    /* encoder in the neutral state - anti-clockwise rotation*/
    if (lrsum == -4)
    {
        lrsum = 0;
        return -1;
    }

    // An impossible rotation has been detected - ignore the movement
    lrsum = 0;
    return 0;
}


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)); 		//rounding up
	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;
		case 220: 
			scaleCalc = 220;
			gauge = " Z";
			// display.print(gauge);
		break;
		case 450: 
			scaleCalc = 450;
			gauge = " T";
			// display.print(gauge);
		break;
	}
	display.print(scaleCalc);
  display.setCursor(115,57);
	display.print(gauge);
	display.display();
}


void setup()
{
  pinMode(SENSOR_L_PIN,INPUT_PULLUP);
	pinMode(SENSOR_R_PIN,INPUT_PULLUP);
  pinMode(buttonPin,	INPUT_PULLUP);		
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize OLED display
  write_to_display();
  display.display();
	

	// The module already has pullup resistors on board
	pinMode(PIN_CLK, INPUT);
	pinMode(PIN_DT, INPUT);

	// But not for the push switch
	pinMode(PIN_SW, INPUT_PULLUP);

	// We need to monitor both pins, rising and falling for all states
	attachInterrupt(digitalPinToInterrupt(PIN_CLK), rotary, CHANGE);
	attachInterrupt(digitalPinToInterrupt(PIN_DT), rotary, CHANGE);
	
}

void loop()
{
    // Has rotary encoder moved?
    if (rotaryEncoder)
    {
        // Get the movement (if valid)
        int8_t rotationValue = checkRotaryEncoder();

        // If valid movement, do something
        if (rotationValue != 0)
        {
					rotationCounter += rotationValue;

					if(rotationCounter > 6 ){
						rotationCounter = 0;
					}
					if(rotationCounter < 0 ){
						rotationCounter = 6;
					}
					
					scaleGauge = scale[rotationCounter];
					Serial.println(scaleGauge);
            write_to_display();

				}

        
    }

    if (digitalRead(PIN_SW) == LOW && millis() - modeLastChanged > 300) {
    		modeLastChanged = millis();
  			units = !units;
    		Serial.print(F("Units set to: "));
				if(!units) Serial.println(F("KPH"));
    		else      Serial.println(F("MPH"));
    		write_to_display();
 

    }
		
		


		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
			
			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 microseconds 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_microseconds is needed for a move of SENSOR_DISTANCE micrometers
			// measured_us * train_length / SENSOR_DISTANCE) time is needed for train_length to pass

      delay(waittime);
      
      display.display();
      Serial.println(F("Waiting for train..."));
      state = 0;
		break;
		}
}