//Variablen

//RC-bezogen
int cnt_CH_1 = 0; //Zähler Channel 1 für Interruptroutine
int frqraw_CH_1 = 0; //Ãœbergabewert Channel 1 aus Interruptroutine
int cnt_CH_2 = 0; //Zähler Channel 2 für Interruptroutine
int frqraw_CH_2 = 0; //Ãœbergabewert Channel 2 aus Interruptroutine
int cnt_CH_3 = 0; //Zähler Channel 3 für Interruptroutine
int frqraw_CH_3 = 0; //Ãœbergabewert Channel 3 aus Interruptroutine
int cnt_CH_4 = 0; //Zähler Channel 4 für Interruptroutine
int frqraw_CH_4 = 0; //Ãœbergabewert Channel 4 aus Interruptroutine


//Programmbezogen
const int deadzone = 10;
const int hlevel = 174; //höchste gelieferte Frequenz
const int llevel = 115; //niedrigste gelieferte Frequenz
const int glevel = 310; //Grenzlevel für Ergebnis

void setup() {
  
  // Controller pins
  DDRD = 0b11000011; //Setzt D2-D5 als Eingang 0 und die restlichen als Ausgang 1
  cli(); // Clear interrupts Interrupts ausschalten

  // Register zurücksetzen
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;

  OCR1A = 20; //Output Compare Register auf Vergleichswert setzen, war 20

  TCCR1B |= (1 << CS11); //Prescale 8
  // 16MHz/8=2MHz mit OCR1A=20 Interrupt alle 10µs

  TCCR1B |= (1 << WGM12); //CTC-Mode einschalten
  TIMSK1 |= (1 << OCIE1A); //Timer Compare Interrupt setzen

  sei(); // Set Interrupts Interrupts einschalten

  Serial.begin(9600); //serielle Verbindung etablieren
  

}

ISR(TIMER1_COMPA_vect) { //die Interruptroutine gibt ein Zehntel der Impulsbreite in µs zurück

  
   if (PIND & (1<<PD2)) {  //Channel 1 rechts horizontal, das ist PIN D2
    cnt_CH_1++; //wenn Eingang High dann Zähler inkrementieren
    
  }
  else if (cnt_CH_1) {          //wenn Eingang Low dann prüfen ob Zähler gestartet
    frqraw_CH_1 = cnt_CH_1 - 1; //wenn Zähler gestartet, stoppen und Wert übergeben
    
    cnt_CH_1 = 0; //Zähler zurücksetzen
  }


  if (PIND & (1<<PD3)) {  //Channel 2 rechts vertikal, das ist PIN D3
    cnt_CH_2++; //wenn Eingang High dann Zähler inkrementieren
    
  }
  else if (cnt_CH_2) {          //wenn Eingang Low dann prüfen ob Zähler gestartet
    frqraw_CH_2 = cnt_CH_2 - 1; //wenn Zähler gestartet, stoppen und Wert übergeben
    
    cnt_CH_2 = 0; //Zähler zurücksetzen
  }

   if (PIND & (1<<PD4)) {  //Channel 3 links vertikal, das ist PIN D4
    cnt_CH_3++; //wenn Eingang High dann Zähler inkrementieren
    
  }
  else if (cnt_CH_3) {          //wenn Eingang Low dann prüfen ob Zähler gestartet
    frqraw_CH_3 = cnt_CH_3 - 1; //wenn Zähler gestartet, stoppen und Wert übergeben
    
    cnt_CH_3 = 0; //Zähler zurücksetzen
  }


  if (PIND & (1<<PD5)) {  //Channel 4 links horizontal , das ist PIN D5
    cnt_CH_4++; //wenn Eingang High dann Zähler inkrementieren
    
  }
  else if (cnt_CH_4) {          //wenn Eingang Low dann prüfen ob Zähler gestartet
    frqraw_CH_4 = cnt_CH_4 - 1; //wenn Zähler gestartet, stoppen und Wert übergeben
    
    cnt_CH_4 = 0; //Zähler zurücksetzen
  }

    
}

void loop() {
  
  Serial.println(pulseToPWM(frqraw_CH_1)); //Wert für Kanal 1 an Console senden
  Serial.println(pulseToPWM(frqraw_CH_2)); //Wert für Kanal 2 an Console senden
  Serial.println(pulseToPWM(frqraw_CH_3)); //Wert für Kanal 3 an Console senden
  Serial.println(pulseToPWM(frqraw_CH_4)); //Wert für Kanal 4 an Console senden
  Serial.println(" ");
  Serial.println(" ");
  delayMicroseconds(16000); // Damit man überhaupt etwas sieht

}

// RC pulse in PWM für Motor wandeln
int pulseToPWM(int pulse) {
  
  if (pulse > 100) { // nur Werte > 100 verwerten, andere sind Störung oder Sender off
    
    pulse = map(pulse, llevel, hlevel, -500, 500); // Ausgabewertebereich verschieben
    
    pulse = constrain(pulse, -glevel, glevel); // und jetzt noch begrenzen
  } else {
    
    pulse = 0; // keine sinnvollen Impulse
  }

  // Ruhezone einrichten
  if (abs(pulse) <= deadzone) {
    pulse = 0;
  }

  return pulse;
}