#define RED A5
#define GREEN A3
#define BLUE A2

byte Col,Row;
byte button;

byte Keys[5][4] = {
  {4, 3, 2, 1},
  {8, 7, 6, 5},
  {12, 11, 10, 9},
  {16, 15, 14, 13},
  {20, 19, 18, 17}
};

bool checkBtn(){
 for(int i=0;i<5;i++){
    if (!(PINB & (1<<i))){                 //Check if one of the rows is pressed (goes LOW), pins 8 to 12
      Row = i+1;                           //Save Row number that went low
      for(int i=0;i<4;i++){                //Scan columns 1 to 4 (pins 4 to 7)
        PORTD |= (1<<i+4);                 //Turns each column High
          if (PINB & (1<<Row-1)){          //As soon as a row becomes High
            Col = i+1;                     //we know the Col now, save it
            PORTD &= ~(0b11110000);        //Reset pins 4 to 7 to LOW
            button = Keys[Row-1][Col-1];   //identify button number pressed
            delay(250);
            Serial.println(button);
            goto get_out;
          }
        }  
    }
  }
get_out:;
}

void setup() {
  pinMode(RED,OUTPUT);
  digitalWrite(RED,HIGH);  //RED LED, ON when LOW
  pinMode(A4,OUTPUT);
  digitalWrite(A4,HIGH);  //Common Anode Always HIGH
  pinMode(GREEN,OUTPUT);
  digitalWrite(GREEN,HIGH);  //GREEN LED, ON when LOW
  pinMode(BLUE,OUTPUT);
  digitalWrite(BLUE,HIGH);  //BLUE LED, ON when LOW
  pinMode(A0,INPUT);      //Vbat input. 
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, INPUT_PULLUP);
  pinMode(9, INPUT_PULLUP);
  pinMode(10, INPUT_PULLUP);
  pinMode(11, INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, LOW);
Serial.begin(115200);
}

void loop() {
 checkBtn();
 switch(button){
   case 0:

   break;

   case 1:
   digitalWrite(RED,LOW);
   digitalWrite(GREEN,HIGH);
   digitalWrite(BLUE,HIGH);
   break;
   
   case 2:
   digitalWrite(RED,HIGH);
   digitalWrite(GREEN,LOW);
   digitalWrite(BLUE,HIGH);   
   break;
   
   case 3:
   digitalWrite(RED,HIGH);
   digitalWrite(GREEN,HIGH);
   digitalWrite(BLUE,LOW);   
   break;
 }
}