from pextant.explorers import Astronaut
from pextant.EnvironmentalModel import load_legacy

"""
Explanation: Path Planning
1. Problem setup
The solver requires an environment and an agent(denominated explorer). The agent determines the cost function definitions.
End of explanation
"""


apollo14_model = load_legacy("Apollo14.txt").loadSubSection(maxSlope=15, \
                                                            cached=True)
agent = Astronaut(80)

import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from matplotlib_scalebar.scalebar import ScaleBar
from pextant.viz.utils import hillshade
from pextant.lib.geoshapely import GeoPoint, GeoPolygon

start = GeoPoint(apollo14_model.ROW_COL, 750, 100)
goal = GeoPoint(apollo14_model.ROW_COL, 650, 900)
waypoints = GeoPolygon([start, goal])

"""
Explanation: To illustrate the use, our example will use the Apollo 14 map. Maximum slope is set to 15 degrees which is reasonable for astronaut traverses. To speed up the solver for later we need to pass in the argument cached=True to the loadSubSection method. This will carry out some preprocessing that will speed up solution time later on.
End of explanation
"""


hillshade(apollo14_model,1)
plt.imshow(apollo14_model.obstacle_mask(), alpha=0.5, cmap='bwr_r')
plt.plot(*waypoints.to(apollo14_model.COL_ROW), \
         linestyle='None', marker='*', markeredgecolor="k", markersize=15)
red_patch = mpatches.Patch(color='red', alpha=0.5, label='Obstacle map')
plt.legend(handles=[red_patch])
plt.gca().add_artist(ScaleBar(apollo14_model.resolution, location=4))
plt.show()

"""
Explanation: We will choose for start the location of the Lunar lander, and for goal the border of the crater rim, this is to very roughly approximate the actual EVA - which has several intermediate points included.
End of explanation
"""


from pextant.solvers.astarMesh import astarSolver

"""
Explanation: 2. Solver setup
End of explanation
"""


pathfinder = astarSolver(apollo14_model, agent, inhouse=False)

%%time 
out,_,_ = pathfinder.solvemultipoint(waypoints)

"""
Explanation: SEXTANT had a slightly unefficient implementation, so this was just changed at the tailgate of the project. It implements a slightly modified version of networkx's astar solver.
End of explanation
"""


hillshade(apollo14_model,1)
plt.imshow(apollo14_model.obstacle_mask(), alpha=0.5, cmap='bwr_r')
plt.plot(*waypoints.to(apollo14_model.COL_ROW), \
         linestyle='None', marker='*', markeredgecolor="k", markersize=15)
plt.plot(*out.coordinates().to(apollo14_model.COL_ROW))
plt.legend(handles=[red_patch])
plt.gca().add_artist(ScaleBar(apollo14_model.resolution, location=4))
plt.show()

"""
Explanation: Next we can overlay the solution on the map visualization:
End of explanation
"""


from pextant.explorers import TraversePath

traverse = TraversePath.frommap(out.coordinates(), apollo14_model)

_, _, dr = agent.path_dl_slopes(traverse)
energies, v = agent.path_energy_expenditure(traverse)

import numpy as np

plt.plot(np.cumsum(dr), np.cumsum(energies)/1000.)
plt.xlabel('distance [m]')
plt.ylabel('energy [kJ]')
plt.title('Cumulative energy for EVA')
plt.show()

"""
Explanation: 3. Post processing of a path
End of explanation
"""


