completed a crude version of A star algorithm