Fixed various bugs, added path finding, and simplified code

Fixed a lot of bugs associated with moving,
added a pathfinder for moving pieces
removed occupied variable.