#define PIN_LED 13
#define PIN_SW1 2
#define PIN_SW2 3
#define PIN_FLASH 5
volatile boolean sw1_flag = false;
volatile boolean sw2_flag = false;

volatile boolean sw1_released = true;
volatile boolean sw2_released = true;

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

unsigned long sw1_lastTimeMicros=0;
unsigned long sw2_lastTimeMicros=0;

#define SWDEBOUNCE 500

float sensordistance = 0.37; //in meters

#define TIMESCOUNT 20
uint8_t times_index=0;
float times[TIMESCOUNT];




void setup() {
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_SW1, INPUT_PULLUP);
  pinMode(PIN_SW2, INPUT_PULLUP);

  pinMode(PIN_FLASH, OUTPUT);

  digitalWrite(PIN_FLASH, HIGH);

  for (uint8_t i=0;i<TIMESCOUNT;i++){
    times[i]=15;
  }
  

  Serial.begin(115200);
}

void loop() {
  //digitalWrite(PIN_LED, sw1_flag);

  if (sw1_released && !digitalRead(PIN_SW1)){ //pressed
    sw1_released=false;
    if (sw1_lastTime+SWDEBOUNCE<millis()){
      sw1_flag=true;
      sw1_lastTime=millis();
      sw1_lastTimeMicros=micros();
    }
  }else if (!sw1_released && digitalRead(PIN_SW1)){ //released
    sw1_released=true;
  }

  
  if (sw2_released && !digitalRead(PIN_SW2)){ //pressed
    sw2_released=false;
    if (sw2_lastTime+SWDEBOUNCE<millis()){
      sw2_flag=true;
      sw2_lastTime=millis();
      sw2_lastTimeMicros=micros();
    }
  }else if (!sw2_released && digitalRead(PIN_SW2)){ //released
    sw2_released=true;
  }



  if (sw1_flag){
    sw1_flag=false;
    //Serial.println("SW1");
  }
  
  if (sw2_flag){
    sw2_flag=false;

    
    
    //Serial.println("SW2");
    unsigned long timediff=sw2_lastTimeMicros-sw1_lastTimeMicros;
    //Serial.println(timediff);
    float meterpersecond=1.0/timediff*sensordistance*1000000;   // meter per s
    
    //Serial.print(meterpersecond);
    //Serial.println(" m/s");

    float kmh=meterpersecond*3.6;

    if (kmh>=1){
  
      if (kmh>getTimesMax()){
        digitalWrite(PIN_FLASH, LOW); //Trigger flash
        delay(100);
        digitalWrite(PIN_FLASH, HIGH);
      }
  
      Serial.println(kmh);
      //Serial.println(" kmh");
  
  
      //add time to last times queue
      times_index++;
      times_index%=TIMESCOUNT-1;
      times[times_index]=kmh;
  
      
    }

  }
  
}

float getTimesMax(){
  float timesmax=0;
  for (uint8_t i=0;i<TIMESCOUNT;i++){
    if (timesmax<times[i]){
      timesmax=times[i];
    }
  }
  return timesmax;
}