/*
 * Copyright (C) 2019 AIRC 01
 *
 * 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.
 *
*/
/* Desc: A TCP communication to send and receive Pose informations of robot entities
 * Author: Zhang Shuai & Zeng Lei
 */

#ifndef _TCPCOMMUNICATION_HH_
#define _TCPCOMMUNICATION_HH_

#include <string>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
// #include <fcntl.h>
#include <unistd.h>

// #include <vector>

// #include <sdf/sdf.hh>

// #include "gazebo/physics/World.hh"
#include "gazebo/util/system.hh"
#include "gazebo/math/Pose.hh"

#define QUEUE 1
#define BUF_SIZE 8192

namespace gazebo
{
namespace physics
{

/// \brief Structure of Pose information communicated with Tcp
struct CommunicationData
{
  uint32_t ID;
  char link_name[16];
  math::Pose link_pose;
};

/// \addtogroup gazebo_physics
/// \{

/// \class TcpServer TcpCommunication.hh physics/physics.hh
/// \brief Send and receive Pose informations of robot entities
///
class GZ_PHYSICS_VISIBLE TcpServer
{
  /// \brief Constructor.
public:
  TcpServer();

  /// \brief Destructor.
public:
  ~TcpServer();

  /// \brief Start Tcp server waiting for the request of client.
public:
  void StartServer();

  /// \brief Send the iterations of world.
public:
  bool sendData(uint64_t nStep);

  /// \brief Receive the iterations of world.
public:
  bool recvData(uint64_t &nStep);

  /// \brief Send Pose informations of robot entities.
public:
  bool sendInfor(CommunicationData *_infor, unsigned int _simula_entity_num);

  /// \brief Receive Pose informations of robot entities.
public:
  bool recvInfor(CommunicationData *_infor, unsigned int _shadow_entity_num);

  /// \brief Set port of network.
public:
  void SetPort(int _port);

  /// \brief Close Tcp server.
public:
  void Close();

  /// \brief Socket of server.
private:
  int server_fd;

  /// \brief Socket of client.
private:
  int client_fd;

  /// \brief Port of network.
private:
  int port;

  /// \brief Structure of Server Network Address.
private:
  struct sockaddr_in server_sockaddr;

  /// \brief Structure of Client Network Address.
private:
  struct sockaddr_in client_sockaddr;

  /// \brief Buffer for data transfer.
private:
  char buff[BUF_SIZE];
};

/// \class TcpClient TcpCommunication.hh physics/physics.hh
/// \brief Send and receive Pose informations of robot entities
///
class GZ_PHYSICS_VISIBLE TcpClient
{
  /// \brief Constructor.
public:
  TcpClient();

  /// \brief Destructor.
public:
  ~TcpClient();

  /// \brief Connect to Tcp server.
public:
  void ConnectServer();

  /// \brief Send the iterations of world.
public:
  bool sendData(uint64_t nStep);

  /// \brief Receive the iterations of world.
public:
  bool recvData(uint64_t &nStep);

  /// \brief Send Pose informations of robot entities.
public:
  bool sendInfor(CommunicationData *_infor, unsigned int _simula_entity_num);

  /// \brief Receive Pose informations of robot entities.
public:
  bool recvInfor(CommunicationData *_infor, unsigned int _shadow_entity_num);

  /// \brief Set IP of network.
public:
  void SetIp(std::string _ip);

  /// \brief Set port of network.
public:
  void SetPort(int _port);

  /// \brief Close Tcp server.
public:
  void Close();

  /// \brief Socket of connection.
private:
  int conn_fd;

  /// \brief IP of network.
private:
  std::string ip;

  /// \brief Port of network.
private:
  int port;

  /// \brief Structure of Server Network Address.
private:
  struct sockaddr_in server_sockaddr;

  /// \brief Buffer for data transfer.
private:
  char buff[BUF_SIZE];
};
/// \}
} // namespace physics
} // namespace gazebo
#endif