From 21ff43523086aa86a0e393c8a5e42bd8d8e4249d Mon Sep 17 00:00:00 2001
From: Shushman <shushmanchoudhury@gmail.com>
Date: Sat, 21 Dec 2019 15:47:39 -0800
Subject: [PATCH] Updated to work with JLD2; NEED JULIA 1.3

---
 Project.toml                          |  1 +
 scripts/test_replanning_heuristics.jl | 24 ++++++++++++++++--------
 src/MultiAgentAllocationTransit.jl    | 10 ++++++++++
 3 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/Project.toml b/Project.toml
index 5e3fff1..d65cf9b 100644
--- a/Project.toml
+++ b/Project.toml
@@ -10,6 +10,7 @@ DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
 Distances = "b4f34e82-e78d-54a5-968a-f98e89d6e8f7"
 Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
 GLPK = "60bf3e95-4087-53dc-ae20-288a0d20c6a6"
+GeometryTypes = "4d00f742-c7ba-57c2-abde-4428a4b178cb"
 Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
 IterTools = "c8e1da08-722c-5040-9ed9-7db0dc04731e"
 JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
diff --git a/scripts/test_replanning_heuristics.jl b/scripts/test_replanning_heuristics.jl
index d1736e5..8f980da 100644
--- a/scripts/test_replanning_heuristics.jl
+++ b/scripts/test_replanning_heuristics.jl
@@ -5,6 +5,7 @@ using MultiAgentAllocationTransit
 using BenchmarkTools
 using Statistics
 using JSON
+using JLD2
 using Logging
 global_logger(SimpleLogger(stderr, Logging.Warn))
 
@@ -17,6 +18,7 @@ const trips_file = "./data/sfmta/trips.json"
 const drone_params_file = "./data/drone_params.toml"
 const bb_params_file = "./data/sfmta/sf_bb_params.toml"
 const out_file = "./data/temp_mult_generic.json"
+const city_travel_time_estimates = "./data/sfmta/sf_halton_tt_estimates.jld2"
 
 # MAPF-TN params
 const TRANSIT_CAP_RANGE = (2, 4)
@@ -43,29 +45,33 @@ lon_dist = Uniform(bb_params.lon_start, bb_params.lon_end)
 
 # Transit Graph Preprocessing
 tg = load_transit_graph_latlong(stop_coords_file, trips_file, TRANSIT_CAP_RANGE, rng)
-tg, stop_idx_to_trips, aug_trips_fws_dists, stops_nn_tree, nn_idx_to_stop =
+tg, stop_idx_to_trips =
                 transit_graph_preprocessing(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, drone_params)
 
+# Load Halton stuff
+@load city_travel_time_estimates halton_nn_tree city_halton_points travel_time_estimates
+
 sites = [LatLonCoords((lat = rand(rng, lat_dist), lon = rand(rng, lon_dist))) for i = 1:N_SITES]
 depot_sites = vcat(depots, sites)
 
 # Load OTG stuff
 otg = OffTransitGraph(depots = depots, sites = sites)
-depot_to_sites_dists = generate_depot_to_sites_dists(otg, tg, stops_nn_tree, nn_idx_to_stop, stop_idx_to_trips,
-                            aug_trips_fws_dists, MultiAgentAllocationTransit.distance_lat_lon_euclidean)
+aug_trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean,
+                                                      length(depots), length(sites),
+                                                      vcat(depots, sites),
+                                                      drone_params)
 state_graph, depot_sites_to_vtx, trip_to_vtx_range = setup_state_graph(tg, otg)
 
 
 # Set the cost function using the wrapper
 env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph,
                      agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range,
-                     stops_nn_tree = stops_nn_tree, nn_idx_to_stop = nn_idx_to_stop, stop_idx_to_trips = stop_idx_to_trips,
-                     aug_trips_fws_dists = aug_trips_fws_dists, depot_to_sites_dists = depot_to_sites_dists,
+                     stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists,
                      drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean,
-                     curr_site_points = [])
-
-cost_fn(i, j) = allocation_cost_fn_wrapper(env, ECBS_WEIGHT, N_DEPOTS, N_SITES, i, j)
+                     curr_site_points = [], threshold_global_conflicts = 10)
 
+cost_fn(i, j) = allocation_cost_wrapper_estimate(env, ECBS_WEIGHT, N_DEPOTS, N_SITES,
+                                                                     halton_nn_tree, travel_time_estimates, i, j)
 
 agent_tours = task_allocation(N_DEPOTS, N_SITES, N_AGENTS,
                               depot_sites, cost_fn)
@@ -96,3 +102,5 @@ solution_copy = deepcopy(solution)
 did_replan_indiv, el_time_indiv = replan_individual!(env, solution, N_DEPOTS, N_SITES, agent_tours, ECBS_WEIGHT)
 
 did_replan_collec, el_time_collec, new_soln_collec = replan_collective!(env_copy, solution_copy, N_DEPOTS, N_SITES, agent_tours, ECBS_WEIGHT)
+
+@show el_time_collec
diff --git a/src/MultiAgentAllocationTransit.jl b/src/MultiAgentAllocationTransit.jl
index 393122f..c7ca6ec 100644
--- a/src/MultiAgentAllocationTransit.jl
+++ b/src/MultiAgentAllocationTransit.jl
@@ -18,6 +18,7 @@ using CSV
 using DataFrames
 using Distances
 using NearestNeighbors
+using GeometryTypes
 using IterTools
 using GLPK
 GLPK.jl_set_preemptive_check(false)
@@ -115,4 +116,13 @@ include("task_allocation.jl")
 include("mapf_transit.jl")
 include("load_transit_env.jl")
 
+
+function Base.convert(::Type{NearestNeighbors.HyperSphere{N,T}}, ghs::GeometryTypes.HyperSphere{N,T}) where {N, T <: AbstractFloat}
+    return NearestNeighbors.HyperSphere{N,T}(ghs.center, ghs.r)
+end
+
+function Base.convert(::Type{GeometryTypes.HyperSphere{N,T}}, nhs::NearestNeighbors.HyperSphere{N,T}) where {N, T <: AbstractFloat}
+    return GeometryTypes.HyperSphere{N,T}(nhs.center, nhs.r)
+end
+
 end # module