a simple c style A star path finder