/hwcomponents/caros_kuka_robot/src/kuka_tcp_client.cpp

#include <caros/kuka_tcp_client.h>
#include <string>

tcp_client::tcp_client(boost::asio::io_service& io_service, boost::asio::ip::tcp::resolver::iterator endpoint_iterator,
                       input_handler function, void* def_state)
    : io_service_(io_service),
      socket_(io_service),
      body_length_(0),
      running_(true),
      terminate_connection_string("bye"),
      input_handle(function),
      v_state(def_state)
{
  do_connect(endpoint_iterator);
}
tcp_client::~tcp_client()
{
}

void tcp_client::set_terminate_connection_string(std::string new_msg)
{
  terminate_connection_string = new_msg;
}

void tcp_client::write(const std::string& msg)
{
  if (msg != "hello")
  {
    std::cerr << "Writing: " << msg << std::endl;
  }
  io_service_.post([this, msg]()
    {
        bool write_in_progress = !write_msgs_.empty();
        write_msgs_.push_back(msg + "\n");
        if (!write_in_progress)
        {
            do_write();
        }
    }
    /*end of code*/);
}

void tcp_client::close()
{
  std::cerr << "client is closed" << std::endl;
  io_service_.post([this]()
  {
    running_ = false;
    socket_.close();
  }
  /*end of code */);
}

void tcp_client::do_connect(boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
{
  boost::asio::async_connect(socket_, endpoint_iterator,
                             [this](boost::system::error_code err, boost::asio::ip::tcp::resolver::iterator)
                             {
                               if (!err)
                               {
                                 do_read_header();
                               }
                               else
                               {
                                 running_ = false;
                               }
                               std::cout << "[SOCKET CLIENT] connected to server" << std::endl;
                             }
                             /*end of code*/);
}

void tcp_client::do_read_header()
{
  boost::asio::async_read(socket_, boost::asio::buffer(msg_len, tcp_client::header_length + 1),
                          [this](boost::system::error_code ec, std::size_t /*length*/)
                          {
                            if (!ec)
                            {
                              // body_length_ =
                              // atoi(std::string(msg_len,tcp_client::header_length-1).c_str());
                              body_length_ = atoi(msg_len);
                              if (body_length_ > 0 && body_length_ < tcp_client::max_msg_length - 2)
                              {
                                do_read_body();
                              }
                              else
                              {
                                std::cerr << "[SOCKET CLIENT] malformed header" << std::endl;
                                // do_read_header();
                              }
                            }
                            else
                            {
                              std::cerr << "[SOCKET CLIENT] error getting header, closing" << std::endl;
                              close();
                            }
                          }
                          /*end of code*/);
}
void tcp_client::do_read_body()
{
  boost::asio::async_read(socket_, boost::asio::buffer(msg_txt, body_length_ + 2),  // windows uses 2 chars for endl
                          [this](boost::system::error_code ec, std::size_t /*length*/)
                          {
                            if (!ec)
                            {
                              read_msg_ = std::string(msg_txt, body_length_);
                              if (read_msg_.find(terminate_connection_string) != std::string::npos)
                              {
                                std::cout << "\n[SOCKET CLIENT] closing server" << std::endl;
                                close();
                              }
                              else
                              {
                                if (v_state != nullptr)
                                {
                                  input_handle(read_msg_, v_state);
                                }
                                do_read_header();
                              }
                            }
                            else
                            {
                              std::cerr << "[SOCKET CLIENT] error getting body, closing" << std::endl;
                              close();
                            }
                          }
                          /*end oc code*/);
}

void tcp_client::do_write()
{
  boost::asio::async_write(socket_, boost::asio::buffer(write_msgs_.front().c_str(), write_msgs_.front().length()),
                           [this](boost::system::error_code err, std::size_t /*length*/)
                           {
                             if (err)
                             {
                               std::cerr << "[SOCKET CLIENT] Writing failed\n";
                               close();
                             }
                             else
                             {
                               write_msgs_.pop_front();
                               if (!write_msgs_.empty())
                               {
                                 do_write();
                               }
                             }
                           }
                           /*end of code*/);
}

void default_set_state(std::string, void*)
{
  std::cout << "calling the wrong function" << std::endl;
}