/******************************************************************************
 * Enum afkortingen Wissel
 ******************************************************************************/
enum wisselStates_t { Recht = 0, Afbuigen};
enum blokStates_t   { Vrij = 0, Bezet};


/******************************************************************************
 * Debug modes
 ******************************************************************************/
//Serial monitor compenstatie, -'1'
#define DEBUG_SERIAL_MONITOR
//Algemene debug info
#define DEBUG_SKETCH

/******************************************************************************
 * Wissel parameters
 ******************************************************************************/
const byte WisselPins[][2] = {{13, 14}, {11, 12}}; //wissel pinnen
const unsigned int WisselSchakeltijd = 200; //Schakeltijd van de wissels in ms
const byte WisselBufferSize = 8;  //Grote van de wissel buffer

/******************************************************************************
 * Serial parameters
 ******************************************************************************/
const unsigned int SerialInterval = 200;  //Tijd waarbinnen een commando binnen moet zijn in ms

/******************************************************************************
 * Wissel variabele
 ******************************************************************************/
byte wisselBuffer[WisselBufferSize];      //Buffer voor wissel adressen
bool wisselStateBuffer[WisselBufferSize]; //buffer voor wissel stateas
byte volgendeWisselInvoer = 0;  //Waar de volgende wissel invoegen
byte volgendeWisselUitvoer = 0; //Waar de volgende wissel schakelen

void setup(){
  for(byte i = 0; i < sizeof(WisselPins); i++){
    pinMode(WisselPins[i][Recht], OUTPUT);
    pinMode(WisselPins[i][Afbuigen], OUTPUT);
  }

  Serial.begin(115200); //tijd voor 115200?
}

void loop() {
  checkIngekomenSerial();
  updateWissels();

}


//Inkomende seriele data controleren
void checkIngekomenSerial() {
  // Ga dan uit dat de serial commando's eruit zien als
  // [commando letter] [adres]
  // Waarbij ieder [] één byte is. Dus geen adressen in ASCII zenden ;)

  static unsigned int lastCommandMillis = 0;
  unsigned int nowMillis = millis();

  if (Serial.available() >= 2) {
    lastCommandMillis = nowMillis;

    switch (Serial.read()) {

      //schakel een wissel recht
      case 'W':
        setWissel(Serial.read(), Recht);

        break;

      //schakel een wissel afbuigend
      case 'w':
        setWissel(Serial.read(), Afbuigen);

        break;

      //Maak blok vrij
      case 'b':
        setBlok(Serial.read(), Vrij);

        break;

      case 'B':
        setBlok(Serial.read(), Bezet);

        break;
    }
  }

  //Is het anders tijd voor het legen van de buffer?
  //Gebeurd als er SerialInterval lang geen geldig commando is ontvangen
  else if(Serial.available() && (nowMillis - lastCommandMillis >= SerialInterval)){
    while(Serial.available()){
      Serial.read();
    }
  }

  //Om te zorgen dat de buffer niet geleegd word als we na een lange tijd weer beginnen met het zenden van een commando
  //updaten we de tijd ook als de buffer gewoon leeg is. Kan ook gezien worden als geldig
  //commando voor do niets ;)
  else if(!Serial.available()){
    lastCommandMillis = millis();
  }
}


//wissel schakelen
void setWissel(byte adres, bool state){

  #if defined(DEBUG_SERIAL_MONITOR)
    //Even compenseren voor ASCII. Werkt alleen voor 1 t/m 9.
    //GEEN oplossing dus. Beter en simpeler om een echte byte te sturen maar dat doet de serial monitor niet
    adres -= '1';
  #endif

  #if defined(DEBUG_SKETCH)
    Serial.print(F("Wissel "));
    Serial.print(adres);
    Serial.print((state ? "/": "|"));
  #endif

  //Als er ruimte is en adres is geldig dan toevoegen aan buffer
  if(wisselBufferRuimte && adres < sizeof(WisselPins) / sizeof(WisselPins[0])){
    #if defined(DEBUG_SKETCH)
      Serial.print(F(" Slot "));
      Serial.print(volgendeWisselInvoer);
    #endif

    //toevoegen aan buffer
    wisselBuffer[volgendeWisselInvoer] = adres;
    wisselStateBuffer[volgendeWisselInvoer] = state;

    //Invoer pointer naar volgende slot
    volgendeWisselInvoer++;
    volgendeWisselInvoer %= WisselBufferSize;
  }
  else{
    Serial.print(F("Wissel buffer vol!"));
  }
  Serial.println();
}

//wissels updaten, oproepen in loop()
void updateWissels(){
  static unsigned int lastMillis = 0;
  static bool wisselActief = false;
  unsigned int nowMillis = millis();

  //als er een wissel actief is
  if(wisselActief){
    //wacht deze af
    if(nowMillis - lastMillis >= WisselSchakeltijd){
      //uitgang deactiveren
      digitalWrite(WisselPins[wisselBuffer[volgendeWisselUitvoer]][wisselStateBuffer[volgendeWisselUitvoer]], LOW);
      //opslaan dat er geen wissel actief is
      wisselActief = false;

      //naar volgende buffer plek
      volgendeWisselUitvoer++;
      volgendeWisselUitvoer %= WisselBufferSize;
    }
  }

  //Als er nu geen wissel actief is kunnen we de volgende schakelen
  else{
    //zolang de uitvoer nog niet bij is met de invoer is er dus een wissel te schakelen
    if(volgendeWisselInvoer != volgendeWisselUitvoer){
      #if defined(DEBUG_SKETCH)
        Serial.print(F("schakelen! "));
        Serial.print(wisselBuffer[volgendeWisselUitvoer]);
        Serial.println((wisselStateBuffer[volgendeWisselUitvoer] ? "/": "|"));
      #endif

      //uitgang schakelen
      digitalWrite(WisselPins[wisselBuffer[volgendeWisselUitvoer]][wisselStateBuffer[volgendeWisselUitvoer]], HIGH);
      //opslaan dat er een wissel actief is
      wisselActief = true;
      //begintijd opslaan
      lastMillis = nowMillis;
    }
  }
}

//als volgende invoer 1 kleiner is dan volgende uitvoer is de buffer vol
inline bool wisselBufferRuimte(){
    return ( ((volgendeWisselInvoer + 1) % WisselBufferSize) != volgendeWisselUitvoer);
}

void setBlok(byte address, bool state) {

  Serial.print("Blok ");
  Serial.print(address);
  if (state) {
    Serial.println(" bezet");
  }
  else {
    Serial.println(" vrij");
  }
}