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

#ifndef BoardSupportComm_HEADER
#define BoardSupportComm_HEADER

#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <DUNE/DUNE.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <json/json.h>
#include <thread>
#include <chrono>

#define CRC_COMPUTE_START_POS 5
#define CRC_COMPUTE_SUM_SIZE 13

struct AUVEmbedded
{
  uint16_t header;    //1:[0,1]
  uint16_t count;     //2:[2,3]
  uint8_t size;       //3:[4]
  uint8_t drive_mode;       //4:[5]
  uint16_t height;       //5:[6,7]
  uint16_t depth;       //6:[8,9]
  uint16_t yaw;       //7:[10,11]
  int16_t pitch;       //8:[12,13]
  int16_t roll;       //9:[14,15]
  int16_t ins_vx;       //10:[16,17]
  int16_t ins_vy;       //11:[18,19]
  int16_t ins_vz;       //12:[20,21]
  int32_t lon;       //13:[22,23,24,25]
  int32_t lat;       //14:[26,27,28,29]
  int16_t alt;       //15:[30,31]
  int16_t dvl_vx;       //16:[32,33]
  int16_t dvl_vy;       //17:[34,35]
  int16_t dvl_vz;       //18:[36,37]
  int16_t rpm;       //19:[38,39]
  uint8_t light_enable;       //20:[40]
  uint16_t battery_voltage;       //21:[41,42]
  uint8_t battery_level;       //22:[43]
  uint16_t battery_temp;       //23:[44,45]
  uint32_t fault_leakSensor;       //24:[46,47,48,49]
  uint8_t fault_battery;       //25:[50] 
  uint8_t fault_emergencyBattery;       //26:[51]
  uint8_t fault_thrust;       //27:[52]
  uint8_t iridium;       //28:[53]
  uint8_t throwing_load;       //29:[54]
  uint8_t dvl_status;       //30:[55]
  uint8_t crc;       //31:[56]
  uint16_t footer;       //32:[57,58]

  float refLon;
  float refLat;
  float refAlt;  
  float north;
  float east;
};

struct AUVExecuteCommand
{
  uint16_t header;    //1:[0,1]
  uint16_t count;     //2:[2,3]
  uint8_t size;       //3:[4]
  uint8_t drive_mode;  //4:[5]
  uint8_t thrust;       //5:[6]
  uint16_t yaw;       //6:[7,8]
  uint16_t depth;       //7:[9,10]    
  uint8_t helm_top_angle;   //8:[11]  
  uint8_t helm_bottom_angle;//9:[12]
  uint8_t helm_left_angle;  //10:[13]
  uint8_t helm_right_angle;//11:[14]
  uint8_t light_enable;  //12:[15]
  uint8_t dvl_enable;     //13:[16]
  uint8_t throwing_load_enable;    //14:[17]
  uint8_t crc;                     //15:[18]
  uint16_t footer;       //16:[19,20]
  bool manual_mode;
};

class BoardSupportComm : public AppCastingMOOSApp
{
 public:
   BoardSupportComm();
   ~BoardSupportComm();

protected:
  bool OnNewMail(MOOSMSG_LIST &NewMail);
  bool Iterate();
  bool OnConnectToServer();
  bool OnStartUp();
  void RegisterVariables();
  bool buildReport();

private:

  void ConvertLLAToENU(std::vector<double> init_lla, 
                            std::vector<double> point_lla, 
                            std::vector<double>& point_enu);
  void ConvertENUToLLA(std::vector<double> init_lla, 
                              std::vector<double> point_enu,
                              std::vector<double> &point_lla);
  void ConvertLLAToNED(std::vector<double> init_lla, 
                            std::vector<double> point_lla, 
                            std::vector<double>& point_ned);
  void ConvertNEDToLLA(std::vector<double> init_lla, 
                            std::vector<double> point_ned,
                            std::vector<double> &point_lla);

  void tcpProcessThread();
  std::string convertEmbeddedFormat(Json::Value &embeddedState);  
  int tcpSockFD;
  int tcpConnectRet;
  char tcpReceiveBuffer[65535];  
  unsigned char tcpSendBuffer[65535];   
  int parseMessage(unsigned char* buffer, int size);
  int serializeMessage(unsigned char* buffer);

  template <typename Type> inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length);
  template <typename Type> inline uint16_t serialize(const Type t, uint8_t* bfr, uint16_t& sumSize);
  uint8_t convertIntToUchar(int src, int min, int max);

  struct AUVEmbedded embeddedInfo;
  struct AUVExecuteCommand executeCommand;

  std::string llaOriginPath;
  int recvLen;
  std::string recvContent;
  std::string castLogStream;

};

#endif