Finished basic implementation. Something is going wrong in the heuristics. Builds and runs though.