from flight_environment import FlightEnvironment
from planners.path_planner import rrt_planner
from planners.trajectory_generator import plan_trajectory_through_path, plot_trajectory
from plot_plotly import plot_cylinders

env = FlightEnvironment(50)
start = (1,2,0)
goal = (18,18,3)

# --------------------------------------------------------------------------------------------------- #
# Call your path planning algorithm here. 
# The planner should return a collision-free path and store it in the variable `path`. 
# `path` must be an N×3 numpy array, where:
#   - column 1 contains the x-coordinates of all path points
#   - column 2 contains the y-coordinates of all path points
#   - column 3 contains the z-coordinates of all path points
# This `path` array will be provided to the `env` object for visualization.

#path = [[0,0,0],[1,1,1],[2,2,2],[3,3,3]]
path = rrt_planner(env, start, goal, step_size=1)

# --------------------------------------------------------------------------------------------------- #
# 3D env plot, choose one - Plotly or Matplotlib - comment the other

# Plotly
plot_cylinders(env, path)

# Matplotlib (laggy)
#env.plot_cylinders(path)

# --------------------------------------------------------------------------------------------------- #
#   Call your trajectory planning algorithm here. The algorithm should
#   generate a smooth trajectory that passes through all the previously
#   planned path points.
#
#   After generating the trajectory, plot it in a new figure.
#   The figure should contain three subplots showing the time histories of
#   x, y, and z respectively, where the horizontal axis represents time (in seconds).
#
#   Additionally, you must also plot the previously planned discrete path
#   points on the same figure to clearly show how the continuous trajectory
#   follows these path points.

t_traj, traj = plan_trajectory_through_path(path, desired_speed=1.0, samples_per_meter=8)
plot_trajectory(t_traj, traj, path)

# --------------------------------------------------------------------------------------------------- #


# You must manage this entire project using Git. 
# When submitting your assignment, upload the project to a code-hosting platform 
# such as GitHub or GitLab. The repository must be accessible and directly cloneable. 
#
# After cloning, running `python3 main.py` in the project root directory 
# should successfully execute your program and display:
#   1) the 3D path visualization, and
#   2) the trajectory plot.
#
# You must also include the link to your GitHub/GitLab repository in your written report.