diff --git a/.gitignore b/.gitignore
index 96ef6c0..26324e7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
 /target
 Cargo.lock
+cache/
+benchmark_report.json
diff --git a/Cargo.toml b/Cargo.toml
index 8dd3043..aff6efd 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -2,6 +2,7 @@
 members = [
     "mapf",
     "mapf-viz",
+    "mapf-bench",
 ]
 
 resolver = "2"
diff --git a/mapf-bench/Cargo.toml b/mapf-bench/Cargo.toml
new file mode 100644
index 0000000..94b5e2d
--- /dev/null
+++ b/mapf-bench/Cargo.toml
@@ -0,0 +1,13 @@
+[package]
+name = "mapf-bench"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+mapf = { path = "../mapf" }
+movingai = "0.2"
+anyhow = "1.0"
+clap = { version = "4.0", features = ["derive"] }
+serde = { version = "1.0", features = ["derive"] }
+serde_json = "1.0"
+indicatif = "0.17"
diff --git a/mapf-bench/src/main.rs b/mapf-bench/src/main.rs
new file mode 100644
index 0000000..aa239c9
--- /dev/null
+++ b/mapf-bench/src/main.rs
@@ -0,0 +1,65 @@
+mod movingai;
+
+use anyhow::Result;
+use clap::Parser;
+use std::path::PathBuf;
+use std::time::Instant;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    #[arg(short, long)]
+    map: PathBuf,
+
+    #[arg(short, long)]
+    scen: PathBuf,
+
+    #[arg(short, long, default_value_t = 0.45)]
+    radius: f64,
+
+    #[arg(short, long, default_value_t = 1.0)]
+    speed: f64,
+
+    #[arg(short, long, default_value_t = 10)]
+    num_agents: usize,
+
+    #[arg(short, long, default_value_t = 30)]
+    timeout: u64,
+}
+
+fn main() -> Result<()> {
+    let args = Args::parse();
+
+    println!("Loading map: {:?}", args.map);
+    let map = movingai::Map::from_file(&args.map)?;
+
+    println!("Loading scenario: {:?}", args.scen);
+    let scenario = movingai::MovingAIScenario::from_file(&args.scen)?;
+
+    let negotiation_scenario = scenario.to_negotiation_scenario(
+        &map,
+        args.num_agents,
+        args.radius,
+        args.speed,
+        60_f64.to_radians(),
+    );
+
+    println!("Running negotiation with {} agents", args.num_agents);
+    let start_time = Instant::now();
+    // We don't have a clean way to pass wall-clock timeout into negotiate yet,
+    // so we'll rely on the parent script to enforce strict timeouts for now,
+    // or we could add a QueueLengthLimit as a proxy.
+    let result = mapf::negotiation::negotiate(&negotiation_scenario, None);
+    let duration = start_time.elapsed();
+
+    match result {
+        Ok((_solution, _arena, _name_map)) => {
+            println!("Negotiation successful in {:?}", duration);
+        }
+        Err(e) => {
+            println!("Negotiation failed: {:?}", e);
+        }
+    }
+
+    Ok(())
+}
diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs
new file mode 100644
index 0000000..51b57fb
--- /dev/null
+++ b/mapf-bench/src/movingai.rs
@@ -0,0 +1,149 @@
+use std::fs::File;
+use std::io::{BufRead, BufReader};
+use std::path::Path;
+use anyhow::Result;
+use std::collections::HashMap;
+
+use mapf::negotiation::{Agent, Scenario};
+use std::collections::BTreeMap;
+
+pub struct Map {
+    pub width: usize,
+    pub height: usize,
+    pub grid: Vec<Vec<char>>,
+}
+
+impl Map {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        let mut width = 0;
+        let mut height = 0;
+
+        // Parse header
+        while let Some(line) = lines.next() {
+            let line = line?;
+            if line.starts_with("type") {
+                continue;
+            } else if line.starts_with("height") {
+                height = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("width") {
+                width = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("map") {
+                break;
+            }
+        }
+
+        let mut grid = Vec::with_capacity(height);
+        for _ in 0..height {
+            if let Some(line) = lines.next() {
+                let line = line?;
+                grid.push(line.chars().collect());
+            }
+        }
+
+        Ok(Map { width, height, grid })
+    }
+
+    pub fn to_occupancy_map(&self) -> HashMap<i64, Vec<i64>> {
+        let mut occupancy = HashMap::new();
+        for y in 0..self.height {
+            let mut row = Vec::new();
+            for x in 0..self.width {
+                let c = self.grid[y][x];
+                if c != '.' && c != 'G' && c != 'S' {
+                    row.push(x as i64);
+                }
+            }
+            if !row.is_empty() {
+                occupancy.insert(y as i64, row);
+            }
+        }
+        occupancy
+    }
+}
+
+pub struct ScenarioEntry {
+    pub bucket: usize,
+    pub map_file: String,
+    pub map_width: usize,
+    pub map_height: usize,
+    pub start_x: usize,
+    pub start_y: usize,
+    pub goal_x: usize,
+    pub goal_y: usize,
+    pub optimal_length: f64,
+}
+
+pub struct MovingAIScenario {
+    pub entries: Vec<ScenarioEntry>,
+}
+
+impl MovingAIScenario {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        // Skip version line
+        lines.next();
+
+        let mut entries = Vec::new();
+        for line in lines {
+            let line = line?;
+            let parts: Vec<&str> = line.split_whitespace().collect();
+            if parts.len() < 9 {
+                continue;
+            }
+
+            entries.push(ScenarioEntry {
+                bucket: parts[0].parse()?,
+                map_file: parts[1].to_string(),
+                map_width: parts[2].parse()?,
+                map_height: parts[3].parse()?,
+                start_x: parts[4].parse()?,
+                start_y: parts[5].parse()?,
+                goal_x: parts[6].parse()?,
+                goal_y: parts[7].parse()?,
+                optimal_length: parts[8].parse()?,
+            });
+        }
+
+        Ok(MovingAIScenario { entries })
+    }
+
+    pub fn to_negotiation_scenario(
+        &self,
+        map: &Map,
+        num_agents: usize,
+        radius: f64,
+        speed: f64,
+        spin: f64,
+    ) -> Scenario {
+        let mut agents = BTreeMap::new();
+        for i in 0..num_agents.min(self.entries.len()) {
+            let entry = &self.entries[i];
+            agents.insert(
+                format!("agent_{}", i),
+                Agent {
+                    start: [entry.start_x as i64, entry.start_y as i64],
+                    yaw: 0.0,
+                    goal: [entry.goal_x as i64, entry.goal_y as i64],
+                    radius,
+                    speed,
+                    spin,
+                },
+            );
+        }
+
+        Scenario {
+            agents,
+            obstacles: Vec::new(),
+            occupancy: map.to_occupancy_map(),
+            cell_size: 1.0,
+            camera_bounds: None,
+        }
+    }
+}
diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs
index f2fdba5..69cc401 100644
--- a/mapf-viz/examples/grid.rs
+++ b/mapf-viz/examples/grid.rs
@@ -1316,7 +1316,14 @@ impl App {
         let elapsed = start_time.elapsed();
         println!("Successful planning took {} seconds", elapsed.as_secs_f64());
         dbg!(node_history.len());
-
+        let mut total_length = 0.0;
+        for solution in solution_node.proposals.values() {
+            total_length += solution.meta.trajectory.windows(2).fold(0.0, |acc, w| {
+                (w[0].position.translation.vector - w[1].position.translation.vector).magnitude() + acc
+            });
+            
+        }
+        println!("Total length: {total_length}");
         assert!(self.canvas.program.layers.3.solutions.is_empty());
         for (i, proposal) in &solution_node.proposals {
             let name = name_map.get(i).unwrap();
diff --git a/scripts/benchmark.py b/scripts/benchmark.py
new file mode 100755
index 0000000..f35d694
--- /dev/null
+++ b/scripts/benchmark.py
@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+
+import os
+import subprocess
+import zipfile
+import urllib.request
+import json
+import time
+import argparse
+from pathlib import Path
+
+# Configuration
+CACHE_DIR = Path("cache")
+MAPS_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-map.zip"
+SCEN_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-scen-random.zip"
+DEFAULT_AGENT_COUNTS = [2, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50]
+
+def download_file(url, dest):
+    if dest.exists():
+        return
+    print(f"Downloading {url} to {dest}...")
+    urllib.request.urlretrieve(url, dest)
+
+def unzip_file(zip_path, extract_to):
+    print(f"Unzipping {zip_path} to {extract_to}...")
+    with zipfile.ZipFile(zip_path, 'r') as zip_ref:
+        zip_ref.extractall(extract_to)
+
+def run_benchmark(map_path, scen_path, num_agents, timeout):
+    cmd = [
+        "cargo", "run", "-p", "mapf-bench", "--release", "--",
+        "--map", str(map_path),
+        "--scen", str(scen_path),
+        "--num-agents", str(num_agents),
+        "--timeout", str(timeout)
+    ]
+    
+    start_time = time.time()
+    try:
+        # Strict timeout enforcement via subprocess
+        result = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout + 5)
+        duration = time.time() - start_time
+        
+        success = "Negotiation successful" in result.stdout
+        return {
+            "success": success,
+            "duration": duration,
+            "stdout": result.stdout,
+            "stderr": result.stderr
+        }
+    except subprocess.TimeoutExpired:
+        return {
+            "success": False,
+            "duration": timeout,
+            "error": "Timeout"
+        }
+    except Exception as e:
+        return {
+            "success": False,
+            "error": str(e)
+        }
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--timeout", type=int, default=30, help="Timeout in seconds per run")
+    parser.add_argument("--max-scenarios", type=int, default=1, help="Max random scenarios per map")
+    parser.add_argument("--maps", nargs="+", default=["empty-32-32.map", "room-32-32-4.map", "maze-32-32-2.map"])
+    args = parser.parse_args()
+
+    CACHE_DIR.mkdir(exist_ok=True)
+    
+    maps_zip = CACHE_DIR / "mapf-map.zip"
+    scen_zip = CACHE_DIR / "mapf-scen-random.zip"
+    
+    download_file(MAPS_ZIP_URL, maps_zip)
+    download_file(SCEN_ZIP_URL, scen_zip)
+    
+    maps_dir = CACHE_DIR / "maps"
+    scen_dir = CACHE_DIR / "scenarios"
+    
+    if not maps_dir.exists():
+        unzip_file(maps_zip, maps_dir)
+    if not scen_dir.exists():
+        unzip_file(scen_zip, scen_dir)
+    
+    report = {}
+
+    print("Building mapf-bench in release mode...")
+    subprocess.run(["cargo", "build", "-p", "mapf-bench", "--release"], check=True)
+
+    for map_name in args.maps:
+        map_path = maps_dir / map_name
+        if not map_path.exists():
+            print(f"Map {map_name} not found.")
+            continue
+            
+        base_name = map_name.replace(".map", "")
+        
+        # Find all matching scenario files
+        scen_pattern = f"{base_name}-random-*.scen"
+        scen_files = list((scen_dir / "scen-random").glob(scen_pattern))
+        scen_files.sort()
+        
+        if not scen_files:
+            print(f"No scenarios found for {map_name} with pattern {scen_pattern}")
+            continue
+
+        selected_scens = scen_files[:args.max_scenarios]
+        
+        for scen_path in selected_scens:
+            scen_name = scen_path.name
+            print(f"\nBenchmarking {map_name} with scenario {scen_name}...")
+            
+            key = f"{map_name}:{scen_name}"
+            report[key] = []
+            
+            for count in DEFAULT_AGENT_COUNTS:
+                print(f"  Agents: {count}", end=" ", flush=True)
+                res = run_benchmark(map_path, scen_path, count, args.timeout)
+                if res["success"]:
+                    print(f"✅ ({res['duration']:.2f}s)")
+                else:
+                    error_msg = res.get("error", "FAILED")
+                    print(f"❌ ({error_msg})")
+                
+                report[key].append({
+                    "agents": count,
+                    "success": res["success"],
+                    "duration": res.get("duration", 0),
+                    "error": res.get("error")
+                })
+
+    # Save report
+    with open("benchmark_report.json", "w") as f:
+        json.dump(report, f, indent=2)
+    
+    # Print summary table
+    print("\nBenchmark Summary:")
+    print(f"{'Scenario':<40} | {'Agents':<6} | {'Status':<8} | {'Time':<8}")
+    print("-" * 75)
+    for key, results in report.items():
+        for res in results:
+            status = "SUCCESS" if res["success"] else (res.get("error") if res.get("error") else "FAILED")
+            print(f"{key[:40]:<40} | {res['agents']:<6} | {status:<8} | {res['duration']:>7.2f}s")
+
+if __name__ == "__main__":
+    main()