// ###########################################################################################
// Documentatie:
// Atmel-7810-Automotive-Microcontrollers-ATmega328P_Datasheet.pdf
// https://ww1.microchip.com/downloads/en/DeviceDoc/Atmel-7810-Automotive-Microcontrollers-ATmega328P_Datasheet.pdf
// Arduino port manipulation: https://www.youtube.com/watch?v=EVm0qVJ56II&list=PLZXbxN9gfETy0AKqki-M48MONF5ToMT3g&index=2
// Arduino registers: https://www.youtube.com/watch?v=6q1yEb_ukw8&list=PLZXbxN9gfETy0AKqki-M48MONF5ToMT3g&index=2
// Arduino external interrupts: https://www.youtube.com/watch?v=J61_PKyWjxU&list=PLZXbxN9gfETy0AKqki-M48MONF5ToMT3g&index=3
// Arduino timer interrupts: https://www.youtube.com/watch?v=2kr5A350H7E&list=PLZXbxN9gfETy0AKqki-M48MONF5ToMT3g&index=4&t=907s
// ###########################################################################################
// Poort registers:
// Poort C ==> Pin 14 tot Pin 19 (Analog A0 -> A5)
// Poort D ==> Pin 0 tot Pin 7 (Digital Pin 0 -> Digital Pin 7, 1= Rx x, 2=Tx)

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// -------------------------------------------------------------------------------------------------------
// Poort B ==> Pin 8 tot Pin 13 (Digital Pin 8 -> Digital Pin 13) 
// DDRB - Poort B data direction register (memory addres 0x04) ==> bepaald of pin een INPUT (0) of OUTPUT (1) is.
// Bit      bit 7       bit 6       bit 5       bit 4       bit 3       bit 2       bit 1       bit 0
// Vector   DDBB7       DDBB6       DDB5        DDB4        DDB3        DDB2        DDB1        DDB0
// R//W     R/W         R/W         R/W         R/W         R/W         R/W         R/W         R/W
// Initial  0           0           0           0           0           0           0           0
// -------------------------------------------------------------------------------------------------------
// PORTB - Poort B data register (memory addres 0x05) poort aansturen/lezen
// Indien DDRB = 0 (dus poort geconfigureerd als input) => 1 = pullup weerstand geactiveerd,  0 = geen pullup
// Indien DDRB = 1 (dus poort geconfigureerd als output)=> 0 = LOW = 0V, 1 = HIGH = 5V
// Bit      bit 7       bit 6       bit 5       bit 4       bit 3       bit 2       bit 1       bit 0
// Vector   PORTB7      PORTB6      PORTB5      PORTB4      PORTB3      PORTB2      PORTB1      PORTB0
// R//W     R/W         R/W         R/W         R/W         R/W         R/W         R/W         R/W
// Initial  0           0           0           0           0           0           0           0
// -------------------------------------------------------------------------------------------------------
// PINB - Poort B Input Puns Address (memory addres 0x03) ==> inlezen van de status van de pin
// deze byte dient behandelt te worden als read-only.
// Bit      bit 7       bit 6       bit 5       bit 4       bit 3       bit 2       bit 1       bit 0
// Vector   PINB7       PINB6       PINB5       PINB4       PINB3       PINB2       PINB1       PINB0
// R//W     R/W         R/W         R/W         R/W         R/W         R/W         R/W         R/W
// Initial  N/A         N/A         N/A         N/A         N/A         N/A         N/A         N/A
// -------------------------------------------------------------------------------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <Arduino.h>
#include <math.h>

//#include <IHCOutput.h> //dingus blok ==> tijdelijk uitgeschakeld, maar nodig indien outputs aangestuurd moeten worden

//Timer Settings
#define KLOKFREQUENTIE_MHZ 16 //klokfrequentie van microcontroller, uitgedrukt in MHz
#define TIMER1_PRESCALER 64 //mogelijke waarden: 1, 8, 64, 256, 1024
#define OCR1A_ 9 //output compare register voor de timer ==> zijn OCR1A+1 ticks: 0,1,2,3,4,5,6,7,8,9,0,1,2,...

//Timer constanten
#define KLOKPERIODE_NS  (1E9/(KLOKFREQUENTIE_MHZ*1E6)) //klokperiode van microcontroller, uitegdrukt in MHz
#define TELPERIODE_MUS (KLOKPERIODE_NS*TIMER1_PRESCALER/1000)
#define START_PULSLENGTE_MUS 4100 //lengte hoog deel van startpuls, uitgedrukt in µs
#define HOOG_PULSLENGTE_MUS 150 //lengte hoog deel van input true, uitgedrukt in µs
#define LAAG_PULSLENGTE_MUS 300 //lengte hoog deel van input false, uitgedrukt in µs
#define ONDERGRENS_STARTPULSLENGTE round(0.9*START_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)))
#define ONDERGRENS_HOOGPULSLENGTE round(0.9*HOOG_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)))
#define ONDERGRENS_LAAGPULSLENGTE round(0.9*LAAG_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)))
#define DETECTIEGRENS_LAAG_HOOG (ONDERGRENS_LAAGPULSLENGTE+ONDERGRENS_HOOGPULSLENGTE)/2
#define AANTALKANALENPERINGANGSMODULE 16

// //IO config
// #define NR_OF_INPUT_MODULES_PORTD 4 //4 aantal IHC input modules
// #define NR_OF_OUTPUT_MODULES 7 // 7 aantal IHC output modules

// //Geldt voor NANO en UNO:
// uint8_t ArduinoPinsIhcInputs[NR_OF_INPUT_MODULES_PORTD]={PD2,PD3,PD4,PD5}; //Geeft aan op welke Arduino pinnen de IHC input modules zitten
// uint8_t ArduinoPinsIhcOutputs[NR_OF_OUTPUT_MODULES]={PB5,PC0,PC1,PC2,PC3,PC4,PC5}; //Geeft aan op welke Arduino pinnen de IHC output modules zitten

//Arduino uno pin nummers: https://www.circuito.io/blog/arduino-uno-pinout/
uint8_t ArduinoPinsIhcInputsPORTB[]={}; //Geeft aan op welke Arduino pinnen van poort B de IHC input modules zitten
uint8_t ArduinoPinsIhcInputsPORTC[]={}; //Geeft aan op welke Arduino pinnen van poort C de IHC input modules zitten
uint8_t ArduinoPinsIhcInputsPORTD[]={PD2,PD3,PD4,PD5}; //Geeft aan op welke Arduino pinnen van poort D de IHC input modules zitten
uint8_t ArduinoPinsIhcOutputs[]={PB5,PC0,PC1,PC2,PC3,PC4,PC5}; //Geeft aan op welke Arduino pinnen de IHC output modules zitten

//IO config
#define NR_OF_INPUT_MODULES_PORTB sizeof(ArduinoPinsIhcInputsPORTB) //4 aantal IHC input modules
#define NR_OF_INPUT_MODULES_PORTC sizeof(ArduinoPinsIhcInputsPORTC) //4 aantal IHC input modules
#define NR_OF_INPUT_MODULES_PORTD sizeof(ArduinoPinsIhcInputsPORTD) //4 aantal IHC input modules
#define NR_OF_INPUT_MODULES NR_OF_INPUT_MODULES_PORTB + NR_OF_INPUT_MODULES_PORTC + NR_OF_INPUT_MODULES_PORTD

#define MAX_NR_OF_INPUT_MODULES_PORTB 8 //maximum aantal I/O pinnen op arduino voor poort B (PB0...PB7)
#define MAX_NR_OF_INPUT_MODULES_PORTC 8 //maximum aantal I/O pinnen op arduino voor poort C (PC0...PC7)
#define MAX_NR_OF_INPUT_MODULES_PORTD 8 //maximum aantal I/O pinnen op arduino voor poort D (PD0...PD7)
#define MAX_NR_OF_INPUT_MODULES MAX_NR_OF_INPUT_MODULES_PORTB + MAX_NR_OF_INPUT_MODULES_PORTC + MAX_NR_OF_INPUT_MODULES_PORTD

#define NR_OF_OUTPUT_MODULES sizeof(ArduinoPinsIhcOutputs) // 7 aantal IHC output modules

//Variabelen
volatile uint8_t InputModulePinNr = 0;
volatile uint8_t InputModuleVolgnr = 0;

volatile int BitHoogCounterPORTB[NR_OF_INPUT_MODULES_PORTB];
volatile int BitHoogCounterPORTC[NR_OF_INPUT_MODULES_PORTC];
volatile int BitHoogCounterPORTD[NR_OF_INPUT_MODULES_PORTD];

volatile uint8_t KanaalNrDatWordtIngelezenPORTB[NR_OF_INPUT_MODULES_PORTB];
volatile uint8_t KanaalNrDatWordtIngelezenPORTC[NR_OF_INPUT_MODULES_PORTC];
volatile uint8_t KanaalNrDatWordtIngelezenPORTD[NR_OF_INPUT_MODULES_PORTD];

volatile int16_t KanaalDataBufferPORTB[NR_OF_INPUT_MODULES_PORTB]; //Kanaaldatabuffer voor alle inputs op poort B. Dit is een WOORD, dus elke bit stelt 1 kanaal voor.
volatile int16_t KanaalDataBufferPORTC[NR_OF_INPUT_MODULES_PORTC]; //Kanaaldatabuffer voor alle inputs op poort C. Dit is een WOORD, dus elke bit stelt 1 kanaal voor.
volatile int16_t KanaalDataBufferPORTD[NR_OF_INPUT_MODULES_PORTD]; //Kanaaldatabuffer voor alle inputs op poort D. Dit is een WOORD, dus elke bit stelt 1 kanaal voor.

volatile int16_t KanaalDataOldPORTB[NR_OF_INPUT_MODULES_PORTB];
volatile int16_t KanaalDataOldPORTC[NR_OF_INPUT_MODULES_PORTC];
volatile int16_t KanaalDataOldPORTD[NR_OF_INPUT_MODULES_PORTD];

volatile int16_t KanaalDataPORTB[NR_OF_INPUT_MODULES_PORTB];
volatile int16_t KanaalDataPORTC[NR_OF_INPUT_MODULES_PORTC];
volatile int16_t KanaalDataPORTD[NR_OF_INPUT_MODULES_PORTD];

volatile boolean KanaalDataAvailablePORTB[NR_OF_INPUT_MODULES_PORTB];
volatile boolean KanaalDataAvailablePORTC[NR_OF_INPUT_MODULES_PORTC];
volatile boolean KanaalDataAvailablePORTD[NR_OF_INPUT_MODULES_PORTD];

boolean KanaalDataOldAvailablePORTB[NR_OF_INPUT_MODULES_PORTB];
boolean KanaalDataOldAvailablePORTC[NR_OF_INPUT_MODULES_PORTC];
boolean KanaalDataOldAvailablePORTD[NR_OF_INPUT_MODULES_PORTD];

volatile unsigned long delmecounter = 0;

uint16_t InputBeeldModule[MAX_NR_OF_INPUT_MODULES]; //Bevat de inputbeelden van de alle inputs die ZOUDEN kunnen aangesloten worden op de arduino

//Delme:
unsigned long Startmillis;
unsigned long Currentmillis;
const unsigned long long Interval = 3000; //[ms]


void setup() 
{
  Serial.begin(115200);
  while (!Serial)
    {
      ;//wacht tot de seriële poort geconnecteerd is
    }
  Serial.println("=========================");
  Serial.println("CONTROLLER IS BOOTING... ");
  Serial.println("");
  

  //Inputpoorten activeren als pin voor IHC inputmodule
  for(uint8_t BitCntr = 0; BitCntr<NR_OF_INPUT_MODULES_PORTB;BitCntr ++)
    {
      DDRB = ~(1<<ArduinoPinsIhcInputsPORTB[BitCntr]) & DDRB; //Zet alle poorten die aangegeven zijn in de array laag; dus dit worden inputs
      PORTB = (1<<ArduinoPinsIhcInputsPORTB[BitCntr]) | DDRB; // Activeer voor alle poorten die aangegeven zijn in de array de pullup resistors
    }
  
  for(uint8_t BitCntr = 0; BitCntr<NR_OF_INPUT_MODULES_PORTC;BitCntr ++)
    {
      DDRC = ~(1<<ArduinoPinsIhcInputsPORTC[BitCntr]) & DDRC; //Zet alle poorten die aangegeven zijn in de array laag; dus dit worden inputs
      PORTC = (1<<ArduinoPinsIhcInputsPORTC[BitCntr]) | DDRC; // Activeer voor alle poorten die aangegeven zijn in de array de pullup resistors
    }

  for(uint8_t BitCntr = 0; BitCntr<NR_OF_INPUT_MODULES_PORTD;BitCntr ++)
    {
      DDRD = ~(1<<ArduinoPinsIhcInputsPORTD[BitCntr]) & DDRD; //Zet alle poorten die aangegeven zijn in de array laag; dus dit worden inputs
      PORTD = (1<<ArduinoPinsIhcInputsPORTD[BitCntr]) | DDRD; // Activeer voor alle poorten die aangegeven zijn in de array de pullup resistors
    }
  
  pinMode(LED_BUILTIN, OUTPUT);
  
  //
  ShowIhcHardwareSetup();

  ShowTimingParameters();
  delay(10);

  //Zet alle settings van TIMER1 goed
  SetupTimer(); 

  //Millis testimer
  Currentmillis = millis();
}
//============================================================================
ISR(TIMER1_COMPA_vect)  //input capture interrupt ISR
{
  //Delmecounter verhogen
  delmecounter++;
  if (delmecounter > 36000) delmecounter=0;

  
  //voor elke inputmodule aangesloten op poort D
  for (InputModulePinNr = 0; InputModulePinNr<NR_OF_INPUT_MODULES_PORTD; InputModulePinNr++)
  {
      //Als de ingang van de inputmodule hoog is ==> eentje bijtellen bij teller  
      if( PIND & (1<<ArduinoPinsIhcInputsPORTD[InputModulePinNr]))
        {
          BitHoogCounterPORTD[InputModulePinNr]++;
        }
      //Anders zijn we laag gevallen en is de bitreeks dus ten einde, tijd om nu te kijken naar de lengte van de bitreeks
      else
        {
          //Zijn er zoveel hoge waarden geteld dat we van een startpuls kunnen spreken?
          if (BitHoogCounterPORTD[InputModulePinNr] >=ONDERGRENS_STARTPULSLENGTE)
            {
              //Ja! Ok, nu kunnen we kanalen beginnen tellen, dit geven we aan door de kanaalteller op 1 te zetten
              KanaalNrDatWordtIngelezenPORTD[InputModulePinNr] = 1;
              //Wis de kanaalbuffer, waarin de waarden van de 16 inputkanalen wordt bewaard
              KanaalDataBufferPORTD[InputModulePinNr] = 0;
            }
          //Nee, we hebben niet met startpuls te maken, maar als we al in de modus zitten dat we kanalen aan het ontvangen zijn gaan we hiermee door.
          //Is de pulslengte lang genoeg om een "0" te zijn?
          else if ((KanaalNrDatWordtIngelezenPORTD[InputModulePinNr] >= 1) && (BitHoogCounterPORTD[InputModulePinNr] >=ONDERGRENS_LAAGPULSLENGTE))
            {
              //Ja! De pulslengte lang genoeg om een "0" te zijn!
              //Kanaaldatabuffer op 0 laten staan
              KanaalNrDatWordtIngelezenPORTD[InputModulePinNr]++;
            }
          //Is de pulslengte lang genoeg om een "1" te zijn?
          else if ((KanaalNrDatWordtIngelezenPORTD[InputModulePinNr] >= 1) && (BitHoogCounterPORTD[InputModulePinNr] >=ONDERGRENS_HOOGPULSLENGTE))
            {
              //Ja! De pulslengte lang genoeg om een "1" te zijn!
              KanaalDataBufferPORTD[InputModulePinNr] = (1<<KanaalNrDatWordtIngelezenPORTD[InputModulePinNr])|KanaalDataBufferPORTD[InputModulePinNr];
              KanaalNrDatWordtIngelezenPORTD[InputModulePinNr]++;
            }
          else
            {
              //Als we hier komen is de lengte van de puls niet lang genoeg om te besluiten of het een startpuls was, een hoog of een laag ==> foutje, vergeet dus deze telegram.
              KanaalNrDatWordtIngelezenPORTD[InputModulePinNr] = 0;
              KanaalDataBufferPORTD[InputModulePinNr] = 0;
            }
        }
    //Bon, hier aanbeland gaan we kijken of we een alle kanalen van een inputmodule hebben ingelezen (=volledig telegram). 
    if (KanaalNrDatWordtIngelezenPORTD[InputModulePinNr]>AANTALKANALENPERINGANGSMODULE)
      {
        //Als we hier komen hebben we inderdaad een volledig telegram ingelezen
        //Nu gaan we kijken of de data gelijk gebleven is aan het vorige telegram voor deze module. Dit doen we om eventuele glitches op te vangen
        if (KanaalDataOldPORTD[InputModulePinNr] == KanaalDataBufferPORTD[InputModulePinNr])
          {
            //vorige telegram kwam overeen met de nieuwe ==> DATA VALID
            //Stuur dus maar door naar inputbeeld
            KanaalDataPORTD[InputModulePinNr] = KanaalDataBufferPORTD[InputModulePinNr];
          
            KanaalDataAvailablePORTD[InputModulePinNr] = true;
          }
        else
          {
            //Data INVALID :(
            ; //Niet aan de kanaaldata komen!
          }
        
        //Sla dit inputbeeld op om te vergelijk met de volgende telegram.
        KanaalDataOldPORTD[InputModulePinNr]= KanaalDataBufferPORTD[InputModulePinNr];

        //Kanaaldatabuffer wissen en kanaalteller ook, klaar om nieuwe telegram te lezen.
        KanaalNrDatWordtIngelezenPORTD[InputModulePinNr] = 0;
        KanaalDataBufferPORTD[InputModulePinNr] = 0;
      }
  }
}
//============================================================================



void loop() {
  // put your main code here, to run repeatedly:
    if (delmecounter > 18000)
      {
        digitalWrite(LED_BUILTIN, HIGH);
      }
    else
      {
        digitalWrite(LED_BUILTIN, LOW);
      }
  
  //Afhandeling inputs poort B
  //TODO

  //Afhandeling inputs poort C
  // TODO

  //Afhandeling inputs poort D
  for (uint8_t InputModuleNr = 0; InputModuleNr<NR_OF_INPUT_MODULES_PORTD; InputModuleNr++)
    {
      //Positieve flankdetectie op beschikbare kanaaldata; (Signaal XOR SingaalOud) 
      if ((KanaalDataAvailablePORTD[InputModuleNr] ^ KanaalDataOldAvailablePORTD[InputModuleNr]) & KanaalDataAvailablePORTD[InputModuleNr])
        {
          //positieve flank gedetecteerd ==> hier moeten we de kanaaldata vertalen voor- en wegschrijven naar inputbeeld
          SetInputBeeldFromKanaalDataBuffer('D', ArduinoPinsIhcInputsPORTD[InputModuleNr]); //bij het tweede argument wordt dus PD0, PD1, PD2,... meegegeven
        }
     KanaalDataOldAvailablePORTD[InputModuleNr] =  KanaalDataAvailablePORTD[InputModuleNr];
     KanaalDataAvailablePORTD[InputModuleNr] = false;
    }

  //Testroutine

  Currentmillis = millis();

  if ((Currentmillis - Startmillis) > Interval)
  {
    Startmillis = Currentmillis;
    if(InputBeeldModule[16] == 0b01010101)
    {
      ;
    }
    else
    {
      Serial.print("inputbeeld fout: ");
      Serial.print(InputBeeldModule[16], BIN);
      Serial.print(" <-> ");
      Serial.println(0b01010101, BIN);
    }
  }
}

//Maak inputbeeld op adhv opgegeven poort en kanaalnummer.
//Kanaaldatabuffer wordt opgemaakt volgens inputmodules die effectief zijn aangesloten om maxime snelheid in de interrupt te garanderen
//De inputbeelden die gemaakt worden, zijn volgens het maximum aantal mogelijke aangesloten inputmodules op de arduino.
//Bijvoorbeeld de kanaaldatabuffer kan data bevatten van informatie die binnen komt op PB3en PB4
//Het inputbeeld is bevat kanaaldata van PB0 tot PB7.
void SetInputBeeldFromKanaalDataBuffer(char PortName, uint8_t InputModuleNr)
  {
    uint8_t myStartNr = 0;
    uint8_t myNrOfInputModules = 0;

    switch(PortName) 
    {
      case 'B':
        //bereken de startoffset
        myStartNr = ArduinoPinsIhcInputsPORTD[InputModuleNr];
        myNrOfInputModules = NR_OF_INPUT_MODULES_PORTB;

        if(myStartNr<= sizeof(InputBeeldModule)) //zit hetgeen we vragen binnen de fysieke grenzen van wat er gedefinieerd is?
        {
          InputBeeldModule[myStartNr] = KanaalDataPORTB[InputModuleNr];
        }
        else
        {
          //Als we hier komen is er een probleem ==> we hebben berekend dat inputbeeldnummer hoger is dan het aantal fysieke inputpinnen
          delay(1);
        }
        break;
      case 'C':
        //bereken de startoffset
        myStartNr = MAX_NR_OF_INPUT_MODULES_PORTB + ArduinoPinsIhcInputsPORTD[InputModuleNr];
        myNrOfInputModules = NR_OF_INPUT_MODULES_PORTC;

        if(myStartNr<= sizeof(InputBeeldModule)) //zit hetgeen we vragen binnen de fysieke grenzen van wat er gedefinieerd is?
        {
          InputBeeldModule[myStartNr] = KanaalDataPORTC[InputModuleNr];
        }
        else
        {
          //Als we hier komen is er een probleem ==> we hebben berekend dat inputbeeldnummer hoger is dan het aantal fysieke inputpinnen
          delay(1);
        }
        break;
      case 'D':
        //bereken de startoffset
        myStartNr = MAX_NR_OF_INPUT_MODULES_PORTB + MAX_NR_OF_INPUT_MODULES_PORTC + ArduinoPinsIhcInputsPORTD[InputModuleNr];
        myNrOfInputModules = NR_OF_INPUT_MODULES_PORTD;

        if(myStartNr<= sizeof(InputBeeldModule)) //zit hetgeen we vragen binnen de fysieke grenzen van wat er gedefinieerd is?
        {
          InputBeeldModule[myStartNr] = KanaalDataPORTD[InputModuleNr];
        }
        else
        {
          //Als we hier komen is er een probleem ==> we hebben berekend dat inputbeeldnummer hoger is dan het aantal fysieke inputpinnen
          delay(1);
        }
        break;
      default:
        //WAAAAAAAAAA ONBEKENDE POORT GEVRAAGD...da kan nie
        break;
    }        
  }
boolean Input(uint8_t ModuleNr, uint8_t InputNr)
  {
    //Input(0,0) = PORTB, module0, kanaal 0
    //Input(0,1) = PORTB, module0, kanaal 1
    //...
    //Input(0,16) = PORTB, module 0, kanaal 16
    //...
    //Input(7,16) = PORTB, module 7, kanaal 16
    //Input(8,0) = PORTC, module 0, kanaal 0
    //...
    //Input(15,16) = PORTC, module 7, kanaal 16
    //Input(16,0) = PORTD, module 0,kanaal 0
    //...
    //Input(23,16) = PORTD, module 7, kanaal 16

    boolean myInput = false;

    //TOT HIE

    return myInput; 
  }

void SetupTimer() 
{
  //programming timer1 for input capture interrupt
  noInterrupts();
  TCCR1A = 0;             //initialize timer1 mode
  TCCR1B = 0;
  TCNT1 = 0;              //initialize timer/counter1 register
  
  //Prescaler laden
  switch(TIMER1_PRESCALER) 
  {
    case 1:
      TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10);
      break;
    case 8:
      TCCR1B |= (0 << CS12) | (1 << CS11) | (0 << CS10);
      break;
    case 64:
      TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
      break;
    case 256:
      TCCR1B |= (1 << CS12) | (0 << CS11) | (0 << CS10);
      break;
    case 1024:
      TCCR1B |= (1 << CS12) | (1 << CS11) | (1 << CS10);
      break;
    default:
      //WAAAAAAAAAA FOUTE PRESCALER INGESTELD, dus hier kiezen we hem default dan maar op 1
      TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10);
      break;
  }

  //Stel de compare match waarde in
  OCR1A = 9 ;//OCR1A_;

  //CTC mode activeren ==> Clear Timer on Compare mode ==> zet timer automatisch op 0 bij een interrupt compare match
  TCCR1B |= (1 << WGM12);

  //Activeer Compare Match A Interrupt ==> genereer een interrupt (TIMER1_COMPA) als de timerwaarde TCNT1 overeenkomt met presetwaarde (OCR1A)
  TIMSK1 |= (1 << OCIE1A);

  interrupts();
}

void ShowIhcHardwareSetup()
{
  Serial.println("------------------------");
  Serial.println("IHC Hardware parameters:");
  Serial.println("------------------------");

  //Input modules
  Serial.print("Aantal input modules = ");
  Serial.println(NR_OF_INPUT_MODULES);
  Serial.print("Hangen op volgende I/O pins: {");
  for (uint8_t i=0; i<NR_OF_INPUT_MODULES_PORTB;i++)
    {
     Serial.print("PB");
     Serial.print(ArduinoPinsIhcInputsPORTB[i]);
     Serial.print(",");
    }
  for (uint8_t i=0; i<NR_OF_INPUT_MODULES_PORTC;i++)
    {
     Serial.print("PC");
     Serial.print(ArduinoPinsIhcInputsPORTC[i]);
     Serial.print(",");
    }
  for (uint8_t i=0; i<NR_OF_INPUT_MODULES_PORTD;i++)
    {
     Serial.print("PD");
     Serial.print(ArduinoPinsIhcInputsPORTD[i]);
     if (i<NR_OF_INPUT_MODULES_PORTD-1) {Serial.print(",");}
    }
  Serial.println("}");

  //Output modules
  Serial.print("Aantal output modules = ");
  Serial.println(NR_OF_OUTPUT_MODULES);
  Serial.print("Hangen op volgende I/O pins: {");
  for (uint8_t i=0; i<NR_OF_OUTPUT_MODULES;i++)
    {
     Serial.print(ArduinoPinsIhcOutputs[i]);
      if (i<NR_OF_OUTPUT_MODULES-1) {Serial.print(",");}
    }
  Serial.println("}");
}

void ShowTimingParameters()
  {
    Serial.println("------------------");
    Serial.println("TIMING PARAMETERS:");
    Serial.println("------------------");

    //Laat klokfrequentie zien
    Serial.print("Klokfrequentie = ");
    Serial.print(KLOKFREQUENTIE_MHZ);
    Serial.println(" MHz");

    //Laat klokperiode zien
    Serial.print("Klokperiode = ");
    Serial.print(KLOKPERIODE_NS, 2);
    Serial.println(" ns");

    //Laat prescaler
    Serial.print("Prescaler instelling = ");
    Serial.println(TIMER1_PRESCALER);

    //Laat telperiode zien
    Serial.print("Telperiode = ");
    Serial.print(TELPERIODE_MUS, 2);
    Serial.println(" µs");

    //OCR1A ==> Output compare register A van timer 1 ==> om de hoeveel ticks er een interrupt moet komen
    Serial.print("OCR1A: Output Compare Register A van Timer 1 = ");
    Serial.println(OCR1A_);
    Serial.print("d.w.z: om de ");
    Serial.print(OCR1A_ +1);
    Serial.println(" ticks een interrupt genereren (OCR1A +1, want we gaan door de nul) ");
    Serial.print("Er komt dus om de ");
    Serial.print(TELPERIODE_MUS * (OCR1A_ +1));
    Serial.println(" µs een interrupt");

    //Aantal compare match interrupts bij hoog signaal voor startblok
    Serial.print("#compare match interrupts bij START blok (4100µs) = ");
    Serial.print(START_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)),1);
    Serial.print(" / detectiegrens = ");
    Serial.print(ONDERGRENS_STARTPULSLENGTE);
    Serial.println(" tellerpulsen");

    //Aantal telpulsen hoog signaal voor "HOOG"
    Serial.print("#compare match interrupts bij HOOG signaal (150µs) = ");
    Serial.print(HOOG_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)),1);
    Serial.print(" / detectiegrens = ");
    Serial.print(ONDERGRENS_HOOGPULSLENGTE);
    Serial.println(" tellerpulsen");

    //Aantal telpulsen hoog signaal voor "LAAG"
    Serial.print("#compare match interrupts bij LAAG signaal (300µs) = ");
    Serial.print(LAAG_PULSLENGTE_MUS/(TELPERIODE_MUS*(OCR1A_+1)),1);
    Serial.print(" / detectiegrens = ");
    Serial.print(ONDERGRENS_LAAGPULSLENGTE);
    Serial.println(" tellerpulsen");

    //In te stellen grens tussen hoog en laag
    Serial.print("in te stellen detectielimiet tussen hoog en laag = ");
    Serial.print(DETECTIEGRENS_LAAG_HOOG);
    Serial.println(" tellerpulsen");
  }