#include "detectorTowerAngleAxis.h"
#include "detectorTowerController.h"
#include "turboPmacController.h"
#include <epicsExport.h>
#include <errlog.h>
#include <iocsh.h>

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

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

static void deferredMovementCollectorLoop(void *drvPvt) {
    detectorTowerAngleAxis *axis = (detectorTowerAngleAxis *)drvPvt;
    while (1) {
        if (axis->receivedTarget_) {
            // Wait for 100 ms and then start the movement with the information
            // available
            axis->startingDeferredMovement_ = true;
            epicsThreadSleep(axis->deferredMovementWait_);
            axis->startCombinedMove();

            // After the movement command has been send, reset the flag
            axis->receivedTarget_ = false;
        }
        // Limit this loop to an idle frequency of 1 kHz
        epicsThreadSleep(0.001);
    }
}

detectorTowerAngleAxis::detectorTowerAngleAxis(detectorTowerController *pC,
                                               int axisNo)
    : turboPmacAxis(pC, axisNo, false), pC_(pC) {

    /*
    The superclass constructor sinqAxis calls in turn its superclass constructor
    asynMotorAxis. In the latter, a pointer to the constructed object this is
    stored inside the array pAxes_:

        pC->pAxes_[axisNo] = this;

    Therefore, the axes are managed by the controller pC. If axisNo is out of
    bounds, asynMotorAxis prints an error (see
    https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp,
    line 40). However, we want the IOC creation to stop completely, since this
    is a configuration error.
    */
    if (axisNo >= pC->numAxes()) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d: FATAL ERROR: "
                  "Axis index %d must be smaller than the total number of axes "
                  "%d",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  axisNo_, pC->numAxes());
        exit(-1);
    }

    // Initialize all member variables
    // Assumed to be not ready by default, this is overwritten in the next poll
    error_ = 0;
    receivedTarget_ = false;
    startingDeferredMovement_ = false;
    deferredMovementWait_ = 0.1; // seconds

    // Will be populated in the init() method
    beamRadius_ = 0.0;

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

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

    // Create a thread which collects all movement commands send to components
    // of the virtual axis. After a component received a new target position,
    // it forwards this information to the thread. The thread then waits a short
    // time to see if other components also received new targets and starts the
    // movement with all targets afterwards.
    epicsThreadCreate("deferredMovement", epicsThreadPriorityLow,
                      epicsThreadGetStackSize(epicsThreadStackMedium),
                      (EPICSTHREADFUNC)deferredMovementCollectorLoop,
                      (void *)this);
}

detectorTowerAngleAxis::~detectorTowerAngleAxis(void) {
    // Since the controller memory is managed somewhere else, we don't need to
    // clean up the pointer pC here.
}

asynStatus detectorTowerAngleAxis::init() {

    // Local variable declaration
    asynStatus status = asynSuccess;
    double motorRecResolution = 0.0;
    char response[pC_->MAXBUF_] = {0};
    int positionState = 0;
    int nvals = 0;

    // The parameter library takes some time to be initialized. Therefore we
    // wait until the status is not asynParamUndefined anymore.
    time_t now = time(NULL);
    time_t maxInitTime = 60;
    while (1) {
        status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
                                     &motorRecResolution);
        if (status == asynParamUndefined) {
            if (now + maxInitTime < time(NULL)) {
                asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                          "Controller \"%s\", axis "
                          "%ddeferredMovementCollectorLoop  => %s, line "
                          "%d\nInitializing the parameter library failed.\n",
                          pC_->portName, axisNo_, __PRETTY_FUNCTION__,
                          __LINE__);
                return asynError;
            }
        } else if (status == asynSuccess) {
            break;
        } else if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorRecResolution_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
    }

    // Read the detector radius
    const char *command = "P459 P358";
    status = pC_->writeRead(axisNo_, command, response, 2);
    if (status != asynSuccess) {
        return status;
    }

    nvals = sscanf(response, "%lf %d", &beamRadius_, &positionState);
    if (nvals != 2) {
        return pC_->couldNotParseResponse(command, response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }

    // Initialize the motorStatusMoving flag
    status = setIntegerParam(pC_->motorStatusMoving(), 0);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "motorStatusMoving_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    status = setIntegerParam(pC_->changeStateRBV(), positionState == 2);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "changeStateRBV", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    return callParamCallbacks();
}

// Perform the actual poll
asynStatus detectorTowerAngleAxis::poll(bool *moving) {
    asynStatus status = asynSuccess;

    // Is this axis the one with the smallest index?
    // If not, just read out the movement status and update *moving
    if (axisNo() < liftAxis()->axisNo() && axisNo() < supportAxis()->axisNo()) {
        status = pC_->pollDetectorAxes(moving, this, liftAxis(), supportAxis());
    } else {
        status = pC_->getIntegerParam(axisNo(), pC_->motorStatusMoving(),
                                      (int *)moving);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorStatusMoving",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
    }
    wasMoving_ = *moving;
    return status;
}

asynStatus detectorTowerAngleAxis::doPoll(bool *moving) {
    asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
              "Controller \"%s\", axis %d  => %s, line %d\nThe doPoll method "
              "of this axis type should not be reachable. This is a bug.\n",
              pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
    return asynError;
}

asynStatus detectorTowerAngleAxis::doMove(double position, int relative,
                                          double min_velocity,
                                          double max_velocity,
                                          double acceleration) {
    double motorRecResolution = 0.0;
    asynStatus plStatus = pC_->getDoubleParam(
        axisNo_, pC_->motorRecResolution(), &motorRecResolution);
    if (plStatus != asynSuccess) {
        return pC_->paramLibAccessFailed(plStatus, "motorRecResolution_",
                                         axisNo_, __PRETTY_FUNCTION__,
                                         __LINE__);
    }

    // Signal to the deferredMovementCollectorLoop that a movement should be
    // started to the defined target position.
    targetPosition_ = position * motorRecResolution;
    receivedTarget_ = true;

    return asynSuccess;
}

asynStatus detectorTowerAngleAxis::startCombinedMove() {

    // Status of read-write-operations of ASCII commands to the controller
    asynStatus rwStatus = asynSuccess;

    // Status of parameter library operations
    asynStatus plStatus = asynSuccess;

    char command[pC_->MAXBUF_] = {0};
    char response[pC_->MAXBUF_] = {0};
    double motorCoordinatesPosition = 0.0;
    int positionState = 0;

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

    plStatus =
        pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState);
    if (plStatus != asynSuccess) {
        return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    // If the axis is in changer position, it must be moved into working
    // position before any move can be started.
    bool isInChangerPos = positionState == 2 || positionState == 3;
    if (pC_->getMsgPrintControl().shouldBePrinted(
            pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
            isInChangerPos, pC_->pasynUser())) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d  => %s, line %d\nAxis cannot be "
                  "moved because it is moving from working to changer "
                  "position, is in changer position or is moving from changer "
                  "to working position.%s\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  pC_->getMsgPrintControl().getSuffix());
    }
    if (isInChangerPos) {

        plStatus = setStringParam(pC_->motorMessageText(),
                                  "Move the axis to working state first.");
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "motorMessageText",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
        plStatus = setIntegerParam(pC_->motorStatusProblem(), true);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }

        callParamCallbacks();
        startingDeferredMovement_ = false;
        return asynError;
    }

    // Set the target positions for beam tilt, detector tilt offset and lift
    // offset
    snprintf(command, sizeof(command), "Q451=%lf Q454=%lf P350=1",
             targetPosition_, liftAxis_->targetPosition_);

    // Lock the access to the controller since this function runs in another
    // thread than the poll method.
    pC_->lock();

    // We don't expect an answer
    rwStatus = pC_->writeRead(axisNo_, command, response, 0);

    // Free the controller again
    pC_->unlock();

    if (rwStatus != asynSuccess) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d\nStarting movement to "
            "target position %lf failed.\n",
            pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
            motorCoordinatesPosition);
        plStatus = setIntegerParam(pC_->motorStatusProblem(), true);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
        callParamCallbacks();
    }

    return rwStatus;
}

asynStatus detectorTowerAngleAxis::stop(double acceleration) {

    // Status of read-write-operations of ASCII commands to the controller
    asynStatus rwStatus = asynSuccess;

    // Status of parameter library operations
    asynStatus plStatus = asynSuccess;

    char response[pC_->MAXBUF_] = {0};

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

    rwStatus = pC_->writeRead(axisNo_, "P350=8", response, 0);

    if (rwStatus != asynSuccess) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d\nStopping the movement "
            "failed.\n",
            pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);

        plStatus = setIntegerParam(pC_->motorStatusProblem(), true);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
        return asynError;
    }

    return rwStatus;
}

// The detector tower axis uses absolute encoders
asynStatus detectorTowerAngleAxis::readEncoderType() {
    asynStatus status = setStringParam(pC_->encoderType(), AbsoluteEncoder);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "encoderType_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }
    return asynSuccess;
}

asynStatus
detectorTowerAngleAxis::toggleWorkingChangerState(bool toChangingPosition) {

    char response[pC_->MAXBUF_] = {0};

    // Status of read-write-operations of ASCII commands to the controller
    asynStatus rwStatus = asynSuccess;

    // Status of parameter library operations
    asynStatus plStatus = asynSuccess;

    bool moving = false;
    int positionState = 0;

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

    rwStatus = poll(&moving);
    if (rwStatus != asynSuccess) {
        return rwStatus;
    }

    plStatus =
        pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState);
    if (plStatus != asynSuccess) {
        return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    if (moving) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d  => %s, line %d\nAxis is not "
                  "idle and can therefore not be moved to %s state.%s\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  toChangingPosition ? "changer" : "working",
                  pC_->getMsgPrintControl().getSuffix());

        plStatus = setStringParam(
            pC_->motorMessageText(),
            "Axis cannot be moved to changer position while it is moving.");
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "motorMessageText_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }

        plStatus = setIntegerParam(pC_->changeStateRBV(), !toChangingPosition);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "changeStateRBV",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }

        return asynError;
    }

    // Axis is already in the correct position
    bool isAlreadyThere = (toChangingPosition == false && positionState == 1) ||
                          (toChangingPosition == true && positionState == 2);

    if (isAlreadyThere) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
                  "Controller \"%s\", axis %d  => %s, line %d\nAxis is already "
                  "in %s position.%s\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  toChangingPosition ? "changer" : "working",
                  pC_->getMsgPrintControl().getSuffix());

        // Update the PV anyway, even though nothing changed.
        plStatus = setIntegerParam(pC_->changeStateRBV(), toChangingPosition);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "changeStateRBV",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }

        return asynSuccess;
    }

    // Move the axis into changer or working position
    if (toChangingPosition) {
        rwStatus = pC_->writeRead(axisNo_, "P350=2", response, 0);
    } else {
        rwStatus = pC_->writeRead(axisNo_, "P350=3", response, 0);
    }

    if (plStatus != asynSuccess) {
        plStatus = setIntegerParam(pC_->changeStateRBV(), !toChangingPosition);
        if (plStatus != asynSuccess) {
            return pC_->paramLibAccessFailed(plStatus, "changeStateRBV",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
    }

    plStatus = setIntegerParam(pC_->changeStateRBV(), toChangingPosition);
    if (plStatus != asynSuccess) {
        return pC_->paramLibAccessFailed(plStatus, "changeStateRBV", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    return rwStatus;
}

asynStatus detectorTowerAngleAxis::adjustOrigin(double newOrigin) {

    asynStatus status = asynSuccess;
    char command[pC_->MAXBUF_] = {0};
    char response[pC_->MAXBUF_] = {0};
    int positionState = 0;

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

    status =
        pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "positionStateRBV_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    // If the axis is in changer position, it must be moved into working
    // position before any move can be started.
    bool isInChangerPos = positionState == 2 || positionState == 3;
    if (pC_->getMsgPrintControl().shouldBePrinted(
            pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
            isInChangerPos, pC_->pasynUser())) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d  => %s, line %d\nAxis cannot be "
                  "moved because it is moving from working to changer "
                  "position, is in changer position or is moving from changer "
                  "to working position.%s\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  pC_->getMsgPrintControl().getSuffix());
    }

    // Set the new origin for the angle axis
    snprintf(command, sizeof(command), "Q556=%lf P350=4", newOrigin);

    // We don't expect an answer
    status = pC_->writeRead(axisNo_, command, response, 0);

    if (status != asynSuccess) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d  => %s, line %d\nSetting new "
                  "angle origin %lf failed.\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  newOrigin);
        status = setIntegerParam(pC_->motorStatusProblem(), true);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorStatusProblem_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
    }

    return status;
}

asynStatus detectorTowerAngleAxis::doReset() {

    char response[pC_->MAXBUF_] = {0};
    asynStatus plStatus = asynSuccess;
    int positionState = 0;

    plStatus =
        pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState);
    if (plStatus != asynSuccess) {
        return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    /*
    Check which action should be performed:
    - If error_ == 10 or 11 (FTZ motor error): P352 = 3 (Recover FTZ)
    - If any other error: P352 = 2 (Reset error)
    - Otherwise: P352 = 1 (Set axis in closed-loop mode)
    */
    if (error_ == 10 || error_ == 11) {
        return pC_->writeRead(axisNo_, "P352=3", response, 0);
    } else if (error_ != 0) {
        return pC_->writeRead(axisNo_, "P352=2", response, 0);
    } else {
        return pC_->writeRead(axisNo_, "P352=1", response, 0);
    }
}

/*************************************************************************************/
/** The following functions are C-wrappers, and can be called directly from
 * iocsh */

extern "C" {

/*
C wrapper for the axis constructor. Please refer to the detectorTower
constructor documentation. The controller is read from the portName.
*/
asynStatus detectorTowerCreateAxis(const char *portName, int angleAxisIdx,
                                   int liftAxisIdx, int supportAxisIdx) {

    /*
    findAsynPortDriver is a asyn library FFI function which uses the C ABI.
    Therefore it returns a void pointer instead of e.g. a pointer to a
    superclass of the controller such as asynPortDriver. Type-safe upcasting
    via dynamic_cast is therefore not possible directly. However, we do know
    that the void pointer is either a pointer to asynPortDriver (if a driver
    with the specified name exists) or a nullptr. Therefore, we first do a
    nullptr check, then a cast to asynPortDriver and lastly a (typesafe)
    dynamic_upcast to Controller
    https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c
     */
    void *ptr = findAsynPortDriver(portName);
    if (ptr == nullptr) {
        /*
        We can't use asynPrint here since this macro would require us
        to get a lowLevelPortUser_ from a pointer to an asynPortDriver.
        However, the given pointer is a nullptr and therefore doesn'taxis
        works w/o that, but doesn't offer the comfort provided
        by the asynTrace-facility
        */
        errlogPrintf("Controller \"%s\" => %s, line %d\nPort not found.",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }
    // Unsafe cast of the pointer to an asynPortDriver
    asynPortDriver *apd = (asynPortDriver *)(ptr);

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

    // Assert that the three indices are different from each other
    if (angleAxisIdx == liftAxisIdx) {
        errlogPrintf("Controller \"%s\" => %s, line %d\nAll axis indices "
                     "must be unique.",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }

    // Prevent manipulation of the controller from other threads while we
    // create the new axis.
    pC->lock();

    /*
    We create a new instance of the axis, using the "new" keyword to
    allocate it on the heap while avoiding RAII.
    https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp
    https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp

    The created object is registered in EPICS in its constructor and can safely
    be "leaked" here.
    */

    detectorTowerAngleAxis *angleAxis =
        new detectorTowerAngleAxis(pC, angleAxisIdx);

#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"
    detectorTowerLiftAxis *liftAxis =
        new detectorTowerLiftAxis(pC, liftAxisIdx, angleAxis);

#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"
    detectorTowerSupportAxis *supportAxis =
        new detectorTowerSupportAxis(pC, supportAxisIdx, angleAxis);

    // Allow manipulation of the controller again
    pC->unlock();
    return asynSuccess;
}

static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)",
                                        iocshArgString};
static const iocshArg CreateAxisArg1 = {"Number of the detector angle axis",
                                        iocshArgInt};
static const iocshArg CreateAxisArg2 = {"Number of the lift axis", iocshArgInt};
static const iocshArg CreateAxisArg3 = {"Number of the support axis",
                                        iocshArgInt};
static const iocshArg *const CreateAxisArgs[] = {
    &CreateAxisArg0,
    &CreateAxisArg1,
    &CreateAxisArg2,
    &CreateAxisArg3,
};
static const iocshFuncDef configDetectorTowerCreateAxis = {"detectorTowerAxis",
                                                           4, CreateAxisArgs};
static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) {
    detectorTowerCreateAxis(args[0].sval, args[1].ival, args[2].ival,
                            args[3].ival);
}

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

/**
 * @brief Set the setDeferredMovementWait (FFI implementation)
 *
 * @param portName                Name of the controller
 * @param axisNo                  Axis number
 * @param scaleMovTimeout         Scaling factor (in seconds)
 * @return asynStatus
 */
asynStatus setDeferredMovementWait(const char *portName, int axisNo,
                                   double deferredMovementWait) {

    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;
    }

    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
    detectorTowerAngleAxis *axis =
        dynamic_cast<detectorTowerAngleAxis *>(asynAxis);
    if (axis == nullptr) {
        errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not "
                     "exist or is not an instance of detectorTowerAngleAxis.",
                     portName, __PRETTY_FUNCTION__, __LINE__, axisNo);
        return asynError;
    }

    axis->deferredMovementWait_ = deferredMovementWait;
    return asynSuccess;
}

static const iocshArg setDeferredMovementWaitArg0 = {"Controller port name",
                                                     iocshArgString};
static const iocshArg setDeferredMovementWaitArg1 = {"Axis number",
                                                     iocshArgInt};
static const iocshArg setDeferredMovementWaitArg2 = {
    "Minimum time a deferred movement will be delayed in order to collect "
    "commands in seconds",
    iocshArgDouble};
static const iocshArg *const setDeferredMovementWaitArgs[] = {
    &setDeferredMovementWaitArg0, &setDeferredMovementWaitArg1,
    &setDeferredMovementWaitArg2};
static const iocshFuncDef setDeferredMovementWaitDef = {
    "setDeferredMovementWait", 3, setDeferredMovementWaitArgs};

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

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

// This function is made known to EPICS in detectorTower.dbd and is
// called by EPICS in order to register both functions in the IOC shell
static void detectorTowerAxisRegister(void) {
    iocshRegister(&configDetectorTowerCreateAxis,
                  configDetectorTowerCreateAxisCallFunc);
    iocshRegister(&setDeferredMovementWaitDef, setDeferredMovementWaitCallFunc);
}
epicsExportRegistrar(detectorTowerAxisRegister);

} // extern "C"