
#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>

#include "groupCommand.hpp"
#include "groupFeedback.hpp"
#include "lookup.hpp"

using namespace Pnd;

// for test
#ifdef WIN32
#include <WS2tcpip.h>
#include <winsock2.h>
#pragma comment(lib, "ws2_32.lib")
#define DllExport __declspec(dllexport)
#else
#include <arpa/inet.h>
#include <sys/time.h>
#include <unistd.h>
#endif

#include <time.h>

const timeval intervalTimeStart() {
  timeval tv_last;
  gettimeofday(&tv_last, NULL);
  return tv_last;
}

double intervalTimeEnd(const timeval &tv_last, const float &frequency) {
  float interval_time;
  if (frequency == 0) {
    interval_time = 0;
  } else {
    interval_time = 1.0 / frequency;
  }
  long double delay_time = interval_time * 1000000;
  long double time = 0;
  timeval now_time;
  while (true) {
    gettimeofday(&now_time, NULL);
    time = ((now_time.tv_sec - tv_last.tv_sec) * 1000000 +
            (now_time.tv_usec - tv_last.tv_usec));
    if (time >= delay_time) {
      break;
    }
    std::this_thread::sleep_for(std::chrono::microseconds(1));
  }
  return time / 1000;
}

#define StartTimeChrono(funName)                                \
  std::chrono::microseconds ms##funName =                       \
      std::chrono::duration_cast<std::chrono::microseconds>(    \
          std::chrono::system_clock::now().time_since_epoch()); \
  long start##funName = ms##funName.count();

#define EndTimeChrono(funName)                                                \
  ms##funName = std::chrono::duration_cast<std::chrono::microseconds>(        \
      std::chrono::system_clock::now().time_since_epoch());                   \
  long end##funName = ms##funName.count();                                    \
  double ms_time = (end##funName - start##funName) / 1000.0;                  \
  std::cout << "The function " << #funName << " runs for " << ms_time << "ms" \
            << std::endl;

int main() {
  std::string str("10.10.10.255");
  // After construction,start the background thread lookup actuator
  Lookup lookup(&str);
  // Wait 1 seconds for the module list to populate, and then print out its
  // contents
  std::this_thread::sleep_for(std::chrono::seconds(1));
  lookup.setLookupFrequencyHz(0);

  auto group = lookup.getGroupFromFamily("Default", 200);

  if (!group) {
    std::cout << "Group not found! Check that the family and name of a module "
                 "on the network"
              << std::endl
              << "matches what is given in the source file." << std::endl;
    return -1;
  }

  std::cout << group->size() << std::endl;
  GroupCommand group_command(group->size());

  GroupFeedback group_feedback(group->size());

  group_command.resetLinearCount(std::vector<float>(group->size(), 0));
  group->sendCommand(group_command);

  group_command.enable(std::vector<float>(group->size(), 1));
  group->sendCommand(group_command);

  std::vector<PosPtInfo> pos_pt_infos, pre_pos_infos;
  PosPtInfo pos;
  int timeout_num = 0;
  int next_feedback_timeout = 0;

  for (int j = 0; j < 1000; ++j) {
    StartTimeChrono(all);
    auto t = intervalTimeStart();
    // StartTimeChrono(feedback);
    auto ret = group->getNextFeedback(group_feedback, 10);
    // EndTimeChrono(feedback);
    if (ret) {
      for (int i = 0; i < group_feedback.size(); ++i) {
        if (std::isnan(group_feedback[i]->position)) {
          timeout_num++;
          pos.pos = pre_pos_infos.at(i).pos;
          // if (timeout_num > 3) return 0;
        } else {
          std::cout << group_feedback[i]->position << " ";
          pos.pos = group_feedback[i]->position + 0.1;
          // pos.pos += 0.01;
        }
        pos.vel = 0.0;
        pos.torque = 0.0;
        pos_pt_infos.push_back(pos);
      }
      pre_pos_infos = pos_pt_infos;
      std::cout << std::endl;
      StartTimeChrono(command);
      group_command.setInputPositionPt(pos_pt_infos);
      group->sendCommand(group_command);
      EndTimeChrono(command);
      pos_pt_infos.clear();
    } else {
      next_feedback_timeout++;
    }
    intervalTimeEnd(t, 400);
    EndTimeChrono(all);
  }

  group_command.enable(std::vector<float>(group->size(), 0));
  group->sendCommand(group_command);
  std::cout << "end!!! " << next_feedback_timeout << std::endl;

  return 0;
}
