/*
 *  Created on: Apr 16, 2018
 *      Author: Poom Pianpak
 */

#include "main.h"
#include "snake.h"

#include <future>

RosbridgeWsClient rbc("localhost:9090");

void advertiseServiceCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message)
{
  // message->string() is destructive, so we have to buffer it first
  std::string messagebuf = in_message->string();
  std::cout << "advertiseServiceCallback(): Message Received: " << messagebuf << std::endl;

  rapidjson::Document document;
  if (document.Parse(messagebuf.c_str()).HasParseError())
  {
    std::cerr << "advertiseServiceCallback(): Error in parsing service request message: " << messagebuf << std::endl;
    return;
  }

  rapidjson::Document values(rapidjson::kObjectType);
  rapidjson::Document::AllocatorType& allocator = values.GetAllocator();
  values.AddMember("success", document["args"]["data"].GetBool(), allocator);
  values.AddMember("message", "from advertiseServiceCallback", allocator);

  rbc.serviceResponse(document["service"].GetString(), document["id"].GetString(), true, values);
}

void callServiceCallback(std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)
{
  std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl;
  connection->send_close(1000);
}

void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& futureObj)
{
  rbc.addClient("topic_publisher");

  rosbridge_msgs::Header header(0, rosbridge_msgs::Time(1,2), "/map");
  rosbridge_msgs::Polygon polygon(std::vector<rosbridge_msgs::Point32>({rosbridge_msgs::Point32(1,1,1),
                                                                        rosbridge_msgs::Point32(2,2,2),
                                                                        rosbridge_msgs::Point32(3,3,3)}));
  rosbridge_msgs::PolygonStamped polygonStamped(header, polygon);
  rapidjson::Document doc;
  rapidjson::Value val = polygonStamped.toJson(doc.GetAllocator());
  val.Swap(doc);

  // Write to stdout
  rapidjson::OStreamWrapper out(std::cout);
  // Write document...
  rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
  doc.Accept(writer);
  std::cout << std::endl;

  while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout)
  {
    rbc.publish("/polygon_stamped_topic", doc);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }

  std::cout << "publisherThread stops()" << std::endl;
}

void subscriberCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message)
{
  std::cout << "subscriberCallback(): Message Received: " << in_message->string() << std::endl;
}

int main() {
//  rbc.addClient("service_advertiser");
//  rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback);

  rbc.addClient("topic_advertiser");
  rbc.advertise("topic_advertiser", "/polygon_stamped_topic", "geometry_msgs/PolygonStamped");

//  rbc.addClient("topic_subscriber");
//  rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback);

  // Test calling a service
//  rapidjson::Document document(rapidjson::kObjectType);
//  document.AddMember("data", true, document.GetAllocator());
//  rbc.callService("/zservice", callServiceCallback, document);

  // Test creating and stopping a publisher
  {
    // Create a std::promise object
    std::promise<void> exitSignal;

    // Fetch std::future object associated with promise
    std::future<void> futureObj = exitSignal.get_future();

    // Starting Thread & move the future object in lambda function by reference
    std::thread th(&publisherThread, std::ref(rbc), std::cref(futureObj));

    // Wait for 10 sec
    std::this_thread::sleep_for(std::chrono::seconds(10));

    std::cout << "Asking publisherThread to Stop" << std::endl;

    // Set the value in promise
    exitSignal.set_value();

    // Wait for thread to join
    th.join();
  }

  // Test removing clients
//  rbc.removeClient("service_advertiser");
  rbc.removeClient("topic_advertiser");
//  rbc.removeClient("topic_subscriber");

  std::cout << "Program terminated" << std::endl;
}