Motion planning implemented, TODO - comments