diff --git a/src/main.rs b/src/main.rs
index 0c8125e..bbf9f92 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -10,7 +10,7 @@
 
 use core::mem::MaybeUninit;
 
-use alloc::boxed::Box;
+use alloc::{boxed::Box, vec::Vec};
 use esp_hal::{
     clock::CpuClock,
     delay::Delay,
@@ -141,79 +141,110 @@ fn main() -> ! {
     );
 
     sx1509.init().unwrap();
-    sx1509.keypad(3, 4, 256, 2, 1);
+    sx1509.keypad(4, 4, 256, 2, 1);
 
-    let delay = Delay::new();
+    let mut at_commands = ATCommands::new(sim_rst, sim_pwr_key, uart);
 
-    let keypad = [
-        ['2', '1', '3'],
-        ['0', '*', '#'],
-        ['8', '7', '9'],
-        ['5', '4', '6'],
-    ];
+    let font_renderer = FontRenderer::create(30);
+    display.clear(Rgb565::black().as_color());
+    font_renderer.render(
+        &mut display,
+        "Please wait",
+        Position::new(120, 120),
+        font::HorizontalAlignment::Center,
+        font::VerticalAlignment::Center,
+        Rgb565::black(),
+        Rgb565::white(),
+    );
+
+    at_commands.init();
+
+    let async_io = AsyncIO::default();
+
+    let mut state_mgr = StateManager {
+        data: state::StateData::from(display, async_io.clone()),
+        curr_state: Box::new(InitATState::default()),
+    };
+
+    static mut THREAD_2_STACK: Stack<{ 30 * 1024 }> = esp_hal::system::Stack {
+        mem: MaybeUninit::new([0u8; 30 * 1024]),
+    };
+
+    let timg0 = TimerGroup::new(peripherals.TIMG0);
+    let software_interrupt = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
+    esp_rtos::start(timg0.timer0, software_interrupt.software_interrupt0);
+    esp_rtos::start_second_core(
+        peripherals.CPU_CTRL,
+        software_interrupt.software_interrupt1,
+        unsafe {
+            #[allow(static_mut_refs)]
+            &mut THREAD_2_STACK
+        },
+        {
+            let io = async_io.clone();
+            || thread_2_main(io, at_commands, sx1509)
+        },
+    );
 
     loop {
-        delay.delay_millis(500);
-        let keypad_data = sx1509.read_keypad();
-        let row = bit_representation_to_number(((keypad_data & 0xFF00) >> 8) as u8) as usize;
-        let col = bit_representation_to_number((keypad_data & 0xFF) as u8) as usize;
-        if row > 0 && col > 0 {
-            log::info!("Key data: {}", keypad[row - 1][col - 1]);
-        }
+        state_mgr.update();
+        state_mgr.draw();
     }
-
-    // let mut at_commands = ATCommands::new(sim_rst, sim_pwr_key, uart);
-
-    // let font_renderer = FontRenderer::create(30);
-    // display.clear(Rgb565::black().as_color());
-    // font_renderer.render(
-    //     &mut display,
-    //     "Please wait",
-    //     Position::new(120, 120),
-    //     font::HorizontalAlignment::Center,
-    //     font::VerticalAlignment::Center,
-    //     Rgb565::black(),
-    //     Rgb565::white(),
-    // );
-
-    // at_commands.init();
-
-    // let async_io = AsyncIO::default();
-
-    // let mut state_mgr = StateManager {
-    //     data: state::StateData::from(display, async_io.clone()),
-    //     curr_state: Box::new(InitATState::default()),
-    // };
-
-    // static mut THREAD_2_STACK: Stack<{ 30 * 1024 }> = esp_hal::system::Stack {
-    //     mem: MaybeUninit::new([0u8; 30 * 1024]),
-    // };
-
-    // let timg0 = TimerGroup::new(peripherals.TIMG0);
-    // let software_interrupt = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
-    // esp_rtos::start(timg0.timer0, software_interrupt.software_interrupt0);
-    // esp_rtos::start_second_core(
-    //     peripherals.CPU_CTRL,
-    //     software_interrupt.software_interrupt1,
-    //     unsafe {
-    //         #[allow(static_mut_refs)]
-    //         &mut THREAD_2_STACK
-    //     },
-    //     {
-    //         let io = async_io.clone();
-    //         || thread_2_main(io, at_commands)
-    //     },
-    // );
-
-    // loop {
-    //     state_mgr.update();
-    //     state_mgr.draw();
-    // }
-
-    // for inspiration have a look at the examples at https://github.com/esp-rs/esp-hal/tree/esp-hal-v1.1.0/examples
 }
 
-fn thread_2_main(async_io: AsyncIO, mut at_commands: ATCommands<'static, 'static>) {
+#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
+pub enum KeypadButton {
+    Keypad1,
+    Keypad2,
+    Keypad3,
+    Keypad4,
+    Keypad5,
+    Keypad6,
+    Keypad7,
+    Keypad8,
+    Keypad9,
+    KeypadStar,
+    KeypadHash,
+    KeypadA,
+    KeypadB,
+    KeypadC,
+    KeypadD,
+}
+
+static KEYPAD_BUTTONS: [[KeypadButton; 4]; 4] = [
+    [
+        KeypadButton::Keypad1,
+        KeypadButton::Keypad2,
+        KeypadButton::Keypad3,
+        KeypadButton::KeypadA,
+    ],
+    [
+        KeypadButton::Keypad4,
+        KeypadButton::Keypad5,
+        KeypadButton::Keypad6,
+        KeypadButton::KeypadB,
+    ],
+    [
+        KeypadButton::Keypad7,
+        KeypadButton::Keypad8,
+        KeypadButton::Keypad9,
+        KeypadButton::KeypadC,
+    ],
+    [
+        KeypadButton::KeypadStar,
+        KeypadButton::Keypad9,
+        KeypadButton::KeypadHash,
+        KeypadButton::KeypadD,
+    ],
+];
+
+fn thread_2_main(
+    async_io: AsyncIO,
+    mut at_commands: ATCommands<'static, 'static>,
+    mut sx1509: Sx1509<'static>,
+) {
+    let delay = Delay::new();
+
     loop {
         if let Some(command) = unsafe { async_io.check_at_command() } {
             let response = match command {
@@ -228,6 +259,28 @@ fn thread_2_main(async_io: AsyncIO, mut at_commands: ATCommands<'static, 'static
         if let Some(event) = at_commands.readline() {
             log::info!("Event: {}", event);
         }
+
+        let mut buttons_pressed = Vec::new();
+
+        loop {
+            let keypad_data = sx1509.read_keypad();
+            let col = bit_representation_to_number(((keypad_data & 0xFF00) >> 8) as u8) as usize;
+            let row = bit_representation_to_number((keypad_data & 0xFF) as u8) as usize;
+            if row == 0 || col == 0 {
+                break;
+            }
+            if row > 4 || col > 4 {
+                break;
+            }
+            let button = KEYPAD_BUTTONS[col - 1][row - 1];
+            if buttons_pressed.contains(&button) {
+                break;
+            }
+            buttons_pressed.push(button);
+            delay.delay_millis(5);
+        }
+
+        log::info!("Pressed: {:?}", buttons_pressed);
     }
 }