import numpy as np
import matplotlib.pyplot as plt
from cpp_algorithms import dist_fill
from cpp_algorithms import darp, stc, bcd, wavefront
from cpp_algorithms import get_drone_map, get_random_coords
from cpp_algorithms import get_all_area_maps, plot, imshow, imshow_scatter
from cpp_algorithms.darp.darp_helpers import get_assigned_count
from cpp_algorithms.coverage_path.pathing_helpers import has_isolated_areas
n = 3
area_maps = get_all_area_maps("test_maps")
area_map = area_maps[0]
start_points = get_random_coords(area_map, n)
start_points = get_random_coords(area_map, n)
A, losses = darp(300, area_map, start_points, pbar=True)
drone_maps = [get_drone_map(A, i) for i in range(n)]
# plt.figure()
print("assigned : ", get_assigned_count(A))
print('has isolated : ', [has_isolated_areas(dm) for dm in drone_maps])
imshow(A, 1, 4, 1, figsize=(20, 5))
imshow_scatter(start_points, color="black")
dist_maps = [dist_fill(drone_maps[i], [start_points[i]]) for i in range(n)]
[imshow(dist_maps[i], 1, 4, i + 2) for i in range(n)]
print("hi")
# plt.show()
# plt.figure()
# coverage_paths = [wavefront(drone_maps[i],start_points[i]) for i in range(n)]
# imshow(A,1,4,1, figsize=(20,5))
# ## Usage
# In[4]:
from cpp_algorithms import bcd
from cpp_algorithms import get_all_area_maps, get_random_coords
from cpp_algorithms.testers.display_funcs import printer
from cpp_algorithms.common_helpers import plot, imshow, imshow_scatter
from cpp_algorithms.testers.metrics import coverage_metrics, fuel_metrics
# ### Getting the coverage path using bcd
# In[5]:
# ---- Get The Map ----
area_maps = get_all_area_maps("./test_maps/") # all area maps in the folder
area_map = area_maps[10]
# ---- Calculate Coverage Path ----
start_point = get_random_coords(
area_map, 1)[0] # returns a random coord not on an obstacle
coverage_path = bcd(area_map, start_point) # calculate coverage path using bcd
end_point = coverage_path[-1]
# ---- Display The Stuff ----
imshow(area_map, figsize=(7, 7), cmap="Blues_r")
# shows the area_map
plot(coverage_path, alpha=0.5, color="orange")
imshow_scatter([start_point], color="green") # show the start_point (green)
imshow_scatter([end_point], color="red") # show the end_point (red)