//
//  Collaboration client tester
//    Picks first avatar it sees, and sends duplicate messages, with offset
//
// #include <zmq.hpp>
#include "zhelpers.hpp"
#include <string>
#include <iostream>

int view = 0;
const int LPOS = 4;
const int LORIENT = 3;

const int HEARTBEAT_INTERVAL = 1000;
const int LIVE_COUNT = 3;

void testMsg(zmq::socket_t &socket, std::string &session, std::string &ID)
{
  int device = 0;
  double pos[LPOS] = { 0.0, 1.33, 2.5, 1.0 };
  double orient[LORIENT] = { 0.0, 30.3, 0.5 };

  // test position message, HMD
  if (within(1000) < 10)
  {
    s_sendmore(socket, "PMVZ");
    s_sendmore(socket, ID);
    s_sendmore(socket, session);
    s_sendmore(socket, "A");

    zmq::message_t devMsg (sizeof(device));
    memcpy (devMsg.data(), &device, sizeof(device));
    socket.send(devMsg, ZMQ_SNDMORE);

    zmq::message_t request (sizeof(pos));
    memcpy (request.data(), &pos[0], sizeof(pos));
    // std::cout << "Sending position " << request_nbr << "..." << std::endl;
    socket.send(request, ZMQ_SNDMORE);

    zmq::message_t request2 (sizeof(orient));
    memcpy (request2.data(), &orient[0], sizeof(orient));
    socket.send(request2);
  }
  // test viewchange message, 0 - 9
  if (within(10000) < 5)
  {
    s_sendmore(socket, "PMVZ");
    s_sendmore(socket, ID);
    s_sendmore(socket, session);
    s_sendmore(socket, "V");
    zmq::message_t devMsg (sizeof(view));
    memcpy (devMsg.data(), &view, sizeof(view));
    socket.send(devMsg);
    view = (view + 1) % 10;
  }
}

int main (int argc, char *argv[])
{
  std::string session = "PMVZ";
  //  Prepare our context and socket
  zmq::context_t context (1);
  zmq::socket_t socket (context, ZMQ_DEALER);

  // First arg is the server hostname
  // std::cout << "args: " << argc << " " << argv[0] << std::endl;
  std::stringstream ss;
  ss << "tcp://" << (argc >= 2 ? argv[1] : "localhost") << ":5555";

  std::cout << "Connecting to avatar server..." << std::endl;
  std::string socketEndpoint = ss.str();
  socket.connect (socketEndpoint);
  ss.str("");
  ss.clear();

  zmq::socket_t subscriber(context, ZMQ_SUB);
  ss << "tcp://" << (argc >= 2 ? argv[1] : "localhost") << ":5556";
  std::string subscriberEndpoint = ss.str();
  subscriber.connect(subscriberEndpoint);
  // Second arg is the session.
  if (argc >= 3) session = argv[2];
  std::cout << "Session " << session << std::endl;
  subscriber.setsockopt(ZMQ_SUBSCRIBE, session.c_str(), session.length());

  // flag for test messages with random timing.
  bool doTestMsg = argc >= 4;

  s_send(socket, "HelloPMVZ");
  std::string ID; // = s_recv(socket);
  // std::cout << "Received ID " << ID << std::endl;
  std::string mirrorID;

  //  Initialize poll set
  zmq::pollitem_t items [] =
  {
    { socket, 0, ZMQ_POLLIN, 0 },
    { subscriber, 0, ZMQ_POLLIN, 0 }
  };

  int request_nbr = 0;
  int64_t last_time = s_clock();
  int64_t needHeartbeat = last_time + HEARTBEAT_INTERVAL;
  int64_t needReply = last_time + HEARTBEAT_INTERVAL * LIVE_COUNT;
  int retryCount = 1;
  int64_t AvatarUpdateTime = 0;

  while (1) {
    bool rc = true;
    double updatePos[4] = { 0 };
    double updateOrient[3] = { 0 };
    int device = -1;

    // timeout is 0, return immediately.
    zmq::poll(&items[0], 2, 0);

    int64_t currTime = s_clock();
    if (items[0].revents & ZMQ_POLLIN)
    {
      // reply from socket message - client ID, or error.
      std::string reply = s_recv(socket);
      if (reply == "ERROR")
      {
        std::cout << "Error return " << ID << std::endl;
      }
      else if (reply == "pong")
      {
        // update server alive time, below.
        std::cout << "pong " << ID << std::endl;
      }
      else
      {
        ID = reply;
        std::cout << "Received ID " << ID << std::endl;
        retryCount = 0;
        // ideally send "J" join message here, but pub-sub not ready yet.
      }
    }
    if (items [1].revents & ZMQ_POLLIN)
    {
      zmq::message_t update;
      if ((rc = subscriber.recv(&update)) == true)
      {
        // the update msg contains the session for the subscription
        //  process other avatar updates
        std::string otherID = s_recv(subscriber);
        std::string type = s_recv(subscriber);
        if (type == "A") {
          subscriber.recv(&update);
          memcpy (&device, update.data(), sizeof(device));
          subscriber.recv(&update);
          memcpy (&updatePos[0], update.data(), sizeof(updatePos));
          subscriber.recv(&update);
          memcpy (&updateOrient[0], update.data(), sizeof(updateOrient));
          // printouts kill responsiveness. Try 1/sec.
          if (currTime - last_time > 1000)
          {
            last_time = currTime;
            std::cout << "Recvd: [" << otherID << ", " <<
              static_cast<int>(device) << "] " <<
              updatePos[0] << ", " <<
              updatePos[1] << ", " <<
              updatePos[2] << " " <<
              std::endl;
              std::cout << "      orient: " <<
              updateOrient[0] << ", " <<
              updateOrient[1] << ", " <<
              updateOrient[2] << " " <<
              std::endl;
          }
          ++request_nbr;

          // identify a mirror, first different ID message we see.
          if (mirrorID.empty() && otherID != ID)
          {
            std::cout << "Sending J time: " << currTime - AvatarUpdateTime << std::endl;
            AvatarUpdateTime = currTime;
            mirrorID = otherID;
            // send join message, to trigger view setup.
            s_sendmore(socket, "PMVZ");
            s_sendmore(socket, ID);
            s_sendmore(socket, session);
            s_sendmore(socket, "J");
            s_send(socket, ID);
          }
          // do we send a mirror update?
          if (otherID == mirrorID)
          {
            updatePos[0] += 1.0;
            updatePos[1] += 0.5;
            s_sendmore(socket, "PMVZ");
            s_sendmore(socket, ID);
            s_sendmore(socket, session);
            s_sendmore(socket, "A");

            zmq::message_t devMsg (sizeof(device));
            memcpy (devMsg.data(), &device, sizeof(device));
            socket.send(devMsg, ZMQ_SNDMORE);

            zmq::message_t request (sizeof(updatePos));
            memcpy (request.data(), &updatePos[0], sizeof(updatePos));
            // std::cout << "Sending position " << request_nbr << "..." << std::endl;
            socket.send(request, ZMQ_SNDMORE);

            zmq::message_t request2 (sizeof(updateOrient));
            memcpy (request2.data(), &updateOrient[0], sizeof(updateOrient));
            socket.send(request2);

            //  Get the reply.
            // std::string ack = s_recv(socket);
            if (device == 0)
            {
              AvatarUpdateTime = currTime;
            }
          }
        }
        else if (type == "P")
        {
          double updateTrans[3] = { 0 };
          double updateDir[3] = {0};
          subscriber.recv(&update);
          memcpy (&device, update.data(), sizeof(device));
          subscriber.recv(&update);
          memcpy (&updateTrans[0], update.data(), sizeof(updateTrans));
          subscriber.recv(&update);
          memcpy (&updateDir, update.data(), sizeof(updateDir));
          std::cout << "Recvd: " << otherID << ", evt: " <<
            type << ", index: " <<
            static_cast<int>(device) << std::endl;
          std::cout << "      trans: " <<
            updateTrans[0] << ", " <<
            updateTrans[1] << ", " <<
            updateTrans[2] << " " <<
            " rotation: " << updateDir[0] << ", " <<
            updateDir[1] << ", " <<
            updateDir[2] << " " <<
            std::endl;
        }
        else if (type == "V" || type == "J")
        {
          std::string extra = s_recv(subscriber);

          // "V" and "J" messages just contain a string, receive and print.
          std::cout << "Recvd: " << otherID << ", evt: " <<
            type << ", extra: " << extra << " " << std::endl;

          if (type == "J" && !mirrorID.empty() && currTime - AvatarUpdateTime < 9800) {
            // test view-list disable
            // s_sendmore(socket, "PMVZ");
            // s_sendmore(socket, ID);
            // s_sendmore(socket, session);
            // s_sendmore(socket, "VL");
            // s_sendmore(socket, "KCGM-view");
            // s_sendmore(socket, "Long2019");
            // s_send(socket, "");
            // test name
            s_sendmore(socket, "PMVZ");
            s_sendmore(socket, ID);
            s_sendmore(socket, session);
            s_sendmore(socket, "N");
            s_send(socket, "Mirror " + mirrorID );
          }
        }
        else
        {
          std::cout << "Recvd: " << otherID << ", evt: " <<
            type << std::endl;
          s_dump(subscriber);
        }
      }
    }
    if (items [0].revents & ZMQ_POLLIN || items [1].revents & ZMQ_POLLIN)
    {
      // got a message, reset heartbeat.
      needHeartbeat = currTime + HEARTBEAT_INTERVAL;
      needReply = currTime + HEARTBEAT_INTERVAL * LIVE_COUNT;
      retryCount = 0;
    }
    else if (currTime > needHeartbeat && !ID.empty())
    {
      // heartbeat only if we have an ID.
      // send ping, expect pong
      std::cout << "sent ping" << std::endl;
      if (retryCount == 0) retryCount = 1;
      s_sendmore(socket, "ping");
      s_send(socket, ID);
      needHeartbeat = currTime + HEARTBEAT_INTERVAL;
      mirrorID = "";
      s_sleep(10);
    }
    // if heartbeat fails multiple times
    if (currTime > needReply)
    {
      if (retryCount > LIVE_COUNT)
      {
        needReply = currTime + HEARTBEAT_INTERVAL * LIVE_COUNT * retryCount;
        std::cout << "Server disconnected, waiting. " << std::endl;
      }
      else
      {
        std::cout << "Heartbeat fail, retry " << retryCount << std::endl;
        ++retryCount;
        needHeartbeat = currTime + HEARTBEAT_INTERVAL;
        needReply = currTime + HEARTBEAT_INTERVAL * LIVE_COUNT * retryCount;
        // disconnect and reconnect sockets, clear ID
        socket.close();
        subscriber.close();
        socket = zmq::socket_t(context, ZMQ_DEALER);
        socket.connect (socketEndpoint);
        // once we close, we want the socket to close immediately, and drop messages.
        int linger = 0;
        socket.setsockopt(ZMQ_LINGER, &linger, sizeof (linger));
        subscriber = zmq::socket_t(context, ZMQ_SUB);
        subscriber.connect(subscriberEndpoint);
        subscriber.setsockopt(ZMQ_SUBSCRIBE, session.c_str(), session.length());
        items[0].socket = socket;
        items[1].socket = subscriber;

        ID.clear();
        mirrorID.clear();
        s_send(socket, "HelloPMVZ");
      }
    }
    if (!ID.empty() && doTestMsg)
    {
      testMsg(socket, session, ID);
    }

    s_sleep(doTestMsg || retryCount ? 1 : 0);
  }
  return 0;
}
