// Board: Adafruit Feather M0

#include <Arduino.h>

#include <Adafruit_GFX.h>
#include "Adafruit_LEDBackpack.h"  // "Adafruit LED Backpack Library" v1.5.0

#define MEASUREMENT_PERIOD_MS 100
#define PULSES_PER_REVOLUTION 8

Adafruit_7segment matrix = Adafruit_7segment();

// --- Tachometer variables ---
const int TACH_INTERRUPT_PIN = 10; // Or any other digital pin capable of interrupts
volatile unsigned long g_isr_pulse_count_total = 0; // ISR increments this continuously
unsigned long g_main_last_pulse_count_snapshot = 0; // Main loop stores previous snapshot here
unsigned long last_measurement_time = 0;

// --- RPM Display Filtering Variables ---
int g_rpm_value_on_display = 0; // Stores the last RPM value shown on the 7-segment display by the loop
bool g_is_first_rpm_calc_in_loop = true; // Flag to handle the first RPM calculation

// --- Interrupt Service Routine (ISR) ---
void count_pulse() {
  g_isr_pulse_count_total++;
}

void show(int num) {
	matrix.print(num, DEC);
	matrix.writeDisplay();
}

void setup() {
	matrix.begin(0x70);

	Serial.begin(115200);
	// Serial.setDebugOutput(true);

  // --- Setup Tachometer Interrupt ---
  pinMode(TACH_INTERRUPT_PIN, INPUT_PULLUP); // Set pin as input with internal pull-up resistor
  attachInterrupt(digitalPinToInterrupt(TACH_INTERRUPT_PIN), count_pulse, RISING);

	delay(1000);

	Serial.println();
	Serial.println();
	Serial.println("================");
	Serial.println("BOOT UP");

	show(99999);

}

void loop() {
  unsigned long current_time = millis();

  if (current_time - last_measurement_time >= MEASUREMENT_PERIOD_MS) {
    unsigned long current_total_snapshot;

    // Atomically read the total pulse count
    noInterrupts();
    current_total_snapshot = g_isr_pulse_count_total;
    interrupts();

    // Calculate pulses collected during this interval
    unsigned long collected_pulses = current_total_snapshot - g_main_last_pulse_count_snapshot;
    
    // Store the current total snapshot for the next interval's calculation
    g_main_last_pulse_count_snapshot = current_total_snapshot;

    unsigned long actual_elapsed_time = current_time - last_measurement_time;

    // Calculate RPM
    // RPM = (pulses / pulses_per_revolution) / (actual_elapsed_time_ms / 1000 / 60)
    // RPM = (pulses / pulses_per_revolution) * (60000 / actual_elapsed_time_ms)
    // Ensure floating point division for accuracy before converting to int for display
    float rpm_float = 0.0;
    if (actual_elapsed_time > 0) { // Avoid division by zero
        rpm_float = ( (float)collected_pulses / PULSES_PER_REVOLUTION ) * (60000.0 / actual_elapsed_time);
    }
    int rpm = (int)rpm_float;

    // RPM Filtering Logic
    if (g_is_first_rpm_calc_in_loop) {
        show(rpm);
        g_rpm_value_on_display = rpm;
        g_is_first_rpm_calc_in_loop = false;
    } else {
        if (rpm == 0 && g_rpm_value_on_display != 0) {
            // If RPM is actually zero, update the display to show zero
            show(rpm);
            g_rpm_value_on_display = rpm;
        } else if (rpm != 0) { // Apply jitter filter only if current rpm is not zero
            float rpm_float_if_one_more_pulse = 0.0;
            if (actual_elapsed_time > 0) { // Avoid division by zero
                rpm_float_if_one_more_pulse = ( (float)(collected_pulses + 1) / PULSES_PER_REVOLUTION ) * (60000.0 / actual_elapsed_time);
            }
            int rpm_if_one_more_pulse = (int)rpm_float_if_one_more_pulse;

            // Check for the specific 1-pulse drop jitter
            if (rpm < g_rpm_value_on_display && rpm_if_one_more_pulse == g_rpm_value_on_display) {
                // Jitter detected (e.g., displayed 6000, current calc is 5925, but 5925 with +1 pulse would be 6000)
                // Do not update the display; keep showing g_rpm_value_on_display.
                Serial.print("Filtered (jitter) RPM: "); Serial.print(rpm);
                Serial.print(" -> Kept displaying: "); Serial.println(g_rpm_value_on_display);
            } else {
                // Not a jitter, or a rise, or a larger drop. Update display.
                show(rpm);
                g_rpm_value_on_display = rpm;
            }
        }
        // If rpm is 0 and g_rpm_value_on_display is already 0, do nothing to avoid redundant show(0)
    }

    // Debugging output
    Serial.print("Pulses: "); Serial.print(collected_pulses);
    Serial.print("\tElapsed ms: "); Serial.print(actual_elapsed_time);
    Serial.print("\tRPM: "); Serial.println(rpm);

    last_measurement_time = current_time;
  }
}