My  A* path finder C++ version
FSM implementation
Late push XD