Split into smaller files, write RRT logic, some helpers not yet implemented
Got it compiling
Get planning working roughly