#include <Arduino.h>

#define RelayCoil1 2
#define RelayCoil2 3
#define RelaySense1 A0
#define RelaySense2 A1

// ---------- Test parameters ----------
const unsigned long TEST_DELAY_US = 355;   // intended pulse width

// ---------- Calibration ----------
unsigned long offsetUs = 0;                // measured overhead
const int     CAL_SAMPLES = 8;             // average N samples

// ---------- Forward decls ----------
unsigned long measureRawUs(unsigned long delayUs);
float         MeasureSpeed();
void          calibrateOffset();

void setup() {
    Serial.begin(115200);
    Serial.println(F("Relay Tester V0.0.0.1\nDevelop: JNOL and OFER"));

    calibrateOffset();                     // ← measure overhead once
}

void loop() {
    Serial.println(F("Testing Will Start"));
    MeasureSpeed();
    delay(1000);
}

// =====================================================
//  Raw measurement — no offset correction
// =====================================================
unsigned long measureRawUs(unsigned long delayUs) {
    unsigned long start = micros();
    delayMicroseconds(delayUs);
    return micros() - start;
}

// =====================================================
//  Calibrate: average overhead of the call chain
// =====================================================
void calibrateOffset() {
    unsigned long sum = 0;
    for (int i = 0; i < CAL_SAMPLES; i++) {
        unsigned long raw = measureRawUs(TEST_DELAY_US);
        sum += raw;
    }
    unsigned long avgRaw = sum / CAL_SAMPLES;

    // Guard against negative (clock drift / very short delays)
    offsetUs = (avgRaw > TEST_DELAY_US) ? (avgRaw - TEST_DELAY_US) : 0;

    Serial.print(F("Calibration: avg raw = "));
    Serial.print(avgRaw);
    Serial.print(F(" us, offset = "));
    Serial.print(offsetUs);
    Serial.println(F(" us"));
}

// =====================================================
//  Corrected measurement
// =====================================================
float MeasureSpeed() {
    unsigned long rawUs = measureRawUs(TEST_DELAY_US);

    // Apply offset, clamp to zero
    unsigned long correctedUs = (rawUs > offsetUs) ? (rawUs - offsetUs) : 0;

    float rawMs       = rawUs       / 1000.0f;
    float correctedMs = correctedUs / 1000.0f;
    float targetMs    = TEST_DELAY_US / 1000.0f;
    float errorMs     = correctedMs - targetMs;

    Serial.print(F("  raw="));
    Serial.print(rawMs, 3);
    Serial.print(F(" ms  corrected="));
    Serial.print(correctedMs, 3);
    Serial.print(F(" ms  target="));
    Serial.print(targetMs, 3);
    Serial.print(F(" ms  err="));
    Serial.print(errorMs * 1000.0f, 1);   // show error in µs
    Serial.println(F(" us"));

    return correctedMs;
}