Option Explicit

Dim RP As Object

Set RP=CreateObject("RazorProbe.C1")

If param1()=1 Then
  G31Init
Else
  If param1()=2 then
      G31Fix
  Else
    MsgBox "Bad parameter (P=" & param1() & ")"
    End
  End If
End If
Set RP=Nothing

End

Sub G31Fix()
  Dim status As Integer
  Dim touch As Boolean
  Dim TX As Double
  Dim TY As Double
  Dim FX As Double
  Dim FY As Double
  Dim G31X As Double
  Dim G31Y As Double
  Dim Xerr As Double
  Dim Yerr As Double
  Dim G01X As Double
  Dim G01Y As Double

  if isLoading() then
    exit sub
  end if

  touch=isActive(22)
  'here's where we subtract probe corrections
  TX=GetVar(2000)-GetVar(1009)
  TY=GetVar(2001)-GetVar(1010)
  FX=GetVar(1003)
  FY=GetVar(1004)

  status=RP.G31Fix(touch, TX, TY, FX, FY, G31X, G31Y, Xerr, Yerr, G01X, G01Y)

  If status=0 Then
    SetVar 1007,G31X
    SetVar 1008,G31Y
    SetVar 1009, Xerr
    SetVar 1010, Yerr
    SetVar 1011,G01X
    SetVar 1012,G01Y
  End If
  
  If status=1 Then 'finished
    code "G00 Z" & GetVar(1002)
    While isMoving()
    Wend
    MsgBox "Finished."
    DoButton (3) 'Stop
    DoButton (2) 'Rewind
  End If

  If status=2 Then 'lost the edge
    DoButton (3) 'Stop
    MsgBox "Lost the edge. Carefully Jog nearer the edge then Press Cycle Start."
  End If
End Sub

Sub G31Init()
  Dim F As Double
  Dim SZ As Double
  Dim SX As Double
  Dim SY As Double
  Dim PZ As Double
  Dim FBO As Double
  Dim G31X As Double
  Dim G31Y As Double
  Dim Xerr As Double
  Dim Yerr As Double

  RP.G31Init F, SZ, SX, SY, PZ, FBO, G31X, G31Y, Xerr, Yerr

  SetVar 1001,F
  SetVar 1002,SZ
  SetVar 1003,SX
  SetVar 1004,SY
  SetVar 1005,PZ
  SetVar 1006,FBO
  SetVar 1007,G31X
  SetVar 1008,G31Y
  SetVar 1009,Xerr
  SetVar 1010,Yerr
End Sub