Implemented feedback blocked point and distance to goal