#define MEDIAN_N  5

volatile uint32_t sig_t { 0 };
volatile uint32_t clock { 0 };

ISR(INT0_vect) {
	clock++;
}

ISR(ANALOG_COMP_vect) {
	sig_t = clock;
}

void setup() {
	Serial.begin(230400);

	/// clock

	pinMode(2, INPUT);

	bitSet(EICRA, ISC00);	//set for rising edge
	bitSet(EICRA, ISC01);

	EIMSK |= (1 << INT0);   // Enable external interrupt INT0

	/// comparator

	DIDR1 |= (1<<AIN0D) | (1<<AIN1D); // Disable Digital Inputs at AIN0 and AIN1

	ADCSRB &= ~(1<<ACME);	//Clear ACME bits in ADCSRB to use external input at AIN1 -ve input

	ACSR = 
		(0 << ACD) |    // Analog Comparator: Enabled
		(0 << ACBG) |   // Clear ACBG to use external input to AIN0 +ve input
		(0 << ACO) |    // Analog Comparator Output: OFF
		(1 << ACI) |    // Analog Comparator Interrupt Flag: Clear Pending Interrupt by setting the bit
		(1 << ACIE) |   // Analog Comparator Interrupt: enabled 
		(1 << ACIC) |   // Analog Comparator Input Capture: enabled
		(0 << ACIS1) | (0 << ACIS0);   // Analog Comparator Interrupt Mode: Comparator Interrupt on output toggle

	/// stop timer interrupts

	TIMSK0 &= ~(1 << OCIE0A);
	TIMSK1 &= ~(1 << OCIE1A);
	TIMSK2 &= ~(1 << OCIE2A);

	Serial.println(F("Go!"));
}

int compare_func(const void *p1, const void *p2) {
	const double vp1 = *reinterpret_cast<const double *>(p1);
	const double vp2 = *reinterpret_cast<const double *>(p2);

	if (vp1 > vp2)
		return 1;
	else if (vp1 < vp2)
		return -1;

	return 0;
}

void loop() {
	static uint32_t prev_sig_t = 0;
	static double   hzs[MEDIAN_N];
	static byte     n_hz = 0;
	static bool     hz_first = true;

	uint32_t copy_t = sig_t;

	if (prev_sig_t == copy_t)
		return;

	if (prev_sig_t != 0) {  // need at least one previous timestamp
		uint32_t took = copy_t - prev_sig_t;

		// 10000: (ext-)clock source is 10kHz
		double hz = 10000. / took / 2;

		bool restart = false;

		if (hz > 40 && hz < 60) {  // sanity check
			if (hz_first) {
				hz_first  = false;

				hzs[n_hz] = hz;
			}
			else {
				hz_first     = true;

				hzs[n_hz]   += hz;
				hzs[n_hz++] /= 2;

				if (n_hz == MEDIAN_N) {  // get MEDIAN_N of those averages and pick median
					qsort(hzs, MEDIAN_N, sizeof(double), compare_func);

					static char buffer[32];
					sprintf(buffer, "%.3f %.4f", millis() / 1000., hzs[2]);
					Serial.println(buffer);

					restart = true;
				}
			}
		}
		else {
			restart = true;
		}

		if (restart) {
			hz_first = true;
			n_hz     = 0;
		}
	}

	prev_sig_t = copy_t;
}