#define SelectButton 9       // input op digital pin 9
#define StartButton 8        // input op digital pin 8
#define LED1 7               // output op digital pin 7
#define LED2 6               // output op digital pin 6
#define LED3 5               // output op digital pin 5
#define LED4 4               // output op digital pin 4

bool StartButton_State = HIGH;   // HIGH or LOW
bool LastStartButton_State = LOW;
bool SelectButton_State = HIGH ; // HIGH or LOW
bool LastSelectButton_State = LOW;   // Last setting SelectButton_State
byte SelectButton_Count = 0;   // continious counting from 1 to 4

const unsigned int StartDelay = 10000;     // Startdelay / 1000 = seconds

void setup() {
  // put your setup code here, to run once:

 Serial.begin(9600);         // tbv debugging
 pinMode (LED1, OUTPUT);     // digital output
 pinMode (LED2, OUTPUT);     // digital output
 pinMode (LED3, OUTPUT);     // digital output
 pinMode (LED4, OUTPUT);     // digital output
 pinMode(SelectButton, INPUT_PULLUP);
 pinMode(StartButton, INPUT_PULLUP);
}

void loop() {
  // put your main code here, to run repeatedly:

  StartDelayTimer();   // see functions at bottom

 //SelectButton_State = digitalRead(SelectButton);  // read SelectButton
 StartButton_State = digitalRead(StartButton);    // read StartButton
 //Serial.print("SelectButton_State= ");            // debugging
 //Serial.print(SelectButton_State);                //    "
 //Serial.print("\t");                              //    "
 //Serial.print("SelectButton_Count= ");            //    "
 //Serial.print(SelectButton_Count);                //    "
 //Serial.print("\t");                              //    "
 //Serial.print("StartButton= ");                   //    "
 //Serial.println(StartButton_State);               // debugging



 if (debounce(SelectButton) == HIGH) { 
   //Serial.print(" SelectButton = ");
   //Serial.print(SelectButton_State);
   //Serial.print("\t");
   //digitalWrite(LED1, !LED1);
   
   SelectButton_State = !debounce(SelectButton); // Because Pull-up, pushed =!pushed
    if (SelectButton_State == HIGH && LastSelectButton_State == LOW) {
      SelectButton_State = digitalRead(SelectButton);  // read SelectButton
      SelectButton_Count ++; // Button is pushed, Selection_Count ++
      LastSelectButton_State = SelectButton_State; // Store LastSelectButton_State into SelectButton_State.
    
     if(SelectButton_Count > 4) {
       SelectButton_Count = 1;
      } 
    }


    switch(SelectButton_Count) {
     case 1:
      digitalWrite (LED1, HIGH);
      digitalWrite (LED2, LOW);
      digitalWrite (LED3, LOW);
      digitalWrite (LED4, LOW);
     break;
     case 2:
      digitalWrite (LED1, LOW);
      digitalWrite (LED2, HIGH);
      digitalWrite (LED3, LOW);
      digitalWrite (LED4, LOW);
     break;
     case 3:
      digitalWrite (LED1, LOW);
      digitalWrite (LED2, LOW);
      digitalWrite (LED3, HIGH);
      digitalWrite (LED4, LOW);
     break;
     case 4:
      digitalWrite (LED1, LOW);
      digitalWrite (LED2, LOW);
      digitalWrite (LED3, LOW);
      digitalWrite (LED4, HIGH);
     break;
     default:
      digitalWrite (LED1, LOW);
      digitalWrite (LED2, LOW);
      digitalWrite (LED3, LOW);
      digitalWrite (LED4, LOW);
    }
  }

 if (debounce(StartButton) == HIGH) { 
   //Serial.print("StartButton = ");
   //Serial.println(StartButton_State);
   //digitalWrite(LED2, !LED2);
   StartButton_State = !debounce(StartButton); // Because Pull-up, pushed =!pushed
    if (StartButton_State == HIGH && LastStartButton_State == LOW)  { 
     LastStartButton_State = StartButton_State; // Store LastStartButton_State into StartButton_State.
     StartCharging();
    }
  }




}

/* 
 * *******************************
 * *  Below are some Functions  **
 * *******************************
*/


void StartDelayTimer() {
  //
  // Timer "StartDelay" to manual select output, before automatically start with first port. (time in sec.)
  // 
  unsigned int current_millis = millis();
  if (current_millis >= StartDelay) {  
    StartCharging(); 
  }  else {
   Serial.print("StartDelayTime = ");
   Serial.print(current_millis /1000);
   Serial.println("s");
  }
}


// 
// Multiple PressButton debounce function
//
boolean debounce(int pin)  {
 boolean state;
 boolean previousState;
 previousState = digitalRead(pin);
 for (int counter = 0; counter < 10 ; counter++)  {
   delay(1);   //wait 1 millisecond
   state = digitalRead(pin);   //read the pin
   if (state != previousState)   {
     counter = 0;   //reset the counter
     previousState = state;
    }
  }
 return state;
} 


//
// Force Charging with default setting (starting at port 1) or Seleted one
//
void StartCharging() {
 Serial.print(" dit is de laad cyclus ");
 Serial.println(SelectButton_Count);
 
 if(SelectButton_Count == 0) { 
   SelectButton_Count = 1;
  }

  
 StartCharging();
}