#include <speed.h>

#define PIN_SW1 D6
#define PIN_SW2 D5
#define PIN_TRIGGER D7

unsigned long sw1_lastTime = 0;
unsigned long sw2_lastTime = 0;
unsigned long sw1_lastTime_e = 0;
unsigned long sw2_lastTime_e = 0;

float flashspeed = 20;              // in kmh
unsigned long flashdeadtime = 1000; // in ms
float calib_distance = 0.1;  // distance of sensors in meters
float lastMeasuredSpeeds[10];
float highscore = 0;

unsigned long last_flash = 0;
bool flashNext = false;

#define SWDEBOUNCE 1000000

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

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

void handleSpeedLoop()
{
    // reset micros within the first half second to care for overflowing micros
    if (micros() < 500000)
    {
        sw1_lastTime = 0;
        sw2_lastTime = 0;
        sw1_lastTime_e = 0;
        sw2_lastTime_e = 0;
        
    }
    if (millis() < 500)
    {
        last_flash = 0;
    }
    if (sw1_lastTime > 0 && sw2_lastTime > 0 && sw2_lastTime - sw1_lastTime > 1200 && sw2_lastTime - sw1_lastTime < 10000000)
    {
        // 0,036 km/h - 300 km/h und sw2 nach sw1 ausgelöst
        doTrigger(getLastSpeed());
        sw2_lastTime = 0;
        sw1_lastTime = 0;
    }
    {
        /* code */
    }
    
    if (flashNext)
    {
        flashNext = false;
        Serial.print("Flashing");
        pinMode(PIN_TRIGGER, INPUT); // high impedance
        delay(100);
        pinMode(PIN_TRIGGER, OUTPUT);
        digitalWrite(PIN_TRIGGER, LOW);
        Serial.println("..");
    }
}

void doTrigger(float speed)
{
    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");
            flash();
        }
        else
        {
            Serial.print(">> Speed=");
            Serial.print(speed);
            Serial.println(" km/h");
        }
    }
}

void flash()
{
    flashNext = true;
}

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

ICACHE_RAM_ATTR void interrupt_sw1()
{
    if (sw1_lastTime_e + SWDEBOUNCE < micros())
    {
        sw1_lastTime_e = micros();
        sw1_lastTime = micros();
        Serial.print("SW1 - ");
        Serial.println(micros());
    }
}

ICACHE_RAM_ATTR void interrupt_sw2()
{
    if (sw2_lastTime_e + SWDEBOUNCE < micros())
    {
        sw2_lastTime_e = micros();
        Serial.print("SW2 - ");
        Serial.println(micros());
        if (sw1_lastTime > 0)
        {
            sw2_lastTime = micros();
        }
    }
}

float getLastSpeed()
{
    return calib_distance / ((sw2_lastTime - sw1_lastTime + 11404) / 1000000.0) * 3.6; // lichtschranke 1 kaputt, hat delay, brauchen 11404 microsekunden mehr, trust me.
}