Add first full implementation of A Star based path finding algorithm