@@ -207,0 +212,4 @@
+    
+    /// Get a mutable reference to a call by index.
+    /// Only available in tests to allow setting call state without exposing internals.
+    #[cfg(test)]

@@ -392,0 +410,4 @@
+    /// Set the current floor for _this_ elevator.
+    /// Only available in tests to avoid exposing this as a general interface.
+    #[cfg(test)]
+    pub fn set_current_floor_for_test(&mut self, floor: Option<Destination>) {

@@ -392,0 +419,4 @@
+
+    /// Set the direction for _this_ elevator.
+    /// Only available in tests to avoid exposing this as a general interface.
+    #[cfg(test)]

@@ -461,0 +509,4 @@
+
+        // Check for unassigned/new
+        for call in calls {
+            if call.get_call_state() != CallState::New {

@@ -612,0 +650,4 @@
+    /// Looks through all calls with CallState New and returns the destination of the closest one in the correct direction
+    /// All calls are considered if the elevator is Idle
+    /// Returns None if there is no calls
+    pub fn get_next_destination(&self) -> Option<Destination> {

@@ -612,0 +669,4 @@
+            // Filter by direction
+            // Elevator moving up, skip calls below current floor
+            // Elevator moving down, skip calls above current floor
+            if ((elevator_direction == Some(Direction::Up)) && (elevator_current_floor >= call_destination))

@@ -834,1 +911,4 @@
 
+    /// Create a SystemState with no calls
+    /// Used for get_next_destination tests
+    fn create_empty_system_state() -> (SystemState, SystemStateChannelHandles) {

@@ -612,0 +660,4 @@
+
+        // Check for unassigned/new
+        for call in calls {
+            if call.get_call_state() != CallState::New {

@@ -612,0 +676,4 @@
+
+            // Check distance
+            let distance = call_destination.abs_diff(elevator_current_floor);
+            new_calls.push((call.get_call_id(), distance, call.get_direction(), call.get_destination()));

@@ -76,1 +76,4 @@
     }
+
+    /// Function takes the floor as a Destination and returns a u8
+    pub fn floor_number(&self) -> u8 {

@@ -77,0 +77,4 @@
+
+    /// Function takes the floor as a Destination and returns a u8
+    pub fn floor_number(&self) -> u8 {
+        *self as u8 + 1

@@ -462,2 +474,4 @@
     }
 
+    /// Get the [HashMap] with [ElevatorInfo] as a reference
+    pub fn get_elevator_hashmap_as_ref(&self) -> &HashMap<ElevatorId, ElevatorInfo> {

@@ -155,3 +156,3 @@
 
     // Busy wait until there is a call to service
-    let mut num_calls_to_service: usize =
+    /*let mut num_calls_to_service: usize =

@@ -186,0 +203,4 @@
+                state.elevators.close_door();
+            }
+            
+            tokio::time::sleep(Duration::from_millis(100)).await;

@@ -297,1 +315,3 @@
-        tokio::select! {
+
+        let result = get_floor_destination(state.clone()).await;
+        assert_eq!(result, None);

@@ -298,0 +316,4 @@
+        let result = get_floor_destination(state.clone()).await;
+        assert_eq!(result, None);
+
+        /*tokio::select! {

@@ -28,3 +28,3 @@
     /// testing the "real" recv data did not return consistently
     async fn recv(sync_notification_rx: &mut mpsc::UnboundedReceiver<()>) {
-        loop {
+        sync_notification_rx.recv().await;

@@ -30,1 +30,3 @@
-        loop {
+        sync_notification_rx.recv().await;
+
+        /*loop {

@@ -673,0 +709,4 @@
+    /// Returns None if there is no calls
+    pub fn get_next_destination(&self) -> (Option<Destination>, Option<Direction>) {
+        /// One single applicable call, with necessary values to calculate next destination
+        struct ApplicableCalls {

@@ -671,2 +704,4 @@
     }
 
+    /// Get the elevator's next [Destination]
+    /// Looks through all calls with CallState New and returns the destination of the closest one in th     e correct direction

@@ -673,0 +707,4 @@
+    /// Looks through all calls with CallState New and returns the destination of the closest one in th     e correct direction
+    /// All calls are considered if the elevator is Idle
+    /// Returns None if there is no calls
+    pub fn get_next_destination(&self) -> (Option<Destination>, Option<Direction>) {

@@ -237,2 +243,4 @@
     /// The timestamp of the last update from this elevator
     last_seen: ElevatorTime,
+    /// The ID of the current call of the elevator
+    current_call_id: Option<SmallUid>,

@@ -141,2 +141,3 @@
 /// Returns the [`Destination`]
-async fn get_floor_destination(system_state: Arc<RwLock<SystemState>>) -> Destination {
+async fn get_floor_destination(system_state: Arc<RwLock<SystemState>>) -> Option<Destination> {
+    debug!("get_floor_destination called");

@@ -155,3 +156,3 @@
 
     // Busy wait until there is a call to service
-    let mut num_calls_to_service: usize =
+    /*let mut num_calls_to_service: usize =

@@ -196,4 +216,1 @@
-    )
-    .await;
-
     debug!("Current floor is {:?}", current_floor);

@@ -186,0 +197,4 @@
+        Some(destination) => destination,
+        None => {
+            // No calls - close doors before returning
+            debug!("No calls found, closing door");

@@ -186,0 +203,4 @@
+                state.elevators.close_door();
+            }
+            
+            tokio::time::sleep(Duration::from_millis(100)).await;

@@ -186,0 +204,4 @@
+            }
+            
+            tokio::time::sleep(Duration::from_millis(100)).await;
+            return;

@@ -259,6 +276,7 @@ async fn elevator_controller(system_state: Arc<RwLock<SystemState>>, elevator: E
 /// Task for the elevator_controller
 pub async fn elevator_controller_task(state: Arc<RwLock<SystemState>>, elevator: Elevator) -> ! {
     loop {
+        debug!("elevator_controller_task loop iteration");

@@ -300,3 +322,3 @@
             },
             _ = tokio::time::sleep(std::time::Duration::from_millis(100)) => {}
-        }
+        }*/

@@ -30,1 +30,3 @@
-        loop {
+        sync_notification_rx.recv().await;
+
+        /*loop {

@@ -673,0 +708,4 @@
+    /// All calls are considered if the elevator is Idle
+    /// Returns None if there is no calls
+    pub fn get_next_destination(&self) -> (Option<Destination>, Option<Direction>) {
+        /// One single applicable call, with necessary values to calculate next destination