How to implement this cycle (coordinate rotation)

G21 G61

; (X1,Y2,Z1)                (X2,Y2,Z2)
;      |                         |
;      |                         |
;      -----------------@---------
;      |                         |
;      |                         |
;      |                         |
;      |                         |
; (X1,Y1,Z4)                (X2,Y1,Z3)

O100 SUB
; normalised positions of the bar and working point
#<pX> = [[#1 - #<_X1>] / [#<_X2> - #<_X1>]]
#<pY> = [[#2 - #<_Y1>] / [#<_Y2> - #<_Y1>]]

; Z height of left and right ends of bar
#<ZL> = [#<_Z4> + #<pY> * [#<_Z1> - #<_Z4>]]
#<ZR> = [#<_Z3> + #<pY> * [#<_Z2> - #<_Z3>]]

(DEBUG, pX = #<pX> pY = #<pY>)

; and return the result
O100 return [#<ZL> + #<pX> * [#<ZR> - #<ZL>]]
O100 endsub

; test data
#<_X1> = -20
#<_X2> = 20
#<_Y1> = -20
#<_Y2> = 20
#<_Z1> = 10
#<_Z2> = -10
#<_Z3> = -15
#<_Z4> = 20

;each corner should return the Z -height of the corner
O100 CALL [-20] [20] 
(DEBUG, #<_value>)
O100 CALL [20] [20] 
(DEBUG, #<_value>)
O100 CALL [-20] [-20] 
(DEBUG, #<_value>)
O100 CALL [20] [-20] 
(DEBUG, #<_value>)

;and an arbitrary point
O100 CALL [11] [-12] 
(DEBUG, #<_value>)

; Trace the plane for the backplot
G0 X #<_X1> Y #<_Y1> Z#<_Z4>
G0 X #<_X1> Y #<_Y2> Z#<_Z1>
G0 X #<_X2> Y #<_Y2> Z#<_Z2>
G0 X #<_X2> Y #<_Y1> Z#<_Z3>
G0 X #<_X1> Y #<_Y1> Z#<_Z4>

; And a circle of 4 quadrants in that plane
G1 F1000
O100 call [20] [0]
G0 X20 Y0 Z#<_value>
O100 call [0] [-20]
G2 X0 Y-20 Z#<_value> R20
O100 call [-20] [0]
G2 X-20 Y0 Z#<_value> R20
O100 call [0] [20]
G2 X0 Y20 Z#<_value> R20
O100 call [20] [0]
G2 X20 Y0 Z#<_value> R20

M2