// Frequency measurement

uint8_t errorDelta = 25; // readings where F change over ~1 sec are greater than this millihz are discarded
volatile uint8_t skip = 5; // only skip this many consecutive assumed erroneous readings
const boolean debug = 1; // set this to non-zero to enable debug output to serial port
volatile uint8_t count = 0;
long skipped = 0;
long lastTime;
long lastMillihz;

ISR(ANALOG_COMP_vect) {
  ++count; // counts up and down transitions
}

// Moving coil meter control

int meterPin1 =  5;    // meter wire connected to digital pin 5
int meterPin2 =  3;    // meter wire connected to digital pin 3

void meterWrite(int scale) {
  if (scale >= 0) {
    if (scale > 250) 
      scale = 250;
    digitalWrite(meterPin2, LOW);
    analogWrite(meterPin1, scale);
  }
  else {
    if (scale < -250)
      scale = -250;
    digitalWrite(meterPin1, LOW);
    analogWrite(meterPin2, -scale);
  }
}

// Print to serial port 

void printLog(long now, long millihz, long skipped) {  
  Serial.print(now);
  Serial.print(" useconds ");
  Serial.print(millihz/1000);
  Serial.print('.');
  Serial.print((millihz/100) % 10, DEC);
  Serial.print((millihz/10) % 10, DEC);
  Serial.print((millihz) % 10, DEC);
  Serial.print(" Hz ");
  if (millihz >= 50000)
    Serial.print('+');
  Serial.print((millihz - 50000) * 0.002);
  Serial.println(" % ");
  if (skipped !=0) {
    Serial.print(" ****** SKIPPED: ");
    Serial.print(skipped);
    Serial.println(" ****** ");
  }
}

// Setup

void setup() {
  Serial.begin(57600);
  Serial.println("\n[hertz]");

  // compare against 1.1v reference
  ACSR = _BV(ACBG) | _BV(ACI) | _BV(ACIE);

  // use MUX to select AIO port 1
  ADCSRA &= ~ bit(ADEN);
  ADCSRB |= bit(ACME);
  ADMUX = 0;

  // initialize the digital pins as an output:
  pinMode(meterPin1, OUTPUT);
  pinMode(meterPin2, OUTPUT);

}

// Main loop

void loop() {
  cli();
  if (count >= 100) {  
    count -= 100;
    sei();
    long now = micros();

    if (lastTime != 0) {
      long millihz = long(50e9 / (now - lastTime));

      // Skip erroneous readings and print debug if enabled
      if ((lastMillihz != 0) && (skipped < skip) && (lastMillihz - millihz > errorDelta || millihz - lastMillihz > errorDelta)) {
        skipped++;
        if (debug !=0)
          printLog(now, millihz, skipped);
        millihz = lastMillihz;
      }
      else {
        skipped = 0;
        if (debug !=0)
          printLog(now, millihz, skipped);
        lastMillihz = millihz;
      }
      
      // Update the meter
      
      meterWrite(((millihz - 50000) * 2));
    }

    lastTime = now;
  } 
  else
    sei();
}