#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(
            "%s => line %d:\nFATAL ERROR (cannot connect to MCU controller).\n"
            "Terminating IOC",
            __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,
                  "%s => line %d:\nFATAL ERROR (setting input EOS failed "
                  "with %s).\nTerminating IOC",
                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
        pasynOctetSyncIO->disconnect(lowLevelPortUser_);
        exit(-1);
    }

    status = callParamCallbacks();
    if (status != asynSuccess) {
        asynPrint(
            this->pasynUserSelf, ASYN_TRACE_ERROR,
            "%s => line %d:\nFATAL ERROR (executing ParamLib callbacks failed "
            "with %s).\nTerminating IOC",
            __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,
            "%s => line %d:\nAxis %d is not an instance of masterMacsAxis",
            __PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
    }
    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,
              "%s => line %d:\nSending command %s\n", __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,
                          "%s => line %d:\nError %s while reading from the "
                          "controller\n",
                          __PRETTY_FUNCTION__, __LINE__,
                          stringifyAsynStatus(status));
                break;
            }

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

    } else {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "%s => line %d:\nError %s while writing to the controller\n",
                  __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,
                  "%s => line %d:\nReturn value: %s\n", __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_",
                                        __PRETTY_FUNCTION__, __LINE__);
        }

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

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

            pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
            if (pl_status != asynSuccess) {
                return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
                                            __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_TRACE_ERROR,
                      "%s => line %d:\nCommunication failed\n",
                      __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,
                      "%s => line %d:\nTried to write with the read-only "
                      "command %s\n",
                      __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_TRACE_ERROR,
                      "%s => line %d:\nMismatched response %s to "
                      "command %s\n",
                      __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.
    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"
    masterMacsController *pController =
        new masterMacsController(portName, ipPortConfigName, numAxes,
                                 movingPollPeriod, idlePollPeriod, comTimeout);

    return asynSuccess;
}

/*
C wrapper for the axis constructor. Please refer to the masterMacsAxis
constructor documentation. The controller is read from the portName.
*/
asynStatus masterMacsCreateAxis(const char *portName, int axis) {
    masterMacsAxis *pAxis;

    /*
    findAsynPortDriver is a asyn library FFI function which uses the C ABI.
    Therefore it returns a void pointer instead of e.g. a pointer to a
    superclass of the controller such as asynPortDriver. Type-safe upcasting
    via dynamic_cast is therefore not possible directly. However, we do know
    that the void pointer is either a pointer to asynPortDriver (if a driver
    with the specified name exists) or a nullptr. Therefore, we first do a
    nullptr check, then a cast to asynPortDriver and lastly a (typesafe)
    dynamic_upcast to Controller
    https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c
     */
    void *ptr = findAsynPortDriver(portName);
    if (ptr == nullptr) {
        /*
        We can't use asynPrint here since this macro would require us
        to get a lowLevelPortUser_ from a pointer to an asynPortDriver.
        However, the given pointer is a nullptr and therefore doesn't
        have a lowLevelPortUser_! printf is an EPICS alternative which
        works w/o that, but doesn't offer the comfort provided
        by the asynTrace-facility
        */
        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
                     __LINE__, portName);
        return asynError;
    }
    // Unsafe cast of the pointer to an asynPortDriver
    asynPortDriver *apd = (asynPortDriver *)(ptr);

    // Safe downcast
    masterMacsController *pC = dynamic_cast<masterMacsController *>(apd);
    if (pC == nullptr) {
        errlogPrintf("%s => line %d:\ncontroller on port %s is not a "
                     "masterMacsController.",
                     __PRETTY_FUNCTION__, __LINE__, portName);
        return asynError;
    }

    // Prevent manipulation of the controller from other threads while we
    // create the new axis.
    pC->lock();

    /*
    We create a new instance of the axis, 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"
    pAxis = new masterMacsAxis(pC, axis);

    // Allow manipulation of the controller again
    pC->unlock();
    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);
}

/*
Same procedure as for the CreateController function, but for the axis
itself.
*/
static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mmacs1)",
                                        iocshArgString};
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
                                                 &CreateAxisArg1};
static const iocshFuncDef configMasterMacsCreateAxis = {"masterMacsAxis", 2,
                                                        CreateAxisArgs};
static void configMasterMacsCreateAxisCallFunc(const iocshArgBuf *args) {
    masterMacsCreateAxis(args[0].sval, args[1].ival);
}

// 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 masterMacsRegister(void) {
    iocshRegister(&configMasterMacsCreateController,
                  configMasterMacsCreateControllerCallFunc);
    iocshRegister(&configMasterMacsCreateAxis,
                  configMasterMacsCreateAxisCallFunc);
}
epicsExportRegistrar(masterMacsRegister);

#endif

} // extern "C"