Added test body to main
Tested and corrected trajectory alghorithm
Trajectory will return tuple with 3 numpy arrays

Issue: trajectory is calculated to one step before returning to earth ? This should be corrected
correction:it now reaches y=0 at the end of trajectory.