Added the bulk of the A* algorithm, but it is still untested.
Fix directory structure
It builds fuck yeah