#include <speed.h>

#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 flashspeed = 20;              // in kmh
unsigned long flashdeadtime = 1000; // in ms
float calib_distance = 0.062;       // distance of sensors in meters
float lastMeasuredSpeeds[10];
float highscore = 0;

unsigned long last_flash = 0;

#define SWDEBOUNCE 100000

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

void handleSetup()
{
    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
}

void handleLoop()
{
    // reset micros within the first half second to care for overflowing micros
    if (micros() < 500000) {
        sw1_flag = false;
        sw1_lastTime = 0;
        sw2_flag = false;
        sw2_lastTime = 0;
    }
    if (millis() < 500) {
        last_flash = 0;
    }
    if (sw1_flag)
    {
        sw1_flag = false;
        sw1_lastTime = micros();
        Serial.println("SW1");
        doTrigger(getLastSpeed1());
    }
    if (sw2_flag)
    {
        sw2_flag = false;
        sw2_lastTime = micros();
        Serial.println("SW2");
        doTrigger(getLastSpeed2());
    }
}

void doTrigger(float speed)
{

    if (speed < 0.1)
    {
        return;
    }

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

        if (speed >= flashspeed)
        {
            addLastSpeed(speed);
            Serial.print("> Speed=");
            Serial.print(speed);
            Serial.println(" km/h");
            flash();
        }
    }
}

void flash() {
    Serial.println("Flash");
    pinMode(PIN_TRIGGER, INPUT); // high impedance
    delay(100);
    pinMode(PIN_TRIGGER, OUTPUT);
    digitalWrite(PIN_TRIGGER, LOW);
}

void addLastSpeed(float speed)
{
    for (int i = 0; i < 9; i++)
    {
        lastMeasuredSpeeds[i] = lastMeasuredSpeeds[i + 1];
    }
    lastMeasuredSpeeds[9] = speed;
    if (highscore < speed) {
        highscore = speed;
    }
}

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) * 3.6;
}
float getLastSpeed2()
{
    return calib_distance / ((sw2_lastTime - sw1_lastTime) / 1000000.0) * 3.6;
}