#include "seleneLiftAxis.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include "seleneGuideController.h"
#include "seleneOffsetAxis.h"
#include "turboPmacController.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 seleneLiftAxis which have been created and is used in
the initialization hook function.
 */
static std::vector<seleneLiftAxis *> 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<seleneLiftAxis *>::iterator itA = axes.begin();
             itA != axes.end(); ++itA) {
            seleneLiftAxis *axis = *itA;
            axis->init();
        }
    }
}

seleneLiftAxis::seleneLiftAxis(seleneGuideController *pC, int axis1No,
                               int axis2No, int axis3No, int axis4No,
                               int axis5No, int axis6No, int liftAxisNo,
                               int angleAxisNo)
    : turboPmacAxis(pC, liftAxisNo, false), pC_(pC) {

    asynStatus status = asynSuccess;

    // Read the offset axes from the controller and write pointers to them into
    // offsetAxes_. If they haven't been created, the configuration is wrong
    // and therefore the IOC creation should abort
    int axisNos[numAxes_];
    axisNos[0] = axis1No;
    axisNos[1] = axis2No;
    axisNos[2] = axis3No;
    axisNos[3] = axis4No;
    axisNos[4] = axis5No;
    axisNos[5] = axis6No;

    seleneOffsetAxis *oAxis;
    for (int i = 0; i < numAxes_; i++) {
        oAxis = dynamic_cast<seleneOffsetAxis *>(pC->getAxis(axisNos[i]));
        if (oAxis == nullptr) {
            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d  => %s, line %d\nFATAL ERROR "
                      "(given axis number %d is not an instance of "
                      "seleneOffsetAxis).\nTerminating IOC.\n",
                      pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                      axisNos[i]);
            exit(-1);
        }
        offsetAxes_[i] = oAxis;

        // Give the lift axis a link to the lift axis
        oAxis->liftAxis_ = this;
    }

    angleAxis_ = new seleneAngleAxis(pC, angleAxisNo, this);

    // Initialize the parameters
    targetSet_ = false;
    virtualAxisMovement_ = true;

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

    // Center between axis 3 and 4
    xLift_ = 0.5 * (offsetAxes_[2]->xOffset_ + offsetAxes_[3]->xOffset_);

    // Selene guide motors 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);
    }

    // Even though this happens already in sinqAxis, a default value for
    // motorMessageText is set here again, because apparently the sinqAxis
    // constructor is not run before the string is accessed?
    status = setStringParam(pC_->motorMessageText(), "");
    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);
    }
}

seleneLiftAxis::~seleneLiftAxis() {}

asynStatus seleneLiftAxis::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__);
        }
    }

    // Assume the virtual axes are not moving at the start
    setAxisParamChecked(this, motorStatusMoving, false);
    setAxisParamChecked(this, motorStatusDone, true);

    setAxisParamChecked(angleAxis_, motorStatusMoving, false);
    setAxisParamChecked(angleAxis_, motorStatusDone, true);

    // The virtual axes should always be seen as enabled, since the underlying
    // hardware is automatically enabled after sending a start command to the
    // controller and automatically disabled after the movement finished.
    setAxisParamChecked(this, motorEnableRBV, true);
    setAxisParamChecked(angleAxis_, motorEnableRBV, true);

    status = normalizePositions();
    if (status != asynSuccess) {
        return status;
    }

    // After the normalization, initialize all axes
    for (int i = 0; i < numAxes_; i++) {
        offsetAxes_[i]->init();
    }
    return asynSuccess;
}

asynStatus seleneLiftAxis::normalizePositions() {

    double lift = 0.0;
    double angle = 0.0;
    asynStatus status = asynSuccess;
    char response[pC_->MAXBUF_] = {0};
    double encoderPositions[6] = {0.0};
    double absolutePositions[6] = {0.0};
    int nvals = 0;

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

    // Read out the absolute positions of all axes
    status = pC_->writeRead(axisNo_, "Q0110 Q0210 Q0310 Q0410 Q0510 Q0610",
                            response, 6);
    if (status != asynSuccess) {
        return status;
    }
    nvals =
        sscanf(response, "%lf %lf %lf %lf %lf %lf", &encoderPositions[0],
               &encoderPositions[1], &encoderPositions[2], &encoderPositions[3],
               &encoderPositions[4], &encoderPositions[5]);
    if (nvals != 6) {
        return pC_->couldNotParseResponse("Q0110 Q0210 Q0310 Q0410 Q0510 Q0610",
                                          response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }

    // Adjust for the individual coordinate system origin of the axes
    for (int i = 0; i < numAxes_; i++) {
        seleneOffsetAxis *axis = offsetAxes_[i];
        absolutePositions[i] = encoderPositions[i] - axis->zOffset_;
    }

    deriveLiftAndAngle(absolutePositions[2], absolutePositions[3], &lift,
                       &angle);

    // Set the position values in the parameter library
    status = setMotorPosition(lift);
    if (status != asynSuccess) {
        return status;
    }

    status = angleAxis_->setMotorPosition(angle);
    if (status != asynSuccess) {
        return status;
    }

    // Calculate the new liftPosition_ of all seleneOffsetAxis
    auto liftPositions = positionsFromLiftAndAngle(lift, angle);
    for (int i = 0; i < numAxes_; i++) {
        double liftPosition = liftPositions[i];
        offsetAxes_[i]->liftPosition_ = liftPosition;
        offsetAxes_[i]->setPositionsFromEncoderPosition(encoderPositions[i]);

        // Write the relative position value to the RBV field
        status = offsetAxes_[i]->setMotorPosition(
            offsetAxes_[i]->absolutePositionLiftCS_ -
            offsetAxes_[i]->liftPosition_);
        if (status != asynSuccess) {
            return status;
        }
    }

    // Update all axes
    callParamCallbacks();
    angleAxis_->callParamCallbacks();
    for (int i = 0; i < numAxes_; i++) {
        offsetAxes_[i]->callParamCallbacks();
    }

    return asynSuccess;
}

asynStatus seleneLiftAxis::doPoll(bool *moving) {
    asynStatus status = asynSuccess;

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

    int nvals = 0;
    int axStatus = 0;
    int wasDone = 0;

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

    getAxisParamChecked(this, motorStatusDone, &wasDone);

    const char *command = "P158";
    status = pC_->writeRead(axisNo_, command, response, 1);
    if (status != asynSuccess) {
        return status;
    }

    nvals = sscanf(response, "%d", &axStatus);
    if (nvals != 1) {
        return pC_->couldNotParseResponse(command, response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }
    *moving = (axStatus != 0);

    // Set the moving status of seleneLiftAxis
    setAxisParamChecked(this, motorStatusMoving, *moving);
    setAxisParamChecked(this, motorStatusDone, !(*moving));

    // Set the moving status of seleneAngleAxis
    setAxisParamChecked(angleAxis_, motorStatusMoving, *moving);
    setAxisParamChecked(angleAxis_, motorStatusDone, !(*moving));

    /*
    If an seleneOffsetAxis is moving, the positions of the virtual axes
    for lift and angle should not change. Therefore the motor position entries
    in the ParamLib of both axes are simply left alone.

    If a virtual axis is moving or was moving during the last poll, the
    positions are calculated from the real positions of axis 3 and 4 corrected
    by their respective motor offset
    */
    if (wasDone == 0 || (!(*moving && !virtualAxisMovement_))) {
        double lift = 0.0;
        double angle = 0.0;
        double offsetAx2Pos = 0.0;
        double offsetAx3Pos = 0.0;

        double motorRecResolution = 0.0;

        getAxisParamChecked(angleAxis_, motorRecResolution,
                            &motorRecResolution);

        status = offsetAxes_[2]->motorPosition(&offsetAx2Pos);
        if (status != asynSuccess) {
            return status;
        }

        status = offsetAxes_[3]->motorPosition(&offsetAx3Pos);
        if (status != asynSuccess) {
            return status;
        }

        deriveLiftAndAngle(
            offsetAxes_[2]->absolutePositionLiftCS_ - offsetAx2Pos,
            offsetAxes_[3]->absolutePositionLiftCS_ - offsetAx3Pos, &lift,
            &angle);

        // Set the position values in the parameter library
        status = setMotorPosition(lift);
        if (status != asynSuccess) {
            return status;
        }

        status = angleAxis_->setMotorPosition(angle);
        if (status != asynSuccess) {
            return status;
        }

        // Calculate the new liftPosition_ of all seleneOffsetAxis
        auto liftPositions = positionsFromLiftAndAngle(lift, angle);
        for (int i = 0; i < numAxes_; i++) {
            offsetAxes_[i]->liftPosition_ = liftPositions[i];
        }
    }

    // Check which one of the individual motors has the smallest distance to its
    // upper and lower limits and derive the lift limits from these.
    double smallestDistHighLimit = std::numeric_limits<double>::infinity();
    double smallestDistLowLimit = std::numeric_limits<double>::infinity();
    for (int i = 0; i < numAxes_; i++) {
        if (smallestDistHighLimit > offsetAxes_[i]->distHighLimit_) {
            smallestDistHighLimit = offsetAxes_[i]->distHighLimit_;
        }
        if (smallestDistLowLimit > offsetAxes_[i]->distLowLimit_) {
            smallestDistLowLimit = offsetAxes_[i]->distLowLimit_;
        }
    }

    // Set the limits for the lift axis
    double motPos = 0.0;
    status = motorPosition(&motPos);
    if (status != asynSuccess) {
        return status;
    }

    setAxisParamChecked(this, motorHighLimitFromDriver,
                        motPos + smallestDistHighLimit);
    setAxisParamChecked(this, motorLowLimitFromDriver,
                        motPos - smallestDistLowLimit);

    // The virtual axes are in an error state, if at least one of the underlying
    // real axes is in an error state.
    for (int i = 0; i < numAxes_; i++) {
        setAxisParamChecked(offsetAxes_[i], motorMessageText, userMessage);
        if (strlen(userMessage) != 0) {
            setAxisParamChecked(this, motorMessageText, userMessage);
            return asynError;
        }
    }

    return asynSuccess;
}

asynStatus seleneLiftAxis::doMove(double position, int relative,
                                  double min_velocity, double max_velocity,
                                  double acceleration) {
    double motorRecResolution = 0.0;
    getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
    setTargetPosition(position * motorRecResolution);
    targetSet_ = true;
    asynStatus status = startCombinedMoveFromVirtualAxis();
    targetSet_ = false;
    return status;
}

double seleneLiftAxis::targetPosition() {
    if (targetSet_) {
        return turboPmacAxis::targetPosition();
    } else {
        double targetPos = 0;
        motorPosition(&targetPos);
        return targetPos;
    }
}

asynStatus seleneLiftAxis::startCombinedMove() {

    char command[pC_->MAXBUF_] = {0};
    char response[pC_->MAXBUF_] = {0};
    double liftTargetPosition = 0.0;
    double angleTargetPosition = 0.0;

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

    liftTargetPosition = targetPosition();
    angleTargetPosition = angleAxis_->targetPosition();
    auto targetPositions =
        positionsFromLiftAndAngle(liftTargetPosition, angleTargetPosition);

    // Apply the offsets of the individual offset axes
    for (int i = 0; i < numAxes_; i++) {
        targetPositions[i] =
            targetPositions[i] + offsetAxes_[i]->targetPosition();
    }

    // Set all target positions and send the move command
    snprintf(command, sizeof(command),
             "Q151=%lf Q152=%lf Q153=%lf Q154=%lf Q155=%lf Q156=%lf P150=1",
             targetPositions[0], targetPositions[1], targetPositions[2],
             targetPositions[3], targetPositions[4], targetPositions[5]);

    // No answer expected
    return pC_->writeRead(axisNo_, command, response, 0);
}

asynStatus seleneLiftAxis::stop(double acceleration) {
    // Status of read-write-operations of ASCII commands to the controller
    asynStatus status = asynSuccess;

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

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

    // Stop all axes
    status = pC_->writeRead(axisNo_, "P150=8", response, 0);

    if (status != 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__);

        setAxisParamChecked(this, motorStatusProblem, true);
    }

    return status;
}

asynStatus seleneLiftAxis::doReset() {

    // Reset all underlying real axes
    for (int i = 0; i < numAxes_; i++) {
        offsetAxes_[i]->reset();
    }
    return asynSuccess;
}

asynStatus seleneLiftAxis::enable(bool on) {
    setAxisParamChecked(this, motorMessageText,
                        "Axis cannot be enabled / disabled");
    return asynSuccess;
}

asynStatus seleneLiftAxis::readEncoderType() {
    setAxisParamChecked(this, encoderType, IncrementalEncoder);
    return asynSuccess;
}

asynStatus seleneLiftAxis::doHome(double minVelocity, double maxVelocity,
                                  double acceleration, int forwards) {

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

    // No answer expected
    return pC_->writeRead(axisNo_,
                          "Q151=0 Q152=0 Q153=0 Q154=0 Q155=0 Q156=0 P150=1",
                          response, 0);
}

std::array<double, 6> seleneLiftAxis::positionsFromLiftAndAngle(double lift,
                                                                double angle) {
    // Initialize the array containing the positions
    std::array<double, 6> positions;

    /*
    Calculate the individual positions by applying
    z = lift + tan(angle)*(x-xLift_).


    Apply the individual zOffset_ of the axis compared to that of the virtual
    axis (which is on the same level as axis 1) as well.
    Angles in degree are converted to radians.

    The angle is inverted by convention: A positive angle means that axes 4 to 6
    go down and 1 to 3 go up
    */

    double tanAngle = tan(-angle * std::numbers::pi / 180.0);

    for (int i = 0; i < numAxes_; i++) {
        positions[i] = lift + tanAngle * (offsetAxes_[i]->xOffset_ - xLift_) -
                       offsetAxes_[i]->zOffset_;
    }
    return positions;
}

void seleneLiftAxis::deriveLiftAndAngle(double pos1, double pos2, double *lift,
                                        double *angle) {
    // Read the x- and y-value of axes 3 and 4
    double x1 = offsetAxes_[2]->xOffset_;
    double x2 = offsetAxes_[3]->xOffset_;

    *lift = 0.5 * (pos1 + pos2);

    /*
    Returned radian value is converted to degree
    The angle is inverted by convention: A positive angle means that axes 4 to 6
    go down and 1 to 3 go up
    */
    *angle = -atan2(pos2 - pos1, x2 - x1) * 180.0 / std::numbers::pi;
}

seleneOffsetAxis *seleneLiftAxis::offsetAxis(int idx) {
    if (idx < numAxes_) {
        return offsetAxes_[idx];
    } else {
        return nullptr;
    }
}

/*************************************************************************************/
/** 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 seleneVirtualCreateAxes(const char *portName, int axis1No,
                                   int axis2No, int axis3No, int axis4No,
                                   int axis5No, int axis6No, int liftAxisNo,
                                   int angleAxisNo) {

    /*
    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
    seleneGuideController *pC = dynamic_cast<seleneGuideController *>(apd);
    if (pC == nullptr) {
        errlogPrintf("Controller \"%s\" => %s, line %d\nController "
                     "is not a seleneGuideController.",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        return asynError;
    }

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

#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"
    seleneLiftAxis *pAxis =
        new seleneLiftAxis(pC, axis1No, axis2No, axis3No, axis4No, axis5No,
                           axis6No, liftAxisNo, angleAxisNo);

    // 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 first physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg2 = {"Number of the second physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg3 = {"Number of the third physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg4 = {"Number of the fourth physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg5 = {"Number of the fifth physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg6 = {"Number of the sixth physical axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg7 = {"Number of the virtual lift axis",
                                        iocshArgInt};

static const iocshArg CreateAxisArg8 = {"Number of the virtual angle axis",
                                        iocshArgInt};

static const iocshArg *const CreateAxisArgs[] = {
    &CreateAxisArg0, &CreateAxisArg1, &CreateAxisArg2,
    &CreateAxisArg3, &CreateAxisArg4, &CreateAxisArg5,
    &CreateAxisArg6, &CreateAxisArg7, &CreateAxisArg8,
};
static const iocshFuncDef configSeleneVirtualCreateAxes = {"seleneVirtualAxes",
                                                           9, CreateAxisArgs};
static void configSeleneVirtualCreateAxesCallFunc(const iocshArgBuf *args) {
    seleneVirtualCreateAxes(args[0].sval, args[1].ival, args[2].ival,
                            args[3].ival, args[4].ival, args[5].ival,
                            args[6].ival, args[7].ival, args[8].ival);
}

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

// 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 seleneVirtualAxesRegister(void) {
    iocshRegister(&configSeleneVirtualCreateAxes,
                  configSeleneVirtualCreateAxesCallFunc);
}
epicsExportRegistrar(seleneVirtualAxesRegister);

} // extern "C"