/************************************************************/
/*    NAME:                                               */
/*    ORGN: MIT, Cambridge MA                               */
/*    FILE: AUV150.h                                          */
/*    DATE: December 29th, 1963                             */
/************************************************************/

#ifndef AUV150_HEADER
#define AUV150_HEADER
#define UNIX
// #include "AUV150_Tcp.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include "MOOS/libMOOS/Comms/XPCTcpSocket.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOSGeodesy/MOOSGeodesy.h"
#include "Controler/Controler.h"
#include "AngleUtils.h"
#include "sqlit3/SQLite.h"

#include <iostream>
#include <string>
#include <vector>
#include <set>
#pragma pack(push, 1)

struct CommandFrame_150AUV {
    uint16_t frameHeader = 0xEBA2;     // 1. 帧头       @0   Uint16  单位:EBA2      固定值 0xEBA2
    uint16_t counter;         // 2. 计数值     @2   Uint16  单位:—        0~65535，满后重置
    uint8_t  dataLength;      // 3. 数据长度   @4   Uint8   单位:字节      从“航行模式设置”到“控制抛载”的字节长度
    uint8_t  navMode;         // 4. 航行模式设置 @5   Uint8   单位:—        0xFF: 默认基本控制响应  
                              //                                 0x02: 客户直接控制推进器、舵机
    uint8_t  mainThruster;    // 5. 主推进器航速指令 @6 Uint8 单位:占空比    首位符号位，-100~100 (%)
    uint16_t heading;         // 6. 航向指令   @7   Uint16  单位:0.1°       无符号，0~3600 对应 0~360°
    uint16_t depth;           // 7. 深度指令   @9   Uint16  单位:0.1m       无符号，0~500 对应 0~50.0m
    uint8_t  rudderUp;        // 8. 直操上舵机角度指令 @11 Uint8 单位:占空比 首位符号位，-30~30 (°)
    uint8_t  rudderDown;      // 9. 直操下舵机角度指令 @12 Uint8 单位:占空比 首位符号位，-30~30 (°)
    uint8_t  rudderLeft;      // 10. 直操左舵机角度指令 @13 Uint8 单位:占空比 首位符号位，-30~30 (°)
    uint8_t  rudderRight;     // 11. 直操右舵机角度指令 @14 Uint8 单位:占空比 首位符号位，-30~30 (°)
    uint8_t  led = 0x01;             // 12. LED灯      @15  Uint8   单位:—        0x00: 关，0x01: 开
    uint8_t  dvlSwitch;       // 13. 传感器开关:DVL开关 @16 Uint8 单位:—   0xFF: 默认，0x00: 关，0x01: 开
    uint8_t  payloadCtrl;     // 14. 控制抛载  @17  Uint8   单位:—        0xFF: 默认，0x01: 执行抛载
    uint8_t  crc;             // 15. CRC校验位 @18  Uint8   单位:—        对“航行模式”到“控制抛载”区间进行求和校验
    uint16_t frameTail = 0xEE2A;       // 16. 帧尾      @19  Uint16  单位:EE2A     固定值 0xEE2A
};

struct FeedbackFrame_150AUV {
    uint16_t frameHeader = 0xEBA1;    // 1. 帧头 @0   Uint16  单位:EBA1           固定值 0xEBA1
    uint16_t counter;        // 2. 计数值 @2   Uint16  单位:—             0~65535，最大后重置
    uint8_t  dataLength;     // 3. 数据长度 @4   Uint8   单位:字节         从“航行模式反馈”到“DVL传感器状态反馈”的字节长度
    uint8_t  navModeFb;      // 4. 航行模式反馈 @5   Uint8   单位:—         0xFF: 默认  
                             //                                   0x02: 直接控制（客户直接控制推进器、舵机）
    uint16_t altimeterHeight;// 5. 高度计：高度 @6   Uint16  单位:0.01m        无符号，0~100 对应 0~1.00m，取小值
    uint16_t depthSensor;    // 6. 深度计：深度 @8   Uint16  单位:0.01m        无符号，0~600 对应 0~6.00m
    uint16_t trueHeading;    // 7. 真航向 @10  Uint16  单位:0.01°        无符号，0~36000 对应 0~360°，惯导无符号
    uint16_t  pitch;          // 8. 俯仰角 @12  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
    uint16_t  roll;           // 9. 横滚角 @14  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
    uint16_t  velocityEast;   // 10. 东向速度 @16 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    uint16_t  velocityNorth;  // 11. 北向速度 @18 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    uint16_t  velocityDown;   // 12. 垂向速度 @20 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    uint32_t insLongitude;   // 13. 惯导：经度 @22 Uint32  单位:°，精度:360/2^32 -180~180，惯导
    uint32_t insLatitude;    // 14. 惯导：纬度 @26 Uint32  单位:°，精度:180/2^32 -90~90，惯导
    uint16_t  insAltitude;    // 15. 惯导：高度 @30 int16   单位:0.01m        无符号，惯导
    uint16_t  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
    uint16_t  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
    uint16_t  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
    uint16_t  thrusterRPM;    // 19. 主推进器实际转速(x1) @38 int16 单位:rpm      -4000~4000，推进器
    uint8_t  ledSwitch;      // 20. 灯的开关 @40   Uint8   单位:—           0x00: 关，0x01: 开，灯
    uint16_t batteryVoltage; // 21. 电池电压反馈 @41 Uint16  单位:mV         约24000，电池状态
    uint8_t  batteryLevel;   // 22. 电池电量反馈 @43 Uint8   单位:%          0~100，电池状态
    uint16_t batteryTemp;    // 23. 电池温度反馈 @44 Uint16  单位:0.1°C       0~1000 对应 0~100.0°C，电池状态
    uint32_t leakStatus;     // 24. 漏水传感器反馈 @46 Uint32  单位:—         漏水传感器状态
    uint8_t  powerModule;    // 25. 操作标志 @50 Uint8   单位:—         操作标志 1 允许控制 0 不允许控制
    uint8_t  backupPower;    // 26. 应急电源模块状态反馈 @51 Uint8   单位:—     应急电源模块状态
    uint8_t  thrusterStatus; // 27. 推进器状态反馈 @52 Uint8   单位:—         推进器状态
    uint8_t  reserved;       // 30. 预留 @55   Uint8   单位:—         
    uint8_t  payloadStatus;  // 28. 抛载状态反馈 @53 Uint8   单位:—         0xFF: 默认，0x00: 已抛载
    uint8_t  dvlStatus;      // 29. DVL传感器状态反馈 @54 Uint8   单位:—       0x00: 关，0x01: 开
    uint8_t  crc;            // 31. CRC校验位 @56  Uint8   单位:—         从“航行模式反馈”到“DVL传感器状态反馈”区间求和校验
    uint16_t frameTail = 0xEE1A;      // 32. 帧尾 @57   Uint16  单位:EE1A        固定值 0xEE1A
};

// static_assert(sizeof(FeedbackFrame_150AUV) == 59, "FeedbackFrame size must be 59 bytes");
// static_assert(sizeof(CommandFrame_150AUV) == 21, "CommandFrame size must be 21 bytes");
#pragma pack(pop)

struct AUV150_Status
{
    unsigned char  navModeFb;      // 4. 航行模式反馈 @5   Uint8   单位:—         0xFF: 默认  
    double altimeterHeight;// 5. 高度计：高度 @6   Uint16  单位:0.01m        无符号，0~100 对应 0~1.00m，取小值
    double depthSensor;    // 6. 深度计：深度 @8   Uint16  单位:0.01m        无符号，0~600 对应 0~6.00m
    double trueHeading;    // 7. 真航向 @10  Uint16  单位:0.01°        无符号，0~36000 对应 0~360°，惯导无符号
    double  pitch;          // 8. 俯仰角 @12  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
    double  roll;           // 9. 横滚角 @14  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
    double  velocityEast;   // 10. 东向速度 @16 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    double  velocityNorth;  // 11. 北向速度 @18 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    double  velocityDown;   // 12. 垂向速度 @20 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
    double insLongitude;   // 13. 惯导：经度 @22 Uint32  单位:°，精度:360/2^32 -180~180，惯导
    double insLatitude;    // 14. 惯导：纬度 @26 Uint32  单位:°，精度:180/2^32 -90~90，惯导
    double  insAltitude;    // 15. 惯导：高度 @30 int16   单位:0.01m        无符号，惯导
    double  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
    double  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
    double  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
    double  thrusterRPM;    // 19. 主推进器实际转速(x1) @38 int16 单位:rpm      -4000~4000，推进器
    unsigned char  ledSwitch;      // 20. 灯的开关 @40   Uint8   单位:—           0x00: 关，0x01: 开，灯
    double batteryVoltage; // 21. 电池电压反馈 @41 Uint16  单位:mV         约24000，电池状态
    unsigned char  batteryLevel;   // 22. 电池电量反馈 @43 Uint8   单位:%          0~100，电池状态
    unsigned short batteryTemp;    // 23. 电池温度反馈 @44 Uint16  单位:0.1°C       0~1000 对应 0~100.0°C，电池状态
    int leakStatus;     // 24. 漏水传感器反馈 @46 Uint32  单位:—         漏水传感器状态
    unsigned char  powerModule;    // 25. 电源模块状态反馈 @50 Uint8   单位:—         电源模块状态
    unsigned char  backupPower;    // 26. 应急电源模块状态反馈 @51 Uint8   单位:—     应急电源模块状态
    unsigned char  thrusterStatus; // 27. 推进器状态反馈 @52 Uint8   单位:—         推进器状态
    unsigned char  reserved;       // 30. 预留 @55   Uint8   单位:—         
    unsigned char  payloadStatus;  // 28. 抛载状态反馈 @53 Uint8   单位:—         0xFF: 默认，0x00: 已抛载
    unsigned char dvlStatus;      // 29. DVL传感器状态反馈 @54 Uint8   单位:—       0x00: 关，0x01: 开
    //以下为转换状态
    double x;
    double y;
    double z;

    double heading_error;
    double pitch_error;
    double depth_error;

    double desired_rudder;
    double desired_elevator;
    double desired_thrust;
};

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

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

 protected: // Standard AppCastingMOOSApp function to overload 
   bool buildReport();

 protected:
   void registerVariables();

 private: // Configuration variables
  
public:
    bool Start();
    void setServerHost(std::string serverHost);
    CommandFrame_150AUV m_CommandFrame;
    FeedbackFrame_150AUV m_FeedbackFrame;
    bool ListenLoop();
    bool WriteLoop();
    bool updateStatus(FeedbackFrame_150AUV &feedbackFrame);
    bool isConnected() { return m_bConnected; }
    double getRealReadFreq() { return m_real_read_freq; }
    double getRealWriteFreq() { return m_real_write_freq; }
    unsigned long int getCounter() { return m_counter; }
    std::set<unsigned char> getFaultCodes() { return m_faultCodes; }
    
    // 状态发布函数
    void postStatusUpdate(std::string prefix);

    //DEBUG
    std::vector<uint16_T> m_debug;
    std::vector<std::string> m_debug_str;

private: // State variables
  XPCTcpSocket* m_TcpSocket;
  CMOOSThread WritingThread_; 
  CMOOSThread ReadingThread_; 
  AUV150_Status m_status;
  std::string m_serverHost;
  int m_nReceiveBufferSizeKB = 1024;
  int m_nSendBufferSizeKB = 1024;
  double m_real_read_freq;
  double m_real_write_freq;
  bool m_bConnected;
  std::set<unsigned char> m_faultCodes;
  unsigned long int m_counter;

  SQLite m_sqlite;
  

  //导航相关
  CMOOSGeodesy m_geodesy;
  bool m_geo_ok;
  double m_pos_x;
  double m_pos_y;
  double m_OriginLatitude;
  double m_OriginLongitude;
  double m_curr_time;  // 当前时间戳

  //控制相关
  double m_controlGap;
  double m_desiredVarTime;
  double m_controlCycle;

  double m_controlTime;
  double m_samplingTime;

  bool m_overrived;
  
  //控制器
  static Controler m_control;
};

#endif
