Implemented A* Agent
- Still have a few errors in the implementation