Fixed the A star algorithm. It is almost ready now