/************************************************************/
/*    NAME: Xiaobin Zeng                                              */
/*    ORGN: MIT                                             */
/*    FILE: ClientViewer.cpp                                        */
/*    DATE:                                                 */
/************************************************************/

#include <iterator>
#include "ClientViewer.h"
//#include "Behavior.pb.h"
#include <json/json.h>

using namespace std;

#define UDP_RECEIVE_PORT 6001
#define TCP_SEND_PORT 8000
#define TCP_FILE_RECEIVE_PORT 8001
//#define TCP_SERVER_ADDRESS "10.25.0.230" //树莓派
#define TCP_SERVER_ADDRESS  "127.0.0.1"
// #define TCP_SERVER_ADDRESS  "10.25.0.163"
// #define TCP_SERVER_ADDRESS  "10.25.0.160"

//---------------------------------------------------------
// Constructor

ClientViewer::ClientViewer()
{
  udpReceiveBuffer = new uint8_t[65535];
  tcpReceiveBuffer = new uint8_t[65535];
}

//---------------------------------------------------------
// Destructor

ClientViewer::~ClientViewer()
{
  // udpCommEvent.Stop();
  // tcpCommEvent.Stop();  
  delete udpReceiveBuffer;
  delete tcpReceiveBuffer;

  // if (sock_tcp_send)
  // {
  //   delete sock_tcp_send;
  //   sock_tcp_send = NULL;
  // }

  // if (sock_udp_receive)
  // {
  //   delete sock_udp_receive;
  //   sock_udp_receive = NULL;
  // }
  
}

//---------------------------------------------------------
// Procedure: OnNewMail

bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
{
  MOOSMSG_LIST::iterator p;
   
  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
  {
    CMOOSMsg &msg = *p;
    string key   = msg.GetKey();
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
    string sval  = msg.GetString(); 
    string msrc  = msg.GetSource();
    double mtime = msg.GetTime();
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();
    std::cout << key << " : " << sval << std::endl;
    if(key == "Command")
    {
      if (sval == "SetPlan1")     //PlanDB
      {
        std::string systemName = "CCU Neptus 0_163";
        std::string plan_1_Spec = SetPlan1(systemName, MOOS::Time());
        DUNE::IMC::PlanDB msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
        msg.plan_id.assign("BHV_Waypoint");
        msg.info = plan_1_Spec;
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);

      }
      if (sval == "SetPlan2")  //PlanDB
      {
        std::string systemName = "CCU Neptus 0_163";
        std::string plan_2_Spec = SetPlan2(systemName, MOOS::Time());  
        //std::cout << plan_2_Spec << std::endl;
        DUNE::IMC::PlanDB msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
        msg.plan_id.assign("BHV_Waypoint");
        msg.info = plan_2_Spec;
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "ModifyPlan1")  //PlanDB
      {
        std::string systemName = "CCU Neptus 0_163";  
        std::string plan_1_Spec = ModifyPlan1(systemName, MOOS::Time()); 
        DUNE::IMC::PlanDB msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
        msg.plan_id.assign("BHV_Waypoint");
        msg.info = plan_1_Spec;
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "GetPlanList")  //PlanDB
      {
        DUNE::IMC::PlanDB msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
        msg.plan_id.assign("Test");  
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }      
      if (sval == "SetThrust")  //SetEntityParameters
      {
        double timeStamp = MOOS::Time();
        DUNE::IMC::SetEntityParameters msg;
        msg.setTimeStamp(timeStamp);      
        msg.name = "Thrust";
        DUNE::IMC::EntityParameter subMsg;
        subMsg.setTimeStamp(timeStamp);
        SetEntityStatus(subMsg, "Pwm", generateEntityValue("30", "int"));
        msg.params.push_back(subMsg);
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);        
      }
      if (sval == "SetIndicatorLight")  //SetEntityParameters
      {
        double timeStamp = MOOS::Time();
        DUNE::IMC::SetEntityParameters msg;
        msg.setTimeStamp(timeStamp);      
        msg.name = "IndicatorLight";
        DUNE::IMC::EntityParameter subMsg;
        subMsg.setTimeStamp(timeStamp);
        SetEntityStatus(subMsg, "Status", generateEntityValue("1", "int"));
        msg.params.push_back(subMsg);
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "StartPlan1")  //PlanControl
      {
        DUNE::IMC::PlanControl msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
        msg.plan_id = "east_waypt_survey";
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "StartPlan2")  //PlanControl
      {
        DUNE::IMC::PlanControl msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
        msg.plan_id = "west_waypt_survey";
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }      
      if (sval == "StopPlan1")  //PlanControl
      {
        DUNE::IMC::PlanControl msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
        msg.plan_id = "east_waypt_survey";
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "StopPlan2")  //PlanControl
      {
        DUNE::IMC::PlanControl msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
        msg.plan_id = "west_waypt_survey";
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "RemoteActions")  //PlanControl
      {
        DUNE::IMC::RemoteActions msg;
        msg.setTimeStamp();
        Json::Value executeCommand;
        executeCommand["Thrust"] = -100;
        executeCommand["Heading"] = -27.5;
        Json::StreamWriterBuilder builder;
        msg.actions = Json::writeString(builder, executeCommand);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "VehicleCommand")  //PlanControl
      {
        DUNE::IMC::VehicleCommand msg;
        msg.setTimeStamp();
        msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST;
        msg.command = 1;    
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "GetLogBook")  //PlanControl
      {
        DUNE::IMC::LogBookControl msg;
        msg.setTimeStamp();
        msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
        DUNE::IMC::LogBookEntry msgLogBookEntry;
        msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
        msgLogBookEntry.context = "LIST";
        msg.msg.push_back(msgLogBookEntry);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "RetrLogBook1")  //PlanControl
      {
        DUNE::IMC::LogBookControl msg;
        msg.setTimeStamp();
        msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
        DUNE::IMC::LogBookEntry msgLogBookEntry;
        msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
        msgLogBookEntry.context = "RETR";

        Json::Value retrFile;
        retrFile["lauv-150/2023-11-16/103058"]["auv"]["file"] = "auvData.mdat";      

        Json::StreamWriterBuilder builder;
        msgLogBookEntry.text = Json::writeString(builder, retrFile);
        msg.msg.push_back(msgLogBookEntry);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "RetrLogBook2")  //PlanControl
      {
        DUNE::IMC::LogBookControl msg;
        msg.setTimeStamp();
        msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
        DUNE::IMC::LogBookEntry msgLogBookEntry;
        msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
        msgLogBookEntry.context = "RETR";

        Json::Value retrFile;
        retrFile["lauv-150/2023-11-16/103058"]["command"]["file"] = "clientCommand.txt";               

        Json::StreamWriterBuilder builder;
        msgLogBookEntry.text = Json::writeString(builder, retrFile); 
        msg.msg.push_back(msgLogBookEntry);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "RetrLogBook3")  //PlanControl
      {
        DUNE::IMC::LogBookControl msg;
        msg.setTimeStamp();
        msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
        DUNE::IMC::LogBookEntry msgLogBookEntry;
        msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
        msgLogBookEntry.context = "RETR";

        Json::Value retrFile;
        retrFile["lauv-150/2023-11-16/103058"]["mission"]["file"] = "missionHistory.txt";         

        Json::StreamWriterBuilder builder;
        msgLogBookEntry.text = Json::writeString(builder, retrFile); 
        msg.msg.push_back(msgLogBookEntry);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }
      if (sval == "DeleLogBook")  //PlanControl
      {
        DUNE::IMC::LogBookControl msg;
        msg.setTimeStamp();
        msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_CLEAR;
        DUNE::IMC::LogBookEntry msgLogBookEntry;
        msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
        msgLogBookEntry.context = "DELE";

        Json::Value retrFile;
        retrFile["lauv-150/2023-11-16/103058"]["auv"]["file"] = "auvData.mdat";
        retrFile["lauv-150/2023-11-16/103058"]["command"]["file"] = "clientCommand.txt";
        retrFile["lauv-150/2023-11-16/105131"]["mission"]["file"] = "missionHistory.txt";

        Json::StreamWriterBuilder builder;
        msgLogBookEntry.text = Json::writeString(builder, retrFile);
        msg.msg.push_back(msgLogBookEntry);        
        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
      }      
    }    
  }
	
   return(true);
}

//---------------------------------------------------------
// Procedure: OnConnectToServer

bool ClientViewer::OnConnectToServer()
{
  RegisterVariables();
  return(true);
}

bool ClientViewer::Iterate()
{
  DUNE::IMC::Message * receiveUDPMessage;

  while ((receiveUDPMessage = imcPollUDP()) != NULL) 
  {
      processMessage(receiveUDPMessage);
      free(receiveUDPMessage);
  }
    
  DUNE::IMC::Message * receiveTCPMessage;  
  while ((receiveTCPMessage = imcPollTCP()) != NULL) 
  {
      processMessage(receiveTCPMessage);
      free(receiveTCPMessage);
  }

  return(true);
}

// bool processMessageCallback(DUNE::IMC::Message * message)
// {
//   int type = message->getId();
//   if (type == DUNE::IMC::Announce::getIdStatic()) 
// 	{
//     DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
//     // printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
//     //   msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
//     printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
//       msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);      
//   }
//   if (type == DUNE::IMC::PlanDB::getIdStatic()) 
// 	{
//     DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
//     printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
//     printf("%s\n", msg->info.c_str());
//   }
//   if (type == DUNE::IMC::PlanControlState::getIdStatic()) 
// 	{
//     DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);

//     printf("server receive %s: %lf, %s, %s, %u\n",   \ 
//       msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
//   }
//   if (type == DUNE::IMC::MsgList::getIdStatic()) 
// 	{
//     DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
//     printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
//     DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
//     for (; iter1 != msgList->msgs.end(); ++iter1)
//     {
//       DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
//       DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
//       for (; iter2 != entityParameter->params.end(); ++iter2)
//       {  
//         DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
//         std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;        
//       }
//     }
//   }
//   if (type == DUNE::IMC::EstimatedState::getIdStatic()) 
// 	{
//     DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
//     // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
//     //         msg->getName(), msg->getTimeStamp(), 
//     //         msg->lat, msg->lon, msg->depth,
//     //         msg->phi, msg->theta, msg->psi);
//     printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
//             msg->getName(), msg->getTimeStamp(), 
//             msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
//             msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);            
//   }
//   if (type == DUNE::IMC::VehicleState::getIdStatic()) 
// 	{
//     DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
//     printf("server receive %s: %lf, %u\n", 
//             msg->getName(), msg->getTimeStamp(), msg->op_mode);            
//   }
//   if (type == DUNE::IMC::LogBookControl::getIdStatic()) 
// 	{
//     DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
//     printf("server receive %s: %lf, %u\n", 
//             msg->getName(), msg->getTimeStamp(), msg->command);

//     DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
//     for (; iter != msg->msg.end(); ++iter)
//     {
//       DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
//       std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->text << std::endl;
//     }
//   }

//   return true;
// }

//---------------------------------------------------------
// Procedure: OnStartUp()
//            happens before connection is open

bool ClientViewer::OnStartUp()
{
  initLon = -70.330400;
  initLat = 43.825300;
  initAlt = 0;

  // udpCommEvent.SetPeriod(1);
  // udpCommEvent.SetCallback(processMessageCallback,NULL);
  // udpCommEvent.Start();

  // tcpCommEvent.SetPeriod(1);
  // tcpCommEvent.SetCallback(processMessageCallback,NULL);
  // tcpCommEvent.Start();  

  sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
  m_poll_0.add(sock_udp_receive);

  try
  {
    sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
    sock_tcp_send.setKeepAlive(true);
    m_poll_1.add(sock_tcp_send);
  }
  catch(const std::exception& e)
  {
    std::cerr << e.what() << '\n';
  }

  char local_ip2[INET_ADDRSTRLEN] = {0};
  get_local_ip_using_create_socket(local_ip2);
  ethernetIP = local_ip2;

  RegisterVariables();

  return(true);
}

std::string ClientViewer::SetPlan1(std::string sourceName, double stamp)
{
  struct WayPointBehavior behavior;
  behavior.source = sourceName;
  behavior.priority = -1;
  behavior.points.clear();
  behavior.name = "east_waypt_survey";
  behavior.priority = 10;
  struct Landmark station_1 = {"station_1", -70.328891,43.824429, 10, 3, -1, -1, "point"};
  struct Landmark station_2 = {"station_2", -70.327885,43.824676, 8, 5, -1, -1, "point"};
  struct Landmark station_3 = {"station_3", -70.327867,43.823622, 6, 7, -1, -1, "point"};
  struct Landmark station_4 = {"station_4", -70.328765,43.823622, 4, 9, -1, -1, "point"};
  behavior.points.push_back(station_1);
  behavior.points.push_back(station_2);
  behavior.points.push_back(station_3);
  behavior.points.push_back(station_4);
  behavior.duration = -1;
  behavior.constSpeed = -1;
  behavior.repeat = 1;     
  behavior.closedLoop = true;
  behavior.perpetual = false;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["taskName"] = behavior.name;
  behaviorConfig["taskId"] = "1";  
  behaviorConfig["sourceName"] = behavior.source;
  behaviorConfig["sourceAddress"] = ethernetIP;
  behaviorConfig["clientStamp"] = stamp;
  behaviorConfig["boardStamp"] = -1;     
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeat"] = behavior.repeat;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  behaviorConfig["origin"]["lon"] = initLon;
  behaviorConfig["origin"]["lat"] = initLat;  
  behaviorConfig["origin"]["altitude"] = initAlt;

  Json::Value station;
  station["name"] = station_1.name;  
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  station["north"] = station_1.north;
  station["east"] = station_1.east;
  station["type"] = station_1.type;
  behaviorConfig["points"].append(station);
  station["name"] = station_2.name;    
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  station["north"] = station_2.north;
  station["east"] = station_2.east;
  station["type"] = station_2.type;    
  behaviorConfig["points"].append(station);
  station["name"] = station_3.name;    
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  station["north"] = station_3.north;
  station["east"] = station_3.east;
  station["type"] = station_3.type;    
  behaviorConfig["points"].append(station);
  station["name"] = station_4.name;    
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  station["north"] = station_4.north;
  station["east"] = station_4.east;
  station["type"] = station_4.type;  
  behaviorConfig["points"].append(station);
  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

std::string ClientViewer::SetPlan2(std::string sourceName, double stamp)
{
  struct WayPointBehavior behavior;
  behavior.source = sourceName;
  behavior.priority = -1;
  behavior.points.clear();
  behavior.name = "west_waypt_survey";
  behavior.priority = 10;
  struct Landmark station_1 = {"station_1", -70.331532,43.824194, 9, 4, -1, -1, "track"};
  struct Landmark station_2 = {"station_2", -70.330328,43.824299, 7, 6, -1, -1, "track"};
  struct Landmark station_3 = {"station_3", -70.330346,43.823518, 5, 8, -1, -1, "track"};
  struct Landmark station_4 = {"station_4", -70.331406,43.823206, 3, 10, -1, -1, "track"};
  behavior.points.push_back(station_1);
  behavior.points.push_back(station_2);
  behavior.points.push_back(station_3);
  behavior.points.push_back(station_4);
  behavior.duration = -1;
  behavior.constSpeed = -1;
  behavior.repeat = -1;     
  behavior.closedLoop = true;
  behavior.perpetual = true;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["taskName"] = behavior.name;
  behaviorConfig["taskId"] = "2";    
  behaviorConfig["sourceName"] = behavior.source;
  behaviorConfig["sourceAddress"] = ethernetIP;
  behaviorConfig["clientStamp"] = stamp;
  behaviorConfig["boardStamp"] = -1;
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeat"] = behavior.repeat;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  behaviorConfig["origin"]["lon"] = initLon;
  behaviorConfig["origin"]["lat"] = initLat;  
  behaviorConfig["origin"]["altitude"] = initAlt;
  Json::Value station;
  station["name"] = station_1.name;  
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  station["north"] = station_1.north;
  station["east"] = station_1.east;
  station["type"] = station_1.type;   
  behaviorConfig["points"].append(station);
  station["name"] = station_2.name;  
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  station["north"] = station_2.north;
  station["east"] = station_2.east;
  station["type"] = station_2.type;  
  behaviorConfig["points"].append(station);
  station["name"] = station_3.name;
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  station["north"] = station_3.north;
  station["east"] = station_3.east;
  station["type"] = station_3.type;  
  behaviorConfig["points"].append(station);
  station["name"] = station_4.name;
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  station["north"] = station_4.north;
  station["east"] = station_4.east;
  station["type"] = station_4.type;  
  behaviorConfig["points"].append(station);
  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

std::string ClientViewer::ModifyPlan1(std::string sourceName, double stamp)
{
  struct WayPointBehavior behavior;
  behavior.source = sourceName;
  behavior.priority = -1;
  behavior.points.clear();
  behavior.name = "east_waypt_survey";
  behavior.priority = 10;
  struct Landmark station_1 = {"station_1", -70.328891,43.824429, 9, 2, -1, -1, "track"};
  struct Landmark station_2 = {"station_2", -70.327885,43.824676, 7, 4, -1, -1, "point"};
  struct Landmark station_3 = {"station_3", -70.327867,43.823622, 5, 6, -1, -1, "track"};
  struct Landmark station_4 = {"station_4", -70.328765,43.823622, 3, 8, -1, -1, "point"};
  behavior.points.push_back(station_1);
  behavior.points.push_back(station_2);
  behavior.points.push_back(station_3);
  behavior.points.push_back(station_4);
  behavior.duration = -1;
  behavior.constSpeed = -1;
  behavior.repeat = 3;     
  behavior.closedLoop = true;
  behavior.perpetual = false;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["taskName"] = behavior.name;
  behaviorConfig["taskId"] = "1";  
  behaviorConfig["sourceName"] = behavior.source;
  behaviorConfig["sourceAddress"] = ethernetIP;
  behaviorConfig["clientStamp"] = stamp;
  behaviorConfig["boardStamp"] = -1;  
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeat"] = behavior.repeat;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  behaviorConfig["origin"]["lon"] = initLon;
  behaviorConfig["origin"]["lat"] = initLat;  
  behaviorConfig["origin"]["altitude"] = initAlt;  
  Json::Value station;
  station["name"] = station_1.name;
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  station["north"] = station_1.north;
  station["east"] = station_1.east;
  station["type"] = station_1.type;   
  behaviorConfig["points"].append(station);
  station["name"] = station_2.name;    
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  station["north"] = station_2.north;
  station["east"] = station_2.east;
  station["type"] = station_2.type;    
  behaviorConfig["points"].append(station);
  station["name"] = station_3.name;      
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  station["north"] = station_3.north;
  station["east"] = station_3.east;
  station["type"] = station_3.type;  
  behaviorConfig["points"].append(station);
  station["name"] = station_4.name;   
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  station["north"] = station_4.north;
  station["east"] = station_4.east;
  station["type"] = station_4.type;
  behaviorConfig["points"].append(station);
  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

DUNE::IMC::Message * ClientViewer::imcPollUDP() 
{
  if (m_poll_0.poll(0)) 
	{
    DUNE::Network::Address addr;
    uint16_t rv = sock_udp_receive.read(udpReceiveBuffer, 65535, &addr);
    DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(udpReceiveBuffer, rv);
    return msg;
  }

  return NULL;
}

DUNE::IMC::Message * ClientViewer::imcPollTCP() 
{
  if (m_poll_1.poll(0)) 
	{
    uint16_t rv = sock_tcp_send.read(tcpReceiveBuffer, 65535);
    DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, rv);
    return msg;
  }

  return NULL;
}

bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port) 
{

  DUNE::Utils::ByteBuffer bb;
  try {
      DUNE::IMC::Packet::serialize(msg, bb);
      return sock_tcp_send.write(bb.getBuffer(), msg->getSerializationSize());
  }
  catch (std::runtime_error& e)
  {
      MOOSTrace ("ERROR sending %s to %s:%d: %s\n", msg->getName(), addr.c_str(), port, e.what());
      return false;
  }

  return true;
}


void ClientViewer::processMessage(DUNE::IMC::Message * message) {

  int type = message->getId();
  if (type == DUNE::IMC::Announce::getIdStatic()) 
	{
    DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
    printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
      msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);      
  }
  if (type == DUNE::IMC::PlanDB::getIdStatic()) 
	{
    DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
    printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
    printf("%s\n", msg->info.c_str());
  }
  if (type == DUNE::IMC::PlanControlState::getIdStatic()) 
	{
    DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);

    printf("server receive %s: %lf, %s, %s, %u\n",   \ 
      msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
  }
  if (type == DUNE::IMC::MsgList::getIdStatic()) 
	{
    DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
    printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
    DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
    for (; iter1 != msgList->msgs.end(); ++iter1)
    {
      DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
      DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
      for (; iter2 != entityParameter->params.end(); ++iter2)
      {  
        DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
        //std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;        
      }
    }
  }
  if (type == DUNE::IMC::EstimatedState::getIdStatic()) 
	{
    DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
    printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
            msg->getName(), msg->getTimeStamp(), 
            msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
            msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);            
  }
  if (type == DUNE::IMC::VehicleState::getIdStatic()) 
	{
    DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
    printf("server receive %s: %lf, %u\n", 
            msg->getName(), msg->getTimeStamp(), msg->op_mode);            
  }
  if (type == DUNE::IMC::LogBookControl::getIdStatic()) 
	{
    DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
    printf("server receive %s: %lf, %u\n", 
            msg->getName(), msg->getTimeStamp(), msg->command);

    DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
    for (; iter != msg->msg.end(); ++iter)
    {
      DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
      std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->context << ", " << msgLogBookEntry->text << std::endl;
    }
  }
}

//---------------------------------------------------------
// Procedure: RegisterVariables

void ClientViewer::RegisterVariables()
{
  Register("Command", 0);
  Register("Key", 0);
  Register("Value", 0);
}

int ClientViewer::SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value)
{
  int result = -1;

  subMsg.name = name;
  subMsg.value = value;

  result = 0;
  return result;
}

std::string ClientViewer::generateEntityName(std::string parentName, std::string childName)
{
  Json::Value outputJsonValue;
  outputJsonValue["parent"] = parentName;
  outputJsonValue["child"] = childName;
  Json::StreamWriterBuilder builder;       
  std::string outputJsonString = Json::writeString(builder, outputJsonValue);
  return outputJsonString;
}

std::string ClientViewer::generateEntityValue(std::string value, std::string type)
{
  Json::Value outputJsonValue;
  outputJsonValue["value"] = value;
  outputJsonValue["type"] = type;
  Json::StreamWriterBuilder builder;       
  std::string outputJsonString = Json::writeString(builder, outputJsonValue);
  return outputJsonString;
}


int ClientViewer::get_local_ip_using_create_socket(char* str_ip) 
{
    int status = -1;
    int af = AF_INET;
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in remote_addr;
    struct sockaddr_in local_addr;
    char *local_ip = NULL;
    socklen_t len = 0;

    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(53);
    remote_addr.sin_addr.s_addr = inet_addr("1.1.1.1");

    len =  sizeof(struct sockaddr_in);
    status = connect(sock_fd, (struct sockaddr*)&remote_addr, len);
    if(status != 0 ){
        printf("connect err \n");
    }

    len =  sizeof(struct sockaddr_in);
    getsockname(sock_fd, (struct sockaddr*)&local_addr, &len);

    local_ip = inet_ntoa(local_addr.sin_addr);
    if(local_ip)
    {
        strcpy(str_ip, local_ip);
        status = 0;
    }
    return status;
}