SOFOpt_exec.c

#include "mex.h"

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef _WIN32
#include <fcntl.h>
#include <io.h>
#include <windows.h>
#else
#include <unistd.h>
#include <sys/wait.h>
#define OF_POPEN popen
#define OF_PCLOSE pclose
#endif

#define OF_MAGIC ((uint32_t)0x3142464fU)
#define OF_SUCCESS 0
#define DEBUG 0

#if DEBUG
#define DEBUG_PRINT(...) mexPrintf(__VA_ARGS__)
#else
#define DEBUG_PRINT(...) ((void)0)
#endif

static void printHelp(void)
{
    mexPrintf("Usage: [Kx_out, Ky_out, init_cost, optim_cost, result] = OutputFeedback_exec(A, B, C, Q, R, X0, Structure, use_P_precond, rho_alpha, beta, r, c)\n");
    mexPrintf("Inputs:\n");
    mexPrintf("  A: State matrix (real double square matrix)\n");
    mexPrintf("  B: Input matrix (real double matrix with rows matching A)\n");
    mexPrintf("  C: Output matrix (real double matrix with columns matching A)\n");
    mexPrintf("  Q: State weighting matrix (real double square matrix matching A dimensions)\n");
    mexPrintf("  R: Input weighting matrix (real double matrix)\n");
    mexPrintf("  X0: Initial state vector (real double vector)\n");
    mexPrintf("  Structure: Logical matrix defining the structure of the feedback gain\n");
    mexPrintf("  use_P_precond: use P ARE matrix as preconditioner (logical scalar, optional)\n");
    mexPrintf("  rho_alpha: spectral abscissa safety ratio (double scalar, optional, default 1e-6)\n");
    mexPrintf("  beta: spectral abscissa smoothing softmax parameter (double scalar, optional, default 100)\n");
    mexPrintf("  r: Performance index relative safety shift (double scalar, optional, default 1e-5)\n");
    mexPrintf("  c: Performance index smoothing ratio (double scalar, optional, default 500)\n");
    mexPrintf("Outputs:\n");
    mexPrintf("  Kx_out: State feedback gain matrix of the output feedback solution\n");
    mexPrintf("  Ky_out: Output feedback gain matrix\n");
    mexPrintf("  init_cost: LQR full state feedback cost\n");
    mexPrintf("  optim_cost: Output feedback cost\n");
    mexPrintf("  result: Solver result string\n");

}

static void writeOrFail(FILE *stream, const void *data, size_t size, size_t count, const char *message)
{
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] fwrite request: size=%zu count=%zu\n", size, count);
#endif
    if (fwrite(data, size, count, stream) != count) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", message);
    }
}

static void readOrFail(FILE *stream, void *data, size_t size, size_t count, const char *message)
{
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] fread request: size=%zu count=%zu\n", size, count);
#endif
    if (fread(data, size, count, stream) != count) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", message);
    }
}

static void validateInputs(int nlhs, int nrhs, const mxArray *prhs[])
{
    if (nrhs == 0) {
        printHelp();
        return;
    }

    if (nrhs < 7 || nrhs > 12) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:invalidNumInputs", "Seven to twelve input arguments required.");
    }
    if (nlhs < 4 || nlhs > 5) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:invalidNumOutputs", "Four or five output arguments required.");
    }

    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) ||
        !mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) ||
        !mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) ||
        !mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) ||
        !mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) ||
        !mxIsDouble(prhs[5]) || mxIsComplex(prhs[5])) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "A, B, C, Q, R and X0 must be real double arrays.");
    }

    if (!mxIsLogical(prhs[6])) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Structure must be a logical matrix.");
    }

    if (nrhs >= 8 && !mxIsLogicalScalar(prhs[7])) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "use_P_precond must be a logical scalar.");
    }
    if (nrhs >= 9 &&
        (!mxIsDouble(prhs[8]) || mxIsComplex(prhs[8]) || mxGetNumberOfElements(prhs[8]) != 1)) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "rho_alpha must be a real double scalar.");
    }
    if (nrhs >= 10 &&
        (!mxIsDouble(prhs[9]) || mxIsComplex(prhs[9]) || mxGetNumberOfElements(prhs[9]) != 1)) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "beta must be a real double scalar.");
    }
    if (nrhs >= 11 &&
        (!mxIsDouble(prhs[10]) || mxIsComplex(prhs[10]) || mxGetNumberOfElements(prhs[10]) != 1)) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "r must be a real double scalar.");
    }
    if (nrhs >= 12 &&
        (!mxIsDouble(prhs[11]) || mxIsComplex(prhs[11]) || mxGetNumberOfElements(prhs[11]) != 1)) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "c must be a real double scalar.");
    }
}

static void checkDimensions(
    int32_t A_rows, int32_t A_cols,
    int32_t B_rows, int32_t B_cols,
    int32_t C_rows, int32_t C_cols,
    int32_t Q_rows, int32_t Q_cols,
    int32_t R_rows, int32_t R_cols,
    int32_t X0_size,
    int32_t Structure_rows, int32_t Structure_cols)
{
    if (A_rows != A_cols) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Matrix A must be square.");
    }
    if (B_rows != A_rows) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Matrix B must have the same number of rows as A.");
    }
    if (C_cols != A_cols) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Matrix C must have the same number of columns as A.");
    }
    if (Q_rows != A_rows || Q_cols != A_cols) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Matrix Q must match A dimensions.");
    }
    if (R_rows != B_cols || R_cols != B_cols) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Matrix R must be square with dimensions matching the number of inputs.");
    }
    if (X0_size != A_rows) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "X0 length must match the number of states.");
    }
    if (Structure_rows != B_cols || Structure_cols != C_rows) {
        mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "Structure must have size size(B,2) x size(C,1).");
    }
}

static void writeMatrix(FILE *stream, const mxArray *array)
{
    const size_t count = mxGetNumberOfElements(array);
    writeOrFail(stream, mxGetPr(array), sizeof(double), count, "Unable to write matrix payload to solver process.");
}

static void writeLogicalMatrix(FILE *stream, const mxArray *array)
{
    const mwSize count = mxGetNumberOfElements(array);
    mxLogical *logicalData = mxGetLogicals(array);
    mwSize i;
    uint8_t *buffer = (uint8_t *)mxMalloc(count * sizeof(uint8_t));

    for (i = 0; i < count; ++i) {
        buffer[i] = logicalData[i] ? 1U : 0U;
    }

    writeOrFail(stream, buffer, sizeof(uint8_t), (size_t)count, "Unable to write Structure payload to solver process.");
    mxFree(buffer);
}

static char *buildCommand(const char *execPath, const char *outputPath)
{
    size_t needed = strlen(execPath) + strlen(outputPath) + 16U;
    char *command = (char *)mxMalloc(needed);
    snprintf(command, needed, "\"%s\" > \"%s\"", execPath, outputPath);
    return command;
}

static char *buildCommandWithStderr(const char *execPath, const char *outputPath, const char *stderrPath)
{
    size_t needed = strlen(execPath) + strlen(outputPath) + strlen(stderrPath) + 32U;
    char *command = (char *)mxMalloc(needed);
    snprintf(command, needed, "\"%s\" > \"%s\" 2> \"%s\"", execPath, outputPath, stderrPath);
    return command;
}

#ifdef _WIN32
static char *buildQuotedCommandLine(const char *execPath)
{
    size_t needed = strlen(execPath) + 3U;
    char *command = (char *)mxMalloc(needed);
    snprintf(command, needed, "\"%s\"", execPath);
    return command;
}

static char *buildWindowsErrorMessage(const char *operation, const char *path, DWORD errorCode)
{
    char systemMessage[512];
    DWORD length = FormatMessageA(
        FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
        NULL,
        errorCode,
        0,
        systemMessage,
        (DWORD)sizeof(systemMessage),
        NULL);
    size_t needed;
    char *message;

    if (length == 0) {
        systemMessage[0] = '\0';
    }

    needed = strlen(operation) + strlen(path) + strlen(systemMessage) + 96U;
    message = (char *)mxMalloc(needed);
    snprintf(
        message,
        needed,
        "%s failed for \"%s\". Windows error %lu: %s",
        operation,
        path,
        (unsigned long)errorCode,
        systemMessage);
    return message;
}

static int solverExecutableExists(const char *execPath)
{
    DWORD attributes = GetFileAttributesA(execPath);
    return attributes != INVALID_FILE_ATTRIBUTES && (attributes & FILE_ATTRIBUTE_DIRECTORY) == 0;
}

typedef struct WindowsSolverProcess
{
    PROCESS_INFORMATION processInfo;
    HANDLE outputHandle;
    HANDLE stderrHandle;
} WindowsSolverProcess;

static FILE *startSolverExecutable(
    const char *execPath,
    const char *outputPath,
    const char *stderrPath,
    WindowsSolverProcess *solverProcess,
    char **launchError)
{
    SECURITY_ATTRIBUTES securityAttributes;
    STARTUPINFOA startupInfo;
    HANDLE stdinReadHandle = INVALID_HANDLE_VALUE;
    HANDLE stdinWriteHandle = INVALID_HANDLE_VALUE;
    HANDLE outputHandle = INVALID_HANDLE_VALUE;
    HANDLE stderrHandle = INVALID_HANDLE_VALUE;
    char *commandLine = NULL;
    int stdinFileDescriptor;
    FILE *stdinFile;

    *launchError = NULL;
    ZeroMemory(solverProcess, sizeof(*solverProcess));
    solverProcess->outputHandle = INVALID_HANDLE_VALUE;
    solverProcess->stderrHandle = INVALID_HANDLE_VALUE;

    if (!solverExecutableExists(execPath)) {
        size_t needed = strlen(execPath) + 96U;
        *launchError = (char *)mxMalloc(needed);
        snprintf(*launchError, needed, "Solver executable path does not exist: \"%s\"", execPath);
        return NULL;
    }

    ZeroMemory(&securityAttributes, sizeof(securityAttributes));
    securityAttributes.nLength = sizeof(securityAttributes);
    securityAttributes.bInheritHandle = TRUE;

    if (!CreatePipe(&stdinReadHandle, &stdinWriteHandle, &securityAttributes, 0)) {
        *launchError = buildWindowsErrorMessage("CreatePipe stdin", execPath, GetLastError());
        return NULL;
    }
    if (!SetHandleInformation(stdinWriteHandle, HANDLE_FLAG_INHERIT, 0)) {
        *launchError = buildWindowsErrorMessage("SetHandleInformation stdin", execPath, GetLastError());
        CloseHandle(stdinWriteHandle);
        CloseHandle(stdinReadHandle);
        return NULL;
    }

    outputHandle = CreateFileA(outputPath, GENERIC_WRITE, FILE_SHARE_READ, &securityAttributes, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
    if (outputHandle == INVALID_HANDLE_VALUE) {
        *launchError = buildWindowsErrorMessage("CreateFile output", outputPath, GetLastError());
        CloseHandle(stdinWriteHandle);
        CloseHandle(stdinReadHandle);
        return NULL;
    }

    stderrHandle = CreateFileA(stderrPath, GENERIC_WRITE, FILE_SHARE_READ, &securityAttributes, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
    if (stderrHandle == INVALID_HANDLE_VALUE) {
        *launchError = buildWindowsErrorMessage("CreateFile stderr", stderrPath, GetLastError());
        CloseHandle(outputHandle);
        CloseHandle(stdinWriteHandle);
        CloseHandle(stdinReadHandle);
        return NULL;
    }

    ZeroMemory(&startupInfo, sizeof(startupInfo));
    startupInfo.cb = sizeof(startupInfo);
    startupInfo.dwFlags = STARTF_USESTDHANDLES;
    startupInfo.hStdInput = stdinReadHandle;
    startupInfo.hStdOutput = outputHandle;
    startupInfo.hStdError = stderrHandle;

    ZeroMemory(&solverProcess->processInfo, sizeof(solverProcess->processInfo));
    commandLine = buildQuotedCommandLine(execPath);
    if (!CreateProcessA(execPath, commandLine, NULL, NULL, TRUE, CREATE_NO_WINDOW, NULL, NULL, &startupInfo, &solverProcess->processInfo)) {
        *launchError = buildWindowsErrorMessage("CreateProcess", execPath, GetLastError());
        mxFree(commandLine);
        CloseHandle(stderrHandle);
        CloseHandle(outputHandle);
        CloseHandle(stdinWriteHandle);
        CloseHandle(stdinReadHandle);
        return NULL;
    }
    mxFree(commandLine);
    CloseHandle(stdinReadHandle);

    stdinFileDescriptor = _open_osfhandle((intptr_t)stdinWriteHandle, _O_BINARY);
    if (stdinFileDescriptor == -1) {
        *launchError = buildWindowsErrorMessage("_open_osfhandle stdin", execPath, GetLastError());
        CloseHandle(stdinWriteHandle);
        TerminateProcess(solverProcess->processInfo.hProcess, 1);
        WaitForSingleObject(solverProcess->processInfo.hProcess, INFINITE);
        CloseHandle(solverProcess->processInfo.hThread);
        CloseHandle(solverProcess->processInfo.hProcess);
        CloseHandle(stderrHandle);
        CloseHandle(outputHandle);
        return NULL;
    }

    stdinFile = _fdopen(stdinFileDescriptor, "wb");
    if (stdinFile == NULL) {
        size_t needed = strlen(execPath) + 96U;
        *launchError = (char *)mxMalloc(needed);
        snprintf(*launchError, needed, "_fdopen failed for solver stdin pipe: \"%s\"", execPath);
        _close(stdinFileDescriptor);
        TerminateProcess(solverProcess->processInfo.hProcess, 1);
        WaitForSingleObject(solverProcess->processInfo.hProcess, INFINITE);
        CloseHandle(solverProcess->processInfo.hThread);
        CloseHandle(solverProcess->processInfo.hProcess);
        CloseHandle(stderrHandle);
        CloseHandle(outputHandle);
        return NULL;
    }

    solverProcess->outputHandle = outputHandle;
    solverProcess->stderrHandle = stderrHandle;
    return stdinFile;
}

static int finishSolverExecutable(FILE *process, WindowsSolverProcess *solverProcess, const char *execPath, char **launchError)
{
    DWORD exitCode = 1;

    if (fclose(process) != 0) {
        *launchError = buildWindowsErrorMessage("fclose stdin", execPath, GetLastError());
        TerminateProcess(solverProcess->processInfo.hProcess, 1);
    }

    WaitForSingleObject(solverProcess->processInfo.hProcess, INFINITE);
    if (!GetExitCodeProcess(solverProcess->processInfo.hProcess, &exitCode)) {
        *launchError = buildWindowsErrorMessage("GetExitCodeProcess", execPath, GetLastError());
        exitCode = 1;
    }

    CloseHandle(solverProcess->processInfo.hThread);
    CloseHandle(solverProcess->processInfo.hProcess);
    CloseHandle(solverProcess->stderrHandle);
    CloseHandle(solverProcess->outputHandle);
    solverProcess->stderrHandle = INVALID_HANDLE_VALUE;
    solverProcess->outputHandle = INVALID_HANDLE_VALUE;
    return (int)exitCode;
}
#endif

#ifndef _WIN32
static FILE *openSolverProcess(const char *command)
{
    return OF_POPEN(command, "w");
}

static int closeSolverProcess(FILE *process)
{
    return OF_PCLOSE(process);
}
#endif

static int shellCommandNotFound(int processExitCode)
{
#ifdef _WIN32
    return processExitCode == 9009;
#else
    if (WIFEXITED(processExitCode) && WEXITSTATUS(processExitCode) == 127) {
        return 1;
    }
    return processExitCode == 32512;
#endif
}

static char *getSolverPath(void)
{
    const char *envPath = getenv("OUTPUTFEEDBACK_SOLVER_EXEC");
#ifdef _WIN32
    const char *defaultPath = ".\\solver_exec.exe";
#else
    const char *defaultPath = "./solver_exec";
#endif

    if (envPath != NULL && envPath[0] != '\0') {
        char *configuredPath = (char *)mxMalloc(strlen(envPath) + 1U);
        strcpy(configuredPath, envPath);
#if DEBUG
        DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Using solver path from environment: %s\n", configuredPath);
#endif
        return configuredPath;
    }

    {
        char *configuredPath = (char *)mxMalloc(strlen(defaultPath) + 1U);
        strcpy(configuredPath, defaultPath);
#if DEBUG
        DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Using default solver path: %s\n", configuredPath);
#endif
        return configuredPath;
    }
}

static char *duplicateString(const char *value)
{
    char *copy = (char *)mxMalloc(strlen(value) + 1U);
    strcpy(copy, value);
    return copy;
}

static char *createResponsePath(void)
{
#ifdef _WIN32
    char tempDir[MAX_PATH + 1];
    char tempFile[MAX_PATH + 1];
    DWORD tempDirLength = GetTempPathA((DWORD)sizeof(tempDir), tempDir);
    if (tempDirLength == 0 || tempDirLength >= sizeof(tempDir)) {
        return NULL;
    }
    if (GetTempFileNameA(tempDir, "ofb", 0, tempFile) == 0) {
        return NULL;
    }
    {
        char *path = duplicateString(tempFile);
#if DEBUG
        if (path != NULL) {
            DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Created temporary response path: %s\n", path);
        }
#endif
        return path;
    }
#else
    char templatePath[] = "/tmp/outputfeedback_exec_XXXXXX";
    int fd = mkstemp(templatePath);
    if (fd == -1) {
        return NULL;
    }
    close(fd);
    {
        char *path = duplicateString(templatePath);
#if DEBUG
        if (path != NULL) {
            DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Created temporary response path: %s\n", path);
        }
#endif
        return path;
    }
#endif
}

static void printFileContentsIfPresent(const char *path, const char *label)
{
    FILE *file = fopen(path, "rb");
    long fileSize;
    char *buffer;

    if (file == NULL) {
#if DEBUG
        DEBUG_PRINT("[OutputFeedback_exec][DEBUG] %s file not available: %s\n", label, path);
#endif
        return;
    }

    if (fseek(file, 0, SEEK_END) != 0) {
        fclose(file);
        return;
    }

    fileSize = ftell(file);
    if (fileSize < 0) {
        fclose(file);
        return;
    }

    if (fseek(file, 0, SEEK_SET) != 0) {
        fclose(file);
        return;
    }

    buffer = (char *)mxMalloc((size_t)fileSize + 1U);
    if (buffer == NULL) {
        fclose(file);
        return;
    }

    if (fileSize > 0) {
        size_t readCount = fread(buffer, 1, (size_t)fileSize, file);
        buffer[readCount] = '\0';
#if DEBUG
        DEBUG_PRINT("[OutputFeedback_exec][DEBUG] %s contents:\n%s\n", label, buffer);
#endif
    } else {
#if DEBUG
        DEBUG_PRINT("[OutputFeedback_exec][DEBUG] %s is empty.\n", label);
#endif
    }

    mxFree(buffer);
    fclose(file);
}

static char *readSmallTextFile(const char *path)
{
    FILE *file = fopen(path, "rb");
    long fileSize;
    char *buffer;
    size_t readCount;

    if (file == NULL) {
        return NULL;
    }
    if (fseek(file, 0, SEEK_END) != 0) {
        fclose(file);
        return NULL;
    }
    fileSize = ftell(file);
    if (fileSize <= 0) {
        fclose(file);
        return NULL;
    }
    if (fileSize > 8192) {
        fileSize = 8192;
    }
    if (fseek(file, 0, SEEK_SET) != 0) {
        fclose(file);
        return NULL;
    }

    buffer = (char *)mxMalloc((size_t)fileSize + 1U);
    readCount = fread(buffer, 1, (size_t)fileSize, file);
    buffer[readCount] = '\0';
    fclose(file);
    return buffer;
}

static void raiseSolverProcessError(int processExitCode, const char *stderrPath, const char *launchError)
{
    char *stderrText = readSmallTextFile(stderrPath);

    if (launchError != NULL && launchError[0] != '\0') {
        if (stderrText != NULL && stderrText[0] != '\0') {
            mexErrMsgIdAndTxt(
                "OutputFeedback_exec:ProcessError",
                "Solver executable failed before producing a response. Exit code: %d. Launch error: %s. Solver stderr:\n%s",
                processExitCode,
                launchError,
                stderrText);
        }
        mexErrMsgIdAndTxt(
            "OutputFeedback_exec:ProcessError",
            "Solver executable failed before producing a response. Exit code: %d. Launch error: %s",
            processExitCode,
            launchError);
    }

    if (shellCommandNotFound(processExitCode)) {
        if (stderrText != NULL && stderrText[0] != '\0') {
            mexErrMsgIdAndTxt(
                "OutputFeedback_exec:ProcessError",
                "Solver executable was not found. Check OUTPUTFEEDBACK_SOLVER_EXEC or rebuild with Build.m. Exit code: %d. Solver stderr:\n%s",
                processExitCode,
                stderrText);
        }
        mexErrMsgIdAndTxt(
            "OutputFeedback_exec:ProcessError",
            "Solver executable was not found. Check OUTPUTFEEDBACK_SOLVER_EXEC or rebuild with Build.m. Exit code: %d.",
            processExitCode);
    }

    if (stderrText != NULL && stderrText[0] != '\0') {
        mexErrMsgIdAndTxt(
            "OutputFeedback_exec:ProcessError",
            "Solver executable failed before producing a response. Exit code: %d. Solver stderr:\n%s",
            processExitCode,
            stderrText);
    }

    mexErrMsgIdAndTxt(
        "OutputFeedback_exec:ProcessError",
        "Solver executable failed before producing a response. Exit code: %d. No stderr was captured.",
        processExitCode);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    int32_t A_rows;
    int32_t A_cols;
    int32_t B_rows;
    int32_t B_cols;
    int32_t C_rows;
    int32_t C_cols;
    int32_t Q_rows;
    int32_t Q_cols;
    int32_t R_rows;
    int32_t R_cols;
    int32_t X0_size;
    int32_t Structure_rows;
    int32_t Structure_cols;
    uint8_t usePPrecond = 1U;
    double rhoAlpha = 1e-6;
    double beta = 100.0;
    double r = 1e-5;
    double c = 500.0;
    uint32_t magic;
    int32_t status;
    int32_t Kx_rows;
    int32_t Kx_cols;
    int32_t Ky_rows;
    int32_t Ky_cols;
    int32_t messageLength;
    double initCost;
    double optimCost;
    uint32_t requestMagic;
    char *execPath;
    char *outputPath;
    char *stderrPath;
    char *command;
    char *launchError;
    char *message;
    FILE *process;
    FILE *responseFile;
    int processExitCode;
#ifdef _WIN32
    WindowsSolverProcess solverProcess;
#endif

    validateInputs(nlhs, nrhs, prhs);
    if (nrhs == 0) {
        return;
    }

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Starting mexFunction with nrhs=%d nlhs=%d\n", nrhs, nlhs);
#endif

    A_rows = (int32_t)mxGetM(prhs[0]);
    A_cols = (int32_t)mxGetN(prhs[0]);
    B_rows = (int32_t)mxGetM(prhs[1]);
    B_cols = (int32_t)mxGetN(prhs[1]);
    C_rows = (int32_t)mxGetM(prhs[2]);
    C_cols = (int32_t)mxGetN(prhs[2]);
    Q_rows = (int32_t)mxGetM(prhs[3]);
    Q_cols = (int32_t)mxGetN(prhs[3]);
    R_rows = (int32_t)mxGetM(prhs[4]);
    R_cols = (int32_t)mxGetN(prhs[4]);
    X0_size = (int32_t)mxGetNumberOfElements(prhs[5]);
    Structure_rows = (int32_t)mxGetM(prhs[6]);
    Structure_cols = (int32_t)mxGetN(prhs[6]);

    checkDimensions(A_rows, A_cols, B_rows, B_cols, C_rows, C_cols, Q_rows, Q_cols, R_rows, R_cols, X0_size, Structure_rows, Structure_cols);

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Dimensions: A=%dx%d B=%dx%d C=%dx%d Q=%dx%d R=%dx%d X0=%d Structure=%dx%d\n",
        A_rows, A_cols, B_rows, B_cols, C_rows, C_cols, Q_rows, Q_cols, R_rows, R_cols, X0_size, Structure_rows, Structure_cols);
#endif

    if (nrhs >= 8) {
        usePPrecond = mxIsLogicalScalarTrue(prhs[7]) ? 1U : 0U;
    }
    if (nrhs >= 9) {
        rhoAlpha = mxGetScalar(prhs[8]);
        if (!mxIsFinite(rhoAlpha) || rhoAlpha < 0.0) {
            mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "rho_alpha must be a non-negative finite scalar.");
        }
    }
    if (nrhs >= 10) {
        beta = mxGetScalar(prhs[9]);
        if (!mxIsFinite(beta) || beta <= 0.0) {
            mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "beta must be a positive finite scalar.");
        }
    }
    if (nrhs >= 11) {
        r = mxGetScalar(prhs[10]);
        if (!mxIsFinite(r) || r < 0.0) {
            mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "r must be a non-negative finite scalar.");
        }
    }
    if (nrhs >= 12) {
        c = mxGetScalar(prhs[11]);
        if (!mxIsFinite(c) || c <= 0.0) {
            mexErrMsgIdAndTxt("OutputFeedback_exec:InvalidInput", "c must be a positive finite scalar.");
        }
    }

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] use_P_precond: %u\n", (unsigned int)usePPrecond);
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] rho_alpha: %g\n", rhoAlpha);
#endif

    execPath = getSolverPath();
    command = NULL;
    launchError = NULL;

    outputPath = createResponsePath();
    if (outputPath == NULL) {
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to create a temporary response path.");
    }

    stderrPath = createResponsePath();
    if (stderrPath == NULL) {
        mxFree(outputPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to create a temporary stderr path.");
    }

#ifdef _WIN32
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] solver executable: %s\n", execPath);
#endif
    process = startSolverExecutable(execPath, outputPath, stderrPath, &solverProcess, &launchError);
#else
    command = buildCommandWithStderr(execPath, outputPath, stderrPath);
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] popen command: %s\n", command);
#endif
    process = openSolverProcess(command);
#endif
    if (process == NULL) {
        if (command != NULL) {
            mxFree(command);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        if (launchError != NULL) {
            mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to start solver executable. %s", launchError);
        }
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to start solver executable.");
    }

    requestMagic = OF_MAGIC;

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Writing request header. magic=0x%08x\n",
        (unsigned int)requestMagic);
#endif

    writeOrFail(process, &requestMagic, sizeof(uint32_t), 1, "Unable to write request header to solver process.");
    writeOrFail(process, &A_rows, sizeof(int32_t), 1, "Unable to write A_rows.");
    writeOrFail(process, &A_cols, sizeof(int32_t), 1, "Unable to write A_cols.");
    writeOrFail(process, &B_rows, sizeof(int32_t), 1, "Unable to write B_rows.");
    writeOrFail(process, &B_cols, sizeof(int32_t), 1, "Unable to write B_cols.");
    writeOrFail(process, &C_rows, sizeof(int32_t), 1, "Unable to write C_rows.");
    writeOrFail(process, &C_cols, sizeof(int32_t), 1, "Unable to write C_cols.");
    writeOrFail(process, &Q_rows, sizeof(int32_t), 1, "Unable to write Q_rows.");
    writeOrFail(process, &Q_cols, sizeof(int32_t), 1, "Unable to write Q_cols.");
    writeOrFail(process, &R_rows, sizeof(int32_t), 1, "Unable to write R_rows.");
    writeOrFail(process, &R_cols, sizeof(int32_t), 1, "Unable to write R_cols.");
    writeOrFail(process, &X0_size, sizeof(int32_t), 1, "Unable to write X0_size.");
    writeOrFail(process, &Structure_rows, sizeof(int32_t), 1, "Unable to write Structure_rows.");
    writeOrFail(process, &Structure_cols, sizeof(int32_t), 1, "Unable to write Structure_cols.");
    writeOrFail(process, &usePPrecond, sizeof(uint8_t), 1, "Unable to write use_P_precond flag.");
    writeOrFail(process, &rhoAlpha, sizeof(double), 1, "Unable to write rho_alpha.");
    writeOrFail(process, &beta, sizeof(double), 1, "Unable to write beta.");
    writeOrFail(process, &r, sizeof(double), 1, "Unable to write r.");
    writeOrFail(process, &c, sizeof(double), 1, "Unable to write c.");

    writeMatrix(process, prhs[0]);
    writeMatrix(process, prhs[1]);
    writeMatrix(process, prhs[2]);
    writeMatrix(process, prhs[3]);
    writeMatrix(process, prhs[4]);
    writeMatrix(process, prhs[5]);
    writeLogicalMatrix(process, prhs[6]);

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Finished writing request payload.\n");
#endif
#ifdef _WIN32
    processExitCode = finishSolverExecutable(process, &solverProcess, execPath, &launchError);
#else
    processExitCode = closeSolverProcess(process);
#endif

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Solver process exit code: %d\n", processExitCode);
    printFileContentsIfPresent(stderrPath, "Solver stderr");
#endif

    responseFile = fopen(outputPath, "rb");
    if (responseFile == NULL) {
        if (processExitCode != 0) {
            raiseSolverProcessError(processExitCode, stderrPath, launchError);
        }
        remove(outputPath);
        remove(stderrPath);
        if (command != NULL) {
            mxFree(command);
        }
        if (launchError != NULL) {
            mxFree(launchError);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to open solver response file.");
    }

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Opened response file for reading: %s\n", outputPath);
#endif

    if (fread(&magic, sizeof(uint32_t), 1, responseFile) != 1) {
        fclose(responseFile);
        if (processExitCode != 0) {
            raiseSolverProcessError(processExitCode, stderrPath, launchError);
        }
        remove(outputPath);
        remove(stderrPath);
        if (command != NULL) {
            mxFree(command);
        }
        if (launchError != NULL) {
            mxFree(launchError);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Unable to read solver response header.");
    }
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Response magic: 0x%08x\n", (unsigned int)magic);
#endif
    if (magic != OF_MAGIC) {
        fclose(responseFile);
        remove(outputPath);
        remove(stderrPath);
        if (command != NULL) {
            mxFree(command);
        }
        if (launchError != NULL) {
            mxFree(launchError);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Invalid solver response header.");
    }

    readOrFail(responseFile, &status, sizeof(int32_t), 1, "Unable to read solver response status.");
    readOrFail(responseFile, &Kx_rows, sizeof(int32_t), 1, "Unable to read Kx_rows.");
    readOrFail(responseFile, &Kx_cols, sizeof(int32_t), 1, "Unable to read Kx_cols.");
    readOrFail(responseFile, &Ky_rows, sizeof(int32_t), 1, "Unable to read Ky_rows.");
    readOrFail(responseFile, &Ky_cols, sizeof(int32_t), 1, "Unable to read Ky_cols.");
    readOrFail(responseFile, &initCost, sizeof(double), 1, "Unable to read init_cost.");
    readOrFail(responseFile, &optimCost, sizeof(double), 1, "Unable to read optim_cost.");
    readOrFail(responseFile, &messageLength, sizeof(int32_t), 1, "Unable to read solver message length.");
#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Response status=%d Kx=%dx%d Ky=%dx%d init_cost=%g optim_cost=%g messageLength=%d\n",
        status, Kx_rows, Kx_cols, Ky_rows, Ky_cols, initCost, optimCost, messageLength);
#endif
    if (messageLength < 0) {
        fclose(responseFile);
        remove(outputPath);
        remove(stderrPath);
        if (command != NULL) {
            mxFree(command);
        }
        if (launchError != NULL) {
            mxFree(launchError);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:IOError", "Invalid solver message length.");
    }

    message = (char *)mxMalloc((size_t)messageLength + 1U);
    if (messageLength > 0) {
        readOrFail(responseFile, message, sizeof(char), (size_t)messageLength, "Unable to read solver message.");
    }
    message[messageLength] = '\0';

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Response message: %s\n", message);
#endif

    if (status != OF_SUCCESS) {
        fclose(responseFile);
        remove(outputPath);
        remove(stderrPath);
        mxFree(message);
        if (command != NULL) {
            mxFree(command);
        }
        if (launchError != NULL) {
            mxFree(launchError);
        }
        mxFree(outputPath);
        mxFree(stderrPath);
        mxFree(execPath);
        mexErrMsgIdAndTxt("OutputFeedback_exec:SolverError", message);
    }

    plhs[0] = mxCreateDoubleMatrix(Kx_rows, Kx_cols, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(Ky_rows, Ky_cols, mxREAL);
    plhs[2] = mxCreateDoubleScalar(initCost);
    plhs[3] = mxCreateDoubleScalar(optimCost);
    if (nlhs >= 5) {
        plhs[4] = mxCreateString(message);
    }

    readOrFail(responseFile, mxGetPr(plhs[0]), sizeof(double), (size_t)Kx_rows * (size_t)Kx_cols, "Unable to read Kx payload.");
    readOrFail(responseFile, mxGetPr(plhs[1]), sizeof(double), (size_t)Ky_rows * (size_t)Ky_cols, "Unable to read Ky payload.");

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] Successfully read Kx and Ky payloads.\n");
#endif

    fclose(responseFile);
    remove(outputPath);
    remove(stderrPath);
    mxFree(message);
    if (command != NULL) {
        mxFree(command);
    }
    if (launchError != NULL) {
        mxFree(launchError);
    }
    mxFree(outputPath);
    mxFree(stderrPath);
    mxFree(execPath);

#if DEBUG
    DEBUG_PRINT("[OutputFeedback_exec][DEBUG] mexFunction completed successfully.\n");
#endif
}