#include <signal.h>
#include <stdlib.h>
#include <time.h>
#include <sys/select.h>

#include <ua_types.h>
#include <ua_server.h>
#include <logger_stdout.h>
#include <networklayer_tcp.h>
#include <ua_config_standard.h>

#include <zmq.h>

#include "TestModel.h"

/*
 * shared data
 */
uint32_t upTime = 0;
double measuredTemperature = 0;




UA_Logger logger = Logger_Stdout;
UA_Boolean running = true;



static void stopHandler(int sign) {
  UA_LOG_INFO(logger, UA_LOGCATEGORY_SERVER, "received ctrl-c");
  running = false;
}

static UA_StatusCode
readInteger(void *handle, const UA_NodeId nodeid, UA_Boolean sourceTimeStamp,
            const UA_NumericRange *range, UA_DataValue *dataValue) {
  dataValue->hasValue = true;
  UA_Variant_setScalarCopy(&dataValue->value, handle, &UA_TYPES[UA_TYPES_INT32]);
  return UA_STATUSCODE_GOOD;
}

static UA_StatusCode
readDouble(void *handle, const UA_NodeId nodeid, UA_Boolean sourceTimeStamp,
	   const UA_NumericRange *range, UA_DataValue *dataValue) {
  dataValue->hasValue = true;
  UA_Variant_setScalarCopy(&dataValue->value, handle, &UA_TYPES[UA_TYPES_DOUBLE]);
  return UA_STATUSCODE_GOOD;
}


uint64_t millis() {
  struct timeval te;
  gettimeofday(&te, NULL);
  uint64_t ms = te.tv_sec * 1000 + te.tv_usec / 1000;
  return ms;
}


int main(int argc, char** argv) {
  signal(SIGINT, stopHandler); /* catches ctrl-c */

  /* initialize the OPC-UA server */
  UA_ServerConfig config = UA_ServerConfig_standard;
  UA_ServerNetworkLayer nl = UA_ServerNetworkLayerTCP(UA_ConnectionConfig_standard, 16664);
  config.networkLayers = &nl;
  config.networkLayersSize = 1;
  UA_Server *server = UA_Server_new(config);

  /* create nodes from nodeset */
  TestModel(server);

  /* assign data sources to OPC-UA variables */
  UA_DataSource uptimeDataSource = (UA_DataSource) {
    .handle = &upTime, .read = readInteger, .write = 0};
  UA_Server_setVariableNode_dataSource(server, UA_NODEID_NUMERIC(2, 6006),
				       uptimeDataSource);


  UA_DataSource measuredTemperatureDataSource = (UA_DataSource) {
    .handle = &measuredTemperature, .read = readDouble, .write = 0};
  UA_Server_setVariableNode_dataSource(server, UA_NODEID_NUMERIC(2, 6004),
				       measuredTemperatureDataSource);

  /* prepare OPC-UA server to run cooperative in a loop (below) */
  UA_StatusCode retval = UA_Server_run_startup(server);
  assert(retval == UA_STATUSCODE_GOOD);


  /* prepare zmq context, connect to server and subscribe with empty filter */
  void *zmqContext = zmq_ctx_new();
  void *zmqSubscriber = zmq_socket(zmqContext, ZMQ_SUB);
  int rc = zmq_connect(zmqSubscriber, "tcp://127.0.0.1:12345");
  assert(rc == 0);
  rc = zmq_setsockopt(zmqSubscriber, ZMQ_SUBSCRIBE, 0, 0);
  assert(rc == 0);

     
  while (running == true) {
    upTime = time(0);
    // measuredTemperature += 0.25;
	
    uint16_t canWait = 0;
      
    canWait = UA_Server_run_iterate(server, 0);
    // UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "canWait: %i", canWait);

    uint64_t beforeZmqRecv = millis();
    double oldMeasuredTemperature = measuredTemperature;

    /* we know we will receive a double, write it directly into the shared variable */
    int rcLen = zmq_recv(zmqSubscriber, &measuredTemperature, 
			 sizeof(double), ZMQ_DONTWAIT);
    if (rcLen == -1) {
      if (errno != EAGAIN) {
	UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "something wrong with zmq_recv: %i", errno);
	exit(1);
      }
    } else {
      UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "something received: %i", rcLen);
    }
      
    if (oldMeasuredTemperature != measuredTemperature) {
      UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "temperature value changed: %f", measuredTemperature);
    }

    /* measure duration of the zmq handling */
    uint64_t durationZmqRecv = millis() - beforeZmqRecv;
    canWait = (canWait > durationZmqRecv) ? (canWait - durationZmqRecv) : 0;
    UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "canWait: %i", canWait);


    /* cooperative wait */  
    struct timeval timeout = { .tv_sec = 0, .tv_usec = canWait * 1000 };
    select(0, 0, 0, 0, &timeout);
  }

    
  /* ctrl-c received -> clean up */
  UA_Server_delete(server);
  nl.deleteMembers(&nl);

  zmq_close(zmqSubscriber);
  zmq_ctx_destroy(zmqContext);
  // return (int)retval;
}