#include <Arduino.h>



#define PIN_SW1 D6
#define PIN_SW2 D7
volatile boolean sw1_flag = false;
volatile boolean sw2_flag = false;

unsigned long sw1_lastTime=0;
unsigned long sw2_lastTime=0;

float calib_distance=0.2; //distance of sensors in meters

#define SWDEBOUNCE 100

void interrupt_sw1();
void interrupt_sw2();
float getLastSpeed();

void setup() {
  pinMode(PIN_SW1, INPUT_PULLUP);
  pinMode(PIN_SW2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_SW1), interrupt_sw1, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_SW2), interrupt_sw2, FALLING);

  Serial.begin(115200);
}

void loop() {

  if (sw1_flag){
    sw1_flag=false;
    sw1_lastTime=millis();
    Serial.println("SW1");
  }
  if (sw2_flag){
    sw2_flag=false;
    sw2_lastTime=millis();
    Serial.println("SW2");

    float speed=getLastSpeed();
    Serial.print("Speed="); Serial.print(speed); Serial.println(" m/s");
    Serial.print("     "); Serial.print(speed*3.6); Serial.println(" km/h");
  }
  
}

void interrupt_sw1() {
  if (sw1_lastTime+SWDEBOUNCE<millis()){
    sw1_flag=true;
  }
}

void interrupt_sw2() {
  if (sw2_lastTime+SWDEBOUNCE<millis()){
    sw2_flag=true;
  }
}

float getLastSpeed() {
  return calib_distance/((sw2_lastTime-sw1_lastTime)/1000.0);
}