*****************************************************************************
***          Position and Heading Controller                              ***
*****************************************************************************
fmod POSANDHEAD-CONSTRAINT is
  pr NAT .
  pr GRID .
  inc CONSTRAINT .

  sort PosAndHeadConstraint .  
  subsort PosAndHeadConstraint < Constraint .

  op ___ : Nat Nat Dir -> PosAndHeadConstraint . 
                   *** for now: constraint is new location and direction
                   *** subsequent versions will allow for delta-x,
                   *** delta-y, delta-theta, max velocity, etc. 

  op _@x : PosAndHeadConstraint -> Nat .
  op _@y : PosAndHeadConstraint -> Nat .
  op _@d : PosAndHeadConstraint -> Dir .

  vars x y : Nat .
  var d : Dir .

  eq (x y d)@x = x .
  eq (x y d)@y = y .
  eq (x y d)@d = d .
endfm

mod POSANDHEAD-CONTROLLER is 
  pr CONTROLLER . 
  inc POSANDHEAD-CONSTRAINT .
  inc POSANDHEAD-STATE-VARIABLE .
  inc PHCONTROLLER-PHACTUATOR-INTERFACE .

  sort PosAndHeadCtrlCid .
  subsort PosAndHeadCtrlCid < CtrlCid .
  
  *** Specification of `satisfy' 
  var phv :   PosAndHeadStateValue . 
  var phcstr : PosAndHeadConstraint .

  eq satisfy(phv,phcstr) = 
     if (phv)@x == (phcstr)@x and (phv)@y == (phcstr)@y
         and (phv)@d == (phcstr)@d
     then true 
     else false
     fi .

  *** Auxiliary functions to determine the COA          
  op turnFromTo : Dir Dir -> Command .
  op turnFor : Nat -> Command .
  op driveFor : Nat -> Command .
  op cmdList :  Command -> CommandList .

  
  *** Specification of `coa'
  *** Right now simple strategy has 3 steps
  *** 1. try to achieve new x loc by: 
  ***    a) possibly turning, then b) driving along x-axis
  *** 2. try to achieve new y loc by 
  ***    a) possibly turning, then b) driving along y-axis
  *** 3. try to achieve new direction by turning  
  
  eq coa(phv,phcstr) = 
     if (phv)@x < (phcstr)@x 
     then (if (phv)@y == (phcstr)@y
           then (cmdList(turnFromTo((phv)@d, E)) ; 
                 cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                 cmdList(turnFromTo(E, (phcstr)@d)))
           else (if (phv)@y < (phcstr)@y
                 then (cmdList(turnFromTo((phv)@d, E)) ; 
                       cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                       cmdList(turnFromTo(E, N)) ;
                       cmdList(driveFor(sd((phcstr)@y,(phv)@y))) ;
                       cmdList(turnFromTo(N, (phcstr)@d)) )
                 else (cmdList(turnFromTo((phv)@d, E)) ; 
                       cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                       cmdList(turnFromTo(E, S)) ;
                       cmdList(driveFor(sd((phv)@y,(phcstr)@y))) ;
	               cmdList(turnFromTo(S, (phcstr)@d)))
                fi)
           fi)
          *** use 'sd' since _-_ not defined for Nat
          *** _-_ also ambiguous with __  
     else (if (phv)@y == (phcstr)@y
           then (cmdList(turnFromTo((phv)@d, W)) ; 
                 cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                 cmdList(turnFromTo(W, (phcstr)@d)))
           else (if (phv)@y < (phcstr)@y
                then (cmdList(turnFromTo((phv)@d, W)) ; 
                      cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                      cmdList(turnFromTo(W, N)) ;
                      cmdList(driveFor(sd((phcstr)@y,(phv)@y))) ;
                      cmdList(turnFromTo(N, (phcstr)@d)))
                else (cmdList(turnFromTo((phv)@d, W)) ; 
                      cmdList(driveFor(sd((phcstr)@x,(phv)@x))) ;
                      cmdList(turnFromTo(W, S)) ;
                      cmdList(driveFor(sd((phv)@y,(phcstr)@y))) ;
	              cmdList(turnFromTo(S, (phcstr)@d)))
                fi)
          fi)
     fi .

     *** driving for n steps yields in a command list with n "drive" commands
     var n : Nat .
     eq cmdList(driveFor(0)) = nil .
     eq cmdList(driveFor(n)) = (drive ; cmdList(driveFor(sd(n,1)))) .
     
     *** since the rover turns in 45 degrees, clockwise, turning from one
     *** direction to another is a sequence of 45 degree turns in clockwise 
     *** direction
     *** dir-diff computes the number of 45 degree turns necessary
     var d d' : Dir .
     op dir-diff : Dir Dir -> Nat .
     eq dir-diff(E,E) = 0 .
     eq dir-diff(E,S) = 2 .
     eq dir-diff(E,W) = 4 .
     eq dir-diff(E,N) = 6 .
     eq dir-diff(S,S) = 0 .
     eq dir-diff(S,W) = 2 .
     eq dir-diff(S,N) = 4 .
     eq dir-diff(S,E) = 6 .
     eq dir-diff(W,W) = 0 .
     eq dir-diff(W,N) = 2 .
     eq dir-diff(W,E) = 4 .
     eq dir-diff(W,S) = 6 .
     eq dir-diff(N,N) = 0 .
     eq dir-diff(N,E) = 2 .
     eq dir-diff(N,S) = 4 .
     eq dir-diff(N,W) = 6 .

     op turnFor : Nat -> Command .
     eq cmdList(turnFromTo(d,d'))
        = cmdList(turnFor(dir-diff(d,d'))) .
     eq cmdList(turnFor(0)) = nil .
     eq cmdList(turnFor(n)) = (turn ; cmdList(turnFor(sd(n,1)))) .
     eq cmdList(turn ; turn ; turn ; turn ; turn ; turn ; turn ; turn) 
        = nil .
endm
		   
mod POSANDHEAD-CONTROLLER-TEST is
  inc POSANDHEAD-CONTROLLER .

  op PosAndHeadCtrl : -> PosAndHeadCtrlCid .

  op noOid : -> Oid .
  op myphctrl : -> Object .
  eq myphctrl =
     < o("MyPosAndHeadCtrl") : PosAndHeadCtrl | 
                 mysv( o("MyPosAndHeadStateVar")) ,
                 myactuator( o("MyPosAndHeadActuator")) ,
                 currentCstr(noCstr) ,
                 currentCstrReq(noOid) ,
                 currentSVVal(uk) ,
                 cmds(noCmd) ,
                 waitAfter(noMsg) > .
endm