#include "asynOctetSyncIO.h"
#include "ev42_events_generated.h"
#include <cstring>
#include <epicsStdio.h>
#include <iocsh.h>

// Just for printing
#define __STDC_FORMAT_MACROS
#include <inttypes.h>

#include "asynStreamGeneratorDriver.h"
#include <epicsExport.h>

/* Wrapper to set config values and error out if needed.
 */
static void set_config(rd_kafka_conf_t *conf, char *key, char *value) {
    char errstr[512];
    rd_kafka_conf_res_t res;

    res = rd_kafka_conf_set(conf, key, value, errstr, sizeof(errstr));
    if (res != RD_KAFKA_CONF_OK) {
        // TODO
        // g_error("Unable to set config: %s", errstr);
        exit(1);
    }
}

static void udpPollerTask(void *drvPvt) {
    asynStreamGeneratorDriver *pSGD = (asynStreamGeneratorDriver *)drvPvt;
    pSGD->receiveUDP();
}

static void monitorProducerTask(void *drvPvt) {
    asynStreamGeneratorDriver *pSGD = (asynStreamGeneratorDriver *)drvPvt;
    pSGD->produceMonitor();
}

static void detectorProducerTask(void *drvPvt) {
    asynStreamGeneratorDriver *pSGD = (asynStreamGeneratorDriver *)drvPvt;
    pSGD->produceDetector();
}

// UDP Packet Definitions
struct __attribute__((__packed__)) UDPHeader {
    uint16_t BufferLength;
    uint16_t BufferType;
    uint16_t HeaderLength;
    uint16_t BufferNumber;
    uint16_t RunCmdID;
    uint16_t Status : 8;
    uint16_t McpdID : 8;
    uint16_t TimeStamp[3];
    uint16_t Parameter0[3];
    uint16_t Parameter1[3];
    uint16_t Parameter2[3];
    uint16_t Parameter3[3];

    inline uint64_t nanosecs() {
        uint64_t nsec{((uint64_t)TimeStamp[2]) << 32 |
                      ((uint64_t)TimeStamp[1]) << 16 | (uint64_t)TimeStamp[0]};
        return nsec * 100;
    }
};

struct __attribute__((__packed__)) DetectorEvent {
    uint64_t TimeStamp : 19;
    uint16_t XPosition : 10;
    uint16_t YPosition : 10;
    uint16_t Amplitude : 8;
    uint16_t Id : 1;
    inline uint32_t nanosecs() { return TimeStamp * 100; }
};

struct __attribute__((__packed__)) MonitorEvent {
    uint64_t TimeStamp : 19;
    uint64_t Data : 21;
    uint64_t DataID : 4;
    uint64_t TriggerID : 3;
    uint64_t Id : 1;
    inline uint32_t nanosecs() { return TimeStamp * 100; }
};

/** Constructor for the asynStreamGeneratorDriver class.
 * Calls constructor for the asynPortDriver base class.
 * \param[in] portName The name of the asyn port driver to be created. */
asynStreamGeneratorDriver::asynStreamGeneratorDriver(const char *portName,
                                                     const char *ipPortName,
                                                     const int numChannels)
    : asynPortDriver(portName, 1, /* maxAddr */
                                  // 5,
                     asynInt32Mask | asynInt64Mask |
                         asynDrvUserMask,           /* Interface mask */
                     asynInt32Mask | asynInt64Mask, /* Interrupt mask */
                     0,  /* asynFlags.  This driver does not block and it is
                                            not multi-device, but has a
                            destructor ASYN_DESTRUCTIBLE our version of the Asyn
                            is too old to support this flag */
                     1,  /* Autoconnect */
                     0,  /* Default priority */
                     0), /* Default stack size*/
      num_channels(numChannels + 1), monitorQueue(1000, false),
      detectorQueue(1000, false) {
    // Parameter Setup
    char pv_name_buffer[100];
    P_Counts = new int[this->num_channels];

    asynStatus status;

    for (size_t i = 0; i < this->num_channels; ++i) {
        memset(pv_name_buffer, 0, 100);
        epicsSnprintf(pv_name_buffer, 100, P_CountsString, i);
        status = createParam(pv_name_buffer, asynParamInt32, P_Counts + i);
        setIntegerParam(P_Counts[i], 0);
        printf("%s %d %d %d\n", pv_name_buffer, P_Counts[i], i, status);
    }

    char errstr[512];

    // Create client configuration
    rd_kafka_conf_t *conf = rd_kafka_conf_new();
    set_config(conf, "bootstrap.servers", "linkafka01:9092");
    set_config(conf, "queue.buffering.max.messages", "1e7");

    // Create the Monitor Producer instance.
    this->monitorProducer =
        rd_kafka_new(RD_KAFKA_PRODUCER, conf, errstr, sizeof(errstr));
    if (!this->monitorProducer) {
        // TODO
        // g_error("Failed to create new producer: %s", errstr);
        exit(1);
    }

    conf = rd_kafka_conf_new();
    set_config(conf, "bootstrap.servers", "linkafka01:9092");
    set_config(conf, "queue.buffering.max.messages", "1e7");

    // Create the Detector Producer instance.
    this->detectorProducer =
        rd_kafka_new(RD_KAFKA_PRODUCER, conf, errstr, sizeof(errstr));
    if (!this->detectorProducer) {
        // TODO
        // g_error("Failed to create new producer: %s", errstr);
        exit(1);
    }

    // Setup for Thread Producing Monitor Kafka Events
    status =
        (asynStatus)(epicsThreadCreate(
                         "monitor_produce", epicsThreadPriorityMedium,
                         epicsThreadGetStackSize(epicsThreadStackMedium),
                         (EPICSTHREADFUNC)::monitorProducerTask, this) == NULL);
    if (status) {
        // printf("%s:%s: epicsThreadCreate failure, status=%d\n", driverName,
        // functionName, status);
        printf("%s:%s: epicsThreadCreate failure, status=%d\n",
               "StreamGenerator", "init", status);
        return;
    }

    // Setup for Thread Producing Detector Kafka Events
    status = (asynStatus)(epicsThreadCreate(
                              "monitor_produce", epicsThreadPriorityMedium,
                              epicsThreadGetStackSize(epicsThreadStackMedium),
                              (EPICSTHREADFUNC)::detectorProducerTask,
                              this) == NULL);
    if (status) {
        // printf("%s:%s: epicsThreadCreate failure, status=%d\n", driverName,
        // functionName, status);
        printf("%s:%s: epicsThreadCreate failure, status=%d\n",
               "StreamGenerator", "init", status);
        return;
    }

    // UDP Receive Setup
    pasynOctetSyncIO->connect(ipPortName, 0, &pasynUDPUser, NULL);

    /* Create the thread that receives UDP traffic in the background */
    status = (asynStatus)(epicsThreadCreate(
                              "udp_receive", epicsThreadPriorityMedium,
                              epicsThreadGetStackSize(epicsThreadStackMedium),
                              (EPICSTHREADFUNC)::udpPollerTask, this) == NULL);
    if (status) {
        // printf("%s:%s: epicsThreadCreate failure, status=%d\n", driverName,
        // functionName, status);
        printf("%s:%s: epicsThreadCreate failure, status=%d\n",
               "StreamGenerator", "init", status);
        return;
    }

    // TODO add exit should perhaps ensure the queue is flushed
    // rd_kafka_poll(producer, 0);
    // epicsStdoutPrintf("Kafka Queue Size %d\n", rd_kafka_outq_len(producer));
    // rd_kafka_flush(producer, 10 * 1000);
    // epicsStdoutPrintf("Kafka Queue Size %d\n", rd_kafka_outq_len(producer));
}

asynStreamGeneratorDriver::~asynStreamGeneratorDriver() {
    // should make sure queues are empty and freed
    // and that the kafka producers are flushed and freed
    delete[] P_Counts;
}

// // TODO pretty sure I don't actually need to overwrite this
// asynStatus asynStreamGeneratorDriver::readInt32(asynUser *pasynUser,
//                                                 epicsInt32 *value) {
//     // asynStatus asynStreamGeneratorDriver::readInt64(asynUser *pasynUser,
//     //                                                 epicsInt64 *value) {
//
//     const char *paramName;
//     int function = pasynUser->reason;
//     asynStatus status;
//
//     // TODO not freed
//     getParamName(function, &paramName);
//
//     bool is_p_counts = false;
//     for (size_t i = 0; i < num_channels; ++i) {
//         is_p_counts = is_p_counts | function == P_Counts[i];
//     }
//
//     if (is_p_counts) {
//         status = getIntegerParam(function, value);
//
//         asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: function %d %s
//         %d\n",
//                   "StreamGenerator", "readInt64", function, paramName,
//                   status);
//         // return status;
//         return asynSuccess;
//     } else {
//         return asynError;
//     }
//     return asynSuccess;
// }

void asynStreamGeneratorDriver::receiveUDP() {
    asynStatus status;
    int isConnected;

    char buffer[1500];
    size_t received;
    int eomReason;

    epicsInt32 val;

    const uint32_t x_pixels = 128;
    const uint32_t y_pixels = 128;

    // TODO epics doesn't seem to support uint64, you would need an array of
    // uint32. It does support int64 though.. so we start with that
    epicsInt32 *counts = new epicsInt32[this->num_channels];

    while (true) {
        // memset doesn't work with epicsInt32
        for (size_t i = 0; i < this->num_channels; ++i) {
            counts[i] = 0;
        }

        // epicsStdoutPrintf("polling!!");
        status = pasynManager->isConnected(pasynUDPUser, &isConnected);
        if (status) {
            asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
                      "%s:%s: error calling pasynManager->isConnected, "
                      "status=%d, error=%s\n",
                      "StreamGenerator", "receiveUDP", status,
                      pasynUDPUser->errorMessage);
            // driverName, functionName, status,
            // pasynUserIPPort_->errorMessage);
        }
        asynPrint(pasynUserSelf, ASYN_TRACEIO_DRIVER,
                  "%s:%s: isConnected = %d\n", //
                  "StreamGenerator", "receiveUDP", isConnected);

        status = pasynOctetSyncIO->read(pasynUDPUser, buffer, 1500,
                                        0, // timeout
                                        &received, &eomReason);

        // if (status)
        //     asynPrint(
        //         pasynUserSelf, ASYN_TRACE_ERROR,
        //         "%s:%s: error calling pasynOctetSyncIO->read, status=%d\n",
        //         "StreamGenerator", "receiveUDP", status);

        // buffer[received] = 0;

        if (received) {
            asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, "%s:%s: received %d\n",
                      "StreamGenerator", "receiveUDP", received);

            UDPHeader *header = (UDPHeader *)buffer;

            size_t total_events = (header->BufferLength - 21) / 3;

            // TODO lots of checks and validation missing everywhere here
            if (received == total_events * 6 + 42) {
                asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
                          "%s:%s: received packet %d with %d events (%" PRIu64
                          ")\n",
                          "StreamGenerator", "receiveUDP", header->BufferNumber,
                          total_events, header->nanosecs());

                for (size_t i = 0; i < total_events; ++i) {
                    char *event = (buffer + 21 * 2 + i * 6);

                    if (event[5] & 0x80) { // Monitor Event
                        MonitorEvent *m_event = (MonitorEvent *)event;

                        // asynPrint(
                        //     pasynUserSelf, ASYN_TRACE_ERROR,
                        //     "%s:%s: event (%03d) on monitor %d (%" PRIu64
                        //     ")\n", "StreamGenerator", "receiveUDP", i,
                        //     m_event->DataID, header->nanosecs() +
                        //     (uint64_t)m_event->nanosecs());

                        counts[m_event->DataID + 1] += 1;

                        // needs to be freed!!!
                        auto nme = new NormalisedMonitorEvent();
                        nme->TimeStamp =
                            header->nanosecs() + (uint64_t)m_event->nanosecs();
                        nme->DataID = m_event->DataID;
                        this->monitorQueue.push(nme);

                    } else { // Detector Event
                        DetectorEvent *d_event = (DetectorEvent *)event;
                        counts[0] += 1;

                        // needs to be freed!!!
                        auto nde = new NormalisedDetectorEvent();
                        nde->TimeStamp =
                            header->nanosecs() + (uint64_t)d_event->nanosecs();
                        nde->PixID =
                            (header->McpdID - 1) * x_pixels * y_pixels +
                            x_pixels * (uint32_t)d_event->XPosition +
                            (uint32_t)d_event->YPosition;
                        this->detectorQueue.push(nde);
                    }
                }

                for (size_t i = 0; i < num_channels; ++i) {
                    getIntegerParam(P_Counts[i], &val);
                    counts[i] += val;
                }

                asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
                          "%s:%s: det: (%d), mon0: (%d), mon1: (%d), mon2: "
                          "(%d), mon3: (%d)\n",
                          "StreamGenerator", "receiveUDP", counts[0], counts[1],
                          counts[2], counts[3], counts[4]);

                lock();
                for (size_t i = 0; i < num_channels; ++i) {
                    setIntegerParam(P_Counts[i], counts[i]);
                }
                callParamCallbacks();
                unlock();
            } else {
                asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
                          "%s:%s: invalid UDP packet\n", "StreamGenerator",
                          "receiveUDP");
            }
        }

        epicsThreadSleep(1); // seconds
    }
}

void asynStreamGeneratorDriver::produceMonitor() {

    flatbuffers::FlatBufferBuilder builder(1024);

    std::vector<uint32_t> tof;
    tof.reserve(9000);

    std::vector<uint32_t> did;
    did.reserve(9000);

    int total = 0;
    epicsTimeStamp last_sent = epicsTime::getCurrent();

    uint64_t message_id = 0;

    while (true) {

        if (!this->monitorQueue.isEmpty()) {

            ++total;
            auto nme = this->monitorQueue.pop();
            tof.push_back(nme->TimeStamp);
            did.push_back(nme->DataID);
            delete nme;

        } else {
            epicsThreadSleep(0.001); // seconds
        }

        epicsTimeStamp now = epicsTime::getCurrent();

        // At least every 0.2 seconds
        if (total >= 8192 ||
            epicsTimeDiffInNS(&now, &last_sent) > 200'000'000ll) {
            last_sent = epicsTime::getCurrent();

            if (total) {
                total = 0;

                builder.Clear();

                auto message = CreateEventMessageDirect(
                    builder, "monitor", message_id++, 0, &tof, &did);

                builder.Finish(message, "ev42");
                // printf("buffer size: %d\n", builder.GetSize());

                rd_kafka_resp_err_t err = rd_kafka_producev(
                    monitorProducer, RD_KAFKA_V_TOPIC("NEWEFU_TEST"),
                    RD_KAFKA_V_MSGFLAGS(RD_KAFKA_MSG_F_COPY),
                    // RD_KAFKA_V_KEY((void *)key, key_len),
                    RD_KAFKA_V_VALUE((void *)builder.GetBufferPointer(),
                                     builder.GetSize()),
                    // RD_KAFKA_V_OPAQUE(NULL),
                    RD_KAFKA_V_END);

                if (err) {
                    // TODO
                    //  g_error("Failed to produce to topic %s: %s", topic,
                    //  rd_kafka_err2str(err));
                }

                // epicsStdoutPrintf("Kafka Queue Size %d\n",
                //                   rd_kafka_outq_len(monitorProducer));

                rd_kafka_poll(monitorProducer, 0);

                printf("Monitor Events Queued before sending %d\n",
                       this->monitorQueue.getHighWaterMark());
                this->monitorQueue.resetHighWaterMark();

                tof.clear();
                did.clear();
            }
        }
    }
}

void asynStreamGeneratorDriver::produceDetector() {

    flatbuffers::FlatBufferBuilder builder(1024);

    std::vector<uint32_t> tof;
    tof.reserve(9000);

    std::vector<uint32_t> did;
    did.reserve(9000);

    int total = 0;
    epicsTimeStamp last_sent = epicsTime::getCurrent();

    uint64_t message_id = 0;

    while (true) {

        if (!this->detectorQueue.isEmpty()) {

            ++total;
            auto nde = this->detectorQueue.pop();
            tof.push_back(nde->TimeStamp);
            did.push_back(nde->PixID);
            delete nde;
        } else {
            epicsThreadSleep(0.001); // seconds
        }

        epicsTimeStamp now = epicsTime::getCurrent();

        // At least every 0.2 seconds
        if (total >= 8192 ||
            epicsTimeDiffInNS(&now, &last_sent) > 200'000'000ll) {
            last_sent = epicsTime::getCurrent();

            if (total) {
                total = 0;

                builder.Clear();

                auto message = CreateEventMessageDirect(
                    builder, "detector", message_id++, 0, &tof, &did);

                builder.Finish(message, "ev42");
                // printf("buffer size: %d\n", builder.GetSize());

                rd_kafka_resp_err_t err = rd_kafka_producev(
                    detectorProducer, RD_KAFKA_V_TOPIC("NEWEFU_TEST2"),
                    RD_KAFKA_V_MSGFLAGS(RD_KAFKA_MSG_F_COPY),
                    // RD_KAFKA_V_KEY((void *)key, key_len),
                    RD_KAFKA_V_VALUE((void *)builder.GetBufferPointer(),
                                     builder.GetSize()),
                    // RD_KAFKA_V_OPAQUE(NULL),
                    RD_KAFKA_V_END);

                if (err) {
                    // TODO
                    //  g_error("Failed to produce to topic %s: %s", topic,
                    //  rd_kafka_err2str(err));
                }

                // epicsStdoutPrintf("Kafka Queue Size %d\n",
                //                   rd_kafka_outq_len(monitorProducer));

                rd_kafka_poll(detectorProducer, 0);

                printf("Detector Events Queued before sending %d\n",
                       this->detectorQueue.getHighWaterMark());
                this->detectorQueue.resetHighWaterMark();

                tof.clear();
                did.clear();
            }
        }
    }
}

/* Configuration routine.  Called directly, or from the iocsh function below */

extern "C" {

/** EPICS iocsh callable function to call constructor for the
 * asynStreamGeneratorDriver class. \param[in] portName The name of the asyn
 * port driver to be created. */
asynStatus asynStreamGeneratorDriverConfigure(const char *portName,
                                              const char *ipPortName,
                                              const int numChannels) {
    new asynStreamGeneratorDriver(portName, ipPortName, numChannels);
    return asynSuccess;
}

/* EPICS iocsh shell commands */

static const iocshArg initArg0 = {"portName", iocshArgString};
static const iocshArg initArg1 = {"ipPortName", iocshArgString};
static const iocshArg initArg2 = {"numChannels", iocshArgInt};
static const iocshArg *const initArgs[] = {&initArg0, &initArg1, &initArg2};
static const iocshFuncDef initFuncDef = {"asynStreamGenerator", 3, initArgs};
static void initCallFunc(const iocshArgBuf *args) {
    asynStreamGeneratorDriverConfigure(args[0].sval, args[1].sval,
                                       args[2].ival);
}

void asynStreamGeneratorDriverRegister(void) {
    iocshRegister(&initFuncDef, initCallFunc);
}

epicsExportRegistrar(asynStreamGeneratorDriverRegister);
}