Arduino 4x4 key matrix read

keymatrixscan.ino

int KO[4]={5,6,7,8};
int KI[4]={9,10,11,12};

void setup() {
  pinMode(KI[0],INPUT_PULLUP);
  pinMode(KI[1],INPUT_PULLUP);
  pinMode(KI[2],INPUT_PULLUP);
  pinMode(KI[3],INPUT_PULLUP);
  pinMode(KO[0],OUTPUT);
  pinMode(KO[1],OUTPUT);
  pinMode(KO[2],OUTPUT);
  pinMode(KO[3],OUTPUT);
  Serial.begin(9600);
  keyscan_init();
}

void loop() {
  int a;
  a=keyscan();
  if (a>=0) Serial.println(a);
}


void keyscan_init() {
  digitalWrite(KO[0],HIGH);
  digitalWrite(KO[1],HIGH);
  digitalWrite(KO[2],HIGH);
  digitalWrite(KO[3],HIGH);
}

int keyscan() { // pressed 0-15 , not pressed -1
  int i,j;
  int n=-1;
  for(i=0;i<4;i++) {
    for(j=0;j<4;j++) {
       if (i==j) {
          digitalWrite(KO[j],LOW);
       } else {
         digitalWrite(KO[j],HIGH);
       }
    }
    for(j=0;j<4;j++) {
      if (n<0 && digitalRead(KI[j])==LOW) {
        n=i+j*4;
      }
      delay(1); // wait for 1ms
    }
  }
  keyscan_init();
  return n;
}