Creating a menu with a 128*64 GLC, keypad and arduino

#include <string.h>
#include<accelstepper.h>
#include <keypad.h>
#include "U8glib.h" 

U8GLIB_ST7920_128X64_1X u8g(12, 11, 10, 13);

//Keypad setup
const byte ROWS = 4;
const byte COLS = 4;
char hexaKeys [ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins [ROWS] = {5, 4, 3, 2}; 
byte colPins [COLS] = {9, 8, 7, 6};
Keypad customKeypad = Keypad (makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);

#define KEY_NONE 0
#define KEY_PREV 1
#define KEY_NEXT 2
#define KEY_SELECT 3 
#define KEY_BACK 4 

uint8_t keycode1 = KEY_NONE;
uint8_t keycode2 = KEY_NONE;
uint8_t keycode = KEY_NONE;

void keymode(void){
  keycode2 = keycode1;
  char keypressed = customKeypad.getKey();
if (keypressed != NO_KEY){
  switch (keypressed){     
 case 'A':
  keycode1 = KEY_PREV;
  Serial.println("A");
  break;
case'B':
  keycode1 = KEY_NEXT;
    Serial.println("B");
  break;
case 'C':
  keycode1 = KEY_SELECT;
    Serial.println("C");
  break;
case 'D':
  keycode1 = KEY_BACK;
    Serial.println("D");
  break;
  }
  if (keypressed != 'A' && keypressed != 'B' && keypressed !='C' && keypressed != 'D') 
  keycode1 = KEY_NONE;
}
 if (keycode2 == keycode1) 
 keycode = keycode1;
 else 
 keycode = KEY_NONE;
}

 #define MENU_ITEMS 4
 const char *menu_strings[MENU_ITEMS] = {"Stepper Speed", "DC Speed", "Distance", "Done"};

 uint8_t menu_current = 0;
 uint8_t menu_redraw = 0;
 uint8_t Last_key_code = KEY_NONE;

 void drawmenu(void){
  uint8_t i, h;
  u8g_uint_t w, d;
  u8g.setFont(u8g_font_6x13);
  u8g.setFontRefHeightText();
  u8g.setFontPosTop();
  
  h = u8g.getFontAscent()-u8g.getFontDescent();
  w = u8g.getWidth();
  for( i = 0; i < MENU_ITEMS; i++ ) {
    d = (w-u8g.getStrWidth(menu_strings[i]))/2;
    u8g.setDefaultForegroundColor();
    if ( i == menu_current ) {
      u8g.drawBox(0, i*h+1, w, h);
      u8g.setDefaultBackgroundColor();
    }
    u8g.drawStr(d, i*h, menu_strings[i]);
  }
 }

 void updatemenu(void){ 
  if (keycode != KEY_NONE && Last_key_code == keycode) { 
    return;
  }
  Last_key_code = keycode;

  switch(keycode){
    case KEY_NEXT:
    menu_current ++;
    if (menu_current >= MENU_ITEMS) 
    menu_current =0;
    menu_redraw = 1;
    
    break;

    case KEY_PREV: 
    if (menu_current ==0) 
    menu_current = MENU_ITEMS;
    menu_current --;
    menu_redraw = 1;
    
    break;
  }
 }

 void setup (){
  Serial.begin(9600);
  menu_redraw = 1;
 }

 void loop (){
  keymode();
  if (menu_redraw !=0) {
    u8g.firstPage();
    do{
      drawmenu();
    }while(u8g.nextPage());
    menu_redraw = 0;
  }
   updatemenu();
 }