#include "turboPmacController.h"
#include "asynInt32SyncIO.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "pmacAsynIPPort.h"
#include "turboPmacAxis.h"
#include <epicsExport.h>
#include <errlog.h>
#include <initHooks.h>
#include <iocsh.h>
#include <netinet/in.h>
#include <registryFunction.h>
#include <string.h>
#include <unistd.h>

/**
 * @brief Copy src into dst and replace all carriage returns with spaces. This
 * allows to print *dst with asynPrint.
 *
 *
 * @param dst                 Buffer for the modified string
 * @param src                 Original string
 */
void adjustResponseForPrint(char *dst, const char *src, size_t buf_length) {
    for (size_t i = 0; i < buf_length; i++) {
        if (src[i] == '\r') {
            dst[i] = ' ';
        } else {
            dst[i] = src[i];
        }
    }
}

struct turboPmacControllerImpl {

    // Timeout for the communication process in seconds
    double comTimeout;

    // User for writing int32 values to the port driver.
    asynUser *pasynInt32SyncIOipPort;

    // Indices of additional ParamLib entries
    int rereadEncoderPosition;
    int readConfig;
    int flushHardware;
    int limFromHardware;

    char lastResponse[sinqController::MAXBUF_];
};
#define NUM_turboPmac_DRIVER_PARAMS 5

turboPmacController::turboPmacController(const char *portName,
                                         const char *ipPortConfigName,
                                         int numAxes, double movingPollPeriod,
                                         double idlePollPeriod,
                                         double comTimeout, int numExtraParams)
    : sinqController(
          portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod,
          /*
          The following parameter library entries are added in this driver:
          - REREAD_ENCODER_POSITION
          - READ_CONFIG
          */
          numExtraParams + NUM_turboPmac_DRIVER_PARAMS),
      pTurboPmacC_(
          std::make_unique<turboPmacControllerImpl>((turboPmacControllerImpl){
              .comTimeout = comTimeout,
              .pasynInt32SyncIOipPort = nullptr, // Populated in constructor
              .rereadEncoderPosition = 0,        // Populated in constructor
              .readConfig = 0,                   // Populated in constructor
              .flushHardware = 0,                // Populated in constructor
              .limFromHardware = 0,              // Populated in constructor
              .lastResponse = {0},
          })) {
    // Initialization of local variables
    asynStatus status = asynSuccess;

    // Maximum allowed number of subsequent timeouts before the user is
    // informed.
    setMaxSubsequentTimeouts(10);

    // =========================================================================
    // Create additional parameter library entries

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

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

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

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

    /*
    Define the end-of-string of a message coming from the device to EPICS.
    It is not necessary to append a terminator to outgoing messages, since
    the message length is encoded in the message header in the getSetResponse
    method.
    */
    const char *message_from_device =
        "\006"; // Hex-code for ACK (acknowledge) -> Each message from the MCU
                // is terminated by this value
    status = pasynOctetSyncIO->setInputEos(pasynOctetSyncIOipPort(),
                                           message_from_device,
                                           strlen(message_from_device));
    if (status != asynSuccess) {
        asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d\nFATAL ERROR "
                  "(setting input EOS failed with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort());
        exit(-1);
    }

    status = callParamCallbacks();
    if (status != asynSuccess) {
        asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\" => %s, line %d\nFATAL ERROR "
                  "(executing ParamLib callbacks failed "
                  "with %s).\nTerminating IOC",
                  portName, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
        pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort());
        exit(-1);
    }

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

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

turboPmacController::~turboPmacController() {}

/*
Access one of the axes of the controller via the axis adress stored in asynUser.
If the axis does not exist or is not a Axis, a nullptr is returned and an
error is emitted.
*/
turboPmacAxis *turboPmacController::getTurboPmacAxis(asynUser *pasynUser) {
    asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
    return dynamic_cast<turboPmacAxis *>(asynAxis);
}

/*
Access one of the axes of the controller via the axis index.
If the axis does not exist or is not a Axis, the function must return Null
*/
turboPmacAxis *turboPmacController::getTurboPmacAxis(int axisNo) {
    asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
    return dynamic_cast<turboPmacAxis *>(asynAxis);
}

asynStatus turboPmacController::writeRead(int axisNo, const char *command,
                                          char *response,
                                          int numExpectedResponses) {

    // Definition of local variables.
    asynStatus status = asynSuccess;
    char drvMessageText[MAXBUF_] = {0};
    char modResponse[MAXBUF_] = {0};
    int motorStatusProblem = 0;
    int numReceivedResponses = 0;

    /*
    asyn defines the following reasons for an end-of-message coming from the MCU
    (https://epics.anl.gov/modules/soft/asyn/R4-14/asynDriver.pdf, p. 28):
    0: Timeout
    1: Request count reached
    2: End of string detected -> In this driver, this is the "normal" case
    4: End indicator detected
    Combinations of reasons are also possible, e.g. eomReason = 5 would mean
    that both the request count was reached and an end indicator was detected.
     */
    int eomReason = 0;

    // Number of bytes of the outgoing message (which is command + the
    // end-of-string terminator defined in the constructor)
    size_t nbytesOut = 0;

    // Number of bytes of the incoming message (which is response + the
    // end-of-string terminator defined in the constructor)
    size_t nbytesIn = 0;

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

    turboPmacAxis *axis = getTurboPmacAxis(axisNo);
    if (axis == nullptr) {
        // We already did the error logging directly in getAxis
        return asynError;
    }
    const size_t commandLength = strlen(command);

    /*
    The writeRead command performs the following steps:
    1) Flush the socket buffer on the IOC side (not the controller!)
    2) Write a command to the controller
    3) Read the response

    If a timeout occurs during writing or reading, inform the user that we're
    trying to reconnect. If the problem persists, ask them to call the support
    */
    status = pasynOctetSyncIO->writeRead(
        pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_,
        pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, &eomReason);

    /*
    If sth. is written to the controller, it needs some time to process the
    command. However, the controller returns the acknowledgment via
    pasynOctetSyncIO->writeRead immediately, signalling to the driver that it is
    ready to receive the next command. In practice, this can result in commands
    getting discarded on the driver side or in bringing the driver in undefined
    states (e.g. stuck in status 1).

    To prevent this, we wait for 20 ms after a write command to give the
    controller enough time to process everything. A write command can be
    identified by looking for the equal sign.
    */
    if (strchr(command, '=')) {
        usleep(20000);
    }

    msgPrintControlKey comKey =
        msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);

    if (status == asynTimeout) {

        if (getMsgPrintControl().shouldBePrinted(comKey, true, pasynUser())) {
            asynPrint(
                this->pasynUser(), ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d => %s, line %d\nTimeout while "
                "writing to the controller. Retrying ...%s\n",
                portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                getMsgPrintControl().getSuffix());
        }

        checkComTimeoutWatchdog(axisNo, drvMessageText, sizeof(drvMessageText));

        int timeoutCounter = 0;
        while (1) {
            checkMaxSubsequentTimeouts(timeoutCounter, axis);
            timeoutCounter += 1;

            if (maxSubsequentTimeoutsExceeded()) {
                break;
            }

            status = pasynOctetSyncIO->writeRead(
                pasynOctetSyncIOipPort(), command, commandLength, response,
                MAXBUF_, pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn,
                &eomReason);
            if (status != asynTimeout) {
                asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
                          "Controller \"%s\", axis %d  => %s, line "
                          "%d\nReconnected after write timeout\n",
                          portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
                break;
            }
        }
    } else if (status != asynSuccess) {
        if (getMsgPrintControl().shouldBePrinted(comKey, true, pasynUser())) {
            asynPrint(
                this->pasynUser(), ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d => %s, line %d\nError %s while "
                "writing to the controller.%s\n",
                portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                stringifyAsynStatus(status), getMsgPrintControl().getSuffix());
        }
    } else {
        getMsgPrintControl().resetCount(comKey, pasynUser());
    }

    if (status != asynSuccess) {
        /*
        Since the communication failed, there is the possibility that the
        controller is not connected at all to the network. In that case, we
        cannot be sure that the information read out in the init method of the
        axis is still up-to-date the next time we get a connection. Therefore,
        an info flag is set which the axis object can use at the start of its
        poll method to try to initialize itself.
        */
        axis->setNeedInit(true);
    }

    // The message should only ever terminate due to reason 2
    msgPrintControlKey terminateKey =
        msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
    if (status == asynSuccess && eomReason != 2) {
        status = asynError;

        char reasonStringified[30] = {0};
        switch (eomReason) {
        case 0:
            snprintf(reasonStringified, sizeof(reasonStringified), "Timeout");
            break;
        case 1:
            snprintf(reasonStringified, sizeof(reasonStringified),
                     "Request count reached");
            break;
        case 2:
            snprintf(reasonStringified, sizeof(reasonStringified),
                     "End of string detected");
            break;
        case 3:
            snprintf(reasonStringified, sizeof(reasonStringified),
                     "End indicator detected");
            break;
        }

        snprintf(drvMessageText, sizeof(drvMessageText),
                 "Terminated message due to reason %s (should be \"End of "
                 "string detected\"). Please call the support.",
                 reasonStringified);

        if (getMsgPrintControl().shouldBePrinted(terminateKey, true,
                                                 pasynUser())) {

            asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d => %s, line %d\nMessage "
                      "terminated due to reason %s.%s\n",
                      portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                      reasonStringified, getMsgPrintControl().getSuffix());
        }
    } else {
        getMsgPrintControl().resetCount(terminateKey, pasynUser());
    }

    /*
    Calculate the number of received responses by counting the number of
    carriage returns "\r" in the response.
    */
    for (size_t i = 0; i < strlen(response); i++) {
        if (response[i] == '\r') {
            numReceivedResponses++;
        }
    }
    msgPrintControlKey numResponsesKey =
        msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
    if (status == asynSuccess && numExpectedResponses != numReceivedResponses) {
        adjustResponseForPrint(modResponse, response, MAXBUF_);

        if (getMsgPrintControl().shouldBePrinted(numResponsesKey, true,
                                                 pasynUser())) {
            asynPrint(
                this->pasynUser(), ASYN_TRACE_ERROR,
                "Controller \"%s\", axis %d => %s, line %d\nUnexpected "
                "response '%s' (carriage returns are replaced with spaces) "
                "for command %s.%s\n",
                portName, axisNo, __PRETTY_FUNCTION__, __LINE__, modResponse,
                command, getMsgPrintControl().getSuffix());
        }

        /*
        The literal part of the message below is 113 bytes long. Cap the two
        embedded strings (constexpr precision) so that the message always fits
        into drvMessageText (MAXBUF_ bytes), otherwise the
        -Wformat-truncation check would be triggered.
        */
        constexpr size_t msgLiteralLen =
            sizeof("Received unexpected response '' (carriage returns are "
                   "replaced with spaces) for command . Please call the "
                   "support") -
            1;
        constexpr size_t msgArgLen = (MAXBUF_ - msgLiteralLen - 1) / 2;
        snprintf(drvMessageText, sizeof(drvMessageText),
                 "Received unexpected response '%.*s' (carriage returns "
                 "are replaced with spaces) for command %.*s. Please call "
                 "the support",
                 (int)msgArgLen, modResponse, (int)msgArgLen, command);
        status = asynError;
    } else {
        getMsgPrintControl().resetCount(numResponsesKey, pasynUser());
    }

    // Create custom error messages for different failure modes, if no error
    // message has been set yet
    if (strlen(drvMessageText) == 0) {
        switch (status) {
        case asynSuccess:
            break; // Communicate nothing
        case asynTimeout:
            snprintf(drvMessageText, sizeof(drvMessageText),
                     "connection timeout for axis %d", axisNo);
            break;
        case asynDisconnected:
            snprintf(drvMessageText, sizeof(drvMessageText),
                     "axis is not connected");
            break;
        case asynDisabled:
            snprintf(drvMessageText, sizeof(drvMessageText),
                     "axis is disabled");
            break;
        default:
            snprintf(drvMessageText, sizeof(drvMessageText),
                     "Communication failed (%s)", stringifyAsynStatus(status));
            break;
        }
    }

    // Log the overall status (communication successfull or not)
    if (status == asynSuccess) {
        setAxisParamChecked(axis, motorStatusCommsError, false);
    } else {
        // Check if the axis already is in an error communication mode. If
        // it is not, upstream the error. This is done to avoid "flooding"
        // the user with different error messages if more than one error
        // ocurred before an error-free communication
        getAxisParamChecked(axis, motorStatusProblem, &motorStatusProblem);

        if (motorStatusProblem == 0) {
            setAxisParamChecked(axis, motorMessageText, drvMessageText);
            setAxisParamChecked(axis, motorStatusProblem, true);
            setAxisParamChecked(axis, motorStatusCommsError, true);
        }
    }
    return status;
}

asynStatus turboPmacController::doFlushHardware() {
    /*
    Temporarily overwrite the "reason" field with the FLUSH_HARDWARE
    constant defined in pmacAsynIPPort.h. This reason is then used within
    the write method of pasynInt32SyncIO to select the flush function.
    */
    int temp = pTurboPmacC_->pasynInt32SyncIOipPort->reason;
    pTurboPmacC_->pasynInt32SyncIOipPort->reason = FLUSH_HARDWARE;
    asynStatus status = (asynStatus)pasynInt32SyncIO->write(
        pTurboPmacC_->pasynInt32SyncIOipPort, 1, pTurboPmacC_->comTimeout);

    // Reset the status afterwards
    pTurboPmacC_->pasynInt32SyncIOipPort->reason = temp;
    return status;
}

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

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

    turboPmacAxis *axis = getTurboPmacAxis(pasynUser);

    // Handle custom PVs
    if (function == rereadEncoderPosition()) {
        return axis->rereadEncoder();
    } else if (function == readConfig()) {
        return axis->init();
    } else if (function == flushHardware()) {
        return doFlushHardware();
    } else {
        return sinqController::writeInt32(pasynUser, value);
    }
}

asynStatus turboPmacController::couldNotParseResponse(const char *command,
                                                      const char *response,
                                                      int axisNo,
                                                      const char *functionName,
                                                      int lineNumber) {
    char modifiedResponse[MAXBUF_] = {0};
    adjustResponseForPrint(modifiedResponse, response, MAXBUF_);
    return sinqController::couldNotParseResponse(
        command, modifiedResponse, axisNo, functionName, lineNumber);
}

int turboPmacController::rereadEncoderPosition() {
    return pTurboPmacC_->rereadEncoderPosition;
}
int turboPmacController::readConfig() { return pTurboPmacC_->readConfig; }
int turboPmacController::flushHardware() { return pTurboPmacC_->flushHardware; }
int turboPmacController::limFromHardware() {
    return pTurboPmacC_->limFromHardware;
}

asynUser *turboPmacController::pasynInt32SyncIOipPort() {
    return pTurboPmacC_->pasynInt32SyncIOipPort;
}

/*************************************************************************************/
/** The following functions are C-wrappers, and can be called directly from
 * iocsh */

extern "C" {

/*
C wrapper for the controller constructor. Please refer to the
turboPmacController constructor documentation.
*/
asynStatus turboPmacCreateController(const char *portName,
                                     const char *ipPortConfigName, int numAxes,
                                     double movingPollPeriod,
                                     double idlePollPeriod, double comTimeout) {
    /*
    We create a new instance of the controller, 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"
    turboPmacController *pController =
        new turboPmacController(portName, ipPortConfigName, numAxes,
                                movingPollPeriod, idlePollPeriod, comTimeout);

    return asynSuccess;
}

/*
Define name and type of the arguments for the CreateController function
in the iocsh. This is done by creating structs with the argument names and
types and then providing "factory" functions
(configCreateControllerCallFunc). These factory functions are used to
register the constructors during compilation.
*/
static const iocshArg CreateControllerArg0 = {"Controller name (e.g. mcu1)",
                                              iocshArgString};
static const iocshArg CreateControllerArg1 = {"Asyn IP port name (e.g. pmcu1)",
                                              iocshArgString};
static const iocshArg CreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg CreateControllerArg3 = {"Moving poll rate (s)",
                                              iocshArgDouble};
static const iocshArg CreateControllerArg4 = {"Idle poll rate (s)",
                                              iocshArgDouble};
static const iocshArg CreateControllerArg5 = {"Communication timeout (s)",
                                              iocshArgDouble};
static const iocshArg *const CreateControllerArgs[] = {
    &CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2,
    &CreateControllerArg3, &CreateControllerArg4, &CreateControllerArg5};
static const iocshFuncDef configTurboPmacCreateController = {
    "turboPmacController", 6, CreateControllerArgs,
    "Create a new instance of a TurboPMAC controller."};
static void configTurboPmacCreateControllerCallFunc(const iocshArgBuf *args) {
    turboPmacCreateController(args[0].sval, args[1].sval, args[2].ival,
                              args[3].dval, args[4].dval, args[5].dval);
}

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

} // extern "C"
