Update 2.1

First Try to implement A*