// Buttons
const int buttonFirstRow = A0;
const int buttonSecondRow = A1;
const int buttonAdd = A2;
const int buttonSub = A3;
const int buttonXXX = A4;

const byte debounceLevel = 5;

// LED Matrix
const int driverFirstRow = 7;
const int driverSecondRow = 6;
const int driverResultRow = 5;

const int driverBit0Column = 8;
const int driverBit1Column = 9;
const int driverBit2Column = 10;
const int driverBit3Column = 11;
const int driverBit4Column = 12;

// Calculator Variables
byte FirstValue = 0;
byte SecondValue = 0;
byte ResultValue = 0;

// Matrix Variables
byte ActiveRow = 0;  // 0 = FirstRow, 1 = SecondRow, 2 = ResultRow

// Timer Variables
unsigned long currentMillis;
unsigned long previousMillis;

// Button debounce
byte buttonFirstRowDebounceCounter = 0;
byte buttonSecondRowDebounceCounter = 0;
byte buttonAddDebounceCounter = 0;
byte buttonSubDebounceCounter = 0;
byte buttonXXXDebounceCounter = 0;

// Button States
bool buttonFirstRowPressed = false;
bool buttonSecondRowPressed = false;
bool buttonAddPressed = false;
bool buttonSubPressed = false;
bool buttonXXXPressed = false;

//
unsigned int startupCounter = 1;

//
// Setup Routine (started o)
// 

void setup() {
  // Setup Buttons
  pinMode(buttonFirstRow, INPUT_PULLUP);
  pinMode(buttonSecondRow, INPUT_PULLUP);
  pinMode(buttonAdd, INPUT_PULLUP);
  pinMode(buttonSub, INPUT_PULLUP);
  pinMode(buttonXXX, INPUT_PULLUP);

  // Setup LED Matrix
  pinMode(driverFirstRow, OUTPUT);
  digitalWrite(driverFirstRow, LOW);
  pinMode(driverSecondRow, OUTPUT);
  digitalWrite(driverSecondRow, LOW);
  pinMode(driverResultRow, OUTPUT);
  digitalWrite(driverResultRow, LOW);
  pinMode(driverBit0Column, OUTPUT);
  digitalWrite(driverBit0Column, LOW);
  pinMode(driverBit1Column, OUTPUT);
  digitalWrite(driverBit1Column, LOW);
  pinMode(driverBit2Column, OUTPUT);
  digitalWrite(driverBit2Column, LOW);
  pinMode(driverBit3Column, OUTPUT);
  digitalWrite(driverBit3Column, LOW);
  pinMode(driverBit4Column, OUTPUT);
  digitalWrite(driverBit4Column, LOW);

  // save current Timestamp to start first Task in 10ms
  previousMillis = millis(); 
}

//
// Main Program (Loop)
//

void loop() {
  // get current Timestamp
  currentMillis = millis(); 

  // Enter this Statement if 10ms difference between both timestamps
  if ((unsigned long)(currentMillis - previousMillis) >= 3) {

    if (startupCounter > 0) {
      
      StartupAnimationTask();
    } else {
      // Check if any button is pressed
      ButtonTask();
    }

    // Update LED Matrix
    LEDMatrixTask();    

    // save current Timestamp to trigger next Task in 10ms
    previousMillis = currentMillis;
  }
}

//
// SubRoutines
//

void ButtonTask() {
  //
  ReadButtonsDebounced();

  if (buttonFirstRowPressed) {
    buttonFirstRowPressed = false;
    FirstValue++;
    if (FirstValue > 0x0F) {
      FirstValue = 0;
    }
  } 

  if (buttonSecondRowPressed) {
    buttonSecondRowPressed = false;
    SecondValue++;
    if (SecondValue > 0x0F) {
      SecondValue = 0;
    }
  } 

  if (buttonAddPressed) {
    buttonAddPressed = false;
    ResultValue = FirstValue + SecondValue;
  }

  if (buttonSubPressed) {
    buttonSubPressed = false;
    ResultValue = FirstValue - SecondValue;
  }

  if (buttonXXXPressed) {
    buttonXXXPressed = false;
    // Program here your own function
    ResultValue = FirstValue xor SecondValue;
  }
}

void ReadButtonsDebounced() {
  // First Row Button
  if (digitalRead(buttonFirstRow) == LOW) {
    if (buttonFirstRowDebounceCounter < debounceLevel) {
      buttonFirstRowDebounceCounter++;
    }
  } else {
    if (buttonFirstRowDebounceCounter >= debounceLevel) {
      buttonFirstRowPressed = true;
    }
    buttonFirstRowDebounceCounter = 0;
  }

  // Second Row Button
  if (digitalRead(buttonSecondRow) == LOW) {
    if (buttonSecondRowDebounceCounter < debounceLevel) {
      buttonSecondRowDebounceCounter++;
    }
  } else {
    if (buttonSecondRowDebounceCounter >= debounceLevel) {
      buttonSecondRowPressed = true;
    }
    buttonSecondRowDebounceCounter = 0;
  }

  // Add Button
  if (digitalRead(buttonAdd) == LOW) {
    if (buttonAddDebounceCounter < debounceLevel) {
      buttonAddDebounceCounter++;
    }
  } else {
    if (buttonAddDebounceCounter >= debounceLevel) {
      buttonAddPressed = true;
    }
    buttonAddDebounceCounter = 0;
  }

  // Sub Button
  if (digitalRead(buttonSub) == LOW) {
    if (buttonSubDebounceCounter < debounceLevel) {
      buttonSubDebounceCounter++;
    }
  } else {
    if (buttonSubDebounceCounter >= debounceLevel) {
      buttonSubPressed = true;
    }
    buttonSubDebounceCounter = 0;
  }

  // XXX Button
  if (digitalRead(buttonXXX) == LOW) {
    if (buttonXXXDebounceCounter < debounceLevel) {
      buttonXXXDebounceCounter++;
    }
  } else {
    if (buttonXXXDebounceCounter >= debounceLevel) {
      buttonXXXPressed = true;
    }
    buttonXXXDebounceCounter = 0;
  }
}

void LEDMatrixTask() {
  if (ActiveRow == 0) {
    DisplayValueInRow(driverFirstRow, FirstValue);
    ActiveRow = 1;
  } else if (ActiveRow == 1) {
    DisplayValueInRow(driverSecondRow, SecondValue);    
    ActiveRow = 2;
  } else {
    DisplayValueInRow(driverResultRow, ResultValue);
    ActiveRow = 0;
  }
}

void DisplayValueInRow(int row, byte value) {
  // disable all rows
  digitalWrite(driverFirstRow, LOW);
  digitalWrite(driverSecondRow, LOW);
  digitalWrite(driverResultRow, LOW);
  
  // set new value
  if (bitRead(value,0) == 1) {
    digitalWrite(driverBit0Column, HIGH);
  } else {
    digitalWrite(driverBit0Column, LOW);
  }
  if (bitRead(value,1) == 1) {
    digitalWrite(driverBit1Column, HIGH);
  } else {
    digitalWrite(driverBit1Column, LOW);
  }
  if (bitRead(value,2) == 1) {
    digitalWrite(driverBit2Column, HIGH);
  } else {
    digitalWrite(driverBit2Column, LOW);
  }
  if (bitRead(value,3) == 1) {
    digitalWrite(driverBit3Column, HIGH);
  } else {
    digitalWrite(driverBit3Column, LOW);
  }
  if (bitRead(value,4) == 1) {
    digitalWrite(driverBit4Column, HIGH);
  } else {
    digitalWrite(driverBit4Column, LOW);
  }
  
  // enable row
  digitalWrite(row, HIGH);
}

void StartupAnimation() {
  startupCounter++;

  if (startupCounter < 150) {
    FirstValue = 15;
    SecondValue = 0;
    ResultValue = 0;
  } else if (startupCounter < 300) {
    FirstValue = 0;
    SecondValue = 15;
    ResultValue = 0;
  } else if (startupCounter < 450) {
    FirstValue = 0;
    SecondValue = 0;
    ResultValue = 31;
  } else if (startupCounter < 600) {
    FirstValue = 1;
    SecondValue = 1;
    ResultValue = 1;
  } else if (startupCounter < 750) {
    FirstValue = 2;
    SecondValue = 2;
    ResultValue = 2;
  } else if (startupCounter < 900) {
    FirstValue = 4;
    SecondValue = 4;
    ResultValue = 4;
  } else if (startupCounter < 1050) {
    FirstValue = 8;
    SecondValue = 8;
    ResultValue = 8;
  } else if (startupCounter < 1200) {
    FirstValue = 0;
    SecondValue = 0;
    ResultValue = 16;
  } else if (startupCounter < 1350) {
    FirstValue = 0;
    SecondValue = 0;
    ResultValue = 0;
    startupCounter = 0;
  }
}