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

#include <iterator>
#include "BoardSupportComm.h"
#include <cmath>


#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS  "127.0.0.1"

//#define MOOS_AUV_SIM
#define MATLAB_AUV_SIM
//#define TRUE_AUV

using namespace std;

BoardSupportComm::BoardSupportComm()
{

  embeddedInfo.header = 0xEBA1;
  embeddedInfo.count = 0;
  embeddedInfo.size = 0;
  embeddedInfo.drive_mode = 0xFF;
  embeddedInfo.height = 0;
  embeddedInfo.depth = 0;
  embeddedInfo.yaw = 0;
  embeddedInfo.pitch = 0;
  embeddedInfo.roll = 0;
  embeddedInfo.ins_vx = 0;
  embeddedInfo.ins_vy = 0;
  embeddedInfo.ins_vz = 0;
  embeddedInfo.lon = 0;
  embeddedInfo.lat = 0;
  embeddedInfo.alt = 0;
  embeddedInfo.dvl_vx = 0;
  embeddedInfo.dvl_vy = 0;
  embeddedInfo.dvl_vz = 0;
  embeddedInfo.rpm = 0;
  embeddedInfo.light_enable = 0;
  embeddedInfo.battery_voltage = 0;
  embeddedInfo.battery_level = 0;
  embeddedInfo.battery_temp = 0;
  embeddedInfo.fault_leakSensor = 0;
  embeddedInfo.fault_battery = 0;
  embeddedInfo.fault_emergencyBattery = 0;
  embeddedInfo.fault_thrust = 0;
  embeddedInfo.iridium = 0xFF;
  embeddedInfo.throwing_load = 0xFF;
  embeddedInfo.dvl_status = 0;
  embeddedInfo.footer = 0xEE1A;

  executeCommand.header = 0xEBA2;               //1:[0,1]
  executeCommand.count = 16;                    //2:[2,3]
  executeCommand.size = 21;                     //3:[4]
  executeCommand.drive_mode = 0x02;            //4:[5]
  executeCommand.thrust = 0;                 //5:[6]
  executeCommand.yaw = 0;                       //6:[7,8]
  executeCommand.depth = 0;                     //7:[9,10]
  executeCommand.helm_top_angle = 0;         //8:[11]
  executeCommand.helm_bottom_angle = 0;      //9:[12]
  executeCommand.helm_left_angle = 0;          //10:[13]
  executeCommand.helm_right_angle = 0;       //11:[14]
  executeCommand.light_enable = 0;             //12:[15]
  executeCommand.dvl_enable = 0;               //13:[16]
  executeCommand.throwing_load_enable = 0;    //14:[17]
  executeCommand.crc = 0;                     //15:[18]
  executeCommand.footer = 0xEE2A;             //16:[19,20]
  executeCommand.manual_mode = false;

  embeddedInfo.header = 0xEBA1;
  embeddedInfo.count = 0;
  tcpConnectRet = -1;
  tcpSockFD = -1;
  recvLen = -1;
}

//---------------------------------------------------------
// Destructor
BoardSupportComm::~BoardSupportComm()
{
  close(tcpSockFD);
}

bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
  AppCastingMOOSApp::OnNewMail(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();

#ifdef MOOS_AUV_SIM
    if(key == "NAV_X")
    {
   	  //E->N
      embeddedInfo.east = (int)dval;     
    }
    if(key == "NAV_Y")
	  {
   	  //N->E
      embeddedInfo.north = (int)dval;     
	  }
    if(key == "NAV_Z")
	  {
   	  //U->D
      embeddedInfo.depth = -1 * dval * 100;
	  }
    if(key == "NAV_YAW")
	  {
      double yawTemp = dval;  //radian
      if (std::abs(yawTemp) <= M_PI)
      {
        if (yawTemp <= 0)
        {
          embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100;
        }
        else
        {
          embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100;
        }  
      }
	  }
    if(key == "NAV_PITCH")
	  {
      embeddedInfo.pitch = dval * 100;
	  }
    if(key == "NAV_LAT")
	  {
      embeddedInfo.lat = dval * 1000000;
	  }
    if(key == "NAV_LONG")
	  {
      embeddedInfo.lon = dval * 1000000;      
	  }
    if(key == "NAV_SPEED")
	  {
      float tempYaw = (float)(embeddedInfo.yaw) / 100.0;
      embeddedInfo.ins_vx = dval * cos(tempYaw) * 100;
      embeddedInfo.ins_vy = -dval * sin(tempYaw) * 100;
      embeddedInfo.ins_vz = 0;

	  }
    if(key == "NAV_DEPTH")
	  {
      embeddedInfo.depth = dval * 100;
	  }
#endif    
    if(key == "Fault_LeakSensor")
    {
      embeddedInfo.fault_leakSensor = (uint32_t)dval;
    }
    if(key == "Fault_Battery")
    {
      embeddedInfo.fault_battery = (uint8_t)dval;
    }
    if(key == "Fault_EmergencyBattery")
    {
      embeddedInfo.fault_emergencyBattery = (uint8_t)dval;      
    }
    if(key == "Fault_Thrust")
    {
      embeddedInfo.fault_thrust = (uint8_t)dval;
    }

    if(key == "uManual_enable_cmd")
    {
      if (sval == "true")
      {
        executeCommand.manual_mode = true;
      }
      else
      {
        executeCommand.manual_mode = false;

      }    
    }

    if(key == "uManual_drive_cmd")
	  {
#ifdef MOOS_AUV_SIM      
      if (executeCommand.manual_mode)
#else
      if (embeddedInfo.drive_mode == 0x02)
#endif
      {
        std::string err;
        Json::Value recvCommand;
        std::istringstream iss(sval);  
        Json::CharReaderBuilder builder;  
        bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
        if (!parsingResult) 
        {  
            std::cerr << "Failed to parse JSON string." << std::endl;  
            return false;  
        }
        int thrust = recvCommand["Thrust"].asInt();  
        if ((thrust < -100) or (thrust > 100))
        {
          return false;
        }
        executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100);
        uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30);        
        executeCommand.depth = 0;
        executeCommand.helm_top_angle = helm_top_bottom_angle;
        executeCommand.helm_bottom_angle = helm_top_bottom_angle;
        executeCommand.helm_left_angle = 0;
        executeCommand.helm_right_angle = 0;
        executeCommand.yaw = 0;
        
        int serializeResult = serializeMessage(tcpSendBuffer);
        std::stringstream ss;
        ss << tcpSockFD;
        ss << ", ";
        ss << serializeResult;
        castLogStream = ss.str();

        if ((serializeResult == 0) && (tcpSockFD != -1))
        {
          try
          {
            write(tcpSockFD, tcpSendBuffer, executeCommand.size);
          }
          catch(const std::exception& e)
          {
            std::cerr << e.what() << '\n';
          }
        }
      }
	  }
    if(key == "uMotion_control_cmd")
	  {
      if (executeCommand.manual_mode)
      {
        return false;
      }      
      std::string err;
      Json::Value recvCommand;
      std::istringstream iss(sval);  
      Json::CharReaderBuilder builder;  
      bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
      if (!parsingResult) 
      {  
          std::cerr << "Failed to parse JSON string." << std::endl;  
          return false;  
      }
      executeCommand.thrust = convertIntToUchar(recvCommand["thrust"].asInt(), -100, 100);
      executeCommand.helm_top_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
      executeCommand.helm_bottom_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
      executeCommand.helm_left_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
      executeCommand.helm_right_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);

      int serializeResult = serializeMessage(tcpSendBuffer);
      // std::cout << "serializeResult: " << serializeResult << std::endl;
      // printf("header: %u, %u\n", tcpSendBuffer[0], tcpSendBuffer[1]);
      // printf("count: %u\n", tcpSendBuffer[2], tcpSendBuffer[3]);
      // printf("size: %u\n", tcpSendBuffer[4]);
      // printf("drive_mode: %u\n", tcpSendBuffer[5]);
      // printf("thrust: %u\n", tcpSendBuffer[6]);
      // printf("yaw: %u, %u\n", tcpSendBuffer[7], tcpSendBuffer[8]);
      // printf("depth: %u, %u\n", tcpSendBuffer[9], tcpSendBuffer[10]); 
      // printf("helm_top_angle: %u\n", tcpSendBuffer[11]);
      // printf("helm_bottom_angle: %u\n", tcpSendBuffer[12]);
      // printf("helm_left_angle: %u\n", tcpSendBuffer[13]);
      // printf("helm_right_angle: %u\n", tcpSendBuffer[14]);
      // printf("light_enable: %u\n", tcpSendBuffer[15]);
      // printf("dvl_enable: %u\n", tcpSendBuffer[16]);
      // printf("throwing_load_enable: %u\n", tcpSendBuffer[17]);
      // printf("crc: %u\n", tcpSendBuffer[18]);
      // printf("footer: %u, %u\n", tcpSendBuffer[19], tcpSendBuffer[20]);
      if ((serializeResult == 0) && (tcpSockFD != -1))
      {
        try
        {
          write(tcpSockFD, tcpSendBuffer, executeCommand.size);
        }
        catch(const std::exception& e)
        {
          std::cerr << e.what() << '\n';
        }
      }      
    }
    if(key == "uMission_origin_cmd")
	  {
      std::string err;
      Json::Value recvCommand;
      std::istringstream iss(sval);  
      Json::CharReaderBuilder builder;  
      bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
      if (!parsingResult) 
      {  
          std::cerr << "Failed to parse JSON string." << std::endl;  
          return false;  
      }
      embeddedInfo.refLon = recvCommand["lon"].asFloat();
      embeddedInfo.refLat = recvCommand["lat"].asFloat();
      embeddedInfo.refAlt = recvCommand["alt"].asFloat();

      struct stat info;
      if (stat(llaOriginPath.c_str(), &info) == 0)
      {
        std::ifstream ifs;
        ifs.open(llaOriginPath, std::ios::in);

        Json::Reader reader;
        Json::Value originJsonValue;
        reader.parse(ifs, originJsonValue);
        ifs.close();
        
        originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
        originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
        originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
        originJsonValue["TaskName"] = recvCommand["task"].asString();
        Json::StreamWriterBuilder builder;   
        std::ofstream ofs;
        ofs.open(llaOriginPath, std::ios::out);        
        ofs << Json::writeString(builder, originJsonValue) << std::endl;
        ofs.close();
      }
      else
      {
        Json::Value originJsonValue;        
        originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
        originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
        originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
        originJsonValue["TaskName"] = recvCommand["task"].asString();  
        Json::StreamWriterBuilder builder;
        std::ofstream ofs;
        ofs.open(llaOriginPath, std::ios::out);        
        ofs << Json::writeString(builder, originJsonValue) << std::endl;
        ofs.close();
      } 
    }
  }

  return(true);
}

uint8_t BoardSupportComm::convertIntToUchar(int src, int min, int max)
{
  uint8_t dst;
  if (src < 0)
  {
    if (src < min)
    {
      dst  = std::abs(min) + 0B10000000;      
    }
    else
    {
      dst  = std::abs(src) + 0B10000000;
    }
  }
  else
  {
    if (src > max)
    {
      dst  = std::abs(max);
    }
    else
    {
      dst  = std::abs(src);
    }

  }
  return dst; 
}

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

std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
{
#ifdef MOOS_AUV_SIM
  embeddedState["driveMode"] = 0xFF;
  embeddedState["height"] = 0;
  embeddedState["depth"] = 0;
  embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;
  embeddedState["pitch"] = 0;
  embeddedState["roll"] = 0;
  embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01;
  embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01;
  embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01;
  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
  embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
  embeddedState["dvlVX"] = 0;
  embeddedState["dvlVY"] = 0;
  embeddedState["dvlVZ"] = 0;
  embeddedState["rpm"] = 0;
  embeddedState["lightEnable"] = 0;
  embeddedState["batteryVoltage"] = 15;
  embeddedState["batteryLevel"] = 60;
  embeddedState["batteryTemp"] = 25;
  embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
  embeddedState["faultBattery"] = embeddedInfo.fault_battery;
  embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
  embeddedState["faultThrust"] = embeddedInfo.fault_thrust;

  embeddedState["iridium"] = 0xFF;
  embeddedState["throwingLoadEnable"] = 0xFF;
  embeddedState["dvlStatus"] = 0;
#endif

#ifdef MATLAB_AUV_SIM
  embeddedState["driveMode"] = embeddedInfo.drive_mode;
  embeddedState["height"] = embeddedInfo.height * 0.01;
  embeddedState["depth"] = embeddedInfo.depth * 0.01;
  embeddedState["roll"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->N
  embeddedState["pitch"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->E    
  embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;    //D->D
  embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; //N->N
  embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; //E->E
  embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; //D->D  
  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
  embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
  embeddedState["dvlVX"] = embeddedInfo.dvl_vx * 0.01;    //N->N
  embeddedState["dvlVY"] = embeddedInfo.dvl_vy * 0.01;    //E->E
  embeddedState["dvlVZ"] = embeddedInfo.dvl_vz * 0.01;   //D->D
  embeddedState["rpm"] = embeddedInfo.rpm;
  embeddedState["lightEnable"] = embeddedInfo.light_enable;
  embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
  embeddedState["batteryLevel"] = embeddedInfo.battery_level;
  embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
  embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
  embeddedState["faultBattery"] = embeddedInfo.fault_battery;
  embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
  embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
  embeddedState["iridium"] = embeddedInfo.iridium;
  embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
  embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif

#ifdef TRUE_AUV
  embeddedState["driveMode"] = embeddedInfo.drive_mode;
  embeddedState["height"] = embeddedInfo.height * 0.01;
  embeddedState["depth"] = embeddedInfo.depth * 0.01;
  embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N
  embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E    
  embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;    //D->D  radian<-degree
  embeddedState["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N
  embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E
  embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D
  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
  embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
  embeddedState["dvlVX"] = embeddedInfo.dvl_vy * 0.01;    //E->N
  embeddedState["dvlVY"] = embeddedInfo.dvl_vx * 0.01;    //N->E
  embeddedState["dvlVZ"] = -embeddedInfo.dvl_vz * 0.01;   //U->D
  embeddedState["rpm"] = embeddedInfo.rpm;
  embeddedState["lightEnable"] = embeddedInfo.light_enable;
  embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
  embeddedState["batteryLevel"] = embeddedInfo.battery_level;
  embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
  embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
  embeddedState["faultBattery"] = embeddedInfo.fault_battery;
  embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
  embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
  embeddedState["iridium"] = embeddedInfo.iridium;
  embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
  embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif

  double currentLon = embeddedInfo.lon * 0.000001;
  double currentLat = embeddedInfo.lat * 0.000001;
  double currentAlt = embeddedInfo.alt * 0.01;
  std::vector<double> reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt};
  std::vector<double> current = {currentLon, currentLat, currentAlt};
  std::vector<double> ned = {0, 0, 0};
  ConvertLLAToNED(reference, current, ned);
  embeddedState["north"] = ned.at(0);
  embeddedState["east"]= ned.at(1);
  embeddedState["referenceLon"]= embeddedInfo.refLon;  
  embeddedState["referenceLat"]= embeddedInfo.refLat;  
  embeddedState["referenceAltitude"]= embeddedInfo.refAlt;    

  Json::StreamWriterBuilder builder;
  return Json::writeString(builder, embeddedState);
}


void BoardSupportComm::tcpProcessThread()
{
  while(1)
  {
    bzero(tcpReceiveBuffer, 0);
    recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
    if(recvLen > 0)
    {
#ifndef MOOS_AUV_SIM
      parseMessage((unsigned char* )tcpReceiveBuffer, recvLen);
#endif
      Json::Value embeddedState;
      recvContent = convertEmbeddedFormat(embeddedState);
      Notify("uDevice_monitor_fb", recvContent);
#ifndef MOOS_AUV_SIM
      Notify("NAV_X", embeddedState["north"].asDouble());
      Notify("NAV_Y", embeddedState["east"].asDouble());
      Notify("NAV_Z", embeddedState["depth"].asDouble()); 
      Notify("NAV_LAT", embeddedState["currentLat"].asDouble());
      Notify("NAV_LONG", embeddedState["currentLon"].asDouble());
      Notify("NAV_HEADING", embeddedState["yaw"].asDouble() * 180 / M_PI);
      double driveSpeed = std::sqrt(std::pow((double)embeddedState["dvlVX"].asDouble(), 2) 
                          + std::pow((double)embeddedState["dvlVY"].asDouble(), 2)
                          + std::pow((double)embeddedState["dvlVZ"].asDouble(), 2));
      Notify("NAV_SPEED", driveSpeed);
      Notify("NAV_DEPTH", embeddedState["depth"].asDouble());
      Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI); 
      Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI);
      Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI);
#endif
    }
  }
}

bool BoardSupportComm::buildReport()
{
  m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl;
  m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl;
  m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", sendContent:" << castLogStream << endl;
  return true;
}

bool BoardSupportComm::Iterate()
{
  AppCastingMOOSApp::Iterate();

  if(tcpSockFD == -1)
  {
    tcpSockFD = socket(AF_INET, SOCK_STREAM, 0);
    if(tcpSockFD == -1)
    {
      return false;
    }
  }

  if(tcpConnectRet == -1)
  { 
    struct sockaddr_in saddr;
    //inet_pton(AF_INET, TCP_SERVER_ADDRESS, &saddr.sin_addr.s_addr);
    saddr.sin_addr.s_addr = inet_addr(TCP_SERVER_ADDRESS);
    saddr.sin_family = AF_INET;
    saddr.sin_port = htons(TCP_RECEIVE_PORT);
    tcpConnectRet = connect(tcpSockFD, (struct sockaddr *)&saddr, sizeof(saddr));
    if(tcpConnectRet == -1)
    {
      return false;
    }
  }

  if ((tcpSockFD != -1) && (tcpConnectRet != -1))
  {

    std::thread t1(&BoardSupportComm::tcpProcessThread, this);
    t1.detach();
  }


#if 0
  printf("header: %hx\n", embeddedInfo.header);
  printf("count: %hu\n", embeddedInfo.count);
  printf("size: %u\n", embeddedInfo.size);
  printf("drive_mode: %u\n", embeddedInfo.drive_mode);
  printf("height: %hu\n", embeddedInfo.height);
  printf("depth: %hu\n", embeddedInfo.depth);
  printf("yaw: %hu\n", embeddedInfo.yaw);
  printf("pitch: %hd\n", embeddedInfo.pitch);
  printf("roll: %hd\n", embeddedInfo.roll);
  printf("ins_vx: %hd\n", embeddedInfo.ins_vx);
  printf("ins_vy: %hd\n", embeddedInfo.ins_vy);
  printf("ins_vz: %hd\n", embeddedInfo.ins_vz);
  printf("lon: %d\n", embeddedInfo.lon);
  printf("lat: %d\n", embeddedInfo.lat);
  printf("alt: %hd\n", embeddedInfo.alt);
  printf("dvl_vx: %hd\n", embeddedInfo.dvl_vx);
  printf("dvl_vy: %hd\n", embeddedInfo.dvl_vy);
  printf("dvl_vz: %hd\n", embeddedInfo.dvl_vz);
  printf("rpm: %hd\n", embeddedInfo.rpm);
  printf("light_enable: %u\n", embeddedInfo.light_enable);
  printf("battery_voltage: %u\n", embeddedInfo.battery_voltage);
  printf("battery_level: %u\n", embeddedInfo.battery_level);
  printf("battery_temp: %u\n", embeddedInfo.battery_temp);
  printf("fault_leakSensor: %hd\n", embeddedInfo.fault_leakSensor);
  printf("fault_battery: %u\n", embeddedInfo.fault_battery);
  printf("fault_emergencyBattery: %u\n", embeddedInfo.fault_emergencyBattery);
  printf("fault_thrust: %u\n", embeddedInfo.fault_thrust);
  printf("iridium: %u\n", embeddedInfo.iridium);
  printf("throwing_load: %u\n", embeddedInfo.throwing_load);
  printf("dvl_status: %u\n", embeddedInfo.dvl_status);
  printf("crc: %u\n", embeddedInfo.crc);
  printf("footer: %u\n", embeddedInfo.footer);
#endif

#ifdef MOOS_AUV_SIM
  executeCommand.header = 0xEBA2;               //1:[0,1]
  executeCommand.count = 16;                    //2:[2,3]
  executeCommand.size = 21;                     //3:[4]
  executeCommand.drive_mode = 0x02;            //4:[5]
  executeCommand.thrust = 178;                 //5:[6],赋值-50
  executeCommand.yaw = 45.3 * 10;             //6:[7,8],赋值45.3
  executeCommand.depth = 298.6 * 10;          //7:[9,10],赋值298.6
  executeCommand.helm_top_angle = 145;         //8:[11],赋值-17
  executeCommand.helm_bottom_angle = 154;      //9:[12],赋值-26
  executeCommand.helm_left_angle = 9;          //10:[13],赋值9
  executeCommand.helm_right_angle = 158;       //11:[14],赋值-30
  executeCommand.light_enable = 1;             //12:[15],赋值1
  executeCommand.dvl_enable = 1;               //13:[16],赋值1
  executeCommand.throwing_load_enable = 1;    //14:[17],赋值1
  executeCommand.crc = 0;                     //15:[18]
  executeCommand.footer = 0xEE2A;             //16:[19,20]

  int serializeResult = serializeMessage(tcpSendBuffer);
  if (serializeResult == 0)
  {
    try
    {
      write(tcpSockFD, tcpSendBuffer, executeCommand.size);
    }
    catch(const std::exception& e)
    {
      std::cerr << e.what() << '\n';
    }
  }
#endif

  AppCastingMOOSApp::PostReport();

  return true;
}

template <typename Type>
inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize)
{
  uint16_t size = sizeof(Type);
  std::memcpy(bfr, &t, size);
  sumSize += size;
  return size;
}

int BoardSupportComm::serializeMessage(unsigned char* buffer)
{
  uint8_t* ptr = buffer;
  uint16_t sumSize = 0;
  ptr += serialize(executeCommand.header, ptr, sumSize);
  ptr += serialize(executeCommand.count, ptr, sumSize);
  ptr += serialize(executeCommand.size, ptr, sumSize);
  ptr += serialize(executeCommand.drive_mode, ptr, sumSize);  
  ptr += serialize(executeCommand.thrust, ptr, sumSize);
  ptr += serialize(executeCommand.yaw, ptr, sumSize);
  ptr += serialize(executeCommand.depth, ptr, sumSize);
  ptr += serialize(executeCommand.helm_top_angle, ptr, sumSize);
  ptr += serialize(executeCommand.helm_bottom_angle, ptr, sumSize);
  ptr += serialize(executeCommand.helm_left_angle, ptr, sumSize);
  ptr += serialize(executeCommand.helm_right_angle, ptr, sumSize);
  ptr += serialize(executeCommand.light_enable, ptr, sumSize);
  ptr += serialize(executeCommand.dvl_enable, ptr, sumSize);
  ptr += serialize(executeCommand.throwing_load_enable, ptr, sumSize);
  DUNE::Algorithms::CRC8 crc(0x07);
  crc.putArray(buffer+CRC_COMPUTE_START_POS, CRC_COMPUTE_SUM_SIZE);
  executeCommand.crc = crc.get();
  ptr += serialize(executeCommand.crc, ptr, sumSize);
  ptr += serialize(executeCommand.footer, ptr, sumSize);
  if (sumSize != (uint16_t)executeCommand.size)
  {
    return -1;
  }
  return 0;
}

template <typename Type>
inline uint16_t BoardSupportComm::deserialize(Type& t, const uint8_t* bfr, uint16_t& length)
{
  uint16_t size = sizeof(Type);

  std::memcpy(&t, bfr, size);
  length -= size;

  return size;
}

int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
{
  const uint8_t* ptr = buffer;
  uint16_t size__ = size;
  uint16_t header;
  ptr += deserialize(embeddedInfo.header, ptr, size__);
  ptr += deserialize(embeddedInfo.count, ptr, size__);
  ptr += deserialize(embeddedInfo.size, ptr, size__);
  ptr += deserialize(embeddedInfo.drive_mode, ptr, size__);
  ptr += deserialize(embeddedInfo.height, ptr, size__);
  ptr += deserialize(embeddedInfo.depth, ptr, size__);
  ptr += deserialize(embeddedInfo.yaw, ptr, size__);
  ptr += deserialize(embeddedInfo.pitch, ptr, size__);
  ptr += deserialize(embeddedInfo.roll, ptr, size__);
  ptr += deserialize(embeddedInfo.ins_vx, ptr, size__);
  ptr += deserialize(embeddedInfo.ins_vy, ptr, size__);
  ptr += deserialize(embeddedInfo.ins_vz, ptr, size__);
  ptr += deserialize(embeddedInfo.lon, ptr, size__);
  ptr += deserialize(embeddedInfo.lat, ptr, size__);
  ptr += deserialize(embeddedInfo.alt, ptr, size__);
  ptr += deserialize(embeddedInfo.dvl_vx, ptr, size__);
  ptr += deserialize(embeddedInfo.dvl_vy, ptr, size__);
  ptr += deserialize(embeddedInfo.dvl_vz, ptr, size__);
  ptr += deserialize(embeddedInfo.rpm, ptr, size__);
  ptr += deserialize(embeddedInfo.light_enable, ptr, size__);
  ptr += deserialize(embeddedInfo.battery_voltage, ptr, size__);
  ptr += deserialize(embeddedInfo.battery_level, ptr, size__);
  ptr += deserialize(embeddedInfo.battery_temp, ptr, size__);
  ptr += deserialize(embeddedInfo.fault_leakSensor, ptr, size__);
  ptr += deserialize(embeddedInfo.fault_battery, ptr, size__);
  ptr += deserialize(embeddedInfo.fault_emergencyBattery, ptr, size__);
  ptr += deserialize(embeddedInfo.fault_thrust, ptr, size__);
  ptr += deserialize(embeddedInfo.iridium, ptr, size__);
  ptr += deserialize(embeddedInfo.throwing_load, ptr, size__);
  ptr += deserialize(embeddedInfo.dvl_status, ptr, size__);
  ptr += deserialize(embeddedInfo.crc, ptr, size__);
  ptr += deserialize(embeddedInfo.footer, ptr, size__);

  return 0;
}

bool BoardSupportComm::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();
  m_MissionReader.GetValue("llaOriginPath", llaOriginPath);
  try
  {
    struct stat info;
    if (stat(llaOriginPath.c_str(), &info) == 0)
    {
      std::ifstream ifs;
      ifs.open(llaOriginPath, std::ios::in);

      Json::Reader reader;
      Json::Value originJsonValue;
      reader.parse(ifs, originJsonValue);
      ifs.close();

      if (originJsonValue.isMember("LongOrigin") && 
          originJsonValue.isMember("LatOrigin") && 
          originJsonValue.isMember("AltOrigin"))
      {
        embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat();
        embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat();
        embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat();        
      }
      else
      {
        throw -1;
      }
    }
    else
    {
      throw -1;
    } 
  }
  catch(...)
  {
    m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon);
    m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat);
    m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt);
  }

  RegisterVariables();
  return(true);
}

void BoardSupportComm::RegisterVariables()
{
  AppCastingMOOSApp::RegisterVariables();

#ifdef MOOS_AUV_SIM
  Register("NAV_X", 0);
  Register("NAV_Y", 0);
  Register("NAV_Z", 0);
  Register("NAV_LAT", 0);
  Register("NAV_LONG", 0);
  Register("NAV_SPEED", 0);
  Register("NAV_DEPTH", 0);
  Register("NAV_YAW", 0);
  Register("NAV_PITCH", 0);
  Register("Fault_LeakSensor", 0);
  Register("Fault_Battery", 0);
  Register("Fault_EmergencyBattery", 0);
  Register("Fault_Thrust", 0);
#endif

  Register("uManual_drive_cmd", 0);
  Register("uMotion_control_cmd", 0);
  Register("uManual_enable_cmd", 0);
  Register("uMission_origin_cmd", 0); 
}

void BoardSupportComm::ConvertLLAToENU(std::vector<double> init_lla,
                            std::vector<double> point_lla,
                            std::vector<double>& point_enu)
{
  static GeographicLib::LocalCartesian local_cartesian;
  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
  local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
                          point_enu.at(0), point_enu.at(1), point_enu.at(2));
}

void BoardSupportComm::ConvertENUToLLA(std::vector<double> init_lla,
                            std::vector<double> point_enu,
                            std::vector<double> &point_lla)
{
  static GeographicLib::LocalCartesian local_cartesian;
  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
  local_cartesian.Reverse(point_enu.at(0), point_enu.at(1), point_enu.at(2),
                          point_lla.at(1), point_lla.at(0), point_lla.at(2));
}


void BoardSupportComm::ConvertLLAToNED(std::vector<double> init_lla,
                            std::vector<double> point_lla,
                            std::vector<double>& point_ned)
{
  static GeographicLib::LocalCartesian local_cartesian;
  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));

  double point_e = 0;
  double point_n = 0;
  double point_u = 0;
  local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
                          point_e, point_n, point_u);
  point_ned.at(0) = point_n;
  point_ned.at(1) = point_e;
  point_ned.at(2) = -point_u;
}


void BoardSupportComm::ConvertNEDToLLA(std::vector<double> init_lla,
                            std::vector<double> point_ned,
                            std::vector<double> &point_lla)
{
  static GeographicLib::LocalCartesian local_cartesian;
  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));

  double point_e = point_ned.at(1);
  double point_n = point_ned.at(0);
  double point_u = -point_ned.at(2);

  local_cartesian.Reverse(point_e, point_n, point_u,
                          point_lla.at(1), point_lla.at(0), point_lla.at(2));
}