#include "sinqController.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include "sinqAxis.h"
#include <errlog.h>

sinqController::sinqController(const char *portName,
                               const char *ipPortConfigName, int numAxes,
                               double movingPollPeriod, double idlePollPeriod,
                               int numExtraParams)
    : asynMotorController(
          portName,
          // As described in the function documentation, an offset of 1 is
          // added for better readability of the configuration.
          numAxes + 1,
          /*
          4 Parameters are added in sinqController:
          - MOTOR_MESSAGE_TEXT
          - MOTOR_TARGET_POSITION
          - ENABLE_AXIS
          - ENABLE_AXIS_RBV
          - ENABLE_MOV_WATCHDOG
          - LIMITS_OFFSET
          */
          NUM_MOTOR_DRIVER_PARAMS + NUM_SINQMOTOR_DRIVER_PARAMS +
              numExtraParams,
          0, // No additional interfaces beyond those in base class
          0, // No additional callback interfaces beyond those in base class
          ASYN_CANBLOCK | ASYN_MULTIDEVICE,
          1, // autoconnect
          0,
          0) // Default priority and stack size
{

    // Initialization of local variables
    asynStatus status = asynSuccess;

    // Initialization of all member variables
    lowLevelPortUser_ = nullptr;

    // =========================================================================;

    /*
    We try to connect to the port via the port name provided by the constructor.
    If this fails, the function is terminated via exit.
    */
    pasynOctetSyncIO->connect(ipPortConfigName, 0, &lowLevelPortUser_, NULL);
    if (status != asynSuccess || lowLevelPortUser_ == nullptr) {
        errlogPrintf(
            "%s => line %d:\nFATAL ERROR (cannot connect to MCU controller).\n"
            "Terminating IOC",
            __PRETTY_FUNCTION__, __LINE__);
        exit(-1);
    }

    // =========================================================================;

    // MOTOR_MESSAGE_TEXT corresponds to the PV definition inside sinqMotor.db.
    // This text is used to forward status messages to NICOS and in turn to the
    // user.
    status =
        createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &motorMessageText_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    // Internal parameter library entry which stores the movement target
    status = createParam("MOTOR_TARGET_POSITION", asynParamFloat64,
                         &motorTargetPosition_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE", asynParamInt32, &motorEnable_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE_RBV", asynParamInt32, &motorEnableRBV_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status =
        createParam("MOTOR_CAN_DISABLE", asynParamInt32, &motorCanDisable_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status =
        createParam("MOTOR_CAN_SET_SPEED", asynParamInt32, &motorCanSetSpeed_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_LIMITS_OFFSET", asynParamFloat64,
                         &motorLimitsOffset_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    /*
    We need to introduce 2 new parameters in order to write the limits from the
    driver to the EPICS record. See the comment in sinqController.h next to
    the declaration of motorHighLimitFromDriver_.
    */
    status = createParam("MOTOR_HIGH_LIMIT_FROM_DRIVER", asynParamFloat64,
                         &motorHighLimitFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_LOW_LIMIT_FROM_DRIVER", asynParamFloat64,
                         &motorLowLimitFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ENABLE_MOV_WATCHDOG", asynParamInt32,
                         &motorEnableMovWatchdog_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VELO_FROM_DRIVER", asynParamFloat64,
                         &motorVeloFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VBAS_FROM_DRIVER", asynParamFloat64,
                         &motorVbasFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_VMAX_FROM_DRIVER", asynParamFloat64,
                         &motorVmaxFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    status = createParam("MOTOR_ACCL_FROM_DRIVER", asynParamFloat64,
                         &motorAcclFromDriver_);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        exit(-1);
    }

    // Poller configuration
    status = startPoller(movingPollPeriod, idlePollPeriod, 1);
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nFATAL ERROR (starting the poller failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        pasynOctetSyncIO->disconnect(lowLevelPortUser_);
        exit(-1);
    }
}

sinqController::~sinqController(void) {
    /*
    Cleanup of the memory allocated in the asynMotorController constructor
    */
    free(this->pAxes_);
}

asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) {
    int function = pasynUser->reason;

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

    asynMotorAxis *asynAxis = getAxis(pasynUser);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
    if (axis == nullptr) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nAxis %d is not an instance of sinqAxis",
                  __PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
        return asynError;
    }

    // Handle custom PVs
    if (function == motorEnable_) {
        return axis->enable(value != 0);
    } else {
        return asynMotorController::writeInt32(pasynUser, value);
    }
}

asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
    if (pasynUser->reason == motorEnableRBV_) {
        // Value is updated in the poll function of an axis
        return asynSuccess;
    } else if (pasynUser->reason == motorCanDisable_) {
        // By default, motors cannot be disabled
        *value = 1;
        return asynSuccess;
    } else {
        return asynMotorController::readInt32(pasynUser, value);
    }
}

asynStatus sinqController::errMsgCouldNotParseResponse(const char *command,
                                                       const char *response,
                                                       int axisNo,
                                                       const char *functionName,
                                                       int lineNumber) {
    asynStatus pl_status = asynSuccess;

    asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
              "%s => line %d:\nCould not interpret response %s for "
              "command %s.\n",
              functionName, lineNumber, response, command);

    pl_status = setStringParam(
        motorMessageText_,
        "Could not interpret MCU response. Please call the support");
    if (pl_status != asynSuccess) {
        return paramLibAccessFailed(pl_status, "motorMessageText_",
                                    __PRETTY_FUNCTION__, __LINE__);
    }

    pl_status = setIntegerParam(motorStatusCommsError_, 1);
    if (pl_status != asynSuccess) {
        return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
                                    __PRETTY_FUNCTION__, __LINE__);
    }

    return asynError;
}

asynStatus sinqController::paramLibAccessFailed(asynStatus status,
                                                const char *parameter,
                                                const char *functionName,
                                                int lineNumber) {

    if (status != asynSuccess) {
        // Log the error message and try to propagate it. If propagating fails,
        // there is nothing we can do here anyway.
        asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
                  "%s => line %d:\n Accessing the parameter library failed for "
                  "parameter %s with error %s.\n",
                  functionName, lineNumber, parameter,
                  stringifyAsynStatus(status));
        setStringParam(motorMessageText_,
                       "Accessing paramLib failed. Please call the support.");
    }

    return status;
}

// Static pointers (valid for the entire lifetime of the IOC). The number behind
// the strings gives the integer number of each variant (see also method
// stringifyAsynStatus)
const char asynSuccessStringified[] = "success";           // 0
const char asynTimeoutStringified[] = "timeout";           // 1
const char asynOverflowStringified[] = "overflow";         // 2
const char asynErrorStringified[] = "error";               // 3
const char asynDisconnectedStringified[] = "disconnected"; // 4
const char asynDisabledStringified[] = "disabled";         // 5
const char asynParamAlreadyExistsStringified[] =
    "parameter already exists";                                     // 6
const char asynParamNotFoundStringified[] = "parameter not found";  // 7
const char asynParamWrongTypeStringified[] = "wrong type";          // 8
const char asynParamBadIndexStringified[] = "bad index";            // 9
const char asynParamUndefinedStringified[] = "parameter undefined"; // 10
const char asynParamInvalidListStringified[] = "invalid list";      // 11
const char inputDidNotMatchAsynStatus[] =
    "Input did not match any variant of asynStatus";

const char *sinqController::stringifyAsynStatus(asynStatus status) {
    // See
    // https://github.com/epics-modules/asyn/blob/master/asyn/asynDriver/asynDriver.h
    // and
    // https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/paramErrors.h
    // for the definition of the error codes
    // The pragma is necessary since the param lib error codes are "tacked onto"
    // the enum, which results in compiler warnings otherwise.
#pragma GCC diagnostic ignored "-Wswitch"
    switch (status) {
    case asynSuccess:
        return asynSuccessStringified;
    case asynTimeout:
        return asynTimeoutStringified;
    case asynOverflow:
        return asynOverflowStringified;
    case asynError:
        return asynErrorStringified;
    case asynDisconnected:
        return asynDisconnectedStringified;
    case asynDisabled:
        return asynDisabledStringified;
    case asynParamAlreadyExists:
        return asynParamAlreadyExistsStringified;
    case asynParamNotFound:
        return asynParamNotFoundStringified;
    case asynParamWrongType:
        return asynParamWrongTypeStringified;
    case asynParamBadIndex:
        return asynParamBadIndexStringified;
    case asynParamUndefined:
        return asynParamUndefinedStringified;
    case asynParamInvalidList:
        return asynParamInvalidListStringified;
    }

    asynPrint(
        pasynUserSelf, ASYN_TRACE_ERROR,
        "%s => line %d:\nInput did not match any variant of asynStatus.\n",
        __PRETTY_FUNCTION__, __LINE__);

    return inputDidNotMatchAsynStatus;
}

// =============================================================================
// Provide the setters to IOC shell
extern "C" {

/**
 * @brief Enable / disable the watchdog (FFI implementation)
 *
 * @param portName                Name of the controller
 * @param axisNo                  Axis number
 * @param enable                  If 0, disable the watchdog, otherwise enable
 * it
 * @return asynStatus
 */
asynStatus setWatchdogEnabled(const char *portName, int axisNo, int enable) {

    sinqController *pC;
    pC = (sinqController *)findAsynPortDriver(portName);
    if (pC == nullptr) {
        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
                     __LINE__, portName);
        return asynError;
    }

    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
    if (axis == nullptr) {
        errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
                     "instance of sinqAxis.",
                     __PRETTY_FUNCTION__, __LINE__, axisNo);
    }

    return axis->setWatchdogEnabled(enable != 0);
}

static const iocshArg setWatchdogEnabledArg0 = {"Controller port name",
                                                iocshArgString};
static const iocshArg setWatchdogEnabledArg1 = {"Axis number", iocshArgInt};
static const iocshArg setWatchdogEnabledArg2 = {
    "Enabling / disabling the watchdog", iocshArgInt};
static const iocshArg *const setWatchdogEnabledArgs[] = {
    &setWatchdogEnabledArg0, &setWatchdogEnabledArg1, &setWatchdogEnabledArg2};
static const iocshFuncDef setWatchdogEnabledDef = {"setWatchdogEnabled", 3,
                                                   setWatchdogEnabledArgs};

static void setWatchdogEnabledCallFunc(const iocshArgBuf *args) {
    setWatchdogEnabled(args[0].sval, args[1].ival, args[2].ival);
}

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

/**
 * @brief Set the offsetMovTimeout (FFI implementation)
 *
 * @param portName                Name of the controller
 * @param axisNo                  Axis number
 * @param offsetMovTimeout        Offset (in seconds)
 * @return asynStatus
 */
asynStatus setOffsetMovTimeout(const char *portName, int axisNo,
                               double offsetMovTimeout) {

    sinqController *pC;
    pC = (sinqController *)findAsynPortDriver(portName);
    if (pC == nullptr) {
        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
                     __LINE__, portName);
        return asynError;
    }

    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
    if (axis == nullptr) {
        errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
                     "instance of sinqAxis.",
                     __PRETTY_FUNCTION__, __LINE__, axisNo);
    }

    return axis->setOffsetMovTimeout(offsetMovTimeout);
}

static const iocshArg setOffsetMovTimeoutArg0 = {"Controller port name",
                                                 iocshArgString};
static const iocshArg setOffsetMovTimeoutArg1 = {"Axis number", iocshArgInt};
static const iocshArg setOffsetMovTimeoutArg2 = {"Offset timeout for movement",
                                                 iocshArgDouble};
static const iocshArg *const setOffsetMovTimeoutArgs[] = {
    &setOffsetMovTimeoutArg0, &setOffsetMovTimeoutArg1,
    &setOffsetMovTimeoutArg2};
static const iocshFuncDef setOffsetMovTimeoutDef = {"setOffsetMovTimeout", 3,
                                                    setOffsetMovTimeoutArgs};

static void setOffsetMovTimeoutCallFunc(const iocshArgBuf *args) {
    setOffsetMovTimeout(args[0].sval, args[1].ival, args[2].dval);
}

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

/**
 * @brief Set the setScaleMovTimeout (FFI implementation)
 *
 * @param portName                Name of the controller
 * @param axisNo                  Axis number
 * @param scaleMovTimeout         Scaling factor (in seconds)
 * @return asynStatus
 */
asynStatus setScaleMovTimeout(const char *portName, int axisNo,
                              double scaleMovTimeout) {

    sinqController *pC;
    pC = (sinqController *)findAsynPortDriver(portName);
    if (pC == nullptr) {
        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
                     __LINE__, portName);
        return asynError;
    }

    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
    if (axis == nullptr) {
        errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
                     "instance of sinqAxis.",
                     __PRETTY_FUNCTION__, __LINE__, axisNo);
    }

    return axis->setScaleMovTimeout(scaleMovTimeout);
}

static const iocshArg setScaleMovTimeoutArg0 = {"Controller port name",
                                                iocshArgString};
static const iocshArg setScaleMovTimeoutArg1 = {"Axis number", iocshArgInt};
static const iocshArg setScaleMovTimeoutArg2 = {
    "Multiplier for calculated move time", iocshArgDouble};
static const iocshArg *const setScaleMovTimeoutArgs[] = {
    &setScaleMovTimeoutArg0, &setScaleMovTimeoutArg1, &setScaleMovTimeoutArg2};
static const iocshFuncDef setScaleMovTimeoutDef = {"setScaleMovTimeout", 3,
                                                   setScaleMovTimeoutArgs};

static void setScaleMovTimeoutCallFunc(const iocshArgBuf *args) {
    setScaleMovTimeout(args[0].sval, args[1].ival, args[2].dval);
}

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

static void sinqControllerRegister(void) {
    iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
    iocshRegister(&setScaleMovTimeoutDef, setScaleMovTimeoutCallFunc);
    iocshRegister(&setWatchdogEnabledDef, setWatchdogEnabledCallFunc);
}
epicsExportRegistrar(sinqControllerRegister);

} // extern C