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

sinqAxis::sinqAxis(class sinqController *pC, int axis)
    : asynMotorAxis((asynMotorController *)pC, axis), pC_(pC) {

    bool initial_poll_ = true;
    int init_poll_counter_ = 0;
}

asynStatus sinqAxis::atFirstPoll() { return asynSuccess; }

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 EPCIS 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.",
                          __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;
        }
    }

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

    // If the poll status is ok, reset the error indicators in the parameter
    // library
    if (poll_status == asynSuccess) {
        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__);
        }
    }

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