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

#ifndef SurfaceSupportComm_HEADER
#define SurfaceSupportComm_HEADER

#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <arpa/inet.h>


#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 "PeriodicUDPEvent.h"

struct Landmark {
	float lon;
	float lat;
	float depth;
	float speed;
};

struct EstimatedState {
  float referenceLon;
  float referenceLat;
  float referenceAltitude;  
  float currentLon;
  float currentLat;
  float currentAltitude;
  float offsetNorth;
  float offsetEast;
  float offsetDown;	
  float roll;
  float pitch;
  float yaw;
  float linearVelocityNorth;
  float linearVelocityEast;
  float linearVelocityDown;
  float height;	        
  float depth;
};

struct DeviceStatus {
  unsigned int batteryVoltage; 
  unsigned int batteryLevel;
  float batteryTemp;
  unsigned int controllerTemp;
  int thrustRpm;
  unsigned int lightEnable;
  unsigned int throwingLoadEnable;
  unsigned int dvlStatus;
};


class SurfaceSupportComm : public CMOOSApp
// class SurfaceSupportComm : public AppCastingMOOSApp
{
public:
  SurfaceSupportComm();
  ~SurfaceSupportComm();

protected: // Standard MOOSApp functions to overload  
  bool OnNewMail(MOOSMSG_LIST &NewMail);
  bool Iterate();
  bool OnConnectToServer();
  bool OnStartUp();
  void RegisterVariables();
  // bool buildReport();

private:
  char recv_buf[2048];
  DUNE::Network::UDPSocket sock_udp_send;
  DUNE::Network::TCPSocket sock_tcp_receive;
  DUNE::IO::Poll m_poll;
  uint16_t header_src, header_dst;
  uint8_t header_src_ent, header_dst_ent;
  std::string planConfigPath;
  struct EstimatedState estimatedState;
  struct DeviceStatus deviceStatus;
  struct Client
  {
    DUNE::Network::TCPSocket* socket; // Socket handle.
    DUNE::Network::Address address; // Client address.
    uint16_t port; // Client port.
    DUNE::IMC::Parser parser; // Parser handle
  };

  typedef std::list<Client> ClientList;
  ClientList m_clients; 

  bool udpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
  void tcpReceiveFromClient(char* buf, unsigned int cap, double timeout);
  void tcpSendToClient(char* buf, unsigned int cap);  
  void acceptNewClient(void);
  void handleClients(char* buf, unsigned int cap);
  void closeConnection(Client& c, std::exception& e);
  void processMessage(DUNE::IMC::Message * message);
  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 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 BatchConvertCoordinate(Json::Value &object);
  void ParsePlanConfig(Json::Value inputJsonValue);
  int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value);   
  int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
  template <typename Type> 
  inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value);

  int testFlag = 0;
  double TimeLast;
  int sendCount = 0;

  PeriodicUDPEvent udpEvent;
  // PeriodicTCPEvent tcpEvent;
  std::string missionStatusString; 
};

#endif