#include "nodeTemplate.h"

nodeTemplate::nodeTemplate(wxTextCtrl *TextBox)// ros::Node("nodeTemplate")
{
   m_pTextBox = TextBox;
   initNode();
   m_pNode = new ros::Node("templateNode");

      //put node-specific subscriptions, advertising, and services here

   m_SubscribeMonitorNodeBus = _T("monitor_node_bus");
   m_pNode->subscribe(std::string(m_SubscribeMonitorNodeBus.mb_str()),monitorNodeBus, &nodeTemplate::monitorNodeBusCallback, this, 100);

   m_AdvertiseNodeBusOut = _T("node_bus_out");
   m_pNode->advertise<wxWidgetsNodeTemplate::ROS_Node_Template>(std::string(m_AdvertiseNodeBusOut.mb_str()), 100);

   m_AdvertiseServiceDataBus = _T("node_service_bus");
   m_pNode->advertiseService("node_service_bus", &nodeTemplate::serviceTemplateCallback, this);

   m_pTextBox->AppendText(_T("node_template is running."));
}

nodeTemplate::~nodeTemplate()
{
   m_pNode->shutdown(); // exit cleanly from the master
   delete m_pNode;
}

void nodeTemplate::initNode()
{
 	char fileName[20] = "ROS_Node_Template";
 	char *pArgumentList[4];
	pArgumentList[0] = fileName;	//argv[0] is the filename.
	pArgumentList[1] = NULL;	// argv[argc] is a null pointer.
	int i = 1;
	ros::init(i,pArgumentList);	// this is typcially called from within a main(int argc, char **argv) routine, so we have to send the correct arguments
                                 // to do: pick up the remappings from the command line and use them here


   // this is an alternative method of calling ros::init, but it does not seem to work
   //ros::VP_string test; // compiler says VP_string is not a member of ros, but documentation says it is the following typedef
   //std::vector<std::pair<std::string, std::string> > test;
	//test[0].first = "nodeTemplate";
	//test[0].second = "nodeTemplate";
	//ros::init(test);   // init refuses this datatype
}


void nodeTemplate::Publish()
{
   wxString tempString = _T("\nPublished on ");
   tempString << m_AdvertiseNodeBusOut << _T("\n");
   m_pTextBox->AppendText(tempString);

   wxString messageOut;
   uint8_t Value8;
   uint16_t Value16 ;
   uint32_t Value32 ;
   uint64_t Value64 ;

   getValuesNodeBusOut( &messageOut, &Value8, &Value16, &Value32, &Value64);

   nodeBusOut.Test_msg = std::string(messageOut.mb_str());
   nodeBusOut.Test_uint8 = Value8;
   nodeBusOut.Test_uint16 = Value16;
   nodeBusOut.Test_uint32 = Value32;
   nodeBusOut.Test_uint64 = Value64;

   m_pNode->publish(std::string(m_AdvertiseNodeBusOut.mb_str()), nodeBusOut);

}

void nodeTemplate::monitorNodeBusCallback()
{
   wxString messageOrigin = _T("\n Received on ");
   messageOrigin << m_SubscribeMonitorNodeBus;
   messageOrigin << _T(": \n DataType = ");
   std::string stlstring = monitorNodeBus.__s_getDataType();
   wxString dataType(stlstring.c_str(), wxConvUTF8); // where the message is from-- we could use this to identify how to decode it, but it is easier just
                           // to set up separate callbacks for each subscription.  This is really useful, however if we want to set the subscription up at runtime
   messageOrigin<< dataType;


   stlstring = monitorNodeBus.Test_msg;
   wxString messageIn(stlstring.c_str(), wxConvUTF8);
   uint8_t Value8 = monitorNodeBus.Test_uint8;
   int16_t Value16 = monitorNodeBus.Test_uint16;
   int32_t Value32 = monitorNodeBus.Test_uint32;
   int64_t Value64 = monitorNodeBus.Test_uint64;

   setValuesMonitorNodeBus(messageIn, Value8, Value16, Value32, Value64);
   messageOrigin << _T("\n Values = ") << m_Value8 <<  _T(", ") << m_Value16 << _T(", ") << m_Value32 <<  _T(", ") << m_Value64 << _T("\n");
   m_pTextBox->AppendText(messageOrigin);
}


bool nodeTemplate::serviceTemplateCallback(serverNode::nodeTemplateServer::Request  &req,
           serverNode::nodeTemplateServer::Response &res )
  {
    wxString tempString = _T("\nService called: \n Message is ");
    int8_t test8;
    int16_t test16;
    test8 = req.testIn8;
    test16 = req.testIn16;
    std::string stlstring = req.testMessageIn;
    wxString Message(stlstring.c_str(), wxConvUTF8);

    stlstring = "Success";
    res.testOut8 = 8;
    res.testOut32 = 32;
    res.testMessageOut = stlstring;

    tempString << Message;
    m_pTextBox->AppendText(tempString);

    return true;
  }

// node specific routines
void nodeTemplate::getValuesNodeBusOut( wxString *pMessageOut, uint8_t *pValue8, uint16_t *pValue16, uint32_t *pValue32, uint64_t *pValue64)
{

   *pMessageOut = _T("\n Hello World");
   *pValue8 = 2;
   *pValue16 = 3;
   *pValue32 = 5;
   *pValue64 = 7;
}


void nodeTemplate::setValuesMonitorNodeBus( wxString messageIn, uint8_t Value8, uint16_t Value16, uint32_t Value32, uint64_t Value64)
{

   m_MessageIn = messageIn;
   m_Value8 = Value8;
   m_Value16 = Value16;
   m_Value32 = Value32;
   m_Value64 = Value64;
}