I think I am ready to merge the PR.

- Got basic route finding working.
- Refactored some of the code
- Removed a lot of unused code
- Got flightapid working