 
{ File ns.sil -  ( Near sol function ) }
 
 
{  This function chooses the nearest of the computed joint }
{  vectors for robot X.  Argument X is the robot object.   }
function near_sol_sx_13_19(x:object): flagged_jvr;
var
  i: integer;
  s: arm_solution;
  dist, mindist: real;
  soli: integer;
  lastj, sol: jvr;
  no_sol, seeking_first_sol:boolean;
begin
  s := all_jvrs(x);
  seeking_first_sol := true;
  soli := 0;
  lastj := jvr_to_get_close_to(x);
  no_sol := false;
  while (soli = 0) do begin
    for i := 1 to nsols(s) do begin
      if valid(select(s,i)) or no_sol then begin
        sol := value(select(s,i));
        dist := abs(lastj[1]-sol[1]) +
                abs(lastj[2]-sol[2]) +
                abs(lastj[3]-sol[3]) +
                abs(lastj[4]-sol[4]) +
                abs(lastj[5]-sol[5]) +
                abs(lastj[6]-sol[6]);
        if seeking_first_sol then begin
          mindist := dist;
          soli := i;
          seeking_first_sol := false;
        end else if dist < mindist then begin
          soli:=i;
          mindist := dist;
        end;
      end;
    end;
    if soli = 0 then no_sol := true;
  end;
  if not(in_env(s)) then
  begin
    write_msg('Goal out of work envelope.');
    last_move_had_err_msg := true;
  end else if no_sol and (mindist = 0.0) then
  begin
    write_msg('Goal out of joint range.');
    last_move_had_err_msg := true;
  end;
  near_sol_sx_13_19 := select(s,soli);
end;
