diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index 8be2ce6..a6418c4 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -260,62 +260,43 @@ fn negotiate_path(
     )>,
 ) {
     for (entity, mut path, mut position, mut velocity, speed, rotation_speed) in query.iter_mut() {
-        let mut new_path = path.0.clone();
         let start_i32 = (
             position.position.translation.x,
             position.position.translation.y,
         )
             .i32();
-        let new_path_clone = new_path.clone();
-        let mut iter = new_path_clone.split(|p| *p == start_i32);
-        if iter.next().is_some() {
-            if let Some(upcoming) = iter.next() {
-                new_path = vec![start_i32];
-                new_path.append(&mut upcoming.to_vec());
-            } else {
-                let start = Vec2::new(
-                    position.position.translation.x,
-                    position.position.translation.y,
-                );
-                let next = path[1];
-                let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
-                let mut direction = next - start;
-                direction = direction.normalize();
-                direction *= **speed;
-                velocity.linvel = direction.into();
-                continue;
-            }
-        } else {
-            println!("Remove path1");
-            commands.entity(entity).remove::<Path>().remove::<Speed>();
-            velocity.linvel = Vec2::ZERO.into();
-        }
+        let mut new_path = path
+            .iter()
+            .cloned()
+            .skip_while(|v| *v == start_i32)
+            .collect::<Vec<(i32, i32)>>();
+        new_path.retain(|v| *v != start_i32);
         **path = new_path;
-        if path.len() >= 2 {
+        if let Some(next) = path.first() {
             let start = Vec2::new(
                 position.position.translation.x,
                 position.position.translation.y,
             );
-            let next = path[1];
             let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
+            let mut direction = next - start;
+            direction = direction.normalize();
+            direction *= **speed;
+            velocity.linvel = direction.into();
             if rotation_speed.is_some() {
                 let v = next - start;
                 let angle = v.y.atan2(v.x);
                 position.position.rotation = UnitComplex::new(angle);
             }
-            let mut direction = next - start;
-            direction = direction.normalize();
-            direction *= **speed;
-            velocity.linvel = direction.into();
+            continue;
         } else {
-            velocity.linvel = Vec2::ZERO.into();
-            println!("Remove path2");
+            println!("Remove path1");
             commands
                 .entity(entity)
                 .remove::<Path>()
                 .remove::<NoPath>()
                 .remove::<Destination>()
                 .remove::<Speed>();
+            velocity.linvel = Vec2::ZERO.into();
         }
     }
 }