&H NOBOUNDSCHECK
DEF RSIPI_Test()
  ; =========================================================================
  ; RSIPI Comprehensive Test Program
  ; =========================================================================
  ; Tests all RSIPI Python library functionality:
  ;   1. RSI initialisation and connection
  ;   2. Cartesian correction (RSI_MOVECORR)
  ;   3. Tech variable exchange (Python <-> KRL)
  ;   4. $SEN_PREA variable reading
  ;   5. Digital I/O coordination
  ;   6. Handshake patterns (wait for Python, signal back)
  ;
  ; Matching Python script: rsi_config/rsipi_test.py
  ;
  ; Protocol (Tech.C11 = state from KRL, Tech.T11 = command from Python):
  ;   State 0: Idle
  ;   State 1: RSI connected, waiting for Python
  ;   State 2: Running corrections (RSI_MOVECORR active)
  ;   State 3: Corrections complete, reading SEN_PREA
  ;   State 4: I/O test phase
  ;   State 5: Complete
  ;   State 99: Error
  ;
  ; Python commands via Tech.T11:
  ;   0: No command
  ;   1: Python ready, start corrections
  ;   2: Stop corrections
  ;   3: SEN_PREA values written, read them
  ;   4: Start I/O test
  ;   5: Shutdown
  ; =========================================================================

  DECL INT ret, CONTID
  DECL INT python_cmd
  DECL REAL sen_val1, sen_val2, sen_val3
  DECL E6POS start_pos

  INI

  ; -- Store start position ------------------------------------------------
  start_pos = $POS_ACT

  ; -- Move to a safe starting position ------------------------------------
  ; (Adjust these coordinates for your robot/cell)
  PTP start_pos

  ; ========================================================================
  ; PHASE 1: Initialise RSI
  ; ========================================================================

  ; Load RSI signal flow configuration
  ret = RSI_CREATE("RSIPI_Full.rsi", CONTID, TRUE)
  IF (ret <> RSIOK) THEN
    MsgNotify("RSI_CREATE failed", "RSIPI_Test")
    HALT
  ENDIF

  ; Activate RSI in RELATIVE mode at 4ms cycle
  ; Change to #ABSOLUTE / #IPO as needed
  ret = RSI_ON(#RELATIVE, #IPO_FAST)
  IF (ret <> RSIOK) THEN
    MsgNotify("RSI_ON failed", "RSIPI_Test")
    HALT
  ENDIF

  ; Signal state 1: RSI connected, waiting for Python
  $TECH.C[11] = 1
  MsgNotify("RSI active - waiting for Python...", "RSIPI_Test")

  ; ========================================================================
  ; PHASE 2: Wait for Python to connect and signal ready
  ; ========================================================================

  ; Poll Tech.T11 for Python's "ready" command (value = 1)
  python_cmd = 0
  WHILE (python_cmd <> 1)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012  ; Check every 12ms
  ENDWHILE

  MsgNotify("Python connected - starting corrections", "RSIPI_Test")

  ; Send current position to Python via Tech.C12-C17
  $TECH.C[12] = $POS_ACT.X
  $TECH.C[13] = $POS_ACT.Y
  $TECH.C[14] = $POS_ACT.Z
  $TECH.C[15] = $POS_ACT.A
  $TECH.C[16] = $POS_ACT.B
  $TECH.C[17] = $POS_ACT.C

  ; ========================================================================
  ; PHASE 3: Sensor-guided motion (Python sends corrections)
  ; ========================================================================

  ; Signal state 2: corrections active
  $TECH.C[11] = 2

  ; RSI_MOVECORR - robot is now purely controlled by Python corrections
  ; The robot will hold position and apply RKorr corrections from Python
  ; Python sends corrections via api.motion.update_cartesian()
  ; This blocks until Python sends stop command (Tech.T11 = 2)
  ; or until RSI is turned off
  RSI_MOVECORR()

  MsgNotify("Corrections phase complete", "RSIPI_Test")

  ; ========================================================================
  ; PHASE 4: Read $SEN_PREA values from Python
  ; ========================================================================

  ; Signal state 3: ready to read SEN_PREA
  $TECH.C[11] = 3

  ; Wait for Python to write SEN_PREA values (command = 3)
  python_cmd = 0
  WHILE (python_cmd <> 3)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012
  ENDWHILE

  ; Read values that Python wrote via MAP2SEN_PREA
  sen_val1 = $SEN_PREA[1]
  sen_val2 = $SEN_PREA[2]
  sen_val3 = $SEN_PREA[3]

  ; Echo them back to Python via Tech.C18-C20
  $TECH.C[18] = sen_val1
  $TECH.C[19] = sen_val2
  $TECH.C[110] = sen_val3

  MsgNotify("SEN_PREA read complete", "RSIPI_Test")

  ; ========================================================================
  ; PHASE 5: Digital I/O test
  ; ========================================================================

  ; Signal state 4: I/O test phase
  $TECH.C[11] = 4

  ; Wait for Python to start I/O test (command = 4)
  python_cmd = 0
  WHILE (python_cmd <> 4)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012
  ENDWHILE

  ; Activate gripper (digital output 1)
  $OUT[1] = TRUE
  MsgNotify("Gripper ON - $OUT[1] = TRUE", "RSIPI_Test")
  WAIT SEC 1.0

  ; Python should see Digout.o1 = TRUE in its send_variables
  ; Signal to Python that gripper is on
  $TECH.C[11] = 41  ; Sub-state: gripper activated

  ; Wait for Python acknowledgement (command = 41)
  python_cmd = 0
  WHILE (python_cmd <> 41)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012
  ENDWHILE

  ; Deactivate gripper
  $OUT[1] = FALSE
  MsgNotify("Gripper OFF - $OUT[1] = FALSE", "RSIPI_Test")
  WAIT SEC 0.5

  ; Signal gripper off
  $TECH.C[11] = 42  ; Sub-state: gripper deactivated

  ; Wait for Python acknowledgement
  python_cmd = 0
  WHILE (python_cmd <> 42)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012
  ENDWHILE

  ; ========================================================================
  ; PHASE 6: Shutdown
  ; ========================================================================

  ; Signal state 5: complete
  $TECH.C[11] = 5
  MsgNotify("Test complete - shutting down RSI", "RSIPI_Test")

  ; Wait for Python to acknowledge shutdown (command = 5)
  python_cmd = 0
  WHILE (python_cmd <> 5)
    python_cmd = $TECH.T[11]
    WAIT SEC 0.012
  ENDWHILE

  ; Clean up RSI
  ret = RSI_OFF()
  IF (ret <> RSIOK) THEN
    MsgNotify("RSI_OFF warning", "RSIPI_Test")
  ENDIF

  ret = RSI_DELETE(CONTID)

  ; Return to start
  PTP start_pos

  MsgNotify("RSIPI_Test complete!", "RSIPI_Test")

END