private double GetCost(RobotTwoWheelCommand command)
{
double lookAheadRemaining = lookAhead;
double cost = 0;
PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2());
PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining);
RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity);
return(newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT); // - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT;
}
