COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Interpreter - Level 2 aux routines }
C00009 00003	(* message passing routines: getReply, whereArm *)
C00012 00004	(* Graph structure routines: eval, feval, invalidate, stvals, change, getDevice, getFrame *)
C00027 00005	(* aux routine: getVal *)
C00029 00006	(* Aux routine: setVal *)
C00031 00007	(* affixment auxiliary routines: affixaux, unfixaux & unfix *)
C00037 00008	(* Aux routines to destroy variables: killVar, killEnv *)
C00048 ENDMK
C⊗;
{$NOMAIN	Interpreter - Level 2 aux routines }

%include ialhdr.pas;

{ Externally defined routines from elsewhere: }

	(* From ALLOC *)
procedure relVector(v: vectorp);				external;
function newTrans: transp;					external;
procedure relTrans(t: transp);					external;
function newNode: nodep;					external;
procedure relNode(n: nodep);					external;
procedure relEentry(n: enventryp);				external;
procedure relCmoncb(n: cmoncbp);				external;
procedure relFrame(n: framep);					external;
procedure relEheader(n: envheaderp);				external;
procedure relEnvironment(n: environp);				external;

	(* Arithmetic routines *)
function ttmul (t1,t2: transp): transp; 			external;
function tinvrt (t: transp): transp; 				external;
function taxis (t: transp): vectorp; 				external;
function tmagn (t: transp): scalar; 				external;

	(* From RSXMSG *)
function startArm: boolean;                                  	external;
procedure initMsg(var buf: messagep; var flag: boolean);     	external;
function SendArm: boolean;                                   	external;
function GetArm: boolean;                                    	external;
procedure signalArm;                                         	external;

	(* From IAUX1A *)
procedure push (n: nodep);					external;
function pop: nodep;						external;
procedure upTrans (var t: transp; tp: transp);			external;
function getEntry (level, offset: byte): enventryp; 		external;
function getVar (level, offset: byte): enventryp;		external;
procedure freePdb(p: pdbp);					external;
procedure freeEvent(e: eventp);					external;
procedure sendCmd;						external;

	(* From IAUX1B *)
procedure msgDispatch;						external;

	(* From PP *)
procedure ppLine; 						external;
procedure ppOutNow; 						external;
procedure ppChar(ch: ascii); 					external;
procedure pp5(ch: c5str; length: integer); 			external;
procedure pp10(ch: cstring; length: integer); 			external;
procedure pp10L(ch: cstring; length: integer);			external;
procedure pp20(ch: c20str; length: integer); 			external;
procedure pp20L(ch: c20str; length: integer); 			external;
procedure ppInt(i: integer); 					external;

(* message passing routines: getReply, whereArm *)

procedure getReply; external;
procedure getReply;
 var ocmd: msgtypes; b: boolean;
 begin
 with msg↑ do
  begin
  ocmd := cmd;			(* remember what we're waiting for *)
  sendCmd;			(* send request to ARM servo *)
  repeat
   b := getArm;			(* try to read a message packet from ARM *)
   if b and (cmd <> ocmd) then	(* if we got one, was it our reply? *)
     begin
     msgDispatch;		(* deal with whatever the ARM servo sent over *)
     b := false;		(* keep waiting for our reply *)
     end
  until b;			(* wait for reply *)
  end;
 end;

function whereArm (mech: integer): transp; external;	(* to read in the arm's position *)
function whereArm ;
 var tp: transp; i,j: integer; b: boolean;
 begin
 tp := newTrans;
 with msg↑,tp↑ do
  begin
  cmd := wherecmd;
  dev := mech;
  getReply;			(* go get 1st message packet *)
  if ok then			(* check there's no error *)
    begin
    for i := 1 to 3 do
     for j := 1 to 2 do val[i,j] := t[i + 3*(j-1)];	(* copy result *)
    repeat b := getArm until b;	(* get 2nd packet (guaranteed to be next) *)
    for i := 1 to 3 do
     for j := 3 to 4 do val[i,j] := t[i + 3*(j-3)];	(* copy result *)
    end
   else
    begin			(* *** ERROR - maybe we should complain??? *** *)
    pp20L('[Error in whereArm!]',20); ppLine; ppOutNow;
    relTrans(tp);		(* don't need this anymore *)
    tp := niltrans;
    end;
  end;
 whereArm := tp;
 end;

(* Graph structure routines: eval, feval, invalidate, stvals, change, getDevice, getFrame *)

procedure nextTime; external;
procedure nextTime;
 begin
 if etime = Maxint then etime := 1 (* should reset all invalid frames, but ... *)
  else etime := etime + 1;
 end;

procedure eval (f: framep); external;
procedure eval ;
 var calc: nodep; b: boolean; f2, tr: transp;
 begin
 if f↑.valid <> etime then	(* Haven't looked at it yet *)
  begin
  f↑.valid := etime;		(* Mark it *)
  calc := f↑.calcs;		(* Get list of calculators *)
  b := true;
  while (calc <> nil) and b do	(* See if someone it's affixed to is now valid *)
   if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
    with calc↑.other↑ do	(* A possibility, look at other frame *)
     begin
     if not ftype then	(* See if it's a device or frame *)
       begin		(* It's a device - use it to compute current value *)
       f2 := whereArm(mech);	(* Get current device pos *)
       b := false;		(* No need to look further *)
       end
      else if (dcntr=0) and (valid=0) then	(* not dynamic & valid frame *)
	    begin f2 := val; b := false end
	    else calc := calc↑.next	(* dynamic or not valid - try next *)
     end
    else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)

  if calc = nil then
   begin  (* Check calcs again - this time trying to evaluate other frame *)
   calc := f↑.calcs;
   b := true;
   while (calc <> nil) and b do
    if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
     begin
     eval(calc↑.other);		(* Try to get a value for it *)
     if calc↑.other↑.valid=0 then		(* Is it now valid? *)
	begin f2 := calc↑.other↑.val; b := false end	(* Yes - all done *)
      else calc := calc↑.next	(* still not valid - try next *)
     end
    else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)
   end;

  if calc <> nil then
    with calc↑ do
     begin	(* use other frame to evaluate desired one & return success *)
     if tvarp then tr := tvar↑.t else tr := tval; (* explicitly named trans? *)
     if not frame1 then tr := tinvrt(tr);  (* second := inv(trans) * first *)
     upTrans(f↑.val,ttmul(tr,f2));	  (* first := trans * second *)
     f↑.valid := 0;			  (* Mark it as now valid *)
     end;
  end;
 end;

function feval (f: framep): transp; external;
function feval ;
 begin
 if not f↑.ftype then 
   begin			(* If device use its current value *)
   feval := whereArm(f↑.mech);	(* Get current device pos *)
   end
  else					(* frame *)
   begin
   if (f↑.dcntr<>0) or (f↑.valid<>0) then  (* dynamic frame or not valid? *)
    begin			(* Need to calculate current value *)
    nextTime;			(* update eval time *)
    eval(f);			(* try to evaluate the variable *)
    end;
   if f↑.valid = 0 then feval := f↑.val		(* copy trans pointer *)
    else feval := niltrans;			(* but always return something *)
   end;
 end;

function invalidate (f: framep): boolean; external;
function invalidate ;
 var calc: nodep; b: boolean;
 begin

(* invalidate frame & all other frames affixed either rigidly or
    non-rigidly with this being frame2,
   else indicate we need to modify non-rigid trans. *)

 b := false;		(* assume no updating of non-rigid relationships *)
 if etime <> f↑.valid then		(* haven't marked this one yet *)
  with f↑ do
   begin
   if valid = 0 then upTrans(val,nil);	(* flush old value *)
   valid := etime;	(* mark us as having an invalid value *)
   calc := calcs;
   while calc <> nil do		(* invalidate everyone we're affixed to *)
     begin			(* rigidly or if we're frame 2 *)
     if (calc↑.ntype = calcnode) and (calc↑.rigid or (not calc↑.frame1))
	then b := b or invalidate(calc↑.other)	(* go invalidate frame *)
	else b := true;		(* found a non-rigid affixment to update *)
     calc := calc↑.next;	(* now repeat with next calc *)
     end;
   end;
 invalidate := b;
 end;

procedure stvals (f: framep); external;
procedure stvals ;
 var calc,c2: nodep; t,val: transp; f2: framep;
 begin
 calc := f↑.calcs;
 val := f↑.val;			(* frames current value *)
 while calc <> nil do		(* update everyone we're affixed to *)
  with calc↑ do
   begin
   f2 := other;
   if (ntype = calcnode) and (rigid or (not frame1)) then
     begin			(* see if we need to update this frame *)
     if f2↑.valid <> 0 then		(* haven't updated it yet *)
	begin
	if tvarp then t := tvar↑.t else t := tval; (* explicitly named trans? *)
	if frame1 then t := tinvrt(t);	(* second := inv(trans) * first *)
	upTrans(f2↑.val,ttmul(t,val));	(* first := trans * second *)
	f2↑.valid := 0;			(* Mark it as now valid *)
	stvals(f2);			(* and go update its affixments *)
	end
     end
    else
     begin			(* need to update relation trans *)
     t := feval(f2);			(* get a value for f2 *)
     t := ttmul(val,tinvrt(t));		(* compute new relation trans *)
     if tvarp then upTrans(tvar↑.t,t) 
      else
       begin
       upTrans(tval,t); (* store it *)
       c2 := f2↑.calcs;		(* now go fix trans up in f2's calc list *)
       while c2↑.other <> f do c2 := c2↑.next;	(* find other calc of pair *)
       upTrans(c2↑.tval,t);	(* copy trans to it too *)
       end;
     end;
   calc := calc↑.next;		(* move on to next one *)
   end;
 end;

procedure change (f: framep; res: nodep); external;
procedure change ;
 var calc: nodep; b: boolean;
 begin
 if f↑.dcntr=0 then		(* if not dynamic *)
   begin
   nextTime;
   b := invalidate(f);	(* b = true if any non-rigid affixments need updating *)
   f↑.val := res↑.t;			(* copy trans pointer *)
   f↑.val↑.refcnt:=f↑.val↑.refcnt + 1;	(* mark trans in use *)
   f↑.valid := 0;			(* mark us as having a valid value *)
   if b then stvals(f);	(* go fix up the non-rigid relationships *)
   end
  else begin
       pp20L('Can''t assign to dyna',20); pp10('mic frames',10); ppLine;
	(* maybe also give name of frame?? *)
       end;
 end;

procedure getDevice (f: framep; r: nodep); external;
procedure getDevice ;
 var i: integer; 
 begin
 if f↑.sdev then 
   with msg↑ do
    begin
    cmd := wherecmd;
    dev := f↑.mech;
    getReply;			(* have ARM servo read in the hand/device value *)
    r↑.s := val;
    r↑.ltype := svaltype;
    end
  else
   r↑.t := whereArm(f↑.mech);	(* go read in the arm's position *)
 end;

procedure getFrame (f: framep; r: nodep); external;
procedure getFrame ;
 begin
 if not f↑.ftype then getDevice(f,r)	(* If device get its current value *)
  else					(* frame *)
   begin
   if (f↑.dcntr<>0) or (f↑.valid<>0) then  (* dynamic frame or not valid? *)
    begin			(* Need to calculate current value *)
    nextTime;			(* update eval time *)
    eval(f);			(* try to evaluate the variable *)
    end;
   r↑.t := f↑.val;		(* copy trans pointer *)
   if r↑.t = nil then r↑.t := niltrans;	(* always return something *)
					(* complain though??? *)
   end;
 end;

(* aux routine: getVal *)

procedure getVal (level, offset: byte); external;
procedure getVal ;
 var entry: enventryp; res: nodep;
 begin
 entry := getVar(level,offset);	(* look up environment entry for variable *)
 res := newNode;
 res↑.ntype := leafnode;
 res↑.ltype := entry↑.etype;		(* copy datatype of result *)
 if entry↑.etype = svaltype then res↑.s := entry↑.s	(* it's a scalar *)
  else if entry↑.etype <> frametype then (* it's a vector, trans or string *)
   with res↑ do
    begin
    v := entry↑.v;		(* copy pointer *)
    str := entry↑.str;
    if v = nil then
     if ltype = vectype then v := nilvect
     else if ltype = transtype then t := niltrans
     else length := 0;
				(* complain??? *)
    end
  else
    begin
    res↑.ltype := transtype;
    getFrame(entry↑.f,res);
    end;
 push(res);			(* store the value on the stack *)
 end;

(* Aux routine: setVal *)

procedure setVal (level, offset: byte); external;
procedure setVal ;
 var entry: enventryp; res: nodep;
 begin
 entry := getVar(level,offset);	(* look up environment entry for variable *)
 res := pop;			(* pop value off of stack *)
 with entry↑ do
  if etype = svaltype then s := res↑.s		(* it's a scalar *)
   else if etype = vectype then
	 begin
	 with res↑.v↑ do refcnt := refcnt + 1;	(* indicate new vector is in use *)
	 if v <> nil then
	  begin
	  v↑.refcnt := v↑.refcnt - 1;		(* we're done with vector now *)
	  if v↑.refcnt <= 0 then relVector(v);	(* release it if no one wants it *)
	  end;
	 v := res↑.v;				(* copy new vector pointer *)
	 end
   else if etype = transtype then upTrans(t,res↑.t) (* update trans with new value *)
   else if etype = strngtype then
	 begin
	 length := res↑.length;
	 str := res↑.str;			(* copy new string pointer *)
	 end
   else change(f,res);	(* change frame's value, updating affixed frames *)
 relNode(res);		(* free node up *)
 end;

(* affixment auxiliary routines: affixaux, unfixaux & unfix *)

procedure affixaux (f, d: framep; cnt: integer); external;
procedure affixaux ;
 var c1,c2,ct: nodep;
 begin
 with f↑ do
  if not (ftype and (dev <> nil)) then		(* haven't marked it yet *)
   begin
   if not ftype then cnt := 1			(* it's a device *)
    else begin dev := d; dcntr := cnt; cnt := cnt + 1; end;	(* mark frame *)
   c1 := calcs;
   ct := nil;
   while c1 <> nil do
    begin				(* mark everyone it's affixed to *)
    if c1↑.rigid or not c1↑.frame1 then affixaux(c1↑.other,d,cnt)
     else if c1↑.other↑.dev = nil then
	   begin		(* need to break non-rigid affixment *)
				(* first splice calcs out of affixment lists *)
	   if ct = nil then calcs := c1↑.next else ct↑.next := c1↑.next;
	   c2 := c1↑.other↑.calcs;
	   ct := nil;
	   while c2↑.other <> f do begin ct := c2; c2 := c2↑.next; end;
	   if ct = nil then c1↑.other↑.calcs := c2↑.next else ct↑.next := c2↑.next;
	   if not c1↑.tvarp then
	     begin 			(* release relation trans *)
	     upTrans(c1↑.tval,nil);
	     upTrans(c2↑.tval,nil);
	     end;
	   relNode(c1);			(* finally release calc nodes *)
	   relNode(c2);
	   c1 := ct;
	   end;
    ct := c1;
    c1 := c1↑.next;
    end;
   end;
  end;

function unfixaux (f: framep; cnt: integer): boolean; external;
function unfixaux ;
 var c: nodep; b: boolean; d: framep;
 begin
 b := false;
 with f↑ do
  if not ftype then affixaux(f,f,1)	(* a device - remark everyone as dynamic *)
   else if dev <> nil then  (* check we're still marked as dynamic, else done *)
    if cnt > dcntr then
      begin
      d := dev; dev := nil;		(* so affixaux will mark us *)
      affixaux(f,d,dcntr);		(* need to remark everyone *)
      end
     else
      begin				(* unmark us *)
      dev := nil;
      dcntr := 0;
      b := true;
      c := calcs;
      while (c <> nil) and b do
	begin
	b := unfixaux(c↑.other,cnt);
	c := c↑.next
	end
      end;
 unfixaux := b;
 end;

procedure unfix (f1,f2: framep); external;
procedure unfix ;
 var t: transp; c1, c2: nodep; b: boolean; i: integer;
 begin
 if f1↑.ftype then t := feval(f1);	(* try to get a value for both *)
 if f2↑.ftype then t := feval(f2);	(* if they're frames *)
 c1 := f1↑.calcs;		(* unfix f1 from f2 *)
 c2 := nil;
 b := true;
 while (c1 <> nil) and b do
  if c1↑.other = f2 then
    begin			(* found calc - splice it out of list *)
    b := false;
    if c2 = nil then f1↑.calcs := c1↑.next else c2↑.next := c1↑.next;
    if not c1↑.tvarp then upTrans(c1↑.tval,nil);   (* release old trans values *)
    relNode(c1);		(* done with calc node *)
    end
   else begin c2 := c1; c1 := c1↑.next end;	(* try next *)
 c1 := f2↑.calcs;		(* now unfix f2 from f1 *)
 c2 := nil;
 b := true;
 while (c1 <> nil) and b do
  if c1↑.other = f1 then
    begin			(* found calc - splice it out of list *)
    b := false;
    if c2 = nil then f2↑.calcs := c1↑.next else c2↑.next := c1↑.next;
    if not c1↑.tvarp then upTrans(c1↑.tval,nil);   (* release old trans values *)
    relNode(c1);		(* done with calc node *)
    end
   else begin c2 := c1; c1 := c1↑.next end;	(* try next *)
 if not f1↑.ftype then b := unfixaux(f2,0)	(* f2 no longer dynamic *)
  else if not f2↑.ftype then b := unfixaux(f1,0)	(* f1 no longer dynamic *)
  else if f1↑.dev <> nil then		(* both currently dynamic *)
	if f1↑.dcntr < f2↑.dcntr then b := unfixaux(f2,f1↑.dcntr) (* unmark f2 *)
	 else b := unfixaux(f1,f2↑.dcntr);	(* unmark f1 *)
 end;
(* Aux routines to destroy variables: killVar, killEnv *)

procedure killVar(e: enventryp); external;
procedure killVar;
 var j,k,size: integer; envhdr: envheaderp; env,eo: environp; ep: enventryp;
     b,bo: nodep; pp: pdbp; cp: cmoncbp;
 begin
  with e↑ do
   case etype of
(* don't need to do anything for scalars & strings *)
vectype:   if v <> nil then			(* check for old value *)
	    begin
	    v↑.refcnt := v↑.refcnt - 1;		(* we're done with vector now *)
	    if v↑.refcnt <= 0 then relVector(v);  (* release it if no one else wants it *)
	    end;
transtype: upTrans(t,nil);
frametype: begin
	   while f↑.calcs <> nil do
	    unfix(f,f↑.calcs↑.other);		(* unfix us from everyone *)
	   upTrans(f↑.val,nil);			(* flush our current value *)
	   relFrame(f);				(* flush frame *)
	   end;
eventtype: begin
	   (* *** what to do with those processes waiting on this event? *** *)
	   pp := evt↑.waitlist;
	   while pp <> nil do
	    begin pp↑.status := nullqueue; pp := pp↑.next end;
	   freeEvent(evt);
	   end;
cmontype:  repeat
	    if c↑.cmon↑.oncond↑.ntype = forcenode then freeEvent(c↑.evt);
	    freePdb(c↑.pdb);		(* now it's ok to flush its pdb *)
	    cp := c↑.oldcmon;		(* did we have several copies active? *)
	    relCmoncb(c);		(* and also free up its cmoncb *)
	    c := cp;
	   until cp = nil;
arraytype: begin
	   b := e↑.bnds;
	   size := b↑.mult * (b↑.ub - b↑.lb + 1); (* get array size *)
	   while b <> nil do begin bo := b; b := b↑.next; relNode(bo) end;
	   envhdr := e↑.a;
	   env := envhdr↑.env[0];
	   relEheader(envhdr);
	   j := -1;
	   for k := 1 to size do
	    begin
	    if j = 9 then
	      begin eo := env; env := env↑.next; relEnvironment(eo); j := 0 end
	     else j := j + 1;
	    ep := env↑.vals[j];
	    killVar(ep);		(* kill variable environment entry *)
	    end;
	   relEnvironment(env);
	   end;
	(* nothing to do for procedures or indirect references *)
otherwise {do nothing};
    end;
   relEentry(e);
   e := nil;
 end;

procedure killEnv; external;
procedure killEnv;
 var envhdr: envheaderp; envir,eo: environp; e: enventryp; j: integer;
 begin
 if (curInt↑.env <> sysEnv) and (curInt↑.env↑.varcnt < 255) then
   begin	(* varcnt check is so flushall doesn't have us kill it twice *)
   with curInt↑ do
    begin
    envhdr := env;
    env := envhdr↑.parent;
    end;
   envhdr↑.varcnt := 255;
   envir := envhdr↑.env[0];
   relEheader(envhdr);
   j := 0;
   while envir <> nil do           (* deallocate variables *)
    begin
    e := envir↑.vals[j];
    if e <> nil then killVar(e);   (* kill var's environment entry *)
    if j = 9 then
      begin
      eo := envir;
      envir := envir↑.next;
      relEnvironment(eo);
      j := 0
      end
     else j := j + 1;
    end;
   end
  else curInt↑.env := sysEnv;
 end;