#include <string>
#include <iostream>

#include <ros/ros.h>
#include <ros/node_handle.h>
#include <json_prolog/prolog.h>

using namespace std;
using namespace json_prolog;

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "main");

  Prolog pl;

  //////////////////////////////////////////////////////////////////////////////
  // Get Percepts from Table 1 - happens in another thread
  // TODO: Where?
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Insert to Knowrob - happens in another thread
  // TODO: Where?
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Query knowrob
  //////////////////////////////////////////////////////////////////////////////
  string q = "comp_misingobj:comp_missingObjectTypes(PerceivedObj, MissingObj, MissingTypes)";
  
  PrologQueryProxy bdgs = pl.query(q);
  for(PrologQueryProxy::iterator it=bdgs.begin(); it != bdgs.end(); it++)
  {
    PrologBindings bdg = *it;
    ROS_INFO_STREAM("PerceivedObj = "<< bdg["PerceivedObj"]);
    ROS_INFO_STREAM("MissingObj = "<< bdg["MissingObj"]);
    ROS_INFO_STREAM("MissingTypes = "<< bdg["MissingTypes"]);
  }

  //////////////////////////////////////////////////////////////////////////////
  //Navigate to Table 2
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Get Percepts from Table 2 - happens in another thread
  // TODO: Where?
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Fetch Object
  // TODO: How? Service, Actionlib?
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Navigate to Table 1
  //////////////////////////////////////////////////////////////////////////////
  
  //////////////////////////////////////////////////////////////////////////////
  // Find empty surface
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Put down the objects
  //////////////////////////////////////////////////////////////////////////////
  return 0;
}