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

#include <iterator>
#include "MBUtils.h"
#include "ACTable.h"
#include "AUV150.h"
#include <iomanip>
#include <string>
#include <iostream>
#include <arpa/inet.h>
// #define DEBUG_

using namespace std;
// until函数
/**
 * @brief 解析int16数值，除以100并根据高位判断正负
 * @param value 待解析的int16数值
 * @return 解析后的浮点数值
 */
float parseInt16WithSign(uint16_t value) {
    // 检查最高位是否为1(负数)
    if (value >= 32768) {
        return -((value-32768));
    }
    // 正数
    return value;
}

/**
 * @brief 解析大端序的度+分.分格式坐标 (无符号版本)
 * @param rawValue 原始读取的 uint32_t 数据 (已经过 ntohl 转换，即主机字节序)
 * @return double 十进制度数
 */
double parseInsCoordinate(uint32_t rawValue) {
    // rawValue 已经通过 ntohl 从大端序转换为主机字节序
    // 所以不需要再进行字节交换
    
    // --- 步骤 2: 分离度和分 ---
    const uint32_t SCALE_DEG = 1000000; // 10^6
    const double SCALE_MIN = 10000.0;   // 10^4
    
    uint32_t degrees = rawValue / SCALE_DEG;       // 获取整数度
    uint32_t minRaw = rawValue % SCALE_DEG;        // 获取分的整数部分 (MM.MMMM)

    // --- 步骤 3 & 4: 计算最终度数 ---
    // 1. 还原分为浮点数 (例如 520630 -> 52.0630)
    double actualMinutes = static_cast<double>(minRaw) / SCALE_MIN;
    
    // 2. 分转度并累加
    double result = static_cast<double>(degrees) + (actualMinutes / 60.0);

    return result;
}
/**
 * @brief 将double编码为uint8，第一位为符号位
 * @param value 要编码的double值
 * @param limit 限幅值，输入将被限制在[-limit, limit]范围内
 * @return 编码后的uint8值，第一位为符号位(0正1负)，后7位为数值
 */
uint8_t encodeDoubleToUint8WithSign(double value, double limit) {
    // 限幅处理
    if (value > limit) value = limit;
    if (value < -limit) value = -limit;
    
    // 直接取整数值
    uint8_t magnitude = static_cast<uint8_t>(round(fabs(value)));
    
    // 确保不会超过127
    magnitude = magnitude > 127 ? 127 : magnitude;
    
    // 设置符号位
    if (value < 0) {
        magnitude |= 0x80; // 设置最高位为1表示负数
    }
    
    return magnitude;
}
static uint8_t calculateCRC(const FeedbackFrame_150AUV& frame) {
    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navModeFb);
    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.dvlStatus) + sizeof(frame.dvlStatus);
    
    uint8_t crc = 0;
    for (const uint8_t* p = start; p < end; ++p) {
        crc += *p;
    }
    return crc;
}
static uint8_t calculateCRC(const CommandFrame_150AUV& frame) {
    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navMode);
    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.payloadCtrl) + sizeof(frame.payloadCtrl);
    
    uint8_t crc = 0;
    for (const uint8_t* p = start; p < end; ++p) {
        crc += *p;
    }
    return crc;
}
void convertToHostByteOrder(FeedbackFrame_150AUV& frame) {
    frame.frameHeader = ntohs(frame.frameHeader);
    frame.counter = ntohs(frame.counter);
    frame.altimeterHeight = ntohs(frame.altimeterHeight);
    frame.depthSensor = ntohs(frame.depthSensor);
    frame.trueHeading = ntohs(frame.trueHeading);
    frame.pitch = ntohs(frame.pitch);
    frame.roll = ntohs(frame.roll);
    frame.velocityEast = ntohs(frame.velocityEast);
    frame.velocityNorth = ntohs(frame.velocityNorth);
    frame.velocityDown = ntohs(frame.velocityDown);
    frame.insLongitude = ntohl(frame.insLongitude);
    frame.insLatitude = ntohl(frame.insLatitude);
    frame.insAltitude = ntohs(frame.insAltitude);
    frame.dvlVelX = ntohs(frame.dvlVelX);
    frame.dvlVelY = ntohs(frame.dvlVelY);
    frame.dvlVelZ = ntohs(frame.dvlVelZ);
    frame.thrusterRPM = ntohs(frame.thrusterRPM);
    frame.batteryVoltage = ntohs(frame.batteryVoltage);
    frame.batteryTemp = ntohs(frame.batteryTemp);
    frame.leakStatus = ntohl(frame.leakStatus);
    frame.frameTail = ntohs(frame.frameTail);
}
void convertCommandFrameToHostByteOrder(CommandFrame_150AUV& frame) {
    frame.frameHeader = ntohs(frame.frameHeader);
    frame.counter = ntohs(frame.counter);
    frame.heading = ntohs(frame.heading);
    frame.depth = ntohs(frame.depth);
    frame.frameTail = ntohs(frame.frameTail);
}

// 添加静态成员定义
Controler AUV150::m_control;

static bool TCP_ReadLoop(void *p)
{
    AUV150 *pAUV150 = (AUV150 *)p;
    return pAUV150->ListenLoop();
}

static bool TCP_WriteLoop(void *p)
{
    AUV150 *pAUV150 = (AUV150 *)p;
    return pAUV150->WriteLoop();
}

//---------------------------------------------------------
// Constructor()

AUV150::AUV150() : m_sqlite("AUV150.db")
{
    if (!m_sqlite.createTable()) {
        std::cerr << "Failed to create database tables" << std::endl;
    }
    m_TcpSocket = new XPCTcpSocket(7007L);
    m_real_read_freq = 0;
    m_real_write_freq = 0;
    m_counter = 0;
    m_bConnected = false;
    m_status = {0};
    m_faultCodes.clear();
    m_pos_x = 0;
    m_pos_y = 0;
    m_OriginLatitude = 39.9042;
    m_OriginLongitude = 116.4074;
    m_controlGap = 0;
    m_desiredVarTime = 0;
    m_controlTime = 0;
    m_samplingTime = 0;

    m_overrived = true; //手动控制
    m_control.initialize();
}

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

AUV150::~AUV150()
{
    if(m_TcpSocket)
    {
        m_TcpSocket->vCloseSocket();
    }
    delete m_TcpSocket;
}

//---------------------------------------------------------
// Procedure: OnNewMail()

bool AUV150::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();
#if 0 // Keep these around just for template
    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();
#endif
    if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE") || (key == "MOOS_MANUAL_OVERRID_AUV150"))
    {
        string sval = msg.GetString();
        m_debug.push_back(m_overrived);
        m_debug_str.push_back(sval);
        if(sval == "false")
        {
            m_overrived = false;
        }
        else
        {
            m_overrived = true;
        }
    }
    else if(key == "DESIRED_HEADING")
    {
        m_control.rtU.heading_cmd = msg.GetDouble();
    }
    else if(key == "DESIRED_SPEED")
    {
        m_control.rtU.speed_cmd = msg.GetDouble();
    }
    else if(key == "DESIRED_DEPTH")
    {
        m_control.rtU.depth_cmd = msg.GetDouble();
    }
    else if(key == "PZ")
    {
        m_debug_str.push_back("PZ");
        m_CommandFrame.payloadCtrl = 1;
        MOOSPause(1000);
        m_CommandFrame.payloadCtrl = 0;
    }
    else if(key == "LED")
    {
        m_debug_str.push_back("LED" + msg.GetString());
        if(m_CommandFrame.led == 1)
        {
            m_CommandFrame.led = 0x00;
        }
        else
        {
            m_CommandFrame.led = 0x01;
        }
    }
    else if(key != "APPCAST_REQ") 
       reportRunWarning("Unhandled Mail: " + key);
   }
	
   return(true);
}

//---------------------------------------------------------
// Procedure: OnConnectToServer()

bool AUV150::OnConnectToServer()
{
   registerVariables();
   return(true);
}

//---------------------------------------------------------
// Procedure: Iterate()
//            happens AppTick times per second

bool AUV150::Iterate()
{
    AppCastingMOOSApp::Iterate();
    // Do your thing here!
    #ifdef DEBUG_
        m_status.insLatitude += 0.000001;
        m_status.insLongitude += 0.000001;
        m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
        postStatusUpdate("NAV");
    #endif
    m_sqlite.insertData(m_status);
    m_sqlite.insertData(m_CommandFrame);
    m_sqlite.insertData(m_FeedbackFrame);
    // 发送状态

    // Notify individual variables for easier parsing
    Notify("rtU_u", m_control.rtU.u);
    Notify("rtU_v", m_control.rtU.v);
    Notify("rtU_w", m_control.rtU.w);
    Notify("rtU_phi", m_control.rtU.phi);
    Notify("rtU_theta", m_control.rtU.theta);
    Notify("rtU_psi", m_control.rtU.psi);
    Notify("rtU_x", m_control.rtU.x);
    Notify("rtU_y", m_control.rtU.y);
    Notify("rtU_z", m_control.rtU.z);
    Notify("rtU_dx", m_control.rtU.dx);
    Notify("rtU_dy", m_control.rtU.dy);
    Notify("rtU_dz", m_control.rtU.dz);
    Notify("rtU_speed_cmd", m_control.rtU.speed_cmd);
    Notify("rtU_heading_cmd", m_control.rtU.heading_cmd);
    Notify("rtU_depth_cmd", m_control.rtU.depth_cmd);

    Notify("rtY_MainThrusterSpeedCmd", m_control.rtY.MainThrusterSpeedCmd);
    Notify("rtY_DirectUpperRudderServoAngleCmd", m_control.rtY.DirectUpperRudderServoAngleCmd);
    Notify("rtY_DirectLowerRudderServoAngleCmd", m_control.rtY.DirectLowerRudderServoAngleCmd);
    Notify("rtY_DirectLeftRudderServoAngleCmd", m_control.rtY.DirectLeftRudderServoAngleCmd);
    Notify("rtY_DirectRightRudderServoAngleCmd", m_control.rtY.DirectRightRudderServoAngleCmd);
    Notify("rtY_heading_error", m_control.rtY.heading_error);
    Notify("rtY_pitch_error", m_control.rtY.pitch_error);
    Notify("rtY_depth_error", m_control.rtY.depth_error);

    AppCastingMOOSApp::PostReport();
    return(true);
}

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

bool AUV150::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();

  STRING_LIST sParams;
  m_MissionReader.EnableVerbatimQuoting(false);
  if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
    reportConfigWarning("No config block found for " + GetAppName());

  STRING_LIST::iterator p;
  for(p=sParams.begin(); p!=sParams.end(); p++) {
    string orig  = *p;
    string line  = *p;
    string param = tolower(biteStringX(line, '='));
    string value = line;

    bool handled = false;
    if(param == "foo") {
      handled = true;
    }
    else if(param == "bar") {
      handled = true;
    }
    else if(param == "server_host")
    {
        setServerHost(value);
        handled = true;
    }
    else if(param == "latorigin")
    {
        m_OriginLatitude = stod(value);
        handled = true;
    }
    else if(param == "longorigin")
    {
        m_OriginLongitude = stod(value);
        handled = true;
    }
    else if(param == "controlfrequency")
    {
        m_controlCycle = 1.0 / stod(value);
        handled = true;
    }
    if(!handled)
      reportUnhandledConfigWarning(orig);
  }
  m_geo_ok = m_geodesy.Initialise(m_OriginLatitude, m_OriginLongitude);
  Start();
  registerVariables();
#ifdef DEBUG_
    m_status.insLatitude = m_OriginLatitude;
    m_status.insLongitude = m_OriginLongitude;
#endif
  return(true);
}

//---------------------------------------------------------
// Procedure: registerVariables()

void AUV150::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  Register("DESIRED_HEADING", 0);
  Register("DESIRED_SPEED", 0);
  Register("DESIRED_DEPTH", 0);
  Register("MOOS_MANUAL_OVERIDE", 0);
  Register("MOOS_MANUAL_OVERRIDE", 0);
  Register("MOOS_MANUAL_OVERIDE*", 0);
  Register("MOOS_MANUAL_OVERRIDE*", 0);
  Register("PZ", 0);
  Register("LED", 0);
}

bool AUV150::Start()
{
    //初始化TcpSocket
    try
    {
        m_TcpSocket->vSetRecieveBuf(m_nReceiveBufferSizeKB * 1024);
        m_TcpSocket->vSetSendBuf(m_nSendBufferSizeKB * 1024);
    }
    catch (XPCException & e)
    {
        std::cerr << "there was trouble configuring socket buffers: "
                << e.sGetException() << "\n";
    }

    //启动线程
    WritingThread_.Initialise(TCP_ReadLoop,this);
    WritingThread_.Start();
    ReadingThread_.Initialise(TCP_WriteLoop,this);
    ReadingThread_.Start();
    return true;
}
void AUV150::setServerHost(std::string serverHost)
{
    m_serverHost = serverHost;
}

bool AUV150::ListenLoop()
{
    double start_time = MOOSTime();
    int message_count = 0;
    
    while(!ReadingThread_.IsQuitRequested())
    {
        char buffer[sizeof(FeedbackFrame_150AUV)] = {0};
        try
        {
            if((m_TcpSocket!=NULL) && m_bConnected)
            {
                int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),10);
                if(count > 0) {
                    std::cout << "Received " << count << " bytes" << std::endl;
                    FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer);
                    convertToHostByteOrder(*p);
                    m_FeedbackFrame = *p;
                    // 处理接受到的数据
                    if(p->frameHeader != 0xEBA1)
                    {
                        std::cout << "Received data is not a valid frame" << std::endl;
                        m_faultCodes.insert(0x11);
                        continue;
                    }
                    if(p->crc != calculateCRC(*p))
                    {
                        std::cout << "Received data is not a valid frame" << std::endl;
                        m_faultCodes.insert(0x14); // 0x14 校验错误
                        continue;
                    }
                    // 解析数据
                    m_counter = p->counter;
                    updateStatus(*p);
                    postStatusUpdate("NAV");
                    if(!m_overrived)
                    {
                        // 计算控制量
                        m_control.rtU.u = m_status.dvlVelX;
                        m_control.rtU.v = m_status.dvlVelY;
                        m_control.rtU.w = m_status.dvlVelZ;
                        m_control.rtU.dx = m_status.velocityEast;
                        m_control.rtU.dy = m_status.velocityNorth;
                        m_control.rtU.dz = m_status.velocityDown;
                        m_control.rtU.phi = m_status.roll;
                        m_control.rtU.psi = (m_status.trueHeading);
                        m_control.rtU.theta = (m_status.pitch);
                        m_control.rtU.x = m_status.x;
                        m_control.rtU.y = m_status.y;
                        m_control.rtU.z = m_status.z;
                        m_control.step();
                        m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(-m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLowerRudderServoAngleCmd,30);
                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLeftRudderServoAngleCmd,30);
                        m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(-m_control.rtY.DirectRightRudderServoAngleCmd,30);
                        m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100);  
                        m_status.depth_error = m_control.rtY.depth_error;
                        m_status.heading_error = m_control.rtY.heading_error;
                        m_status.pitch_error = m_control.rtY.pitch_error;
                    }
                    else
                    {
                        m_CommandFrame.rudderUp = 0; 
                        m_CommandFrame.rudderDown = 0;
                        m_CommandFrame.rudderLeft = 0;
                        m_CommandFrame.rudderRight = 0;
                        m_CommandFrame.mainThruster = 0;   
                    }
            }
            try
            {   
                m_CommandFrame.crc = calculateCRC(m_CommandFrame);

                convertCommandFrameToHostByteOrder(m_CommandFrame);
                m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
                m_controlGap =  MOOSTime() - m_desiredVarTime;
                m_controlTime = MOOSTime();
                m_bConnected = true;
            }
            catch(XPCException &e)
            {
                m_bConnected = false;
                std::cerr << "there was trouble sending message: "
                        << e.sGetException() << "\n";
            }

            // 计算控制频率
            m_samplingTime = MOOSTime();
            message_count++;
            double elapsed_time = m_samplingTime - start_time;
            if(elapsed_time >= 1.0) {  // 每秒更新一次频率
                m_real_read_freq = message_count / elapsed_time;
                message_count = 0;
                start_time = MOOSTime();
            }
            //正常情况下清除错误码
            m_faultCodes.erase(0x11);
            m_faultCodes.erase(0x12);
            m_faultCodes.erase(0x13);
        }
        else
        {
            m_faultCodes.insert(0x12); //无数据接收
        }
        }
        catch (XPCException & e)
        {
            m_faultCodes.insert(0x13);
            std::cerr << "there was trouble recieving message: "
                    << e.sGetException() << "\n";
            return false;
        }
    }
    return true;
}

bool AUV150::WriteLoop()
{   
    while(!WritingThread_.IsQuitRequested()){
        if(!m_bConnected)
        {
            m_real_read_freq = 0;
            m_real_write_freq = 0;
            try 
            {   
                if(m_TcpSocket) {
                    m_TcpSocket->vCloseSocket();
                    delete m_TcpSocket;
                }
                // m_TcpSocket->vCloseSocket();
                m_TcpSocket = new XPCTcpSocket(7007L);
                m_TcpSocket->vSetRecieveBuf(m_nReceiveBufferSizeKB * 1024);
                m_TcpSocket->vSetSendBuf(m_nSendBufferSizeKB * 1024);
                m_TcpSocket->vConnect(m_serverHost.c_str());
                m_bConnected = true;
            } catch(XPCException &e) 
            {
                std::cerr << "连接失败: " << e.sGetException() << std::endl;
                MOOSTime(1.0);
            }
        }
    }
    return true;
}

bool AUV150::buildReport()
{
    auto displayWidth = [&](const std::string& str) {
        std::mbstate_t state;
        std::memset(&state, 0, sizeof(state));
        const char* ptr = str.c_str();
        size_t remaining = str.size();
        int count = 0;
        while (remaining > 0) {
            wchar_t wc;
            size_t ret = std::mbrtowc(&wc, ptr, remaining, &state);
            if (ret == (size_t)-1 || ret == (size_t)-2 || ret == 0) {
                // invalid or null or incomplete, treat a byte as a char
                ptr++;
                remaining--;
            } else {
                ptr += ret;
                remaining -= ret;
            }
            count++;
        }
        return count;
    };

    // Helper to center text within a given display width
    auto centerText = [&](const std::string& text, int width) {
        int len = displayWidth(text);
        int pad = width - len;
        int padLeft = pad / 2;
        int padRight = pad - padLeft;
        return std::string(padLeft, ' ') + text + std::string(padRight, ' ');
    };

    // Helper to print a formatted table
    auto printTable = [&](const std::vector<std::string>& headers,
                          const std::vector<std::string>& values)
    {
        size_t cols = headers.size();
        std::vector<int> colWidths(cols);
        // Compute max display width per column and add padding
        for (size_t i = 0; i < cols; ++i) {
            int h = displayWidth(headers[i]);
            int v = displayWidth(values[i]);
            colWidths[i] = std::max(h, v) + 2;
        }

        std::ostringstream oss;
        // Header row
        for (size_t i = 0; i < cols; ++i) {
            oss << centerText(headers[i], colWidths[i]);
            if (i + 1 < cols) oss << " | ";
        }
        m_msgs << oss.str() << std::endl;

        // Divider row
        oss.str(""); oss.clear();
        for (size_t i = 0; i < cols; ++i) {
            oss << std::string(colWidths[i], '-');
            if (i + 1 < cols) oss << " | ";
        }
        m_msgs << oss.str() << std::endl;

        // Value row
        oss.str(""); oss.clear();
        for (size_t i = 0; i < cols; ++i) {
            oss << centerText(values[i], colWidths[i]);
            if (i + 1 < cols) oss << " | ";
        }
        m_msgs << oss.str() << std::endl << std::endl;
    };

    //=================DEGUB==========================
    m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl;
    m_msgs << "DirectUpperRudderServoAngleCmd : " << m_control.rtY.DirectUpperRudderServoAngleCmd << std::endl;
    m_msgs << "DirectLowerRudderServoAngleCmd : " << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl;
    m_msgs << "DirectLeftRudderServoAngleCmd  : " << m_control.rtY.DirectLeftRudderServoAngleCmd << std::endl;
    m_msgs << "DirectRightRudderServoAngleCmd : " << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl;
    m_msgs << "mainThruster                   : " << m_control.rtY.MainThrusterSpeedCmd << std::endl;
    for (const auto& debug_value : m_debug)
    {
        m_msgs << "debug_value: " << debug_value << std::endl;
    }
    m_debug.clear();
    for (const auto& debug_str : m_debug_str)
    {
        m_msgs << debug_str << std::endl;
    }
    m_debug_str.clear();
    // =============== Navigation Info ===============
      printTable(
      {"Connect", "Fre","Cont" ,"ip"},
      {
          (isConnected() ? "Yes" : "No"),
          doubleToString(getRealReadFreq(), 4) + " Hz",
          ulintToString(getCounter()),
          m_serverHost,
        //   doubleToString(m_OriginLatitude, 5) + "deg",
        //   doubleToString(m_OriginLongitude, 5) + "deg",
        //   doubleToString(m_controlGap, 5) + "s"
      }
    );
    m_msgs << "Fault Codes: ";
    for (uint8_t code : getFaultCodes()) {
        m_msgs << uintToString(code) + " ";
    }
    m_msgs << std::endl;
    m_msgs << "Navigation Mode  : " << uintToString(m_status.navModeFb) << std::endl;
    // =============== Heading & Attitude ===============
    printTable(
        {"Heading", "Pitch", "Roll", "X", "Y", "Z", "H"},
        {
            doubleToString(m_status.trueHeading, 2) + "deg",
            doubleToString(m_status.pitch, 2) + "deg",
            doubleToString(m_status.roll, 2) + "deg",
            doubleToString(m_pos_x, 4) + "m",
            doubleToString(m_pos_y, 4) + "m",
            doubleToString(m_status.depthSensor, 2) + "m",
            doubleToString(m_status.altimeterHeight, 2) + "m"
        }
    );
    m_msgs << "---------------INS------------------\n";

    // =============== Velocity Info ===============
    printTable(
    {"Lat", "Lon", "Alt","East Vel","North Vel","Down Vel"},
    {
        doubleToString(m_status.insLatitude, 4) + "deg",
        doubleToString(m_status.insLongitude, 4) + "deg",
        doubleToString(m_status.insAltitude, 2) + "m",
        doubleToString(m_status.velocityEast, 2) + "m/s",
        doubleToString(m_status.velocityNorth, 2) + "m/s",
        doubleToString(m_status.velocityDown, 2) + "m/s"
    }
    );
    m_msgs << "----------------DVL-----------------\n";
    printTable(
        {"DVL X Vel", "DVL Y Vel", "DVL Z Vel"},
        {
            doubleToString(m_status.dvlVelX, 2) + "m/s",
            doubleToString(m_status.dvlVelY, 2) + "m/s",
            doubleToString(m_status.dvlVelZ, 2) + "m/s"
        }
    );
    m_msgs << "---------------------------------\n";

    // =============== Propulsion System ===============
    printTable(
        {"Thruster RPM", "Thruster CMD" ,"up", "down", "left", "right"},
        {
            doubleToString(m_status.thrusterRPM, 1) + "rpm",
            doubleToString(m_CommandFrame.mainThruster, 1) + "%",
            doubleToString(m_CommandFrame.rudderUp, 1) + "deg",
            doubleToString(m_CommandFrame.rudderDown, 1) + "deg",
            doubleToString(m_CommandFrame.rudderLeft, 1) + "deg",
            doubleToString(m_CommandFrame.rudderRight, 1) + "deg",
        }
    );
    // m_msgs << "---------------------------------\n";

    // =============== Power System ===============
    printTable(
        {"Battery Voltage", "Battery Level", "Battery Temperature"},
        {
            doubleToString(m_status.batteryVoltage, 2) + "V",
            uintToString(m_status.batteryLevel) + "%",
            doubleToString(m_status.batteryTemp, 1) + "°C"
        }
    );
    m_msgs << "---------------------------------\n";

    // =============== System Status ===============
    printTable(
        {"Light", "Leak", "CtlFlg" , "Emergency power" , "PZ", "DVL","Thruster Status"},
        {
            m_status.ledSwitch ? "ON" : "OFF",
            m_status.leakStatus ? "LEAK" : "NORMAL",
            uintToString(m_status.powerModule),
            uintToString(m_status.backupPower),
            m_status.payloadStatus ?"DROPPED" : "NORMAL",
            m_status.dvlStatus?"ON" : "OFF",
            uintToString(m_status.thrusterStatus)
        }
    );
    // m_msgs << "---------------------------------\n";

    return true;
}

bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
{
    m_status.navModeFb = feedbackFrame.navModeFb;
    m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0f;
    m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
    m_status.trueHeading = feedbackFrame.trueHeading / 10.0f;

    // m_status.pitch = feedbackFrame.pitch / 10.0f;
    m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
    m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f;
    m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
    m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
    m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
    m_status.insLongitude = parseInsCoordinate(feedbackFrame.insLongitude);
    m_status.insLatitude = parseInsCoordinate(feedbackFrame.insLatitude);
    m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
    m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
    m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;
    m_status.dvlVelZ = parseInt16WithSign(feedbackFrame.dvlVelZ) / 100.0f;
    m_status.thrusterRPM = feedbackFrame.thrusterRPM;
    m_status.ledSwitch = feedbackFrame.ledSwitch;
    m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;
    m_status.batteryLevel = feedbackFrame.batteryLevel;
    m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0f;
    m_status.leakStatus = feedbackFrame.leakStatus;
    m_status.powerModule = feedbackFrame.powerModule;
    m_status.backupPower = feedbackFrame.backupPower;
    m_status.thrusterStatus = feedbackFrame.thrusterStatus;
    m_status.reserved = feedbackFrame.reserved;
    m_status.payloadStatus = feedbackFrame.payloadStatus;
    m_status.dvlStatus = feedbackFrame.dvlStatus;

    m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
    // 以下为转换的数据
    m_status.x = m_pos_x;
    m_status.y = m_pos_y;
    m_status.z = m_status.depthSensor;
    return true;
}

void AUV150::postStatusUpdate(std::string prefix)
{
    m_curr_time = MOOSTime();
    
    // 发布位置信息
    Notify(prefix+"_X", m_pos_x, m_curr_time);
    Notify(prefix+"_Y", m_pos_y, m_curr_time);
    
    if(m_geo_ok) {
        double lat, lon;
        m_geodesy.LocalGrid2LatLong(m_pos_x, m_pos_y, lat, lon);
        Notify(prefix+"_LAT", lat, m_curr_time);
        Notify(prefix+"_LONG", lon, m_curr_time);
    }

    // 发布运动状态
    Notify(prefix+"_HEADING", m_status.trueHeading, m_curr_time);
    Notify(prefix+"_SPEED", m_status.dvlVelX, m_curr_time); 
    Notify(prefix+"_DEPTH", m_status.depthSensor, m_curr_time);
    
    // 发布姿态信息
    Notify(prefix+"_Z", -m_status.depthSensor, m_curr_time);
    Notify(prefix+"_PITCH", m_status.pitch, m_curr_time);
    Notify(prefix+"_YAW", m_status.trueHeading*3.1415926/180, m_curr_time);
    Notify("TRUE_X", m_pos_x, m_curr_time);
    Notify("TRUE_Y", m_pos_y, m_curr_time);

    // 发布对地速度
    Notify(prefix+"_HEADING_OVER_GROUND", m_status.trueHeading, m_curr_time);
    Notify(prefix+"_SPEED_OVER_GROUND", m_status.dvlVelX, m_curr_time);
    
    // 发布高度信息
    if(m_status.insAltitude > 0) {
        Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
    }

    // 发布AUV信息
    prefix = "AUV150";
    Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time);
    Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time);
    Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time);

    Notify(prefix+"_DESIRED_RUDDER",m_status.desired_rudder);
    Notify(prefix+"_DESIRED_EVEVATOR",m_status.desired_elevator);
    Notify(prefix+"_DESIRED_THRUST",m_status.desired_thrust);
}
