#include "masterMacsController.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "masterMacsAxis.h"
#include <epicsExport.h>
#include <errlog.h>
#include <iocsh.h>
#include <netinet/in.h>
#include <registryFunction.h>
#include <string.h>
#include <string>
#include <unistd.h>

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

/**
 * @brief Construct a new masterMacsController::masterMacsController object
 *
 * @param portName              See documentation of sinqController
 * @param ipPortConfigName      See documentation of sinqController
 * @param numAxes               See documentation of sinqController
 * @param movingPollPeriod      See documentation of sinqController
 * @param idlePollPeriod        See documentation of sinqController
 * @param comTimeout  Time after which a communication timeout error
 * is declared in writeRead (in seconds)
 * @param extraParams           See documentation of sinqController
 */
masterMacsController::masterMacsController(const char *portName,
                                           const char *ipPortConfigName,
                                           int numAxes, double movingPollPeriod,
                                           double idlePollPeriod,
                                           double comTimeout)
    : sinqController(
          portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod,
          /*
          The following parameter library entries are added in this driver:
          - REREAD_ENCODER_POSITION
          - READ_CONFIG
          */
          NUM_masterMacs_DRIVER_PARAMS)

{

    // Initialization of local variables
    asynStatus status = asynSuccess;

    // Initialization of all member variables
    lowLevelPortUser_ = nullptr;
    comTimeout_ = comTimeout;

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

    /*
    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("Controller \"%s\" => %s, line %d:\nFATAL ERROR (cannot "
                     "connect to MCU controller).\nTerminating IOC",
                     portName, __PRETTY_FUNCTION__, __LINE__);
        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 = "\x03"; // Hex-code for ETX
    status = pasynOctetSyncIO->setInputEos(
        lowLevelPortUser_, message_from_device, strlen(message_from_device));
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, 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(lowLevelPortUser_);
        exit(-1);
    }

    status = callParamCallbacks();
    if (status != asynSuccess) {
        asynPrint(this->pasynUserSelf, 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(lowLevelPortUser_);
        exit(-1);
    }
}

/*
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.
*/
masterMacsAxis *masterMacsController::getAxis(asynUser *pasynUser) {
    asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
    return masterMacsController::castToAxis(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
*/
masterMacsAxis *masterMacsController::getAxis(int axisNo) {
    asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
    return masterMacsController::castToAxis(asynAxis);
}

masterMacsAxis *masterMacsController::castToAxis(asynMotorAxis *asynAxis) {

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

    // If the axis slot of the pAxes_ array is empty, a nullptr must be returned
    if (asynAxis == nullptr) {
        return nullptr;
    }

    // Here, an error is emitted since asyn_axis is not a nullptr but also not
    // an instance of Axis
    masterMacsAxis *axis = dynamic_cast<masterMacsAxis *>(asynAxis);
    if (axis == nullptr) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nAxis is not "
                  "an instance of masterMacsAxis",
                  portName, axis->axisNo_, __PRETTY_FUNCTION__, __LINE__);
    }
    return axis;
}

asynStatus masterMacsController::read(int axisNo, int tcpCmd, char *response) {
    return writeRead(axisNo, tcpCmd, NULL, response);
}

asynStatus masterMacsController::write(int axisNo, int tcpCmd,
                                       const char *payload) {
    return writeRead(axisNo, tcpCmd, payload, NULL);
}

asynStatus masterMacsController::writeRead(int axisNo, int tcpCmd,
                                           const char *payload,
                                           char *response) {

    // Definition of local variables.
    asynStatus status = asynSuccess;
    asynStatus pl_status = asynSuccess;
    char fullCommand[MAXBUF_] = {0};
    char fullResponse[MAXBUF_] = {0};
    char printableCommand[MAXBUF_] = {0};
    char printableResponse[MAXBUF_] = {0};
    char drvMessageText[MAXBUF_] = {0};
    int motorStatusProblem = 0;

    int valueStart = 0;
    int valueStop = 0;

    // Send the message and block the thread until either a response has
    // been received or the timeout is triggered
    int eomReason = 0; // Flag indicating why the message has ended

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

    // Do we expect an response?
    bool isRead = response != NULL;

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

    masterMacsAxis *axis = getAxis(axisNo);
    if (axis == nullptr) {
        // We already did the error logging directly in getAxis
        return asynError;
    }

    /*
    PSI SINQ uses a custom protocol which is described in
    PSI_TCP_Interface_V1-8.pdf (p. // 4-17).
    A special case is the message length, which is specified by two bytes
    LSB and MSB: MSB = message length / 256 LSB = message length % 256. For
    example, a message length of 47 chars would result in MSB = 0, LSB = 47,
    whereas a message length of 356 would result in MSB = 1, LSB = 100.

    The full protocol looks as follows:
    0x05 -> Start of protocol frame ENQ
    [LSB]
    [MSB]
    0x19 -> Data type PDO1
    value [Actual message] It is not necessary to append a terminator, since
    this protocol encodes the message length in LSB and MSB.
    0x0D -> Carriage return (ASCII alias \r)
    0x03 -> End of text ETX
    */

    fullCommand[0] = '\x05'; // ENQ
    fullCommand[1] = 1;      // Placeholder value, can be anything other than 0
    fullCommand[2] = 1;      // Placeholder value, can be anything other than 0
    fullCommand[3] = '\x19'; // PD01

    // Create the command and add CR and ETX at the end
    if (isRead) {
        snprintf(&fullCommand[4], MAXBUF_ - 4, "%dR%02d\x0D\x03", axisNo,
                 tcpCmd);
    } else {
        snprintf(&fullCommand[4], MAXBUF_ - 4, "%dS%02d=%s\x0D\x03", axisNo,
                 tcpCmd, payload);
    }

    // Calculate the command length
    const size_t fullCommandLength = strlen(fullCommand);

    // Length of the command without ENQ and ETX
    const size_t lenWithMetadata = fullCommandLength - 2;

    // Perform both division and modulo operation at once.
    div_t lenWithMetadataSep = std::div(lenWithMetadata, 256);

    // Now set the actual command length
    fullCommand[1] = lenWithMetadataSep.rem;  // LSB
    fullCommand[2] = lenWithMetadataSep.quot; // MSB

    adjustForPrint(printableCommand, fullCommand, MAXBUF_);
    asynPrint(
        this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
        "Controller \"%s\", axis %d => %s, line %d:\nSending command %s\n",
        portName, axisNo, __PRETTY_FUNCTION__, __LINE__, printableCommand);

    // Send out the command
    status =
        pasynOctetSyncIO->write(lowLevelPortUser_, fullCommand,
                                fullCommandLength, comTimeout_, &nbytesOut);

    if (status == asynSuccess) {

        // Try to read the answer repeatedly
        int maxTrials = 2;
        for (int i = 0; i < maxTrials; i++) {
            status =
                pasynOctetSyncIO->read(lowLevelPortUser_, fullResponse, MAXBUF_,
                                       comTimeout_, &nbytesIn, &eomReason);

            if (status == asynSuccess) {
                status = parseResponse(fullCommand, fullResponse,
                                       drvMessageText, &valueStart, &valueStop,
                                       axisNo, tcpCmd, isRead);
                if (status == asynSuccess) {
                    // Received the correct message
                    break;
                }
            } else {
                asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                          "Controller \"%s\", axis %d => %s, line %d:\nError "
                          "%s while reading from the controller\n",
                          portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                          stringifyAsynStatus(status));
                break;
            }

            if (i + 1 == maxTrials && status == asynError) {
                asynPrint(
                    this->pasynUserSelf, ASYN_TRACE_ERROR,
                    "Controller \"%s\", axis %d => %s, line %d:\nFailed "
                    "%d times to get the correct response. Aborting read.\n",
                    portName, axisNo, __PRETTY_FUNCTION__, __LINE__, maxTrials);
            }
        }

    } else {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nError %s while "
                  "writing to the controller\n",
                  portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                  stringifyAsynStatus(status));
    }

    // MasterMACS needs a bit of time between messages, therefore thr program
    // execution is paused after the communication happened.
    // usleep(1500);

    // Create custom error messages for different failure modes
    switch (status) {
    case asynSuccess:

        if (isRead) {
            /*
            If a property has been read, we need just the part between the
            "=" (0x3D) and the [ACK] (0x06). Therefore, we remove all
            non-needed parts after evaluating the second-to-last char before
            returning the response.
             */
            for (int i = 0; i + valueStart < valueStop; i++) {
                response[i] = fullResponse[i + valueStart];
            }
        }

        break;
    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;
    case asynError:
        // Do nothing - error message drvMessageText has already been set.
        break;
    default:
        snprintf(drvMessageText, sizeof(drvMessageText),
                 "Communication failed (%s)", stringifyAsynStatus(status));
        break;
    }

    // Log the overall status (communication successfull or not)
    if (status == asynSuccess) {
        adjustForPrint(printableResponse, fullResponse, MAXBUF_);
        asynPrint(
            lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
            "Controller \"%s\", axis %d => %s, line %d:\nReturn value: %s\n",
            portName, axisNo, __PRETTY_FUNCTION__, __LINE__, printableResponse);
        pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
    } 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
        pl_status =
            getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem);
        if (pl_status != asynSuccess) {
            return paramLibAccessFailed(pl_status, "motorStatusProblem_",
                                        axisNo, __PRETTY_FUNCTION__, __LINE__);
        }

        if (motorStatusProblem == 0) {
            pl_status = axis->setStringParam(motorMessageText_, drvMessageText);
            if (pl_status != asynSuccess) {
                return paramLibAccessFailed(pl_status, "motorMessageText_",
                                            axisNo, __PRETTY_FUNCTION__,
                                            __LINE__);
            }

            pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
            if (pl_status != asynSuccess) {
                return paramLibAccessFailed(pl_status, "motorStatusProblem",
                                            axisNo, __PRETTY_FUNCTION__,
                                            __LINE__);
            }

            pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
            if (pl_status != asynSuccess) {
                return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
                                            axisNo, __PRETTY_FUNCTION__,
                                            __LINE__);
            }
        }
    }

    return status;
}

/*
A response looks like this (note the spaces, they are part of the
message!):
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[ACK][CR] (No error)
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[NAK][CR] (Communication failed)
- [ENQ][LSB][MSB][PDO1] 1 S 10 [CAN][CR] (Driver tried to write with
a read-only command) Read out the second-to-last char of the
response and check if it is NAK or CAN.
*/
asynStatus masterMacsController::parseResponse(
    const char *fullCommand, const char *fullResponse, char *drvMessageText,
    int *valueStart, int *valueStop, int axisNo, int tcpCmd, bool isRead) {

    bool responseValid = false;
    int responseStart = 0;
    char printableCommand[MAXBUF_] = {0};
    char printableResponse[MAXBUF_] = {0};

    // We don't use strlen here since the C string terminator 0x00
    // occurs in the middle of the char array.
    for (uint32_t i = 0; i < MAXBUF_; i++) {
        if (fullResponse[i] == '\x19') {
            responseStart = i;
        } else if (fullResponse[i] == '=') {
            *valueStart = i + 1;
        } else if (fullResponse[i] == '\x06') {
            *valueStop = i;
            responseValid = true;
            break;
        } else if (fullResponse[i] == '\x15') {
            // NAK
            snprintf(drvMessageText, MAXBUF_, "Communication failed.");
            asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
                      "Controller \"%s\", axis %d => %s, line "
                      "%d:\nCommunication failed\n",
                      portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
            break;
        } else if (fullResponse[i] == '\x18') {
            // CAN
            snprintf(drvMessageText, MAXBUF_,
                     "Tried to write with a read-only command. This is a "
                     "bug, please call the support.");
            asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                      "Controller \"%s\", axis %d => %s, line %d:\nTried to "
                      "write with the read-only command %s\n",
                      portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                      printableCommand);
            responseValid = false;
            break;
        }
    }

    if (responseValid) {
        // Check if the response matches the expectations. Each response
        // contains the string "axisNo R tcpCmd" (including the spaces)
        char expectedResponseSubstring[MAXBUF_] = {0};

        // The response does not contain a leading 0 if tcpCmd only has
        // a single digit!
        if (isRead) {
            snprintf(expectedResponseSubstring, MAXBUF_ - 4, "%d R %d", axisNo,
                     tcpCmd);
        } else {
            snprintf(expectedResponseSubstring, MAXBUF_ - 4, "%d S %d", axisNo,
                     tcpCmd);
        }

        if (strstr(&fullResponse[responseStart], expectedResponseSubstring) ==
            NULL) {
            adjustForPrint(printableCommand, fullCommand, MAXBUF_);
            adjustForPrint(printableResponse, fullResponse, MAXBUF_);

            asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
                      "Controller \"%s\", axis %d => %s, line %d:\nMismatched "
                      "response %s to command %s\n",
                      portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
                      printableResponse, printableCommand);

            snprintf(drvMessageText, MAXBUF_,
                     "Mismatched response %s to command %s. Please call the "
                     "support.",
                     printableResponse, printableCommand);
            return asynError;
        }
    }
    return asynSuccess;
}

asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
    // masterMacs can be disabled
    if (pasynUser->reason == motorCanDisable_) {
        *value = 1;
        return asynSuccess;
    } else {
        return asynMotorController::readInt32(pasynUser, value);
    }
}

/***************************************************************************/
/** 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
masterMacsController constructor documentation.
*/
asynStatus masterMacsCreateController(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. 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"
    masterMacsController *pController =
        new masterMacsController(portName, ipPortConfigName, numAxes,
                                 movingPollPeriod, idlePollPeriod, comTimeout);

    return asynSuccess;
}

/*
This is boilerplate code which is used to make the FFI functions
CreateController and CreateAxis "known" to the IOC shell (iocsh).
*/

#ifdef vxWorks
#else

/*
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. mmacs1)",
                                              iocshArgString};
static const iocshArg CreateControllerArg1 = {
    "Asyn IP port name (e.g. pmmacs1)", 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 configMasterMacsCreateController = {
    "masterMacsController", 6, CreateControllerArgs};
static void configMasterMacsCreateControllerCallFunc(const iocshArgBuf *args) {
    masterMacsCreateController(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 masterMacs.dbd and is called by
// EPICS in order to register both functions in the IOC shell
static void masterMacsControllerRegister(void) {
    iocshRegister(&configMasterMacsCreateController,
                  configMasterMacsCreateControllerCallFunc);
}
epicsExportRegistrar(masterMacsControllerRegister);

#endif

} // extern "C"