Added A star search algorithm. Need to fix and implement. May be broken code.