#include <TimerOne.h>
// Inngang og utgangspinner
#define r_in 2
#define u_out 6
#define b_out 7

// De to frekvensene som sjekkes som
const unsigned int f_0 = 330;
const unsigned int f_1 = 750;

// Verdier for å kompensere for at signalet ikke nødvendigvis er helt perfekt
// Nå kan signalet være +- 5%
const float lowB = 0.95;
const float highB = 1.05;

// 10000 mikrosekund mellom hver gang den regner ut frekvensen -> gir Baudrate = 100Hz
const unsigned int baudrate = 100000;

// Antal målinger som skal summes og taes gjennomsnitt av
const unsigned int meassures = 10;

// Globale variabler
unsigned long lastMess;
unsigned int periods[meassures];

// Gjør at dersom det gjøres kalkulasjoner med listen så blir den ikke endret hvis det kommer en ny interupt
bool calc = false;


void setup() {
  // Setter opp en interupt på r_in pinnen, måler perioden
  attachInterrupt(digitalPinToInterrupt(r_in), FreqMess, RISING);

  // Setter utgangene riktig
  pinMode(u_out, OUTPUT);
  pinMode(b_out, OUTPUT);

  // Oppsett av timer interrupt
  Timer1.initialize(baudrate); // baudrate mikrosekund mellom hver gang den regner ut frekvensen
  // Argumentet i "attachInterrupt" bestemmer hvilken funskjon som er interrupt handler
  Timer1.attachInterrupt(calcFreq);

  //Debug
  Serial.begin(9600);
}

void loop() {
  // Wait for interupt
  delay(10);
}

// Denne regner om en liste med perioder om til en frekvens
unsigned int microPeriodToFreq() {
  unsigned int period = 0;

  for (int i = 0; i < meassures; ++i) {
    period += periods[i];
  }
  Serial.println(period);
  period = period / meassures;
  
  return (1 / period) * pow(10, 6);
}

// Flytter listen med perioder nedover og lager plass til neste.
void movePeriods() {
  for (int i = 0; i < meassures - 1; ++i) {
    periods[i + 1] = periods[i];
  }
}

// Denne måler faktisk frekvensen. 
void FreqMess() {
  // Dersom det regnes og leses fra listen, ikke endre på den. 
  if (!calc) {
    movePeriods();
    Serial.println("Meassure");
    // Setter den nye målingen ved å finne differansen siden sist den ble målt
    periods[0] = micros() - lastMess;
    lastMess = micros();
  }

}

// Denne trigges med baudrate hastighet, Endrer på utgangene i forhold til kravene som er satt i variablene f_0 og f_1
void calcFreq() {
  calc = true;
  unsigned int freq = microPeriodToFreq();
  Serial.println(freq);
  if (freq > f_0 * lowB || freq < f_0 * highB) {
    digitalWrite(b_out, LOW);
    digitalWrite(u_out, HIGH);
  }
  else if (freq > f_1 * lowB || freq < f_1 * highB) {
    digitalWrite(b_out, HIGH);
    digitalWrite(u_out, HIGH);
  }
  else {
    digitalWrite(b_out, LOW);
    digitalWrite(u_out, LOW);
  }
  calc = false;
}
