def inject_rsi_to_krl(input_file, output_file=None, rsi_config="RSIGatewayv1.rsi"):
    """
    Injects RSI commands into a KUKA KRL (.src) program file.

    Args:
        input_file (str): Path to the original KRL file.
        output_file (str, optional): Path to the output KRL file. Defaults to input_file if None.
        rsi_config (str): RSI configuration filename.
    """
    if output_file is None:
        output_file = input_file

    rsi_start = """
; RSI Variable Declarations
DECL INT ret
DECL INT CONTID
"""

    rsi_middle = f"""
; Create RSI Context
ret = RSI_CREATE("{rsi_config}", CONTID, TRUE)
IF (ret <> RSIOK) THEN
    HALT
ENDIF

; Start RSI Execution
ret = RSI_ON(#RELATIVE)
IF (ret <> RSIOK) THEN
    HALT
ENDIF
"""

    rsi_end = """
; Stop RSI Execution
ret = RSI_OFF()
IF (ret <> RSIOK) THEN
    HALT
ENDIF
"""

    with open(input_file, "r") as file:
        lines = file.readlines()

    header_end, ini_end, end_start = None, None, None

    for i, line in enumerate(lines):
        if line.strip().startswith("DEF"):
            header_end = i
        elif line.strip().startswith(";ENDFOLD (INI)"):
            ini_end = i
        elif line.strip().startswith("END"):
            end_start = i

    if header_end is None or ini_end is None or end_start is None:
        raise ValueError("Required markers (DEF, ;ENDFOLD (INI), END) not found in KRL file.")

    with open(output_file, "w") as file:
        # Write header and RSI declarations
        file.writelines(lines[:header_end + 1])
        file.write(rsi_start)

        # Write INI section
        file.writelines(lines[header_end + 1:ini_end + 1])

        # Write RSI context creation and activation
        file.write(rsi_middle)

        # Write main code section
        file.writelines(lines[ini_end + 1:end_start])

        # Write RSI deactivation
        file.write(rsi_end)

        # Write END line
        file.write(lines[end_start])


# Example usage:
if __name__ == "__main__":
    inject_rsi_to_krl("my_program.src", "my_program_rsi.src")