#include "beamShiftAxis.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include <cmath>
#include <errlog.h>
#include <initHooks.h>
#include <limits>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <vector>

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

beamShiftAxis::beamShiftAxis(turboPmacController *pC, int axisNo)
    : turboPmacAxis(pC, axisNo), pC_(pC) {

    asynStatus status = asynSuccess;

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

    // Provide initial values for some parameter library entries
    status = pC_->setIntegerParam(axisNo, pC_->rereadEncoderPosition(), 0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d:\nFATAL ERROR "
            "(setting a parameter value failed with %s)\n. Terminating IOC",
            pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
            pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // The girder translation cannot be disabled
    status = pC_->setIntegerParam(axisNo, pC_->motorCanDisable(), 0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d:\nFATAL ERROR "
            "(setting a parameter value failed with %s)\n. Terminating IOC",
            pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
            pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // The girder translation speed cannot be changed
    status = pC_->setIntegerParam(axisNo, pC_->motorCanSetSpeed(), 0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d:\nFATAL ERROR "
            "(setting a parameter value failed with %s)\n. Terminating IOC",
            pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
            pC_->stringifyAsynStatus(status));
        exit(-1);
    }
}

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

asynStatus beamShiftAxis::init() {

    // Local variable declaration
    asynStatus status = asynSuccess;
    double motorRecResolution = 0.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__);
        }
    }

    // Update the parameter library immediately
    status = callParamCallbacks();
    if (status != asynSuccess) {
        // If we can't communicate with the parameter library, it doesn't make
        // sense to try and upstream this to the user -> Just log the error
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d\ncallParamCallbacks "
            "failed with %s.\n",
            pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
            pC_->stringifyAsynStatus(status));
        return status;
    }

    return this->readEncoderType();
}

// Perform the actual poll
asynStatus beamShiftAxis::doPoll(bool *moving) {

    // Return value for the poll
    asynStatus errorStatus = asynSuccess;

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

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

    char command[pC_->MAXBUF_] = {0};
    char response[pC_->MAXBUF_] = {0};
    char userMessage[pC_->MAXBUF_] = {0};
    int nvals = 0;

    int direction = 0;
    int error = 0;
    double currentPosition = 0.0;
    double previousPosition = 0.0;
    double highLimit = 0.0;
    double lowLimit = 0.0;
    double limitsOffset = 0.0;
    int enabled = 0;

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

    // Read the previous motor position
    pl_status = motorPosition(&previousPosition);
    if (pl_status != asynSuccess) {
        return pl_status;
    }

    /*
    Query the following parameter:
    - Moving flag
    - Position
    - Error
    - Upper limit
    - Lower limit
    */
    snprintf(command, sizeof(command), "P154 Q110 P158 P159 Q113 Q114");
    status = pC_->writeRead(axisNo(), command, response, 6);
    if (status != asynSuccess) {
        return status;
    };

    nvals = sscanf(response, "%d %lf %d %d %lf %lf", (int *)moving,
                   &currentPosition, &enabled, &error, &highLimit, &lowLimit);
    if (nvals != 6) {
        return pC_->couldNotParseResponse(command, response, axisNo(),
                                          __PRETTY_FUNCTION__, __LINE__);
    }

    /*
    The axis limits are set as: ({[]})
    where [] are the positive and negative limits set in EPICS/NICOS, {} are the
    software limits set on the MCU and () are the hardware limit switches. In
    other words, the EPICS/NICOS limits should be stricter than the software
    limits on the MCU which in turn should be stricter than the hardware limit
    switches. For example, if the hardware limit switches are at [-10, 10], the
    software limits could be at [-9, 9] and the EPICS / NICOS limits could be at
    [-8, 8]. Therefore, we cannot use the software limits read from the MCU
    directly, but need to shrink them a bit. In this case, we're shrinking them
    by limitsOffset on both sides.
    */
    getAxisParamChecked(this, motorLimitsOffset, &limitsOffset);
    highLimit = highLimit - limitsOffset;
    lowLimit = lowLimit + limitsOffset;

    // Update the enablement PV.
    setAxisParamChecked(this, motorEnableRBV, enabled);

    if (*moving) {
        // If the axis is moving, evaluate the movement direction
        if ((currentPosition - previousPosition) > 0) {
            direction = 1;
        } else {
            direction = 0;
        }
    }

    errorStatus = handleError(error, userMessage, sizeof(userMessage));
    setAxisParamChecked(this, motorMessageText, userMessage);

    // Update the parameter library
    if (error != 0) {
        setAxisParamChecked(this, motorStatusProblem, true);
    }

    if (*moving == false) {
        setAxisParamChecked(this, motorMoveToHome, false);
    }

    setAxisParamChecked(this, motorStatusMoving, *moving);
    setAxisParamChecked(this, motorStatusMoving, !(*moving));
    setAxisParamChecked(this, motorStatusDirection, direction);
    setAxisParamChecked(this, motorHighLimitFromDriver, highLimit);
    setAxisParamChecked(this, motorLowLimitFromDriver, lowLimit);

    pl_status = setMotorPosition(currentPosition);
    if (pl_status != asynSuccess) {
        return pl_status;
    }
    return errorStatus;
}

asynStatus beamShiftAxis::handleError(int error, char *userMessage,
                                      int sizeUserMessage) {
    asynStatus status = asynSuccess;

    // Create the unique callsite identifier manually so it can be used later in
    // the shouldBePrinted calls.
    msgPrintControlKey keyError = msgPrintControlKey(
        pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
    bool resetError = true;

    switch (error) {
    case 0:
        // No error
        break;
    case 1:
        // Axis is not switched on.

        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nAxis must "
                      "be enabled first.%s\n",
                      pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                      pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage, "Axis must be enabled first.");
        status = asynError;
        break;
    case 2:
        // Axis received a new command while it was moving
        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(
                pC_->pasynUser(), ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d  => %s, line %d\nAxis is "
                "still moving, but received another move command. EPICS "
                "should prevent this, check if *moving is set correctly.%s\n",
                pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage,
                 "Axis received move command, but is still moving. Please "
                 "call the support.");
        status = asynError;
        break;
    case 3:
        // Axis received a new command while it was moving
        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nAxis 1 "
                      "error during motion.%s\n",
                      pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                      pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage,
                 "Axis 1 got an error while moving. Please call the support.");
        status = asynError;
        break;
    case 4:
        // Axis received a new command while it was moving
        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nAxis 2 "
                      "error during motion.%s\n",
                      pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                      pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage,
                 "Axis 2 got an error while moving. Please call the support.");
        status = asynError;
        break;
    case 5:
        // Axis received a new command while it was moving
        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nTarget "
                      "position exceeds limits.%s\n",
                      pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                      pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage,
                 "Target position exceeds limits. Please call the support.");
        status = asynError;
        break;
    case 6:
        // Axis received a new command while it was moving
        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nAxis is not "
                      "ready, delete error first.%s\n",
                      pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                      pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage, "Delete error first.");
        status = asynError;
        break;
    default:

        if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
                                                      pC_->pasynUser())) {
            asynPrint(
                pC_->pasynUser(), ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d  => %s, line %d\nUnknown error "
                "P%2.2d01 = %d.%s\n",
                pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
                axisNo(), error, pC_->getMsgPrintControl().getSuffix());
        }
        resetError = false;

        snprintf(userMessage, sizeUserMessage,
                 "Unknown error P%2.2d01 = %d. Please call the support.",
                 axisNo(), error);

        setAxisParamChecked(this, motorMessageText, userMessage);

        status = asynError;
        break;
    }

    if (resetError) {
        pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
    }
    return status;
}

asynStatus beamShiftAxis::doMove(double position, int relative,
                                 double minVelocity, double maxVelocity,
                                 double acceleration) {

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

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

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

    getAxisParamChecked(this, motorEnableRBV, &enabled);
    getAxisParamChecked(this, motorRecResolution, &motorRecResolution);

    if (enabled == 0) {
        asynPrint(
            pC_->pasynUser(), ASYN_TRACE_ERROR,
            "Controller \"%s\", axis %d  => %s, line %d\nAxis is disabled.\n",
            pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
        return asynSuccess;
    }

    // Convert from EPICS to user / motor units
    motorCoordinatesPosition = position * motorRecResolution;

    asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
              "Controller \"%s\", axis %d  => %s, line %d\nStart of axis to "
              "position %lf.\n",
              pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__, position);

    // Set target position and start the move command
    snprintf(command, sizeof(command), "Q251=%lf P150=1",
             motorCoordinatesPosition);

    // 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\nStarting movement to "
            "target position %lf failed.\n",
            pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__,
            motorCoordinatesPosition);
        setAxisParamChecked(this, motorStatusProblem, true);
    }
    return status;
}

asynStatus beamShiftAxis::stop(double acceleration) {

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

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

    return pC_->writeRead(axisNo(), "P150=8", response, 0);
}

asynStatus beamShiftAxis::doReset() {

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

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

    // Delete the error
    return pC_->writeRead(axisNo(), "P152=2", response, 0);
}

/*
This is a no-op.
*/
asynStatus beamShiftAxis::doHome(double min_velocity, double max_velocity,
                                 double acceleration, int forwards) {
    return asynSuccess;
}

/*
Encoder type is absolute encoder
*/
asynStatus beamShiftAxis::readEncoderType() {
    setAxisParamChecked(this, encoderType, AbsoluteEncoder);
    return asynSuccess;
}

asynStatus beamShiftAxis::enable(bool on) {
    char command[pC_->MAXBUF_] = {0};
    char response[pC_->MAXBUF_] = {0};
    char userMessage[pC_->MAXBUF_] = {0};
    int nvals = 0;
    int error = 0;
    int enabled = 0;

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

    // Axis can only be switched on, trying to switch it off creates an error
    // message
    if (on) {
        // Check if the axis is currently switched off
        snprintf(command, sizeof(command), "P158 P159");
        asynStatus status = pC_->writeRead(axisNo(), command, response, 2);
        if (status != asynSuccess) {
            return status;
        }

        nvals = sscanf(response, "%d %d", &enabled, &error);
        if (nvals != 2) {
            return pC_->couldNotParseResponse(command, response, axisNo(),
                                              __PRETTY_FUNCTION__, __LINE__);
        }

        if (enabled == 0) {
            snprintf(command, sizeof(command), "P152=1");
            return pC_->writeRead(axisNo(), command, response, 0);
        } else {
            // Status is ignored on purpose, we always return asynError in this
            // path.
            handleError(error, userMessage, sizeof(userMessage));

            setAxisParamChecked(this, motorStatusProblem, true);
            setAxisParamChecked(this, motorMessageText, userMessage);
            return asynError;
        }
        return asynSuccess;
    } else {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d  => %s, line %d\nAxis cannot be "
                  "switched off.\n",
                  pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
        setAxisParamChecked(this, motorMessageText, "Cannot be switched off.");
        return asynError;
    }
}

/*************************************************************************************/
/** 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 beamShiftAxis
constructor documentation. The controller is read from the portName.
*/
asynStatus beamShiftCreateAxis(const char *portName, int axis) {

    /*
    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 an asynUser from a pointer to an asynPortDriver.
        However, the given pointer is a nullptr and therefore doesn't
        have an asynUser! 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 not found.",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }
    // Unsafe cast of the pointer to an asynPortDriver
    asynPortDriver *apd = (asynPortDriver *)(ptr);

    // Safe downcast
    turboPmacController *pC = dynamic_cast<turboPmacController *>(apd);
    if (pC == nullptr) {
        errlogPrintf("Controller \"%s\" => %s, line %d\nController "
                     "is not a turboPmacController.",
                     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.
    */
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"
    beamShiftAxis *pAxis = new beamShiftAxis(pC, axis);

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

/*
Same procedure as for the CreateController function, but for the axis
itself.
*/
static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)",
                                        iocshArgString};
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
                                                 &CreateAxisArg1};
static const iocshFuncDef configbeamShiftCreateAxis = {
    "beamShiftAxis", 2, CreateAxisArgs,
    "Create an instance of a beamShift axis. The first argument is the "
    "controller this axis should be attached to, the second argument is the "
    "axis number."};
static void configBeamShiftCreateAxisCallFunc(const iocshArgBuf *args) {
    beamShiftCreateAxis(args[0].sval, args[1].ival);
}

// This function is made known to EPICS in beamShift.dbd and is called
// by EPICS in order to register both functions in the IOC shell
static void beamShiftAxisRegister(void) {
    iocshRegister(&configbeamShiftCreateAxis,
                  configBeamShiftCreateAxisCallFunc);
}
epicsExportRegistrar(beamShiftAxisRegister);

} // extern "C"