Refactor tree.py to use OOP

[wrongsyntax]

Making a Waypoint and Tree class could make the code more clear.

Not a necessary change as functionality would not change but could be done.