#include <Arduino.h>

float flashspeed=20; //in kmh
unsigned long flashdeadtime=1000; //in ms

#define PIN_SW1 D6
#define PIN_SW2 D5
#define PIN_TRIGGER 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.062; //distance of sensors in meters

#define SWDEBOUNCE 100000

ICACHE_RAM_ATTR void interrupt_sw1();
ICACHE_RAM_ATTR void interrupt_sw2();
float getLastSpeed1();
float getLastSpeed2();
void doTrigger1();
void doTrigger2();

void setup() {
  pinMode(PIN_SW1, INPUT_PULLUP);
  pinMode(PIN_SW2, INPUT_PULLUP);
  pinMode(PIN_TRIGGER, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(PIN_SW1), interrupt_sw1, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_SW2), interrupt_sw2, FALLING);
  digitalWrite(PIN_TRIGGER, HIGH); //active low

  Serial.begin(115200);
}

void loop() {

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


void doTrigger1() {
  static unsigned long last_flash=0;

  
  float speed=getLastSpeed1();
  
  if (speed*3.6<0.1) {
    return;
  }

  if (millis()-last_flash>flashdeadtime) { //deadtime
    last_flash=millis();

    if (speed*3.6 >= flashspeed) {
      Serial.print("> Speed="); Serial.print(speed*3.6); Serial.println(" km/h");
      Serial.println("Flash");
      pinMode(PIN_TRIGGER, INPUT); //high impedance
      delay(100);      
      pinMode(PIN_TRIGGER, OUTPUT); digitalWrite(PIN_TRIGGER, LOW);
    }
  }
}


void doTrigger2() {
  static unsigned long last_flash=0;
  
  float speed=getLastSpeed2();
  

  if (speed*3.6<0.1) {
    return;
  }

  if (millis()-last_flash>flashdeadtime) { //deadtime
    last_flash=millis();

    if (speed*3.6 >= flashspeed) {
      Serial.print("> Speed="); Serial.print(speed*3.6); Serial.println(" km/h");
      Serial.println("Flash");
      pinMode(PIN_TRIGGER, INPUT); //high impedance
      delay(100);      
      pinMode(PIN_TRIGGER, OUTPUT); digitalWrite(PIN_TRIGGER, LOW);
    }
  }
}



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

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

float getLastSpeed1() {
  return calib_distance/((sw1_lastTime-sw2_lastTime)/1000000.0);
}
float getLastSpeed2() {
  return calib_distance/((sw2_lastTime-sw1_lastTime)/1000000.0);
}