/*
 * ur_realtime_communication.cpp
 *
 * Copyright 2015 Thomas Timm Andersen
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ur_modern_driver/ur_realtime_communication.h"

UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
                                                 unsigned int safety_count_max)
{
  robot_state_ = new RobotStateRT(msg_cond);
  bzero((char*)&serv_addr_, sizeof(serv_addr_));
  sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
  if (sockfd_ < 0)
  {
    print_fatal("ERROR opening socket");
  }
  server_ = gethostbyname(host.c_str());
  if (server_ == NULL)
  {
    print_fatal("ERROR, no such host");
  }
  serv_addr_.sin_family = AF_INET;
  bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
  serv_addr_.sin_port = htons(30003);
  flag_ = 1;
  setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
  setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
  setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
  fcntl(sockfd_, F_SETFL, O_NONBLOCK);
  connected_ = false;
  keepalive_ = false;
  safety_count_ = safety_count_max + 1;
  safety_count_max_ = safety_count_max;
}

bool UrRealtimeCommunication::start()
{
  fd_set writefds;
  struct timeval timeout;

  keepalive_ = true;
  print_debug("Realtime port: Connecting...");

  connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
  FD_ZERO(&writefds);
  FD_SET(sockfd_, &writefds);
  timeout.tv_sec = 10;
  timeout.tv_usec = 0;
  select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
  unsigned int flag_len;
  getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
  if (flag_ < 0)
  {
    print_fatal("Error connecting to RT port 30003");
    return false;
  }
  sockaddr_in name;
  socklen_t namelen = sizeof(name);
  int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
  if (err < 0)
  {
    print_fatal("Could not get local IP");
    close(sockfd_);
    return false;
  }
  char str[18];
  inet_ntop(AF_INET, &name.sin_addr, str, 18);
  local_ip_ = str;
  comThread_ = std::thread(&UrRealtimeCommunication::run, this);
  return true;
}

void UrRealtimeCommunication::halt()
{
  keepalive_ = false;
  comThread_.join();
}

void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
  int bytes_written;
  if (inp.back() != '\n')
  {
    inp.append("\n");
  }
  if (connected_)
    bytes_written = write(sockfd_, inp.c_str(), inp.length());
  else
    print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
}

void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
{
  char cmd[1024];
  if (robot_state_->getVersion() >= 3.1)
  {
    sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc);
  }
  else
  {
    sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc);
  }
  addCommandToQueue((std::string)(cmd));
  if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.)
  {
    // If a joint speed is set, make sure we stop it again after some time if the user doesn't
    safety_count_ = 0;
  }
}

void UrRealtimeCommunication::run()
{
  uint8_t buf[2048];
  int bytes_read;
  bzero(buf, 2048);
  struct timeval timeout;
  fd_set readfds;
  FD_ZERO(&readfds);
  FD_SET(sockfd_, &readfds);
  print_debug("Realtime port: Got connection");
  connected_ = true;
  while (keepalive_)
  {
    while (connected_ && keepalive_)
    {
      timeout.tv_sec = 0;        // do this each loop as selects modifies timeout
      timeout.tv_usec = 500000;  // timeout of 0.5 sec
      select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
      bytes_read = read(sockfd_, buf, 2048);
      if (bytes_read > 0)
      {
        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
        robot_state_->unpack(buf);
        if (safety_count_ == safety_count_max_)
        {
          setSpeed(0., 0., 0., 0., 0., 0.);
        }
        safety_count_ += 1;
      }
      else
      {
        connected_ = false;
        close(sockfd_);
      }
    }
    if (keepalive_)
    {
      // reconnect
      print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
      sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
      if (sockfd_ < 0)
      {
        print_fatal("ERROR opening socket");
      }
      flag_ = 1;
      setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
      setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));

      setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
      fcntl(sockfd_, F_SETFL, O_NONBLOCK);
      while (keepalive_ && !connected_)
      {
        std::this_thread::sleep_for(std::chrono::seconds(10));
        fd_set writefds;

        connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
        FD_ZERO(&writefds);
        FD_SET(sockfd_, &writefds);
        select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
        unsigned int flag_len;
        getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
        if (flag_ < 0)
        {
          print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 "
                      "seconds...");
        }
        else
        {
          connected_ = true;
          print_info("Realtime port: Reconnected");
        }
      }
    }
  }
  setSpeed(0., 0., 0., 0., 0., 0.);
  close(sockfd_);
}

void UrRealtimeCommunication::setSafetyCountMax(uint inp)
{
  safety_count_max_ = inp;
}

std::string UrRealtimeCommunication::getLocalIp()
{
  return local_ip_;
}