diff --git a/src/lib.rs b/src/lib.rs
index d96d55e..77e98c7 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -4,6 +4,7 @@ use core::marker::PhantomData;
 
 use arduino_hal::{
     delay_us,
+    hal::port::{PB4, PB5, PE6},
     port::{
         mode::{Input, PullUp},
         Pin, PinOps,
@@ -13,6 +14,7 @@ use arduino_hal::{
 use avr_device::asm::delay_cycles;
 
 use static_pins::StaticPinOps;
+use ufmt::derive::uDebug;
 
 mod structures;
 
@@ -24,15 +26,18 @@ const SERIAL_DELAY: u32 = 4;
 const FIRST_HALF_SERIAL_DELAY: u32 = SERIAL_DELAY / 2;
 const SECOND_HALF_SERIAL_DELAY: u32 = SERIAL_DELAY - FIRST_HALF_SERIAL_DELAY;
 
-const READING_ADJUST: u32 = 16;
-const FIRST_ENTRY_READING: u32 = 30;
-const MSB: u16 = 0x0200;
+const READING_ADJUST: u32 = 10;
+const TX_DELAY_CYCLES: u32 = 22;
+const MSB: u16 = 0x0800;
+const PACKET_MASK: u16 = 0x0FFF;
 
 #[inline(always)]
 fn crc_calculate(value: u8) -> u8 {
     ((value >> 6) | (value >> 4) | (value >> 2) | value) & 0x0F
 }
 
+#[cfg(feature = "ufmt")]
+#[derive(uDebug)]
 pub enum PollError {
     NotFound,
     NotReady,
@@ -42,40 +47,81 @@ pub struct HalfDuplexSerial<P> {
     _pin: PhantomData<Pin<Input<PullUp>, P>>,
 }
 
+#[cfg(debug_assertions)]
+#[inline(always)]
+fn debug_pb4_low() {
+    PB4::set_low();
+}
+
+#[cfg(debug_assertions)]
+#[inline(always)]
+fn debug_pb4_high() {
+    PB4::set_high();
+}
+
+#[allow(dead_code)]
+#[cfg(debug_assertions)]
+#[inline(always)]
+fn debug_pb4_pulse_positive() {
+    debug_pb4_low();
+    debug_pb4_high();
+}
+
+#[allow(dead_code)]
+#[cfg(debug_assertions)]
+#[inline(always)]
+fn debug_pb4_pulse_negative() {
+    debug_pb4_high();
+    debug_pb4_low();
+}
+
 impl<P> HalfDuplexSerial<P>
 where
     P: PinOps + StaticPinOps,
 {
-    #[inline]
     pub fn new(_pin: Pin<Input<PullUp>, P>) -> Self {
+        if cfg!(debug_assertions) {
+            PB4::into_output_high();
+            PB5::into_output_high();
+            PE6::into_output_high();
+        }
         Self {
             _pin: PhantomData {},
         }
     }
 
     pub fn poll(&self) -> PollResult {
+        PE6::set_low();
         P::into_output();
-        delay_cycles(4);
+
+        delay_cycles(2);
+
         P::into_pull_up_input();
 
-        delay_us(SERIAL_DELAY);
+        delay_us(SERIAL_DELAY + FIRST_HALF_SERIAL_DELAY);
 
         if P::is_low() {
+            PE6::set_high();
             return PollResult::Err(PollError::NotFound);
         }
 
         delay_us(SERIAL_DELAY);
 
         if P::is_high() {
+            PE6::set_high();
             return PollResult::Err(PollError::NotReady);
         }
 
         while P::is_low() {}
 
+        PE6::set_high();
+
         PollResult::Ok(())
     }
 
     pub fn response(&self) {
+        PE6::set_low();
+
         P::into_output_high();
 
         delay_us(FIRST_HALF_SERIAL_DELAY);
@@ -84,23 +130,42 @@ where
 
         delay_us(SERIAL_DELAY);
 
+        P::set_high();
         P::into_pull_up_input();
+
+        PE6::set_high();
     }
 
-    #[inline(never)]
+    #[inline]
     pub fn sync_transmitter(&self) {
+        if cfg!(debug_assertions) {
+            PB5::set_low();
+        }
+
         P::into_output();
 
         delay_us(SERIAL_DELAY);
 
         P::set_high();
+
+        if cfg!(debug_assertions) {
+            PB5::set_high();
+        }
     }
 
-    #[inline(never)]
+    #[inline]
     pub fn sync_reciever(&self) {
+        if cfg!(debug_assertions) {
+            PB5::set_low();
+        }
+
         while P::is_high() {}
 
         while P::is_low() {}
+
+        if cfg!(debug_assertions) {
+            PB5::set_high();
+        }
     }
 
     #[inline]
@@ -120,7 +185,7 @@ pub trait SoftSerialReader<P, T>: SoftSerialByteReader<P>
 where
     P: PinOps + StaticPinOps,
 {
-    fn read_bytes(&self, recieve_data: T);
+    fn read_bytes(&self, recieve_data: T) -> Result<(), CorruptedData>;
 }
 
 pub trait SoftSerialByteWriter<P>
@@ -132,16 +197,22 @@ where
         let mut data = ((transmit_data as u16) << 4) | (crc_calculate(transmit_data) as u16);
 
         for _ in 0..(u8::BITS + 4) {
+            debug_pb4_low();
+
             if data & MSB == 0 {
                 P::set_high();
             } else {
                 P::set_low();
             }
 
+            data = (data << 1) & PACKET_MASK;
             delay_us(SERIAL_DELAY);
 
-            data <<= 1;
+            debug_pb4_high();
         }
+
+        P::set_high();
+        delay_cycles(TX_DELAY_CYCLES);
     }
 }
 
@@ -153,24 +224,25 @@ where
     fn read_byte(&self) -> ReadByteResult {
         let mut packet = 0u16;
 
-        delay_cycles(FIRST_ENTRY_READING);
         for _ in 0..(u8::BITS + 4) {
-            delay_us(FIRST_HALF_SERIAL_DELAY);
+            debug_pb4_low();
 
-            packet <<= 1;
+            delay_us(FIRST_HALF_SERIAL_DELAY);
+            delay_cycles(READING_ADJUST);
 
             if P::is_low() {
-                packet |= 1;
+                packet = (packet << 1) | 1;
             } else {
-                packet |= 0;
+                packet <<= 1;
             }
 
-            delay_cycles(READING_ADJUST);
             delay_us(SECOND_HALF_SERIAL_DELAY);
+
+            debug_pb4_high();
         }
 
+        let data = (packet >> 4) as u8;
         let received_crc = (packet & 0x000F) as u8;
-        let data = packet.overflowing_shr(4).0 as u8;
         let calculated_crc = crc_calculate(data);
 
         if received_crc != calculated_crc {
@@ -200,12 +272,11 @@ impl<P> SoftSerialReader<P, &mut [u8]> for HalfDuplexSerial<P>
 where
     P: PinOps + StaticPinOps,
 {
-    fn read_bytes(&self, recieve_data: &mut [u8]) {
+    fn read_bytes(&self, recieve_data: &mut [u8]) -> Result<(), CorruptedData> {
         for byte in recieve_data {
-            if let Ok(data) = self.read_byte() {
-                *byte = data;
-            }
+            *byte = self.read_byte()?;
             self.sync_reciever();
         }
+        Ok(())
     }
 }