implement intra three opt

Cargo.toml

@@ -13,3 +13,4 @@ categories = ["Algorithms"]
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+itertools = "0.12.0"

src/routing.rs

@@ -2,6 +2,7 @@ use crate::vehicle::Vehicle;
 use crate::location::Location;
 use crate::route::Route;
 use crate::errors::InfeasableProblem;
+use itertools::Itertools;
 
 pub struct UnsolvedProblem { 
     vehicles: Vec<Vehicle>,
@@ -99,7 +100,7 @@ impl SolvedProblem {
         fn two_opt(route: &mut Route) {
             let n_nodes = 2;
             let mut best_cost = route.cost();
-            for i in 0..route.destinations.len()-n_nodes {
+            for i in 0..route.destinations.len()-(n_nodes-1) {
                 let mut candidate_route_destinations = route.destinations.clone();
                 candidate_route_destinations.swap(i, i+n_nodes);
 
@@ -120,8 +121,45 @@ impl SolvedProblem {
         self
     }
 
-    pub fn intra_three_opt(self) -> SolvedProblem { 
-        todo!()
+    pub fn intra_three_opt(mut self) -> SolvedProblem { 
+        fn three_opt(route: &mut Route) { 
+            let n_nodes: usize = 3;
+            let mut best_cost = route.cost();
+
+            let route_len = route.destinations.len();
+            for i in 0..route_len-(n_nodes-1) {
+                for j in i..route_len-(n_nodes-2) { 
+                    for k in j..route_len {
+                        let index = [i,j,k];
+                        let options = index.iter().permutations(3);
+                        'options: for option in options { 
+                            let i2 = *option[0]; // vec length should be 3 for 3-opt
+                            let j2 = *option[1]; // vec length should be 3 for 3-opt
+                            let k2 = *option[2]; // vec length should be 3 for 3-opt
+
+                            let mut candidate_route_destinations = route.destinations.clone();
+                            candidate_route_destinations.swap(i, i2);
+                            candidate_route_destinations.swap(j, j2);
+                            candidate_route_destinations.swap(k, k2);
+
+                            std::mem::swap(&mut route.destinations, &mut candidate_route_destinations);
+                            let cost = route.cost();
+                            if cost < best_cost { 
+                                best_cost = cost;
+                                break 'options;
+                            } else {
+                                std::mem::swap(&mut route.destinations, &mut candidate_route_destinations);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        for route in self.routes.iter_mut() { 
+            three_opt(route)
+        }
+
+        self
     }