diff --git a/src/kernel/thread_manager.rs b/src/kernel/thread_manager.rs
index b8f19d3..da4518e 100644
--- a/src/kernel/thread_manager.rs
+++ b/src/kernel/thread_manager.rs
@@ -69,25 +69,21 @@ impl ThreadManager {
     /// 
     /// **next_thread** thread to dispatch to the CPU
     pub fn switch_to(&mut self, machine: &mut Machine, next_thread: Rc<RefCell<Thread>>) {
-        match self.get_g_current_thread() {
-            Some(old) => {
-                let old1 = Rc::clone(old);
-                let old2 = Rc::clone(old);
-                self.thread_save_processor_state(machine, old1);
-                // old_thread.save_simulator_state();
-                if old2 != next_thread {
-                    self.debug(format!("switching from \"{}\" to \"{}\"", old2.borrow().get_name(), next_thread.borrow().get_name()));
-                    self.thread_restore_processor_state(machine, Rc::clone(&next_thread));
-                    // next_thread.restore_simulator_state();
-                    debug_assert!(!self.ready_list.contains(&next_thread));
-                    self.set_g_current_thread(Some(next_thread));
-                }
-            },
-            None => {
+        if let Some(old_thread) = self.get_g_current_thread() {
+            let old_thread = old_thread.clone();
+            self.thread_save_processor_state(machine, old_thread.clone());
+            // old_thread.save_simulator_state();
+            if old_thread != next_thread {
+                self.debug(format!("switching from \"{}\" to \"{}\"", old_thread.borrow().get_name(), next_thread.borrow().get_name()));
                 self.thread_restore_processor_state(machine, Rc::clone(&next_thread));
                 // next_thread.restore_simulator_state();
+                debug_assert!(!self.ready_list.contains(&next_thread));
                 self.set_g_current_thread(Some(next_thread));
             }
+        } else {
+            self.thread_restore_processor_state(machine, Rc::clone(&next_thread));
+            // next_thread.restore_simulator_state();
+            self.set_g_current_thread(Some(next_thread));
         }
 
         if let Some(th) = self.get_g_thread_to_be_destroyed() {
@@ -122,7 +118,6 @@ impl ThreadManager {
     /// Cannot use yield as a function name -> reserved name in rust
     pub fn thread_yield(&mut self, machine: &mut Machine, thread: Rc<RefCell<Thread>>) {
         let old_status = machine.interrupt.set_status(crate::simulator::interrupt::InterruptStatus::InterruptOff);
-            
         
         self.debug(format!("Yeilding thread: {}", thread.borrow().get_name()));
         debug_assert_eq!(&Option::Some(Rc::clone(&thread)), self.get_g_current_thread());
@@ -192,26 +187,22 @@ impl ThreadManager {
     /// - *id_sema* id of the semaphore, stored in [`ObjAddr`], id given by user program thought exceptions
     /// - *machine* Current state of the machine
     pub fn sem_p(&mut self, id_sema: i32, machine: &mut Machine) -> Result<MachineOk, MachineError> {
-        match self.get_g_current_thread() {
-            Some(thread) => {
-                let rc1_thread = Rc::clone(thread);
-                let rc2_thread = Rc::clone(thread);
-                let sema = self.get_obj_addrs().search_semaphore(id_sema as i32);
-                if let Some(sema) = sema {
-                    let old_status = machine.interrupt.set_status(InterruptStatus::InterruptOff);
-                    sema.counter -= 1;
-                    if sema.counter < 0 {
-                        sema.waiting_queue.push(rc1_thread);
-                        self.thread_sleep(machine, rc2_thread);
-                    }
-                    machine.interrupt.set_status(old_status);
-                    Ok(MachineOk::Ok)
-                } else {
-                    Err("Cannot find semaphore")?
-                }
-            },
-            None => unreachable!("Current thread should not be None")
-        }   
+        let old_status = machine.interrupt.set_status(InterruptStatus::InterruptOff);
+        let thread = match self.get_g_current_thread() {
+            Some(thread) => Rc::clone(thread),
+            None => Err("sem_p error: current thread should not be None")?
+        };
+        let sema = match self.get_obj_addrs().search_semaphore(id_sema) {
+            Some(sema) => sema,
+            None => Err("sem_p error: cannot find semaphore")?
+        };
+        sema.counter -= 1;
+        if sema.counter < 0 {
+            sema.waiting_queue.push(thread.clone());
+            self.thread_sleep(machine, thread);
+        }
+        machine.interrupt.set_status(old_status);
+        Ok(MachineOk::Ok)
     }
 
     /// Increment semaphore value, waking up a waiting thread if any.
@@ -225,23 +216,18 @@ impl ThreadManager {
     /// - *id_sema* id of the semaphore, stored in [`ObjAddr`], id given by user program thought exceptions
     /// - **machine** the machine where the threads are executed
     pub fn sem_v(&mut self, id_sema: i32, machine: &mut Machine) -> Result<MachineOk, MachineError> {
-        let sema = self.get_obj_addrs().search_semaphore(id_sema as i32);
-        match sema {
-            Some(sema) => {
-                let old_status = machine.interrupt.set_status(InterruptStatus::InterruptOff);
-                sema.counter += 1;
-                match sema.waiting_queue.pop() {
-                    Some(thread) => self.ready_to_run(thread),
-                    None => ()
-                }
-                machine.interrupt.set_status(old_status);
-                Ok(MachineOk::Ok)
-            },
-            None => {
-                Err("Cannot find semaphore")?
-            }
+        let sema = match self.get_obj_addrs().search_semaphore(id_sema) {
+            Some(sema) => sema,
+            None => Err("sem_v error: cannot find semaphore")?
+        };
+        let old_status = machine.interrupt.set_status(InterruptStatus::InterruptOff);
+        sema.counter += 1;
+        match sema.waiting_queue.pop() {
+            Some(thread) => self.ready_to_run(thread),
+            None => ()
         }
-        
+        machine.interrupt.set_status(old_status);
+        Ok(MachineOk::Ok)
     }