#include "sinqAxis.h"
#include "sinqController.h"
#include <math.h>
#include <unistd.h>

sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
    : asynMotorAxis((asynMotorController *)pC, axisNo), pC_(pC) {
    asynStatus status = asynSuccess;

    initial_poll_ = true;
    watchdogMovActive_ = false;
    init_poll_counter_ = 0;
    scaleMovTimeout_ = 2.0;
    offsetMovTimeout_ = 30;

    // Motor is assumed to be enabled
    status = pC_->setIntegerParam(axisNo_, pC_->motorEnableRBV_, 1);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
            "with %s)\n. Terminating IOC",
            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // By default, motors cannot be disabled
    status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
            "with %s)\n. Terminating IOC",
            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // Provide a default value for the motor position.
    status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
            "with %s)\n. Terminating IOC",
            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // We assume that the motor has no status problems initially
    status = pC_->setIntegerParam(axisNo_, pC_->motorStatusProblem_, 0);
    if (status != asynSuccess) {
        asynPrint(
            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
            "with %s)\n. Terminating IOC",
            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
        exit(-1);
    }
}

asynStatus sinqAxis::atFirstPoll() {
    asynStatus status = asynSuccess;
    int variableSpeed = 0;

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

    if (variableSpeed == 1) {
        status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, 0.0);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
        status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_,
                                     1000000000.0);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
    } else {
        status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, 0.0);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
        status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, 0.0);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
    }

    return status;
}

asynStatus sinqAxis::poll(bool *moving) {
    // Local variable declaration
    asynStatus pl_status = asynSuccess;
    asynStatus poll_status = asynSuccess;

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

    // If this poll is the initial poll, check if the parameter library has
    // already been initialized. If not, force EPICS to repeat the poll until
    // the initialization is complete (or until a timeout is reached). Once the
    // parameter library has been initialized, read configuration data from the
    // motor controller into it.
    if (initial_poll_) {
        poll_status = atFirstPoll();

        if (poll_status == asynSuccess) {
            initial_poll_ = false;
        } else {
            // Send a message to the IOC shell every 10 trials.
            init_poll_counter_ += 1;
            if (init_poll_counter_ % 10 == 0) {
                asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
                          "%s => line %d:\nRunning function 'atFirstPoll' "
                          "failed %d times with error %s.\n",
                          __PRETTY_FUNCTION__, __LINE__, init_poll_counter_,
                          pC_->stringifyAsynStatus(poll_status));
            }

            // Wait for 100 ms until trying the entire poll again
            usleep(100000);
            return poll_status;
        }
    }

    /*
    At the beginning of the poll, it is assumed that the axis has no status
    problems and therefore all error indicators are reset. This does not affect
    the PVs until callParamCallbacks has been called!

    The motorStatusProblem_ field changes the motor record fields SEVR and STAT.
    */
    pl_status = setIntegerParam(pC_->motorStatusProblem_, false);
    if (pl_status != asynSuccess) {
        pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
                                  __PRETTY_FUNCTION__, __LINE__);
    }
    pl_status = setIntegerParam(pC_->motorStatusCommsError_, false);
    if (pl_status != asynSuccess) {
        pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_",
                                  __PRETTY_FUNCTION__, __LINE__);
    }
    pl_status = setStringParam(pC_->motorMessageText_, "");
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    // The poll function is just a wrapper around doPoll and
    // handles mainly the callParamCallbacks() function. This wrapper is used
    // to make sure callParamCallbacks() is called in case of a premature
    // return.
    poll_status = doPoll(moving);

    // The poll did not succeed: Something went wrong and the motor has a status
    // problem.
    if (poll_status != asynSuccess) {
        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
        if (pl_status != asynSuccess) {
            pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
                                      __PRETTY_FUNCTION__, __LINE__);
        }
    }

    // Check and update the watchdog
    if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
        return asynError;
    }

    if (pl_status != asynSuccess) {
        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFunction isEnabled failed with %s.\n",
                  __PRETTY_FUNCTION__, __LINE__,
                  pC_->stringifyAsynStatus(poll_status));
        pl_status = setStringParam(pC_->motorMessageText_,
                                   "Could not check whether the motor is "
                                   "enabled or not. Please call the support");
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
    }

    // According to the function documentation of asynMotorAxis::poll, this
    // function should be called at the end of a poll implementation.
    pl_status = callParamCallbacks();
    if (pl_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_->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\ncallParamCallbacks failed with %s for axis %d",
            __PRETTY_FUNCTION__, __LINE__,
            pC_->stringifyAsynStatus(poll_status), axisNo_);
        poll_status = pl_status;
    }

    return poll_status;
}

asynStatus sinqAxis::doPoll(bool *moving) { return asynSuccess; }

asynStatus sinqAxis::move(double position, int relative, double minVelocity,
                          double maxVelocity, double acceleration) {

    double targetPosition = 0.0;

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

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

    // Calculate the (absolute) target position
    if (relative) {

        double motorPosition = 0.0;
        pl_status =
            pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        targetPosition = position + motorPosition;
    } else {
        targetPosition = position;
    }

    // Set the target position
    pl_status = setDoubleParam(pC_->motorTargetPosition_, targetPosition);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    return doMove(position, relative, minVelocity, maxVelocity, acceleration);
}

asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,
                            double maxVelocity, double acceleration) {
    return asynSuccess;
}

asynStatus sinqAxis::home(double minVelocity, double maxVelocity,
                          double acceleration, int forwards) {

    double targetPosition = 0.0;
    double position = 0.0;
    double highLimit = 0.0;
    double lowLimit = 0.0;

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

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

    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &position);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &highLimit);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorHighLimit_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &lowLimit);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    if (std::fabs(position - highLimit) > std::fabs(position - lowLimit)) {
        targetPosition = highLimit;
    } else {
        targetPosition = lowLimit;
    }

    // Set the target position
    pl_status = setDoubleParam(pC_->motorTargetPosition_, targetPosition);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    return doHome(minVelocity, maxVelocity, acceleration, forwards);
}

asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
                            double acceleration, int forwards) {
    return asynSuccess;
}

asynStatus sinqAxis::enable(bool on) { return asynSuccess; }

asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
    asynStatus status = asynSuccess;
    int variableSpeed = 0;

    // Can the speed of the motor be varied?
    status =
        pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_, &variableSpeed);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }
    if (variableSpeed == 1) {

        // Check the inputs and create corresponding error messages
        if (vbas > vmax) {
            asynPrint(pC_->lowLevelPortUser_, ASYN_TRACE_ERROR,
                      "%s => line %d:\nLower speed limit vbas=%lf must not be "
                      "smaller than upper limit vmax=%lf.\n",
                      __PRETTY_FUNCTION__, __LINE__, vbas, vmax);

            status = setStringParam(
                pC_->motorMessageText_,
                "Lower speed limit must not be smaller than upper speed limit");
            if (status != asynSuccess) {
                return pC_->paramLibAccessFailed(status, "motorMessageText_",
                                                 __PRETTY_FUNCTION__, __LINE__);
            }
            return asynError;
        }
        if (velo < vbas || velo > vmax) {
            asynPrint(pC_->lowLevelPortUser_, ASYN_TRACE_ERROR,
                      "%s => line %d:\nActual speed velo=%lf must be between "
                      "lower limit vbas=%lf and upper limit vmax=%lf.\n",
                      __PRETTY_FUNCTION__, __LINE__, velo, vbas, vmax);

            status = setStringParam(pC_->motorMessageText_,
                                    "Speed is not inside limits");
            if (status != asynSuccess) {
                return pC_->paramLibAccessFailed(status, "motorMessageText_",
                                                 __PRETTY_FUNCTION__, __LINE__);
            }
            return asynError;
        }

        status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, vbas);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, vmax);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
    } else {
        // Set minimum and maximum speed equal to the set speed
        status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, velo);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, velo);
        if (status != asynSuccess) {
            return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }
    }
    return status;
}

asynStatus sinqAxis::setAcclField(double accl) {

    if (accl <= 0.0) {
        return asynError;
    }

    asynStatus status =
        pC_->setDoubleParam(axisNo_, pC_->motorAcclFromDriver_, accl);
    if (status != asynSuccess) {
        return pC_->paramLibAccessFailed(status, "motorAcclFromDriver_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }
    return status;
}

asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
    return pC_->setIntegerParam(axisNo_, pC_->motorEnableMovWatchdog_, enable);
}

asynStatus sinqAxis::startMovTimeoutWatchdog() {
    asynStatus pl_status;
    int enableMovWatchdog = 0;

    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog_,
                                     &enableMovWatchdog);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    if (enableMovWatchdog == 1) {
        // These parameters are only needed in this branch
        double motorPosition = 0.0;
        double motorPositionRec = 0.0;
        double motorTargetPositionRec = 0.0;
        double motorTargetPosition = 0.0;
        double motorVelocity = 0.0;
        double motorVelocityRec = 0.0;
        double motorAccel = 0.0;
        double motorAccelRec = 0.0;
        double motorRecResolution = 0.0;
        time_t timeContSpeed = 0;
        time_t timeAccel = 0;

        // Activate the watchdog
        watchdogMovActive_ = true;

        /*
        NOTE: This function must not be called in the constructor (e.g. in order
        to save the read result to the member variable earlier), since the
        parameter library is updated at a later stage!
        */
        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
                                        &motorRecResolution);
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_,
                                        &motorPositionRec);
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorPosition",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        motorPosition = motorPositionRec * motorRecResolution;

        /*
        We use motorVelocity, which corresponds to the record field VELO.
        From https://epics.anl.gov/docs/APS2015/14-Motor-Record.pdf:
        * VELO = motorVelocity_ = Slew velocity
        * VBAS = motorVelBase_ = Only used for stepper motors to minimize
        resonance.
        As documented in
        https://epics.anl.gov/docs/APS2015/17-Motor-Driver.pdf, the
        following relations apply: motorVelBase = VBAS / MRES motorVelocity
        = VELO / MRES motorAccel = (motorVelocity - motorVelBase) / ACCL
        Therefore, we need to correct the values from the parameter library.
         */

        // Read the velocity
        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorVelocity_,
                                        &motorVelocityRec);

        // Only calculate timeContSpeed if the motorVelocity has been populated
        // with a sensible value (e.g. > 0)
        if (pl_status == asynSuccess && motorVelocityRec > 0.0) {
            // Convert back to the value in the VELO field
            motorVelocity = motorVelocityRec * motorRecResolution;

            pl_status = pC_->getDoubleParam(axisNo_, pC_->motorTargetPosition_,
                                            &motorTargetPositionRec);
            motorTargetPosition = motorTargetPositionRec * motorRecResolution;
            if (pl_status == asynSuccess) {

                timeContSpeed =
                    std::ceil(std::fabs(motorTargetPosition - motorPosition) /
                              motorVelocity);
            }
        }

        pl_status =
            pC_->getDoubleParam(axisNo_, pC_->motorAccel_, &motorAccelRec);
        if (pl_status == asynSuccess && motorVelocityRec > 0.0 &&
            motorAccelRec > 0.0) {

            // Convert back to the value in the ACCL field
            motorAccel = motorVelocityRec / motorAccelRec;

            // Calculate the time
            timeAccel = 2 * std::ceil(motorVelocity / motorAccel);
        }

        // Calculate the expected arrival time
        expectedArrivalTime_ =
            time(NULL) + offsetMovTimeout_ +
            scaleMovTimeout_ * (timeContSpeed + 2 * timeAccel);
    } else {
        watchdogMovActive_ = false;
    }
    return asynSuccess;
}

asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
    asynStatus pl_status;
    int enableMovWatchdog = 0;

    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog_,
                                     &enableMovWatchdog);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    // Not moving or watchdog not active / enabled
    if (enableMovWatchdog == 0 || !moving || !watchdogMovActive_) {
        watchdogMovActive_ = false;
        return asynSuccess;
    }

    // Check if the expected time of arrival has been exceeded.
    if (expectedArrivalTime_ < time(NULL)) {
        // Check the watchdog
        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nAxis %d exceeded the expected arrival time "
                  "%ld (current time is %ld).\n",
                  __PRETTY_FUNCTION__, __LINE__, axisNo_, expectedArrivalTime_,
                  time(NULL));

        pl_status = setStringParam(
            pC_->motorMessageText_,
            "Exceeded expected arrival time. Check if the axis is blocked.");
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
                                             __PRETTY_FUNCTION__, __LINE__);
        }

        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
        if (pl_status != asynSuccess) {
            pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
                                      __PRETTY_FUNCTION__, __LINE__);
        }

        // Even if the movement timed out, the rest of the poll should continue.
        return asynSuccess;
    }
    return asynSuccess;
}