/* 
 * test scetch measure speed pulses via period duration with interupt
 * Arduino Mega interupt pins: 2, 3, 18, 19, 20, 21
 */



#include <Wire.h>
#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 
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); 
//Adafruit_SSD1306 display(OLED_RESET);  // original



//pins
const int RESET_BTN = 0;  // speed reset button
const int SPEED_PIN = 2;  // speed sensor input pin

// OUD //volatile unsigned int counter_speedpulses = 0;  // Counter variable for revolutions

//general timing vars
unsigned long previousMillis = 0;  // Variable to store previous time in milli seconds
unsigned long currentMillis = 0; // Variable to store current time

//Speed Vars:
 //Variables shared between ISR functions and normal functions should be declared "volatile". 
volatile unsigned long previousMicros_spd = 0; //to measure time between pulses.
volatile unsigned long currentMicros_spd = 0; //to measure time between pulses.
volatile unsigned long speed_time_between_pulses_micros= 0;
unsigned int speed_pulses_hr =0;
const float pi = 3.14159;
   // nu constant, later instelbare en in EEPROM:
volatile const float speed_dist_p_puls_mm = 450.0; // Wheel circumference in mm / pulses per revolution
float speed_kmh = 0;
float maxspeed_kmh = 0;

//ODO and TRIP distance vars:
unsigned long ODO_km = 999999; //unsigned long max 4,294,967,295, this is enough for the odo km
float TRIP_km = 1999.99;

unsigned long TRIP_mm = 1999.99; //unsigned long max 4,294,967,295, this is enough for the odo km
unsigned long ODO_mm = 999999; //counts the km's behind the komma in mm's //add to ODO_km each km.

//RPM Vars:
unsigned int RPM = 0;  // Engine RPM value
 
// ====================================================================================
// ||                            INTERUPT SERVICE ROUTINES                            ||
// ====================================================================================
// Interupt service routine (ISR) - speedsensor:
void ISR_speedpinchange() {  
  previousMicros_spd = currentMicros_spd; //currentMicros is from last isr call, put in previousMicros
  currentMicros_spd = micros();           //realtime put in currentMicros
  speed_time_between_pulses_micros = currentMicros_spd - previousMicros_spd;

}    // end of ISR_speedpinchange

 

// ====================================================================================
// ||                            SETUP                                               ||
// ====================================================================================
void setup() {
  pinMode(SPEED_PIN, INPUT_PULLUP);
  pinMode(RESET_BTN, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(SPEED_PIN), ISR_speedpinchange, FALLING);
  Serial.begin(9600); // open the serial port at 9600 bps:
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  //display.setCursor(0, 0);
  //display.println("TEG");
  //display.display();
  //delay(2000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(30, 0); 
  display.println("Speed");  
  display.setCursor(20, 20);  
  display.println("Checker"); 
  display.display();
  delay(1000);
}

// ====================================================================================
// ||                            MAIN LOOP                                           ||
// ====================================================================================
void loop() {
  currentMillis = millis();
  
  //execute this if-loop only once per 500ms
  if (currentMillis - previousMillis >= 500UL) {      
    //detachInterrupt(digitalPinToInterrupt(SPEED_PIN));  //interupts ignored untill attach
    // Calculate speed:
      //speed_time_between_pulses_micros = currentMicros_spd - previousMicros_spd;
      //previousMicros_spd =0;
    speed_pulses_hr = 3600000000UL / speed_time_between_pulses_micros;//   3600/T(s)
    speed_kmh = (speed_pulses_hr*speed_dist_p_puls_mm) /1000000.0; //div 1000000 for mm to km

//print to monitor for diagnose

    Serial.print("Spd pulse time: ");  // prints a label
    Serial.print("\t");         // prints a tab
    Serial.print("puls/hr: ");  // prints a label
    Serial.print("\t");         // prints a tab
    Serial.print("kmh: ");  // prints a label
    Serial.println();         // new line

    Serial.print(speed_time_between_pulses_micros);
    Serial.print("\t\t");  
    Serial.print("\t");        
    Serial.print(speed_pulses_hr);
    Serial.print("\t\t");  
    Serial.print(speed_kmh);
    Serial.print("\t");  
    Serial.println();         // new line


    if(speed_kmh >=maxspeed_kmh){
      maxspeed_kmh=speed_kmh;
    }
    if((digitalRead(RESET_BTN)==LOW)){
      maxspeed_kmh=0;
    }
  
  
    //attachInterrupt(digitalPinToInterrupt(SPEED_PIN), ISR_speedpinchange, FALLING); //attach again
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1,2);
    display.setCursor(0, 5);
    display.print("SPEED: ");
    display.println(speed_kmh);
    display.setCursor(100, 5);
    display.print("Km/h ");
    display.setCursor(0, 25);
    display.print("Max Speed: ");
    display.println(maxspeed_kmh);
    display.setCursor(0, 45);
    display.print("PPH  : ");
    display.println(speed_pulses_hr);

    }
    display.display();
  
  
}