#include "sinqController.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include "sinqAxis.h"
#include <errlog.h>
#include <vector>

/*
Contains all instances of turboPmacAxis which have been created and is used in
the initialization hook function.
 */
static std::vector<sinqController *> controller;

/**
 * @brief Hook function to perform certain actions during the IOC initialization
 *
 * @param iState
 */
void sinqController::epicsInithookFunction(initHookState iState) {
    if (iState == initHookAfterIocRunning) {
        // Iterate through all axes of each and call the initialization method
        // on each one of them.
        for (std::vector<sinqController *>::iterator itC = controller.begin();
             itC != controller.end(); ++itC) {

            sinqController *controller = *itC;
            controller->startPoller(controller->movingPollPeriod_,
                                    controller->idlePollPeriod_, 1);
        }
    }
}

sinqController::sinqController(const char *portName,
                               const char *ipPortConfigName, int numAxes,
                               double movingPollPeriod, double idlePollPeriod,
                               int numExtraParams)
    : asynMotorController(
          portName,
          // As described in the function documentation, an offset of 1 is
          // added for better readability of the configuration.
          numAxes + 1,
          NUM_MOTOR_DRIVER_PARAMS + NUM_SINQMOTOR_DRIVER_PARAMS +
              numExtraParams,
          0, // No additional interfaces beyond those in base class
          0, // No additional callback interfaces beyond those in base class
          ASYN_CANBLOCK | ASYN_MULTIDEVICE,
          1,     // autoconnect
          0, 0), // Default priority and stack size
      msgPrintControl_(4) {

    asynStatus status = asynSuccess;

    // Handle to the asynUser of the IP port asyn driver
    pasynOctetSyncIOipPort_ = nullptr;

    // Initial values for the average timeout mechanism, can be overwritten
    // later by a FFI function
    comTimeoutWindow_ = 3600; // seconds

    // Number of timeouts which may occur before an error is forwarded to the
    // user
    maxNumberTimeouts_ = 60;

    // Queue holding the timeout event timestamps
    timeoutEvents_ = {};

    // Inform the user after 10 timeouts in a row (default value)
    maxSubsequentTimeouts_ = 10;
    maxSubsequentTimeoutsExceeded_ = false;

    // Store the poll period information. The poller itself will be started
    // later (after the IOC is running in epicsInithookFunction)
    movingPollPeriod_ = movingPollPeriod;
    idlePollPeriod_ = idlePollPeriod;

    // =========================================================================;

    /*
    We try to connect to the port via the port name provided by the constructor.
    If this fails, the function is terminated via exit.
    */
    pasynOctetSyncIO->connect(ipPortConfigName, 0, &pasynOctetSyncIOipPort_,
                              NULL);
    if (status != asynSuccess || pasynOctetSyncIOipPort_ == nullptr) {
        errlogPrintf("Controller \"%s\" => %s, line %d:\nFATAL ERROR (cannot "
                     "connect to MCU controller).\n"
                     "Terminating IOC",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        exit(-1);
    }

    // =========================================================================;

    // MOTOR_MESSAGE_TEXT corresponds to the PV definition inside sinqMotor.db.
    // This text is used to forward status messages to NICOS and in turn to the
    // user.
    status =
        createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &motorMessageText_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE", asynParamInt32, &motorEnable_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_RESET", asynParamInt32, &motorReset_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE_RBV", asynParamInt32, &motorEnableRBV_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status =
        createParam("MOTOR_CAN_DISABLE", asynParamInt32, &motorCanDisable_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status =
        createParam("MOTOR_CAN_SET_SPEED", asynParamInt32, &motorCanSetSpeed_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_LIMITS_OFFSET", asynParamFloat64,
                         &motorLimitsOffset_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    /*
    We need to introduce 2 new parameters in order to write the limits from the
    driver to the EPICS record. See the comment in sinqController.h next to
    the declaration of motorHighLimitFromDriver_.
    */
    status = createParam("MOTOR_HIGH_LIMIT_FROM_DRIVER", asynParamFloat64,
                         &motorHighLimitFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_LOW_LIMIT_FROM_DRIVER", asynParamFloat64,
                         &motorLowLimitFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE_MOV_WATCHDOG", asynParamInt32,
                         &motorEnableMovWatchdog_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VELO_FROM_DRIVER", asynParamFloat64,
                         &motorVeloFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VBAS_FROM_DRIVER", asynParamFloat64,
                         &motorVbasFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VMAX_FROM_DRIVER", asynParamFloat64,
                         &motorVmaxFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ACCL_FROM_DRIVER", asynParamFloat64,
                         &motorAcclFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("ADAPTIVE_POLLING", asynParamInt32, &adaptivePolling_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_FORCE_STOP", asynParamInt32, &motorForceStop_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
                  "parameter failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        exit(-1);
    }

    // Register the hook function during construction of the first axis object
    if (controller.empty()) {
        initHookRegister(&epicsInithookFunction);
    }

    // Collect all axes into this list which will be used in the hook function
    controller.push_back(this);
}

sinqController::~sinqController(void) {

    // Free all axes
    for (int axisNo = 0; axisNo < numAxes_; axisNo++) {
        if (pAxes_[axisNo] != nullptr) {
            delete pAxes_[axisNo];
        }
    }

    // Cleanup of the array allocated in the asynMotorController constructor
    free(this->pAxes_);
}

msgPrintControl &sinqController::getMsgPrintControl() {
    return msgPrintControl_;
}

asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) {
    int function = pasynUser->reason;

    // =====================================================================

    asynMotorAxis *asynAxis = getAxis(pasynUser);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);

    if (axis == nullptr) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an "
                  "instance of sinqAxis",
                  portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }

    // Handle custom PVs
    if (function == motorEnable_) {
        return axis->enable(value != 0);
    } else if (function == motorReset_) {
        return axis->reset();
    } else if (function == motorForceStop_) {
        return axis->stop(0.0);
    } else {
        return asynMotorController::writeInt32(pasynUser, value);
    }
}

asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {

    // Casting into a sinqAxis is necessary to get access to the field axisNo()
    asynMotorAxis *asynAxis = getAxis(pasynUser);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);

    if (axis == nullptr) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an "
                  "instance of sinqAxis.\n",
                  portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }

    if (pasynUser->reason == motorEnableRBV_) {
        return getIntegerParam(axis->axisNo(), motorEnableRBV_, value);
    } else if (pasynUser->reason == motorCanDisable_) {
        return getIntegerParam(axis->axisNo(), motorCanDisable_, value);
    } else {
        return asynMotorController::readInt32(pasynUser, value);
    }
}

asynStatus sinqController::couldNotParseResponse(const char *command,
                                                 const char *response,
                                                 int axisNo,
                                                 const char *functionName,
                                                 int line) {
    asynStatus pl_status = asynSuccess;

    asynPrint(pasynOctetSyncIOipPort_, ASYN_TRACE_ERROR,
              "Controller \"%s\", axis %d => %s, line %d:\nCould not interpret "
              "response \"%s\" for command \"%s\".\n",
              portName, axisNo, functionName, line, response, command);

    pl_status = setStringParam(
        motorMessageText_,
        "Could not interpret controller response. Please call the support");
    if (pl_status != asynSuccess) {
        return paramLibAccessFailed(pl_status, "motorMessageText_", axisNo,
                                    __PRETTY_FUNCTION__, __LINE__);
    }

    pl_status = setIntegerParam(motorStatusCommsError_, 1);
    if (pl_status != asynSuccess) {
        return paramLibAccessFailed(pl_status, "motorStatusCommsError_", axisNo,
                                    __PRETTY_FUNCTION__, __LINE__);
    }

    return asynError;
}

asynStatus sinqController::paramLibAccessFailed(asynStatus status,
                                                const char *parameter,
                                                int axisNo,
                                                const char *functionName,
                                                int line) {

    if (status != asynSuccess) {
        asynPrint(pasynOctetSyncIOipPort_, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\n Accessing the "
                  "parameter library failed for parameter %s with error %s.\n",
                  portName, axisNo, functionName, line, parameter,
                  stringifyAsynStatus(status));

        // Log the error message and try to propagate it. If propagating fails,
        // there is nothing we can do here anyway.
        setStringParam(motorMessageText_,
                       "Accessing paramLib failed. Please call the support.");
    }

    return status;
}

asynStatus sinqController::checkComTimeoutWatchdog(int axisNo,
                                                   char *motorMessage,
                                                   size_t motorMessageSize) {

    asynStatus paramLibStatus = asynSuccess;

    // Add a new timeout event to the queue
    timeoutEvents_.push_back(time(NULL));

    // Remove every event which is older than the time window from the deque
    while (1) {
        if (timeoutEvents_.empty()) {
            break;
        }
        if (timeoutEvents_[0] + comTimeoutWindow_ <= time(NULL)) {
            timeoutEvents_.pop_front();
        } else {
            break;
        }
    }

    // Check if the maximum allowed number of events has been exceeded
    bool wantToPrint = timeoutEvents_.size() > maxNumberTimeouts_;

    if (msgPrintControl_.shouldBePrinted(portName, axisNo, __PRETTY_FUNCTION__,
                                         __LINE__, wantToPrint,
                                         pasynUserSelf)) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nMore than %ld "
                  "communication timeouts in %ld "
                  "seconds.%s\n",
                  portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                  maxNumberTimeouts_, comTimeoutWindow_,
                  msgPrintControl_.getSuffix());
    }

    if (wantToPrint) {
        snprintf(motorMessage, motorMessageSize,
                 "More than %ld communication timeouts in %ld seconds. Please "
                 "call the support.",
                 maxNumberTimeouts_, comTimeoutWindow_);

        paramLibStatus = setIntegerParam(motorStatusCommsError_, 1);
        if (paramLibStatus != asynSuccess) {
            return paramLibAccessFailed(paramLibStatus,
                                        "motorStatusCommsError_", axisNo,
                                        __PRETTY_FUNCTION__, __LINE__);
        }

        return asynError;
    } else {
        return asynSuccess;
    }
}

asynStatus sinqController::checkComTimeoutWatchdog(sinqAxis *axis) {

    char motorMessage[200] = {0};

    asynStatus status =
        checkComTimeoutWatchdog(axis->axisNo(), motorMessage, 200);
    if (status == asynError) {
        status = axis->setStringParam(motorMessageText_, motorMessage);
        if (status != asynSuccess) {
            return paramLibAccessFailed(status, "motorMessageText_",
                                        axis->axisNo(), __PRETTY_FUNCTION__,
                                        __LINE__);
        }
    }
    return status;
}

asynStatus sinqController::checkMaxSubsequentTimeouts(int timeoutNo, int axisNo,
                                                      char *motorMessage,
                                                      size_t motorMessageSize) {
    asynStatus paramLibStatus = asynSuccess;

    if (timeoutNo >= maxSubsequentTimeouts_) {
        if (!maxSubsequentTimeoutsExceeded_) {
            snprintf(motorMessage, motorMessageSize,
                     "Communication timeout between IOC and motor controller. "
                     "Trying to reconnect ...");
            asynPrint(
                this->pasynUserSelf, ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d => %s, line %d:\nMore than %d "
                "subsequent communication "
                "timeouts\n",
                this->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                maxSubsequentTimeouts_);

            paramLibStatus = setIntegerParam(motorStatusCommsError_, 1);
            if (paramLibStatus != asynSuccess) {
                return paramLibAccessFailed(paramLibStatus,
                                            "motorStatusCommsError_", axisNo,
                                            __PRETTY_FUNCTION__, __LINE__);
            }
            maxSubsequentTimeoutsExceeded_ = true;
        }

        return asynError;
    } else {
        maxSubsequentTimeoutsExceeded_ = false;
        motorMessage[0] = '\0';
        return asynSuccess;
    }
}

asynStatus sinqController::checkMaxSubsequentTimeouts(int timeoutNo,
                                                      sinqAxis *axis) {

    char motorMessage[200] = {0};

    asynStatus status = checkMaxSubsequentTimeouts(axis->axisNo(), timeoutNo,
                                                   motorMessage, 200);
    if (status == asynError) {
        status = axis->setStringParam(motorMessageText_, motorMessage);
        if (status != asynSuccess) {
            return paramLibAccessFailed(status, "motorMessageText_",
                                        axis->axisNo(), __PRETTY_FUNCTION__,
                                        __LINE__);
        }
    }
    return status;
}

// Static pointers (valid for the entire lifetime of the IOC). The number behind
// the strings gives the integer number of each variant (see also method
// stringifyAsynStatus)
const char asynSuccessStringified[] = "success";           // 0
const char asynTimeoutStringified[] = "timeout";           // 1
const char asynOverflowStringified[] = "overflow";         // 2
const char asynErrorStringified[] = "error";               // 3
const char asynDisconnectedStringified[] = "disconnected"; // 4
const char asynDisabledStringified[] = "disabled";         // 5
const char asynParamAlreadyExistsStringified[] =
    "parameter already exists";                                     // 6
const char asynParamNotFoundStringified[] = "parameter not found";  // 7
const char asynParamWrongTypeStringified[] = "wrong type";          // 8
const char asynParamBadIndexStringified[] = "bad index";            // 9
const char asynParamUndefinedStringified[] = "parameter undefined"; // 10
const char asynParamInvalidListStringified[] = "invalid list";      // 11
const char inputDidNotMatchAsynStatus[] =
    "Input did not match any variant of asynStatus";

const char *sinqController::stringifyAsynStatus(asynStatus status) {
    // See
    // https://github.com/epics-modules/asyn/blob/master/asyn/asynDriver/asynDriver.h
    // and
    // https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/paramErrors.h
    // for the definition of the error codes
    // The pragma is necessary since the param lib error codes are "tacked onto"
    // the enum, which results in compiler warnings otherwise.
#pragma GCC diagnostic ignored "-Wswitch"
    switch (status) {
    case asynSuccess:
        return asynSuccessStringified;
    case asynTimeout:
        return asynTimeoutStringified;
    case asynOverflow:
        return asynOverflowStringified;
    case asynError:
        return asynErrorStringified;
    case asynDisconnected:
        return asynDisconnectedStringified;
    case asynDisabled:
        return asynDisabledStringified;
    case asynParamAlreadyExists:
        return asynParamAlreadyExistsStringified;
    case asynParamNotFound:
        return asynParamNotFoundStringified;
    case asynParamWrongType:
        return asynParamWrongTypeStringified;
    case asynParamBadIndex:
        return asynParamBadIndexStringified;
    case asynParamUndefined:
        return asynParamUndefinedStringified;
    case asynParamInvalidList:
        return asynParamInvalidListStringified;
    }

    asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
              "%s, line %d:\nInput did not match any "
              "variant of asynStatus.\n",
              __PRETTY_FUNCTION__, __LINE__);

    return inputDidNotMatchAsynStatus;
}

// =============================================================================
// IOC shell functions
extern "C" {

// =============================================================================

/**
 * @brief Set the threshold for the communication timeout frequency (FFI
 * implementation)
 *
 * @param comTimeoutWindow        Size of the time window used to calculate
 * the moving average of timeout events in seconds. Set this value to 0 to
 * deactivate the watchdog.
 * @param maxNumberTimeouts       Maximum number of timeouts which may occur
 * within the time window before the watchdog is triggered.
 * @return asynStatus
 */
asynStatus setThresholdComTimeout(const char *portName, int comTimeoutWindow,
                                  int maxNumberTimeouts) {

    sinqController *pC;
    pC = (sinqController *)findAsynPortDriver(portName);
    if (pC == nullptr) {
        errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
                     portName, __PRETTY_FUNCTION__, __LINE__, portName);
        return asynError;
    }

    return pC->setThresholdComTimeout(comTimeoutWindow, maxNumberTimeouts);
}

static const iocshArg setThresholdComTimeoutArg0 = {"Controller port name",
                                                    iocshArgString};
static const iocshArg setThresholdComTimeoutArg1 = {
    "Time window communication timeout frequency", iocshArgInt};
static const iocshArg setThresholdComTimeoutArg2 = {
    "Maximum allowed number of communication timeouts within the window",
    iocshArgInt};
static const iocshArg *const setThresholdComTimeoutArgs[] = {
    &setThresholdComTimeoutArg0, &setThresholdComTimeoutArg1,
    &setThresholdComTimeoutArg2};
static const iocshFuncDef setThresholdComTimeoutDef = {
    "setThresholdComTimeout", 3, setThresholdComTimeoutArgs};

static void setThresholdComTimeoutCallFunc(const iocshArgBuf *args) {
    setThresholdComTimeout(args[0].sval, args[1].ival, args[2].ival);
}

// =============================================================================

/**
 * @brief Set the maximum number of subsequent timeouts (FFI implementation)
 *
 * @param comTimeoutWindow        Size of the time window used to calculate
 * the moving average of timeout events. Set this value to 0 to deactivate
 * the watchdog.
 * @param maxNumberTimeouts       Maximum number of timeouts which may occur
 * within the time window before the watchdog is triggered.
 * @return asynStatus
 */
asynStatus setMaxSubsequentTimeouts(const char *portName,
                                    int maxSubsequentTimeouts) {
    void *ptr = findAsynPortDriver(portName);
    if (ptr == nullptr) {
        /*
        We can't use asynPrint here since this macro would require us
        to get a pasynOctetSyncIOipPort_ from a pointer to an asynPortDriver.
        However, the given pointer is a nullptr and therefore doesn't
        have a pasynOctetSyncIOipPort_! printf is an EPICS alternative which
        works w/o that, but doesn't offer the comfort provided
        by the asynTrace-facility
        */
        errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
                     portName, __PRETTY_FUNCTION__, __LINE__, portName);
        return asynError;
    }
    // Unsafe cast of the pointer to an asynPortDriver
    asynPortDriver *apd = (asynPortDriver *)(ptr);

    // Safe downcast
    sinqController *pC = dynamic_cast<sinqController *>(apd);
    if (pC == nullptr) {
        errlogPrintf(
            "Controller \"%s\" => %s, line %d:\ncontroller on port %s is not a "
            "turboPmacController.",
            portName, __PRETTY_FUNCTION__, __LINE__, portName);
        return asynError;
    }

    // Set the new value
    pC->setMaxSubsequentTimeouts(maxSubsequentTimeouts);

    return asynSuccess;
}

static const iocshArg SetMaxSubsequentTimeoutsArg0 = {
    "Controller name (e.g. mcu1)", iocshArgString};
static const iocshArg SetMaxSubsequentTimeoutsArg1 = {
    "Maximum number of subsequent timeouts before the user receives an error "
    "message",
    iocshArgInt};
static const iocshArg *const SetMaxSubsequentTimeoutsArgs[] = {
    &SetMaxSubsequentTimeoutsArg0, &SetMaxSubsequentTimeoutsArg1};
static const iocshFuncDef setMaxSubsequentTimeoutsDef = {
    "setMaxSubsequentTimeouts", 2, SetMaxSubsequentTimeoutsArgs};
static void setMaxSubsequentTimeoutsCallFunc(const iocshArgBuf *args) {
    setMaxSubsequentTimeouts(args[0].sval, args[1].ival);
}

// =============================================================================

// This function is made known to EPICS in sinqMotor.dbd and is called by EPICS
// in order to register all functions in the IOC shell
static void sinqControllerRegister(void) {
    iocshRegister(&setThresholdComTimeoutDef, setThresholdComTimeoutCallFunc);
    iocshRegister(&setMaxSubsequentTimeoutsDef,
                  setMaxSubsequentTimeoutsCallFunc);
}
epicsExportRegistrar(sinqControllerRegister);

} // extern C