"""A planner that plans for each robot sequentially."""

def __init__(self, my_map, starts, goals):

"""my_map - list of lists specifying obstacle positions

self.my_map = my_map

self.starts = starts

self.goals = goals

self.num_of_agents = len(goals)

self.CPU_time = 0

# compute heuristics for the low-level search

self.heuristics = []

for goal in self.goals:

self.heuristics.append(compute_heuristics(my_map, goal))

def find_solution(self):

""" Finds paths for all agents from their start locations to their goal locations."""

start_time = timer.time()

result = []

constraints = []

#constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 4}]

#constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 10}]

#constraints = [{'agent': 1,'loc': [(1,1),(1,2)],'timestep': 1}]

# constraints = [

# {'agent':1, 'loc':[(1,2),(1,2)], 'timestep':1},

# {'agent':1, 'loc':[(1,3),(1,2)], 'timestep':2},

# {'agent':1, 'loc':[(1,3),(1,3)], 'timestep':2},

# {'agent':1, 'loc':[(1,4)], 'timestep':2},

# ]

for i in range(self.num_of_agents): # Find path for each agent

path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints)

if path is None:

raise BaseException('No solutions')

result.append(path)

##############################

# Task 2: Add constraints here

# Useful variables:

# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]

# * self.num_of_agents has the number of total agents

# * constraints: array of constraints to consider for future A* searches

##############################

# Add constraints to the list of constraints

for j in range(len(path)):

for k in range(self.num_of_agents):

if k != i:

# Vertex constraint

constraints.append({'agent': k,'loc': [path[j]],'timestep': j})

# Edge constraint

if j != 0:

constraints.append({'agent': k,'loc': [path[j],path[j-1]],'timestep': j})

# add goal constraint

constraints.append(path[-1])

self.CPU_time = timer.time() - start_time

print("\n Found a solution! \n")

print("CPU time (s): {:.2f}".format(self.CPU_time))

print("Sum of costs: {}".format(get_sum_of_cost(result)))

print(result)