   updateRegisterX('E', PARSE_INPUT_PORTE);
   updateRegisterX('H', PARSE_INPUT_PORTH);
   updateRegisterX('J', PARSE_INPUT_PORTJ);
-
-}
-
+}
 
 int extractBitsBetween(int lsb, int msb, int DATA) {
   return  (DATA >> lsb) & ~(~0 << (msb - lsb + 1));
 }
 
 void setRegisterDirection() {
+  //Outputs, Direction Register moet 1 zijn om uitgang te zijn
   DDRB = USEDPINS_PORTB;
   DDRD = USEDPINS_PORTD;
   DDRE = USEDPINS_PORTE;
   DDRH = USEDPINS_PORTH;
   DDRJ = USEDPINS_PORTJ;
 
-  //HIER HETZELFDE DOEN VOOR DE INGANGEN
+  //Inputs, Direction Register moet 0 zijn om ingang te zijn.
+  DDRF = ~USEDPINS_PORTF;
+  DDRL = ~USEDPINS_PORTL;
+  DDRG = ~USEDPINS_PORTG;
+  DDRD = ~USEDPINS_PORTD; // Deze moet eigenlijk niet omdat DDRD bij de uitgangen al werd juist gezet.
+
 }
 
 //Deze functie gaat een register updaten met de waarden in NEWVALUE.

  Serial.print("=>");
  Serial.println(PORTJ, BIN);

  
}

//Deze functie gaat de poorten (registers) lezen en serieel terugsturen.
//Nu in binair maar kan ook in HEX.
void readStatusPorts(){
  
  unsigned int partB = (PORTB & USEDPINS_PORTB)<<8;
  unsigned int partD = (PORTD & USEDPINS_PORTD)>>2;
  unsigned int partE1 = (PORTE & 0x08)<<5;
  unsigned int partE2 = (PORTE & 0x30)<<2;
  unsigned int partH1 = (PORTH & 0x78)<<6;
  unsigned int partH2 = (PORTH & 0x03)<<2;
  unsigned int partJ = (PORTJ & USEDPINS_PORTJ)<<4;
  
FEEDBACK_STATUS_PORT=0x0000;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partB;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partD;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partE1;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partE2;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partH1;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partH2;
FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT|partJ;

Serial.print(F("Status van de poorten na aanpssen van registers[BIN]: "));
print_binary(FEEDBACK_STATUS_PORT,16);
Serial.print(F("Status van de poorten na aanpssen van registers [HEX]: 0x"));
Serial.println(FEEDBACK_STATUS_PORT,HEX);
  
}
  
//Print de mapping van de poorten
void printInfoPoorten(){
 Serial.print(F("Uitgang:        "));
 Serial.println(F("0  1   2   3   4  5   6  7  8  9  10  11  12  13  14  15"));
 Serial.print(F("Port# arduino:  "));
 Serial.println(F("13 12  11  9   8  7   6  5  3  2  14  15  16  17  18  19"));
 Serial.print(F("Port.BIT:       "));
 Serial.println(F("B7 B6  B5  H6  H5 H4  H3 E3 E5 E4 J1  J0  H1  H0  D3  D2"));

 Serial.println(F("Je kan nu een de uitgangen sturen door tussen [] een HEX-waarde mee te geven."));
 Serial.println(F("Bv: [FFFF] => alles aan | [0000] => alles uit | [1800] => uitgagen 3&4 aan, de rest uit"));
  }

      ;
  }
}


//Deze functie gaat de poorten (registers) lezen en serieel terugsturen.
//Nu in binair maar kan ook in HEX.
void readStatusOutputPorts() {

  unsigned int partB = (PORTB & USEDPINS_PORTB) << 8;
  unsigned int partD = (PORTD & USEDPINS_PORTD) >> 2;
  unsigned int partE1 = (PORTE & 0x08) << 5;
  unsigned int partE2 = (PORTE & 0x30) << 2;
  unsigned int partH1 = (PORTH & 0x78) << 6;
  unsigned int partH2 = (PORTH & 0x03) << 2;
  unsigned int partJ = (PORTJ & USEDPINS_PORTJ) << 4;

  FEEDBACK_STATUS_PORT = 0x0000;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partB;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partD;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partE1;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partE2;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partH1;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partH2;
  FEEDBACK_STATUS_PORT = FEEDBACK_STATUS_PORT | partJ;

  Serial.print(F("Status van de poorten na aanpssen van registers[BIN]: "));
  print_binary(FEEDBACK_STATUS_PORT, 16);
  Serial.print(F("Status van de poorten na aanpssen van registers [HEX]: 0x"));
  Serial.println(FEEDBACK_STATUS_PORT, HEX);

}

void readStatusInputPorts() {

  byte temp_PINF = ((PINF & 0x01) << 7) | ((PINF & 0x02) << 5) | ((PINF & 0x04) << 3) | ((PINF & 0x08) << 1) | ((PINF & 0x10) >> 1) | ((PINF & 0x20) >> 3) | ((PINF & 0x40) >> 5) | ((PINF & 0x80) >> 7);

}

void leesStatusIngangen() {
  float temp1 = micros();
  temp_INPUT_STATUS = 0x0000;

  INPUT_1[index] = (PINF & 0x01);
  INPUT_2[index] = (PINF & 0x02);
  INPUT_3[index] = (PINF & 0x04);
  INPUT_4[index] = (PINF & 0x08);

  INPUT_5[index] = (PINF & 0x10);
  INPUT_6[index] = (PINF & 0x20);
  INPUT_7[index] = (PINF & 0x40);
  INPUT_8[index] = (PINF & 0x80);

  INPUT_9[index] = (PINL & 0x02);
  INPUT_10[index] = (PING & 0x02);
  INPUT_11[index] = (PING & 0x01);
  INPUT_12[index] = (PINL & 0x80);

  INPUT_13[index] = (PINL & 0x01);
  INPUT_14[index] = (PINL & 0x04);
  INPUT_15[index] = (PIND & 0x80);
  INPUT_16[index] = (PINL & 0x40);

  if (checkElementsArray(INPUT_1)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_1[index] << 15);
  } if (checkElementsArray(INPUT_2)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_2[index] << 13);
  } if (checkElementsArray(INPUT_3)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_3[index] << 11);
  } if (checkElementsArray(INPUT_4)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_4[index] << 9);
  }

  if (checkElementsArray(INPUT_5)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_5[index] << 7);
  } if (checkElementsArray(INPUT_6)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_6[index] << 5);
  } if (checkElementsArray(INPUT_7)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_7[index] << 3);
  } if (checkElementsArray(INPUT_8)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_8[index] << 1);
  }

  if (checkElementsArray(INPUT_9)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_9[index] << 6);
  } if (checkElementsArray(INPUT_10)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_10[index] << 5);
  } if (checkElementsArray(INPUT_11)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_11[index] << 5);
  } if (checkElementsArray(INPUT_12)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_12[index] >> 3);
  }

  if (checkElementsArray(INPUT_13)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_13[index] << 3);
  } if (checkElementsArray(INPUT_14)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_14[index]);
  } if (checkElementsArray(INPUT_15)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_15[index] >> 6);
  } if (checkElementsArray(INPUT_16)) {
    temp_INPUT_STATUS = temp_INPUT_STATUS | (INPUT_16[index] >> 6);
  }

  //-----Index verhogen, indien aan de laatste terug naat nul.
  if (index == (aantalSamples - 1)) {
    index = 0;
  }
  else {
    index++;
  }
  float temp2 = micros();
  //---------------Einde index verhogen----------------------
  float x = temp2 - temp1;


  if (temp_INPUT_STATUS != INPUT_STATUS_PORT) {
    Serial.print("Status van de ingangen: 0x");
    Serial.println(temp_INPUT_STATUS, HEX);
    Serial.print("Status van de ingangen in binair: ");
    print_binary(temp_INPUT_STATUS, 16);
    INPUT_STATUS_PORT = temp_INPUT_STATUS;

    //For debug only
    Serial.print("tijd voor berekenen: ");
    Serial.println(x);
  }
}

//Functie gaat kijken of alle elementen dezelfde zijn. Indien dit is krijgen we True anders False.
bool checkElementsArray(byte temp[aantalSamples]) {
  for (int i = 1; i < aantalSamples; i++) {
    if (temp[i] != temp[0]) {
      return false;
    }
  }
  return true;
}

//Dit is gewoon om de status van de poorten te printen in de seriële console.
//DEBUG, Niet nodig in finale code
void printStatusPorts() {
  Serial.print("PORTB: 0x");
  Serial.print(PORTB, HEX);
