common.maude

fmod SIGN is
 pr FLOAT .

 var V : Float .

 op sign : Float -> Float .
 eq sign(0.0) = 0.0 .
 ceq sign(V) = - 1.0
  if V < 0.0 .
 ceq sign(V) = 1.0
  if V > 0.0 .
endfm

fmod COSTMAP is
 pr CONVERSION .
 pr INT .

 vars X Y NC NR N N' : Int .
 var  F FX FY FNC : Float .
 vars NL NL' : IntList .
 var  CM : CostMap .
 vars S S' : String .
 var  NT : Nat .

 sort IntList CostMap .
 subsort Int < IntList .

 op mtIL : -> IntList [ctor] .
 op _,_ : IntList IntList -> IntList [ctor assoc id: mtIL] .
 op {_} : IntList -> CostMap [ctor] .

 op |_| : IntList -> Nat .
 eq | N | = 1 .
 eq | N, NL | = s(| NL |) .

 ***        CostMap   X     Y   NCOLS
 op open? : CostMap Float Float  Float  -> Bool [memo] .
 ceq open?(CM, FX, FY, FNC) = open?(CM, X, Y, NC)
  if X := float2nat(FX) /\
     Y := float2nat(FY) /\
     NC := float2nat(FNC) .

 op open? : CostMap Nat Nat Nat  -> Bool .
 eq open?(CM, X, Y, NC) = check(get(CM, X, Y, NC)) .

 *** Uses faster Python version if available
 *** op get : CostMap Nat Nat Nat -> Float .
 op get : CostMap Nat Nat Nat -> Float [special (id-hook SpecialHubSymbol)] .
 ceq get(CM, X, Y, NC) = float(N')
  if N := (NC * Y) + X /\
     N' := skipN&Get(CM, N) .

 op skipN&Get : CostMap Nat -> Int .
 eq skipN&Get({N, NL'}, 0) = N .
 eq skipN&Get({N, NL'}, s(NT)) = skipN&Get({NL'}, NT) .
 eq skipN&Get(CM, NT) = - 1 [owise] .

 op check : Float -> Bool .
 eq check(F) = F < 254.0 .

 ops movCost diagMovCost cellCost obstacleCost mapCost costNeutral stepSize pathEpsilon : -> Float .
 eq movCost = 100.0 .
 eq diagMovCost = 140.0 .
 eq cellCost = 50.0 .
 eq obstacleCost = 254.0 .
 eq mapCost = 50.0 .
 eq costNeutral = 50.0 .
 eq stepSize = 0.5 .
 *** This number (2.0 ^ -23.0) is the machine epsilon of the single precision
 *** floating-point type in IEEE 754 (the float type of C++ used by ROS).
 *** No constant can be found such that all oscillations are detected exactly
 *** as in ROS, because 5.5 times this number makes some be detected earlier
 *** and some others later.
 eq pathEpsilon = 1.1920928955078125e-07 .

 op float2nat : Float -> Nat [memo] .
 eq float2nat(F) = rat(floor(F)) .
endfm

fmod POSE is
 pr EXT-BOOL .
 pr COSTMAP .
 pr SIGN .
 pr INT .

 vars N NR NC X Y Z X' Y' Z' : Nat .
 vars Q Q' : Quaternion .
 vars H V FX FY DX DY : Float .
 var  CM : CostMap .
 var  NOW : Pose .

 sorts BasicPose Pose Quaternion Point .

 *** Simplified representation for angles
 subsort Int < Quaternion .

 *** Simplified pose for 2D
 subsort BasicPose < Pose .

 op getCost : Int ~> Float [memo] .
 eq getCost(N) = if (N rem 90) == 0
                 then movCost
                 else diagMovCost
                 fi .

 op {_,_,_} : Nat Nat Nat -> Point [ctor] .
 op {_,_,_} : Float Float Float -> Point [ctor] .
 op {_,_,_,_} : Float Float Float Float -> Quaternion [ctor] .
 op __ : Point Quaternion -> Pose [ctor] .

 op {_,_} : Nat Nat -> BasicPose [ctor] .
 op {_,_} : Float Float -> BasicPose [ctor] .

 op noPose : -> Pose [ctor] .

 op h : Pose Pose -> Float .
 ceq h({X, Y, Z} Q, {X', Y', Z'} Q') = sqrt((H ^ 2.0) + (V ^ 2.0)) * costNeutral
  if H := float(sd(X, X')) /\
     V := float(sd(Y, Y')) .
 ceq h({X, Y}, {X', Y'}) = sqrt((H ^ 2.0) + (V ^ 2.0)) * costNeutral
  if H := float(sd(X, X')) /\
     V := float(sd(Y, Y')) .

 op closest : BasicPose Float Float ~> BasicPose .
 eq closest({FX, FY}, DX, DY) = closest({FX + sign(DX) * closest(abs(DX)), FY + sign(DY) * closest(abs(DY))}) .

 op closest : BasicPose ~> BasicPose .
 eq closest({FX, FY}) = {float2nat(FX), float2nat(FY)} .

 op closest : Float -> Float .
 eq closest(FX) = if _-_(ceiling(FX), FX) <= _-_(FX, floor(FX))
                  then ceiling(FX)
                  else floor(FX)
                  fi .
endfm

view Pose from TRIV to POSE is
 sort Elt to Pose .
endv

fmod POTENTIAL is
 pr POSE .
 pr INT .

 ops infinite minusInfinite : -> Float .
 eq infinite = 1e10 .
 eq minusInfinite = - 1e10 .

 sort Row Potential .
 subsort Float < Row < Potential .

 op noNum : -> Row [ctor] .
 op _._ : Row Row -> Row [ctor assoc id: noNum] .

 op noRow : -> Potential [ctor] .
 op __ : Potential Potential -> Potential [ctor assoc id: noRow] .

 vars N N' N1 N2 NR NC X Y : Nat .
 vars I I' F F' : Float .
 vars P P' : Potential .
 vars R R' : Row .
 var  NOW : Pose .

 op _@[_,_] : Potential Nat Nat ~> Float .
 eq (R P) @ [N, 0] = R [N] .
 eq (R P) @ [N, s(N')] = P @ [N, N'] .

 op _[_] : Row Nat ~> Float .
 eq (I . R) [0] = I .
 eq (I . R) [s(N)] = R [N] .

 op _[_;_\\_] : Potential Nat Nat Float ~> Potential .
 eq (R P) [N ; 0 \\ I] = (R [N \\ I]) P .
 eq (R P) [N ; s(N') \\ I] = R (P [N ; N' \\ I]) .

 op _[_\\_] : Row Nat Float ~> Row .
 eq (I . R) [0 \\ I'] = I' . R .
 eq (I . R) [s(N) \\ I'] = I . (R [N \\ I']) .

 ***          Rows Cols  Init Init
 op initialP* : Nat  Nat   Nat Nat  -> Potential .
 ceq initialP*(NC, NR, N1, N2) = P [N1 ; N2 \\ 0.0]
  if P := initialP(NC, NR) .

 op initialP : Nat Nat -> Potential .
 eq initialP(NC, 0) = noRow .
 eq initialP(NC, s(NR)) = initialRow(NC) initialP(NC, NR) .

 op initialRow : Nat -> Row [memo] .
 eq initialRow(0) = noNum .
 eq initialRow(s(N)) = infinite . initialRow(N) .

 *** It returns true if all exist and are open
 op allNeighborsOpen : Potential Pose Nat Nat -> Bool .
 eq allNeighborsOpen(P, NOW, NC, NR) = $allNeighborsOpen(P, NOW, NC, NR, 0) .

 op $allNeighborsOpen : Potential Pose Nat Nat Nat -> Bool .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 270) =
               if (s(Y) < NR)
               then (P @[X, s(Y)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 315) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 225) =
               if (X > 0) and (s(Y) < NR)
               then (P @[sd(X, 1), s(Y)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 270) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 180) =
               if X > 0
               then (P @[sd(X, 1), Y] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 225) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 135) =
               if X > 0 and Y > 0
               then (P @[sd(X, 1), sd(Y, 1)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 180) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 90) =
               if Y > 0
               then (P @[X, sd(Y, 1)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 135) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 45) =
               if (s(X) < NC) and (Y > 0)
               then (P @[s(X),  sd(Y, 1)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 90) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 0) =
               if (s(X) < NC)
               then (P @[s(X),  Y] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 45) .
 eq $allNeighborsOpen(P, {X, Y}, NC, NR, 315) =
               if (s(X) < NC) and (s(Y) < NR)
               then (P @[s(X), s(Y)] < infinite)
               else false
               fi and-then
               $allNeighborsOpen(P, {X, Y}, NC, NR, 360) .
 eq $allNeighborsOpen(P, NOW, NC, NR, N) = true [owise] .
endfm

fmod PATH is
 inc LIST{Pose} * (sort List{Pose} to Path,
                   sort NeList{Pose} to NePath,
                   op nil to noPath) .
 vars PT PT' : Path .
 var  P : Pose .

 *** We cannot remove poses because we need repetitions for detecting loops
 *** eq P PT P = P PT .

 op _in_ : Pose Path -> Bool .
 eq P in (PT P PT') = true .
 eq P in PT = false [owise] .
endfm

fmod GRADIENT is
 pr FLOAT .
 pr NAT .

 sort GCell GRow Gradient .
 subsort GCell < GRow < Gradient .

 op <_,_> : Float Float -> GCell [ctor] .

 op mtGR : -> GRow [ctor] .
 op __ : GRow GRow -> GRow [ctor assoc id: mtGR] .

 op mtGradient : -> Gradient [ctor] .
 op _,_ : Gradient Gradient -> Gradient [ctor assoc id: mtGradient] .

 vars X Y N N' : Nat .
 vars GC GC' : GCell .
 var  G : Gradient .
 var  GR : GRow .
 var  F : Float .

 op _[[_,_]] : Gradient Nat Nat -> GCell .
 eq (GR, G) [[0, Y]] = GR [Y] .
 eq (GR, G) [[s(X), Y]] = G [[X, Y]] .
 eq G [[X,Y]] = < 0.0, 0.0 > [owise] .

 op _[_] : GRow Nat -> GCell .
 eq (GC GR) [0] = GC .
 eq (GC GR) [s(Y)] = GR [Y] .
 eq GR [Y] = < 0.0, 0.0 > [owise] .

 op initialGradient : Nat Nat -> Gradient .
 eq initialGradient(0, N') = mtGradient .
 eq initialGradient(s(N), N') = initialGRow(N'), initialGradient(N, N') .

 op initialGRow : Nat -> GRow [memo] .
 eq initialGRow(0) = mtGR .
 eq initialGRow(s(N)) = < 0.0, 0.0 > initialGRow(N) .

 op _|_,_|->_ : Gradient Nat Nat GCell -> Gradient .
 eq (GR, G) | 0, Y |-> GC = (GR [Y] -> GC), G .
 eq (GR, G) | s(X), Y |-> GC = GR, (G | X, Y |-> GC) .
 eq G | X,Y |-> GC = G [owise] .

 op _[_]->_ : GRow Nat GCell -> GRow .
 eq (GC GR) [0] -> GC' = GC' GR .
 eq (GC GR) [s(Y)] -> GC' = GC (GR [Y] -> GC') .
 eq GR [Y] -> GC' = GR [owise] .
endfm

fmod BASIC-TRAVERSE is
 pr POTENTIAL .
 pr LIST{Nat} .
 pr PATH .

 vars H T D DX DY SS FX FY FX' FY' FX'' FY'' : Float .
 vars X Y Z X' Y' Z' XI YI XC YC XN YN NR NC I NEW N V MINV : Nat .
 vars INIT GOAL ACCP NOW : Pose .
 vars Q Q' : Quaternion .
 vars P P' : Potential .
 var  LN : List{Nat} .
 var  CM : CostMap .
 var  PT : Path .

 *** Basic computation without gradient
 op computePath : Pose Pose Potential Nat Nat -> Path .
 ceq computePath({X, Y, Z} Q, GOAL, P, NC, NR) = noPath
  if P @ [X, Y] == infinite .
 eq computePath({X, Y, Z} Q, GOAL, P, NC, NR) = $computePath({X, Y}, GOAL, P, NC, NR, noPath) [owise] .

 op $computePath : Pose Pose Potential Nat Nat Path -> Path .
 eq $computePath({X, Y}, {X, Y, Z} Q, P, NC, NR, PT) = PT ({X, Y, Z} Q) .
 ceq $computePath({X, Y}, GOAL, P, NC, NR, PT) = $computePath(ACCP, GOAL, P, NC, NR, PT ({X, Y, 0} 0))
  if ACCP := getMin({X, Y}, P, NC, NR) [owise] .

 op getMin : Pose Potential Nat List{Nat} -> Pose .
 eq getMin({X, Y}, P, NC, NR) = getMin({X, Y}, P, NC, NR, {X, Y}, float2nat(P @ [X, Y]), 135 90 45 180 0 225 270 315) .

 op getMin : Pose Potential Nat Nat Pose Nat List{Nat} -> Pose .
 ceq getMin({X, Y}, P, NC, NR, ACCP, MINV, N LN) = if V < MINV
                                                   then getMin({X, Y}, P, NC, NR, {X', Y'}, V, LN)
                                                   else getMin({X, Y}, P, NC, NR, ACCP, MINV, LN)
                                                   fi
  if < X', Y' > := getMove({X, Y}, N, NC, NR) /\
     V := float2nat(P @ [X', Y']) .
 ceq getMin({X, Y}, P, NC, NR, ACCP, MINV, N LN) = getMin({X, Y}, P, NC, NR, ACCP, MINV, LN)
  if not (getMove({X, Y}, N, NC, NR) :: Pair) .
 eq getMin(INIT, P, NC, NR, ACCP, MINV, nil) = ACCP .

 sort Pair .
 op <_,_> : Nat Nat -> Pair [ctor] .
 op getMove : Pose Nat Nat Nat ~> Pair .
 ceq getMove({X, Y}, 270, NC, NR)   = <  X, s(Y) >
  if s(Y) < NR .
 ceq getMove({X, Y}, 225, NC, NR)  = <  sd(X, 1), s(Y) >
  if X > 0 /\
     s(Y) < NR .
 ceq getMove({X, Y}, 180, NC, NR)  = <  sd(X, 1), Y >
  if X > 0 .
 ceq getMove({X, Y}, 135, NC, NR) = < sd(X, 1), sd(Y, 1) >
  if X > 0 /\
     Y > 0 .
 ceq getMove({X, Y}, 90, NC, NR) = < X,  sd(Y, 1) >
  if Y > 0 .
 ceq getMove({X, Y}, 45, NC, NR) = < s(X),  sd(Y, 1) >
  if s(X) < NC /\
     Y > 0 .
 ceq getMove({X, Y}, 0, NC, NR) = <  s(X),  Y >
  if s(X) < NC .
 ceq getMove({X, Y}, 315, NC, NR) = <  s(X),  s(Y) >
  if s(X) < NC /\
     s(Y) < NR .
endfm

fmod GRADIENT-TRAVERSAL is
 pr BASIC-TRAVERSE .
 pr POTENTIAL .
 pr GRADIENT .
 pr PATH .
 pr SIGN .

 vars V T D DX DY DX' DY' DX'' DY'' DDX DDY SS FX FY FX' FY' FX'' FY'' FZ FZ' NORM EX SX SEX EY SY SEY CX CY XX YY DXAUX1 DXAUX2 DYAUX1 DYAUX2 : Float .
 vars X Y Z X' Y' Z' XI YI XC YC XN YN NR NC I NEW N : Nat .
 vars G G' G1 G2 G3 : Gradient .
 vars INIT GOAL CURR PS : Pose .
 var  P : Potential .
 var  CM : CostMap .
 var  PT : Path .

 ***                                INIT GOAL STEP SIZE
 op computePath : Potential Pose Pose   Float   Gradient Nat Nat Nat ~> Path .
 eq computePath(P, {X, Y}, {X', Y'}, SS, G, NC, NR, N) = if P @ [X, Y] == infinite then noPath else
      computePath(P, {float(X'), float(Y')}, SS, G, NC, NR, N, {float(X), float(Y)}, 0.0, 0.0, noPath) fi .

 ***                                GOAL STEP SIZE                            DX    DY    ACC
 op computePath : Potential Pose   Float   Gradient Nat Nat Nat Pose Float Float Path ~> Path .
 eq computePath(P, GOAL, SS, G, NC, NR, 0, CURR, DX, DY, PT) = noPath .
 ceq computePath(P, {FX, FY}, SS, G, NC, NR, s(N), {FX', FY'}, DX, DY, PT) = PT ({FX, FY, 0.0} 0) *** ({FX', FY', 0.0} 0)
  if {X, Y} := closest({FX', FY'}, DX, DY) /\
     (P @ [X, Y]) < costNeutral [print "Eq2 " FX' ", " FY'] .
 ceq computePath(P, GOAL, SS, G, NC, NR, s(N), {FX, FY}, DX, DY, PT) =
                               computePath(P, GOAL, SS, G, NC, NR, N, {float(X'), float(Y')}, 0.0, 0.0, PT ({FX', FY', 0.0} 0))
  if FX' := FX + DX /\
     FY' := FY + DY /\
     {X, Y} := closest({FX, FY}, DX, DY) /\
     (P @ [X, Y]) >= costNeutral /\
     ((not allNeighborsOpen(P, {float2nat(FX),float2nat(FY)}, NC, NR)) or loop(PT)) /\
     {X', Y'} := getMin({float2nat(FX),float2nat(FY)}, P, NC, NR) [print "Eq3 " FX ", " FY " Con desplazamiento: " FX' ", " FY' " Cercano: " X ", " Y] .
 ceq computePath(P, GOAL, SS, G, NC, NR, s(N), {FX, FY}, DX, DY, PT) = noPath
  if X' := float2nat(FX) /\
     Y' := float2nat(FY) /\
     {X, Y} := closest({FX, FY}, DX, DY) /\
     (P @ [X, Y]) >= costNeutral /\
     allNeighborsOpen(P, {X',Y'}, NC, NR) /\
     not loop(PT) /\
     < CX, CY, G1 > := update&get(G, P, X', Y', NC, NR) /\
     < SX, SY, G2 > := update&get(G1, P, s(X'), Y', NC, NR) /\
     < SEX, SEY, G3 > := update&get(G2, P, s(X'), s(Y'), NC, NR) /\
     < EX, EY, G' > := update&get(G3, P, X', s(Y'), NC, NR) /\
     DDX := _-_(1.0, DX) /\
     DDY := _-_(1.0, DY) /\
     XX := (DDY * (DDX * CX) + (DX * EX)) + DY * ((DDX * SX) + (DX * SEX)) /\
     YY := (DDY * (DDX * CY) + (DX * EY)) + DY * ((DDX * SY) + (DX * SEY)) /\
     XX == 0.0 /\
     YY == 0.0 . *** [print "Eq4 " FX ", " FY " Con desplazamiento: " FX' ", " FY' ] .
 ceq computePath(P, GOAL, SS, G, NC, NR, s(N), {FX, FY}, DX, DY, PT) =
                               computePath(P, GOAL, SS, G', NC, NR, N, {FX'', FY''}, DX'', DY'', PT ({FX', FY', 0.0} 0))
  if FX' := FX + DX /\
     FY' := FY + DY /\
     X' := float2nat(FX) /\
     Y' := float2nat(FY) /\
     {X, Y} := closest({FX, FY}, DX, DY) /\
     (P @ [X, Y]) >= costNeutral /\
     allNeighborsOpen(P, {X',Y'}, NC, NR) /\
     not loop(PT) /\
     < CX, CY, G1 > := update&get(G, P, X', Y', NC, NR) /\
     < SX, SY, G2 > := update&get(G1, P, s(X'), Y', NC, NR) /\
     < SEX, SEY, G3 > := update&get(G2, P, s(X'), s(Y'), NC, NR) /\
     < EX, EY, G' > := update&get(G3, P, X', s(Y'), NC, NR) /\
     DDX := _-_(1.0, DX) /\
     DDY := _-_(1.0, DY) /\
     XX := DDY * (DDX * CX + DX * SX) + DY * (DDX * EX + DX * SEX) /\
     YY := DDY * (DDX * CY + DX * SY) + DY * (DDX * EY + DX * SEY) /\
     ((XX =/= 0.0) or (YY =/= 0.0)) /\
     DX' := DX + XX * (SS / sqrt((XX * XX) + (YY * YY))) /\
     DY' := DY + YY * (SS / sqrt((XX * XX) + (YY * YY))) /\
     FX'' := if abs(DX') > 1.0
             then FX + sign(DX')
             else FX
             fi /\
     DX'' := if abs(DX') > 1.0
             then _-_(DX', sign(DX'))
             else DX'
             fi /\
     FY'' := if abs(DY') > 1.0
             then FY + sign(DY')
             else FY
             fi /\
     DY'' := if abs(DY') > 1.0
             then _-_(DY', sign(DY'))
             else DY'
             fi [print "Eq3 " FX ", " FY " Con desplazamiento: " FX' ", " FY' " y " DX ", " DY] .

 sort GradientUpdateRes .
 op <_,_,_> : Float Float Gradient -> GradientUpdateRes [ctor] .

 op update&get : Gradient Potential Nat Nat Nat Nat -> GradientUpdateRes .
 ceq update&get(G, P, X, Y, NC, NR) = < FX, FY, G >
  if < FX, FY > := (G [[X, Y]]) /\
     < FX, FY > =/= < 0.0, 0.0 > .
 ceq update&get(G, P, X, Y, NC, NR) = < 0.0, 0.0, G >
  if (G [[X, Y]]) == < 0.0, 0.0 > /\
     ((X == 0) or (s(X) == NC) or (Y == 0) or (s(Y) == NR)) .
 ceq update&get(G, P, X, Y, NC, NR) = < DX', DY', G' >
  if G [[X, Y]] == < 0.0, 0.0 > /\
     ((X =/= 0) and (s(X) =/= NC) and (Y =/= 0) and (s(Y) =/= NR)) /\
     (P @ [X, Y]) == infinite /\
     DX := if (P @ [sd(X, 1), Y]) < infinite
           then - obstacleCost
           else if (P @ [s(X), Y]) < infinite
                then obstacleCost
                else 0.0
                fi
           fi /\
     DY := if (P @ [X, sd(Y, 1)]) < infinite
           then - obstacleCost
           else if (P @ [X, s(Y)]) < infinite
                then obstacleCost
                else 0.0
                fi
           fi /\
     NORM := sqrt((DX * DX) + (DY * DY)) /\
     DX' := if NORM > 0.0 then DX / NORM else DX fi /\
     DY' := if NORM > 0.0 then DY / NORM else DY fi /\
     G' := (G | X, Y |-> < DX', DY' >) .
 ceq update&get(G, P, X, Y, NC, NR) = < DX', DY', G' >
  if G [[X, Y]] == < 0.0, 0.0 > /\
     ((X =/= 0) and (s(X) =/= NC) and (Y =/= 0) and (s(Y) =/= NR)) /\
     (P @ [X, Y]) < infinite /\
     DXAUX1 := if (P @ [s(X), Y]) < infinite
               then _-_((P @ [X, Y]), (P @ [s(X), Y]))
               else 0.0
               fi /\
     DXAUX2 := if (P @ [sd(X, 1), Y]) < infinite
               then _-_((P @ [sd(X, 1), Y]), (P @ [X, Y]))
               else 0.0
               fi /\
     DX := DXAUX1 + DXAUX2 /\
     DYAUX1 := if (P @ [X, sd(Y, 1)]) < infinite
               then _-_((P @ [X, sd(Y, 1)]), (P @ [X, Y]))
               else 0.0
               fi /\
     DYAUX2 := if (P @ [X, s(Y)]) < infinite
               then _-_((P @ [X, Y]), (P @ [X, s(Y)]))
               else 0.0
               fi /\
     DY := DYAUX1 + DYAUX2 /\
     NORM := sqrt((DX * DX) + (DY * DY)) /\
     DX' := if NORM > 0.0 then DX / NORM else DX fi /\
     DY' := if NORM > 0.0 then DY / NORM else DY fi /\
     G' := (G | X, Y |-> < DX', DY' >) .

 op loop : Path -> Bool .
 eq loop(PT ({FX, FY, FZ} 0) PS ({FX', FY', FZ'} 0)) = (abs(_-_(FX, FX')) < pathEpsilon) and (abs(_-_(FY, FY')) < pathEpsilon) .
 eq loop(PT) = false [owise] .
endfm