diff --git a/src/main.rs b/src/main.rs
index 79e9192..949743e 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -140,56 +140,68 @@ fn main() -> ! {
         Output::new(peripherals.GPIO15, Level::High, OutputConfig::default()),
     );
 
-    sx1509.init();
+    sx1509.init().unwrap();
+    sx1509.set_pin_dir(15, sx1509::PinDirection::Output, true);
 
-    let mut at_commands = ATCommands::new(sim_rst, sim_pwr_key, uart);
+    let mut is_high = true;
 
-    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)
-        },
-    );
+    let delay = Delay::new();
 
     loop {
-        state_mgr.update();
-        state_mgr.draw();
+        delay.delay_millis(500);
+        is_high = !is_high;
+        sx1509.write_pin(15, is_high);
+        log::info!("Pin: {}", is_high)
     }
 
+    // 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
 }
 
diff --git a/src/sx1509.rs b/src/sx1509.rs
index 342cb52..85b8a8c 100644
--- a/src/sx1509.rs
+++ b/src/sx1509.rs
@@ -1,11 +1,24 @@
 use esp_hal::{Blocking, delay::Delay, gpio::Output, i2c::master::I2c};
 
 pub enum Register {
+    RegPullUpB = 0x6,
+    RegPullDownB = 0x8,
+    RegDirB = 0x0E,
+    RegDataB = 0x10,
+
     RegInterruptMaskA = 0x13,
 
     RegMisc = 0x1F,
 }
 
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+pub enum PinDirection {
+    Output,
+    AnalogOutput,
+    Input,
+    InputPullUp,
+}
+
 pub struct Sx1509<'d> {
     i2c: I2c<'d, Blocking>,
     rst: Output<'d>,
@@ -74,4 +87,66 @@ impl<'d> Sx1509<'d> {
             .unwrap();
         ((buffer[0] as u16) << 8) | (buffer[1] as u16)
     }
+
+    pub fn set_pin_dir(&mut self, pin: u8, dir: PinDirection, initial_level: bool) {
+        let mut is_input = false;
+        if dir == PinDirection::Output || dir == PinDirection::AnalogOutput {
+            let mut temp_reg_data = self.read_16(Register::RegDataB);
+            if !initial_level {
+                temp_reg_data &= !(1 << pin);
+                self.write_16(Register::RegDataB, temp_reg_data);
+            }
+        } else {
+            is_input = true;
+        }
+
+        let mut temp_reg_data = self.read_16(Register::RegDirB);
+        if !is_input {
+            temp_reg_data |= 1 << pin;
+        } else {
+            temp_reg_data &= !(1 << pin);
+        }
+
+        self.write_16(Register::RegDirB, temp_reg_data);
+
+        if dir == PinDirection::InputPullUp {
+            self.write_pin(pin, true);
+        }
+
+        if dir == PinDirection::AnalogOutput {
+            self.led_driver_init(pin);
+        }
+    }
+
+    pub fn write_pin(&mut self, pin: u8, high: bool) {
+        let temp_reg_dir = self.read_16(Register::RegDirB);
+        if ((0xFFFF ^ temp_reg_dir) & (1 << pin)) != 0 {
+            // Pin is an output, write high/low
+            let mut temp_reg_data = self.read_16(Register::RegDataB);
+            if high {
+                temp_reg_data |= 1 << pin;
+            } else {
+                temp_reg_data &= !(1 << pin);
+            }
+            self.write_16(Register::RegDataB, temp_reg_data);
+        } else {
+            // Pin is an input, pull-up/down
+            let mut temp_pull_up = self.read_16(Register::RegPullUpB);
+            let mut temp_pull_down = self.read_16(Register::RegPullDownB);
+
+            if high {
+                temp_pull_up |= 1 << pin;
+                temp_pull_down &= !(1 << pin);
+            } else {
+                temp_pull_down |= 1 << pin;
+                temp_pull_up &= !(1 << pin);
+            }
+            self.write_16(Register::RegPullUpB, temp_pull_up);
+            self.write_16(Register::RegPullDownB, temp_pull_down);
+        }
+    }
+
+    pub fn led_driver_init(&mut self, pin: u8) {
+        todo!()
+    }
 }