/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-12 09:52:27
 * @LastEditors: zhaojingkui 1553836110@qq.com
 * @LastEditTime: 2023-11-28 11:32:48
 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
// #define DEBUG
#include"Emulator.hpp"
#include <exception>
#include"ACTable.h"

bool _ListenCB(void * pParam)
{
  Emulator* pMe = (Emulator* )pParam;
  return pMe->receiveUdpDate();    
}

bool _ListenBSC(void* pParam)
{
  _150server* pMe = (_150server*)pParam;
  return pMe->listenInfo();
}

bool _Connect(void* pParam)
{
  Emulator* pMe = (Emulator* )pParam;
  return pMe->_150Connect();
}

bool Emulator::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.m_sKey;
    string sval = msg.m_sVal;
    double dval = msg.m_dfVal;
    double dfT;
    msg.IsSkewed(m_curr_time, &dfT);
    // if(key == "DESIRED_RUDDER")
    // {
    //   sendBuf[0] = dval;
    //   remus100.rudder = dval;
    // }
    // else if(key == "DESIRED_ELEVATOR")
    // {
    //   sendBuf[1] = dval;
    //   remus100.elevator = dval;
    // }
    // else if(key == "DESIRED_THRUST")
    // {
    //   sendBuf[2] = dval;
    //   remus100.thrust = dval;
    // }
    // else 
    if(key == "DESIRED_DEPTH")
      sendBuf[3] = dval;
    else if(key == "DESIRED_HEADING")
      sendBuf[4] = dval;
    else if(key == "DESIRED_SPEED")
      sendBuf[5] = dval;
    //   reportRunWarning("UnKown var:" + key);
  }
  
  return true;
}
bool Emulator::Iterate()
{
  AppCastingMOOSApp::Iterate();
  if(_150ServerThread.IsThreadRunning())
  { // 这里需要转换数据
    if(p_150server_1._150cmd.helm_top_angle > 128)
      sendBuf[0] = -(p_150server_1._150cmd.helm_top_angle - 128);      
    else
      sendBuf[0] = p_150server_1._150cmd.helm_top_angle;   
    if(p_150server_1._150cmd.helm_right_angle > 128) 
      sendBuf[1] = -(p_150server_1._150cmd.helm_right_angle - 128);
    else
      sendBuf[1] = p_150server_1._150cmd.helm_right_angle;
    sendBuf[2] = (uint8_t)p_150server_1._150cmd.thrust * 1525 / 100;
    set150Info();
    p_150server_1.postInfo();
    isConnect = " 150 Server...";
  }
  else
    isConnect = "No 150 Connect...";
  udp->iSendMessageTo((void*)sendBuf, sendBufSize, matalb_port, matlab_host);
  //postNodeRecordUpdate(prefix, record);
  AppCastingMOOSApp::PostReport(); 
  return true;
}

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

bool Emulator::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();
//TODO: 添加初始状态设置
  STRING_LIST sParams;
  m_MissionReader.GetConfiguration(GetAppName(), sParams);
  STRING_LIST::iterator p;
  
  for(p=sParams.begin(); p!=sParams.end(); p++) 
  {
      bool unhandled = false;
      string orig  = *p;
      string line  = *p;
      string param = toupper(biteStringX(line, '='));
      string value = line;
      double dval  = atof(value.c_str());
      cout << param << endl;
      if(param == "MATLAB_HOST")
      {
        matlab_host = value;
      }
      else if(param == "MATLAB_PORT")
      {
        matalb_port = (long int)dval;
      }
      else if(param == "LOCAL_PORT")
      {
        local_port = (long int)dval;
      }
      else if(param == "PREFIX")
      {
        setNonWhiteVarOnString(prefix, value);
      }  
      else if(param == "START_X")
      {
        start_x = dval;
      }
      else if(param == "START_Y")
      {
        start_y = dval;
      }
      else if(param == "START_Z")
      {
        start_z = dval;
      }
      else if(param == "START_HEADING")
      {
        start_heading = dval;
      }
      else
        unhandled = true;
      if(unhandled)
        reportUnhandledConfigWarning(orig);
  }
  //添加UDP 设置
  //TODO：添加try
  try
  {
    udp = new XPCUdpSocket(local_port);
    udp->vSetBroadcast(true);
    udp->vBindSocket();
  }
  catch(const std::exception& e)
  {
    std::cerr << e.what() << '\n';
    reportRunWarning("Local port Open Field");
  }
  //初始化数据
  receiveBufSize = receiveDateSize*sizeof(double);
  receiveBuf = new char[receiveBufSize];
  sendBufSize = sendDataSize*sizeof(double);
  sendBuf = new double[sendDataSize];
  for(int i=0; i<sendDataSize; i++)
    sendBuf[i] = 0;
  for(int i=0; i<receiveBufSize; i++)
    receiveBuf[i] = 0;
  // sendBuf[0] = 0.1;
  // sendBuf[1] = 0.1;
  // sendBuf[2] = 1525;
  
  record.setX(start_x);
  record.setY(start_y);
  record.setDepth(start_z);
  record.setHeading(start_heading);

  receiveThread.Initialise(_ListenCB,this);
  receiveThread.Start();

  _150ConnectThread.Initialise(_Connect, this);
  _150ConnectThread.Start();
  //启动150 server
  // 
  // 
  return true;
  
}
bool Emulator::_150Connect()
{
  cout << "Connect ... " << endl;
  isConnect = "Connect...";
  p_150server_1._150_startServer();
  _150ServerThread.Initialise(_ListenBSC,&(this->p_150server_1));
  _150ServerThread.Start();
  _150ConnectThread.RequestQuit();
  isConnect = "Connected and start server";
  return true;
}
void Emulator::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  Register("DESIRED_RUDDER",0);
  Register("DESIRED_THRUST",0);
  Register("DESIRED_ELEVATOR",0);
  Register("DESIRED_DEPTH");
  Register("DESIRED_HEADING");
  Register("DESIRED_SPEED");
}
bool Emulator::buildReport()
{
  // for(int i=0; i<sendDataSize; i++)
  // {
  //   m_msgs << sendBuf[i] << "!#";
  // }
  m_msgs << "MATLAB      : " << matlab_host << " : " << matalb_port << endl;
  m_msgs << "LOCAL       : " << local_port << endl;
  m_msgs << "_150 Connect: " << isConnect << endl;
  m_msgs << "_150 Cmd Recive: " << p_150server_1._150_recive << endl;
  m_msgs << "=============================================" << endl;
  double *s = (double*)receiveBuf;
  m_msgs << "DESIRED_RUDDER   : " << sendBuf[0] << " (deg)" << endl;
  m_msgs << "DESIRED_ELEVATOR : " << sendBuf[1] << " (deg)" << endl;
  m_msgs << "DESIRED_THRUST   : " << sendBuf[2] << " (rpm)" << endl;
  
  m_msgs << "Pose  : " << "[ "  <<  doubleToStringX(s[6],1) << " , " <<  doubleToStringX(s[7],1) << " ]" << endl;
  m_msgs << "Depth : " <<  doubleToStringX(s[8],1) << " (m)" << endl;

  m_msgs << "u     : " << doubleToStringX(s[0],2) << " (m/s)" << endl;
  m_msgs << "v     : " << doubleToStringX(s[1],2) << " (m/s)" << endl;
  m_msgs << "w     : " << doubleToStringX(s[2],2) << " (m/s)" << endl;

  m_msgs << "p     : " << doubleToStringX(s[3],1) << " (deg/s)" << endl;
  m_msgs << "q     : " << doubleToStringX(s[4],1) << " (deg/s)" << endl;
  m_msgs << "r     : " << doubleToStringX(s[5],1) << " (deg/s)" << endl;

  m_msgs << "roll  : " <<  doubleToStringX(s[9],1) << " (deg)" << endl;
  m_msgs << "pitch : " <<  doubleToStringX(s[10],1) << " (deg)" << endl;
  m_msgs << "yaw   : " <<  doubleToStringX(s[11],1) << " (deg)" << endl;
  
  m_msgs << "Lat   : " <<  doubleToStringX(s[12],5) << " (du)" << endl;
  m_msgs << "Lon   : " <<  doubleToStringX(s[13],5) << " (du)" << endl;
  m_msgs << "Alt   : " <<  doubleToStringX(s[14],5) << " (m)" << endl;
  

  return(true);
}

void Emulator::postNodeRecordUpdate(std::string, const NodeRecord& record)
{
  double nav_x = record.getX();
  double nav_y = record.getY();

  Notify(prefix+"_X", nav_x, m_curr_time);
  Notify(prefix+"_Y", nav_y, m_curr_time);

  if(geoOk) {
    double nav_lat = record.getLat();
    double nav_lon = record.getLon();
    Notify(prefix+"_LAT", nav_lat, m_curr_time);
    Notify(prefix+"_LONG", nav_lon, m_curr_time);
  }

  double new_speed = record.getSpeed();
  new_speed = snapToStep(new_speed, 0.01);

  Notify(prefix+"_HEADING", record.getHeading(), m_curr_time);
  Notify(prefix+"_SPEED", new_speed, m_curr_time);
  Notify(prefix+"_DEPTH", record.getDepth(), m_curr_time);

  // Added by HS 120124 to make it work ok with iHuxley
  // Notify("SIMULATION_MODE","TRUE", m_curr_time);
  Notify(prefix+"_Z", -record.getDepth(), m_curr_time);
  Notify(prefix+"_PITCH", record.getPitch(), m_curr_time);
  Notify(prefix+"_YAW", record.getYaw(), m_curr_time);
  Notify("TRUE_X", nav_x, m_curr_time);
  Notify("TRUE_Y", nav_y, m_curr_time);

  double hog = angle360(record.getHeadingOG());
  double sog = record.getSpeedOG();

  Notify(prefix+"_HEADING_OVER_GROUND", hog, m_curr_time);
  Notify(prefix+"_SPEED_OVER_GROUND", sog, m_curr_time);
  
  if(record.isSetAltitude()) 
    Notify(prefix+"_ALTITUDE", record.getAltitude(), m_curr_time);
}

bool Emulator::receiveUdpDate()
{
  double u,v,w;
  double U;
  while(!receiveThread.IsQuitRequested())
  {
    int iRx = udp->iRecieveMessage(receiveBuf,receiveBufSize);
    if(iRx)
    {
      for(int i=0; i<receiveDateSize; i++)
      {
        // MOOSTrace(doubleToString(*((double*)receiveBuf+i))+"\n");
        double data = *((double*)receiveBuf+i);
        switch (i)
        {
        case 0: //u
          u = data;
          remus100.u = u*100;
          break;
        case 1: //v
          // record.set
          v = data;
          remus100.v = v*100;
          break;
        case 2: //w
          // record
          w = data;
          remus100.w = w*100;
          U = sqrt(u*u + v*v + w*w);
          record.setSpeed(U);
          break;
        case 3: //p
          remus100.p = data*100;
          break;
        case 4: //q
          remus100.q = data*100;
          break;
        case 5: //r
          remus100.r = data*100;
          break;
        case 6: //x
          remus100.x = data*100;
          record.setY(data);
          break;
        case 7: //y
          remus100.y = data*100;
          record.setX(data);
          break;
        case 8: //z
          remus100.z = data*100;
          record.setDepth(data);
          break;
        case 9: //phi
          remus100.roll = data*100;
          break;
        case 10: //theta
          remus100.pitch = data*100;
          record.setPitch(data);
          break;
        case 11: //psi
          remus100.yaw = data*100;
          record.setHeading(data);
          break;
        case 12: 
          remus100.lat = data;
          record.setLat(data);
          break;
        case 13:
          remus100.lon = data;
          record.setLon(data);
          break;
        case 14:
          remus100.alt = data;
          break;
        default:
          break;
        }
      } 
    }  
  }
  return true;
}

void Emulator::set150Info()
{
   p_150server_1.embeddedInfoSrc.header = 0xEBA1;     //0:[0,1]
   p_150server_1.embeddedInfoSrc.count = (uint16_t)m_iteration;     //2:[2,3]
   p_150server_1.embeddedInfoSrc.size = (uint8_t)51;       //3:[4]
   p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0xFF;       //4:[5]
   p_150server_1.embeddedInfoSrc.height = (uint16_t)(150-remus100.z);       //5:[6,7]
   p_150server_1.embeddedInfoSrc.depth = (uint16_t)remus100.z;       //6:[8,9]
   p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw;       //7:[10,11]
   p_150server_1.embeddedInfoSrc.pitch = (int16_t)remus100.pitch;       //8:[12,13]
   p_150server_1.embeddedInfoSrc.roll = (int16_t)remus100.roll;       //9:[14,15]
   p_150server_1.embeddedInfoSrc.ins_vx = (int16_t)remus100.u;       //10:[16,17]
   p_150server_1.embeddedInfoSrc.ins_vy = (int16_t)remus100.v;       //11:[18,19]
   p_150server_1.embeddedInfoSrc.ins_vz = (int16_t)remus100.w;       //12:[20,21]
   p_150server_1.embeddedInfoSrc.lon = (int32_t)(remus100.lon * 1000000);       //13:[22,23,24,25]
   p_150server_1.embeddedInfoSrc.lat = (int32_t)(remus100.lat * 1000000)       ;       //14:[26,27,28,29]
   p_150server_1.embeddedInfoSrc.alt = (int16_t)(remus100.alt * 100);//15:[30,31]
   p_150server_1.embeddedInfoSrc.dvl_vx = (int16_t)remus100.u;       //16:[32,33]
   p_150server_1.embeddedInfoSrc.dvl_vy = (int16_t)remus100.v;       //17:[34,35]
   p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w;       //18:[36,37]
   p_150server_1.embeddedInfoSrc.rpm = (int16_t)remus100.thrust;       //19:[38,39]
   p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01;       //20:[40]
   p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)30;       //21:[41,42]
   p_150server_1.embeddedInfoSrc.battery_level = 60;       //22:[43]
   p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1;       //23:[44,45]
   p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x0;       //24:[46,47,48,49]
   p_150server_1.embeddedInfoSrc.fault_battery = 0x0;       //25:[50] 
   p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0;       //26:[51]
   p_150server_1.embeddedInfoSrc.fault_thrust = 0x0;       //27:[52]
   p_150server_1.embeddedInfoSrc.iridium = 0xFF;       //28:[53]
   p_150server_1.embeddedInfoSrc.throwing_load = 0xFF;       //29:[54]
   p_150server_1.embeddedInfoSrc.dvl_status = 0x00;       //30:[55]
   p_150server_1.embeddedInfoSrc.crc = 0XFF;       //31:[56]
   p_150server_1.embeddedInfoSrc.footer = 0XEE1A;       //32:[57,58]
}