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

/*
Contains all instances of detectorTowerSupportAxis which have been created and
is used in the initialization hook function.
 */
static std::vector<detectorTowerSupportAxis *> 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<detectorTowerSupportAxis *>::iterator itA =
                 axes.begin();
             itA != axes.end(); ++itA) {
            detectorTowerSupportAxis *axis = *itA;
            axis->init();
        }
    }
}

detectorTowerSupportAxis::detectorTowerSupportAxis(
    detectorTowerController *pC, int axisNo, detectorTowerAngleAxis *angleAxis)
    : 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);
    }

    // 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);

    // Link the axes to each other
    angleAxis_ = angleAxis;
    angleAxis_->supportAxis_ = this;
}

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

asynStatus detectorTowerSupportAxis::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 %d  => %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__);
        }
    }

    // Initialize the motorStatusMoving flag
    setAxisParamChecked(this, motorStatusMoving, false);

    // Check if we are currently in the changer position and update the PV
    // accordingly
    status = pC_->writeRead(axisNo_, "P358", response, 1);
    nvals = sscanf(response, "%d", &positionState);
    if (nvals != 1) {
        return pC_->couldNotParseResponse("P358", response, axisNo(),
                                          __PRETTY_FUNCTION__, __LINE__);
    }

    setAxisParamChecked(this, changeStateRBV, positionState == 2);
    return callParamCallbacks();
}

// Perform the actual poll
asynStatus detectorTowerSupportAxis::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() < angleAxis()->liftAxis()->axisNo() &&
        axisNo() < angleAxis()->axisNo()) {
        status = pC_->pollDetectorAxes(moving, angleAxis(),
                                       angleAxis()->liftAxis(), this);
    } else {
        getAxisParamChecked(this, motorStatusMoving, moving);
    }
    setWasMoving(*moving);
    return status;
}

asynStatus detectorTowerSupportAxis::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 detectorTowerSupportAxis::stop(double acceleration) {
    return angleAxis()->stop(acceleration);
}

asynStatus detectorTowerSupportAxis::reset() { return angleAxis()->reset(); };

asynStatus detectorTowerSupportAxis::readEncoderType() {
    return angleAxis()->readEncoderType();
}

asynStatus detectorTowerSupportAxis::adjustOrigin(double newOrigin) {

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

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

    getAxisParamChecked(this, positionStateRBV, &positionState);

    // 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 adjust for the lift axis
    snprintf(command, sizeof(command), "Q656=%lf P350=6", 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 "
                  "lift origin %lf failed.\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  newOrigin);

        setAxisParamChecked(this, motorStatusProblem, true);
    }

    return status;
}