/*
 * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @Date: 2023-09-21 11:04:00
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-10-07 10:37:30
 * @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSend.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/************************************************************/
/*    NAME: zjk                                              */
/*    ORGN: MIT                                             */
/*    FILE: TaskSend.cpp                                        */
/*    DATE:                                                 */
/************************************************************/

#include <list>
#include <iterator>
#include "MBUtils.h"
#include "TaskSend.h"
#include <iostream>

#include <json/json.h>

using namespace std;

#define UDP_RECEIVE_PORT 8000
#define TCP_SEND_PORT 6000 
#define TCP_SERVER_ADDRESS "127.0.0.1" 

using namespace std;

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

TaskSend::TaskSend()
{
}

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

TaskSend::~TaskSend()
{
}

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

bool TaskSend::OnNewMail(MOOSMSG_LIST &NewMail)
{
   MOOSMSG_LIST::iterator p;
   
   for(p=NewMail.begin(); p!=NewMail.end(); p++) 
   {
      CMOOSMsg &msg = *p;
      p->Trace();
      // current_msg = msg;
      if(p->GetName()== "SendTask")
        if(p->GetString() == "true")
        {
          editView=true;
        }
      
      if(p->GetName()=="BHV_WARNING")
        if(p->GetString() != "")
        {
          error = true;
        }
        
   }
   
   return(true);
}

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

bool TaskSend::OnConnectToServer()
{
   Notify("SendTask","false");
   RegisterVariables();
   return(true);
}

//---------------------------------------------------------
// Procedure: Iterate()

bool TaskSend::Iterate()
{
   // happens AppTick times per second
   if(editView)
   {
      error=false;
      view = taskEditView();
      view->show();
      // delete view;
      // Fl::grab();
      Fl::run();                                                   /* 6. 运行FLTK主循环 */
      cout << "editView = true" << endl;
      editView=false;
   }
   
   if(error)
   {
      error=true;
      disp1->value("parameter incorrect");

   }

  cout << "error=" << error << "editView" << editView << endl;
      

  return(true);
}

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

bool TaskSend::OnStartUp()
{
  list<string> sParams;                                                                                         
  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {         
    list<string>::iterator p;                                           
    for(p=sParams.begin(); p!=sParams.end(); p++) {                 
      string original_line = *p;                                    
      string line = *p;                                    
      string param = stripBlankEnds(toupper(biteString(line, '=')));  
      string value = stripBlankEnds(line);
                                                           
      if(param == "FOO") {                                      
        //handled                                               
      }                                                             
      else if(param == "BAR") {                                  
        //handled                                     
      }                            
    }
  }

  RegisterVariables();

#if 1
  sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
  sock_tcp_send.setKeepAlive(true);
#endif

  return(true);
}

std::string TaskSend::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 = {-70.328891,43.824429, 10, 3};
  struct Landmark station_2 = {-70.327885,43.824676, 8, 5};
  struct Landmark station_3 = {-70.327867,43.823622, 6, 7};
  struct Landmark station_4 = {-70.328765,43.823622, 4, 9};
  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.repeate = -1;     
  behavior.closedLoop = true;
  behavior.perpetual = true;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["name"] = behavior.name;
  behaviorConfig["source"] = behavior.source;
  behaviorConfig["client stamp"] = stamp;
  behaviorConfig["board stamp"] = -1;     
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeate"] = behavior.repeate;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  Json::Value station;
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  behaviorConfig["points"].append(station);
  behaviorConfig["client stamp"] = stamp;
  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

std::string TaskSend::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 = {-70.331532,43.824194, 9, 4};
  struct Landmark station_2 = {-70.330328,43.824299, 7, 6};
  struct Landmark station_3 = {-70.330346,43.823518, 5, 8};
  struct Landmark station_4 = {-70.331406,43.823206, 3, 10};
  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.repeate = -1;     
  behavior.closedLoop = true;
  behavior.perpetual = true;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["name"] = behavior.name;
  behaviorConfig["source"] = behavior.source;
  behaviorConfig["client stamp"] = stamp;
  behaviorConfig["board stamp"] = -1;
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeate"] = behavior.repeate;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  Json::Value station;
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  behaviorConfig["points"].append(station);

  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

std::string TaskSend::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 = {-70.328891,43.824429, 9, 2};
  struct Landmark station_2 = {-70.327885,43.824676, 7, 4};
  struct Landmark station_3 = {-70.327867,43.823622, 5, 6};
  struct Landmark station_4 = {-70.328765,43.823622, 3, 8};
  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.repeate = -1;     
  behavior.closedLoop = true;
  behavior.perpetual = true;
  behavior.minDepth = -1;
  behavior.maxDepth = -1;

  Json::Value behaviorConfig;
  behaviorConfig["name"] = behavior.name;
  behaviorConfig["source"] = behavior.source;
  behaviorConfig["client stamp"] = stamp;
  behaviorConfig["board stamp"] = -1;  
  behaviorConfig["priority"] = behavior.priority;
  behaviorConfig["duration"] = behavior.duration;
  behaviorConfig["closedLoop"] = behavior.closedLoop;
  behaviorConfig["constSpeed"] = behavior.constSpeed;
  behaviorConfig["repeate"] = behavior.repeate;
  behaviorConfig["perpetual"] = behavior.perpetual;
  behaviorConfig["minDepth"] = behavior.minDepth;
  behaviorConfig["maxDepth"] = behavior.maxDepth;
  Json::Value station;
  station["lon"] = station_1.lon;
  station["lat"] = station_1.lat;  
  station["depth"] = station_1.depth;
  station["speed"] = station_1.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_2.lon;
  station["lat"] = station_2.lat;  
  station["depth"] = station_2.depth;
  station["speed"] = station_2.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_3.lon;
  station["lat"] = station_3.lat;  
  station["depth"] = station_3.depth;
  station["speed"] = station_3.speed;
  behaviorConfig["points"].append(station);
  station["lon"] = station_4.lon;
  station["lat"] = station_4.lat;  
  station["depth"] = station_4.depth;
  station["speed"] = station_4.speed;
  behaviorConfig["points"].append(station);

  Json::StreamWriterBuilder builder;
  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);

  return behaviorSpecString;
}

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

void TaskSend::RegisterVariables()
{
  Register("SendTask",0);
  Register("BHV_WARNING",0);
  // Register("FOOBAR", 0);
}

Fl_Window * TaskSend::taskEditView()
{
    Fl_Window *window = new Fl_Window(405, 600, "task config");
    // Fl_Double_Window *window(450, 350, "Simple Table");               /* 1. 创建一个窗口 */
    Fl_Group* pGroup = new Fl_Group(0, 0, 400, 70);                  /* 2. 创建一个分组 */
    pGroup->box(FL_GTK_UP_BOX);

    Fl_Button* Button_1 = new Fl_Button(5, 5, 90, 30, "send task 1");   //发送任务
    Button_1->callback(st_sendTask1Callback, (void*) this);
    
    Fl_Button* Button_2 = new Fl_Button(105, 5, 90, 30, "send task 2");   //发送任务
    Button_2->callback(st_sendTask2Callback, (void*) this);

    Fl_Button* Button_3 = new Fl_Button(205, 5, 90, 30, "modify task 1");   //发送任务
    Button_3->callback(st_ModifyTask1Callback, (void*) this);    

    Fl_Button* Button_4 = new Fl_Button(295, 5, 90, 30, "load task");
    Button_4->callback(st_FeedbackTaskCallback, (void*) this);  

    Fl_Output* TaskMsgDisp = new Fl_Output(5,35,390,30);
    TaskMsgDisp->box(FL_FLAT_BOX);
    disp1 = TaskMsgDisp;
    pGroup->end();                                                      /* 4. 结束上个容器的创建 */
    
    Fl_Group* pGroup1 = new Fl_Group(0, 90, 400, 70,"task parameter");                  /* 2. 创建一个分组 */
    pGroup1->box(FL_GTK_UP_BOX);
    
    Fl_Menu_Button* taskType =new Fl_Menu_Button(5,95,100,25,"task type");
    taskType->add("path tracking");
    taskType->add("fixed yaw");
    taskType->add("fixed depth");
    taskType->add("fixed speed");
    // taskType->add("");
    Fl_Input* pwt = new Fl_Input(200,90,100,30,"task priority");
    pwt->value("100");
    Fl_Input* duration = new Fl_Input(200,35+90,100,30,"task duration");
    duration->value("no-time-limit");
    // pGroup1->add(taskType);
    pGroup1->end();

    Fl_Group* pGroup2 = new Fl_Group(0, 180, 400, 300,"path tracking parameter");                 
    pGroup2->box(FL_GTK_UP_BOX);
    pGroup2->add(capture_radius);
    capture_radius->value("5");
    pGroup2->add(capture_line);
    capture_line->value("true");
    pGroup2->add(slip_radius);
    slip_radius->value("15.0");
    pGroup2->add(lead);
    lead->value("8");
    pGroup2->add(lead_damper);
    lead_damper->value("1");
    pGroup2->add(speed);
    speed->value("10.0");
    pGroup2->add(repeat);
    repeat->value("3");
    pGroup2->add(polygon);


    pGroup2->end();
    window->end();                                                       /* 4. 结束上个容器的创建 */
    // window->show();                                                      /* 5. 显示窗口 */
    return window;
    // delete window;
}

inline int TaskSend::sendTaskCallback(Fl_Widget *w)
{
    disp1->value("Task Sending...");
    Notify("TaskNum","t1");
    string taskMsg = "WPT_UPDATE";
    string taskMsgconit="";
    // if(polygon->value()=="")
    // disp1->value(polygon->value());
    taskMsgconit = "capture_radius=";
    taskMsgconit += capture_radius->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "capture_line=";
    taskMsgconit += capture_line->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "slip_radius=";
    taskMsgconit += slip_radius->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "lead=";
    taskMsgconit += lead->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "lead_damper=";
    taskMsgconit += lead_damper->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "speed=";
    taskMsgconit += speed->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "repeat=";
    taskMsgconit += repeat->value();
    Notify(taskMsg,taskMsgconit);

    taskMsgconit = "polygon=";
    taskMsgconit += polygon->value();    
    Notify(taskMsg,taskMsgconit);
    
    if(error)
      disp1->value("configure incorrect");
    else
      disp1->value("Task Send complete");
    // disp1->value("Task Send complete");
    
}

inline int TaskSend::sendTask1Callback(Fl_Widget *w)
{
  std::string systemName = "neptus-client-1";
  std::string plan_1_Spec = SetPlan1(systemName, getTimeStamp());
  std::cout << plan_1_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_1_Spec;
  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);

}

inline int TaskSend::sendTask2Callback(Fl_Widget *w)
{
  std::string systemName = "neptus-client-1";
  std::string plan_2_Spec = SetPlan2(systemName, getTimeStamp());
  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);
}

inline int TaskSend::ModifyTask1Callback(Fl_Widget *w)
{
  std::string systemName = "neptus-client-1";  
  std::string plan_1_Spec = ModifyPlan1(systemName, getTimeStamp());
  std::cout << plan_1_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_1_Spec;
  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}

inline int TaskSend::FeedbackTaskCallback(Fl_Widget *w)
{
  DUNE::IMC::PlanDB msg;
  msg.setTimeStamp();
  msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
  msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}


bool TaskSend::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;
}

double TaskSend::getTimeStamp()
{
  struct timeval tv;
  gettimeofday(&tv,NULL);
  double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
  return stamp;  
}