/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/obs/gnss_messages_novatel.h>

#include <iostream>
#include <map>

using namespace std;
using namespace mrpt::obs::gnss;

// ------------
void Message_NV_OEM6_GENERIC_FRAME::dumpToStream(std::ostream& out) const
{
  out << mrpt::format(
      "[Novatel OEM6 GENERIC FRAME]\n"
      " Message ID: %u\n",
      (unsigned)this->header.msg_id);
}
void Message_NV_OEM6_GENERIC_FRAME::internal_writeToStream(mrpt::serialization::CArchive& out) const
{
  out.WriteBuffer(&header, sizeof(header));
  out.WriteAs<uint32_t>(msg_body.size());
  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
}
void Message_NV_OEM6_GENERIC_FRAME::internal_readFromStream(mrpt::serialization::CArchive& in)
{
  in.ReadBuffer(&header, sizeof(header));
  const uint32_t nBytesInStream = in.ReadAs<uint32_t>();
  msg_body.resize(nBytesInStream);
  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
  fixEndianness();
}
// ------------
void Message_NV_OEM6_GENERIC_SHORT_FRAME::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 GENERIC SHORT FRAME]\n";
  out << mrpt::format(" Message ID: %u\n", (unsigned)this->header.msg_id);
}
void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_writeToStream(
    mrpt::serialization::CArchive& out) const
{
  out.WriteBuffer(&header, sizeof(header));
  out.WriteAs<uint32_t>(msg_body.size());
  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
}
void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_readFromStream(mrpt::serialization::CArchive& in)
{
  in.ReadBuffer(&header, sizeof(header));
  const uint32_t nBytesInStream = in.ReadAs<uint32_t>();
  msg_body.resize(nBytesInStream);
  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
  fixEndianness();
}

// ------------
const std::string& nv_oem6_solution_status::enum2str(int val)
{
  static bool init_map = false;
  static std::map<int, std::string> val2str;
  if (!init_map)
  {
    init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
    DEF_TYPESTR(SOL_COMPUTED)
    DEF_TYPESTR(INSUFFICIENT_OBS)
    DEF_TYPESTR(NO_CONVERGENCE)
    DEF_TYPESTR(SINGULARITY)
    DEF_TYPESTR(COV_TRACE)
    DEF_TYPESTR(TEST_DIST)
    DEF_TYPESTR(COLD_START)
    DEF_TYPESTR(V_H_LIMIT)
    DEF_TYPESTR(VARIANCE)
    DEF_TYPESTR(RESIDUALS)
    DEF_TYPESTR(DELTA_POS)
    DEF_TYPESTR(NEGATIVE_VAR)
    DEF_TYPESTR(INTEGRITY_WARNING)
    DEF_TYPESTR(INS_INACTIVE)
    DEF_TYPESTR(INS_ALIGNING)
    DEF_TYPESTR(INS_BAD)
    DEF_TYPESTR(IMU_UNPLUGGED)
    DEF_TYPESTR(PENDING)
    DEF_TYPESTR(INVALID_FIX)
#undef DEF_TYPESTR
  }
  auto it = val2str.find(val);
  static const std::string nullstr("???");
  return (it == val2str.end()) ? nullstr : it->second;
}

const std::string& nv_oem6_position_type::enum2str(int val)
{
  static bool init_map = false;
  static std::map<int, std::string> val2str;
  if (!init_map)
  {
    init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
    DEF_TYPESTR(NONE)
    DEF_TYPESTR(FIXEDPOS)
    DEF_TYPESTR(FIXEDHEIGHT)
    DEF_TYPESTR(Reserved)
    DEF_TYPESTR(FLOATCONV)
    DEF_TYPESTR(WIDELANE)
    DEF_TYPESTR(NARROWLANE)
    DEF_TYPESTR(DOPPLER_VELOCITY)
    DEF_TYPESTR(SINGLE)
    DEF_TYPESTR(PSRDIFF)
    DEF_TYPESTR(WAAS)
    DEF_TYPESTR(PROPAGATED)
    DEF_TYPESTR(OMNISTAR)
    DEF_TYPESTR(L1_FLOAT)
    DEF_TYPESTR(IONOFREE_FLOAT)
    DEF_TYPESTR(NARROW_FLOAT)
    DEF_TYPESTR(L1_INT)
    DEF_TYPESTR(WIDE_INT)
    DEF_TYPESTR(NARROW_INT)
    DEF_TYPESTR(RTK_DIRECT_INS)
    DEF_TYPESTR(INS)
    DEF_TYPESTR(INS_PSRSP)
    DEF_TYPESTR(INS_PSRDIFF)
    DEF_TYPESTR(INS_RTKFLOAT)
    DEF_TYPESTR(INS_RTKFIXED)
    DEF_TYPESTR(OMNISTAR_HP)
    DEF_TYPESTR(OMNISTAR_XP)
    DEF_TYPESTR(CDGPS)
#undef DEF_TYPESTR
  }
  auto it = val2str.find(val);
  static const std::string nullstr("???");
  return (it == val2str.end()) ? nullstr : it->second;
}

const std::string& nv_oem6_ins_status_type::enum2str(int val)
{
  static bool init_map = false;
  static std::map<int, std::string> val2str;
  if (!init_map)
  {
    init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
    DEF_TYPESTR(INS_INACTIVE)
    DEF_TYPESTR(INS_ALIGNING)
    DEF_TYPESTR(INS_HIGH_VARIANCE)
    DEF_TYPESTR(INS_SOLUTION_GOOD)
    DEF_TYPESTR(INS_SOLUTION_FREE)
    DEF_TYPESTR(INS_ALIGNMENT_COMPLETE)
    DEF_TYPESTR(DETERMINING_ORIENTATION)
    DEF_TYPESTR(WAITING_INITIALPOS)
#undef DEF_TYPESTR
  }
  auto it = val2str.find(val);
  static const std::string nullstr("???");
  return (it == val2str.end()) ? nullstr : it->second;
}

void generic_dump_BESTPOS(const Message_NV_OEM6_BESTPOS::content_t& fields, std::ostream& out)
{
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
  out << mrpt::format(
      " Solution status: `%s`\n", nv_oem6_solution_status::enum2str(fields.solution_stat).c_str());
  out << mrpt::format(
      " Position type  : `%s`\n", nv_oem6_position_type::enum2str(fields.position_type).c_str());
  out << mrpt::format(
      " Longitude: %.09f deg (std dev: %.06f m)  Latitude: %.09f deg (std "
      "dev: %.06f m)\n",
      fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
  out << mrpt::format(
      " Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m   "
      "(Sum: %.04f m)\n",
      fields.hgt, fields.hgt_sigma, fields.undulation, fields.hgt + fields.undulation);
  out << mrpt::format(" Diff age: %.03f  Solution age: %.03f\n", fields.diff_age, fields.sol_age);
  out << mrpt::format(" Base station ID: `%.*s`\n", 4, fields.base_station_id);
  out << mrpt::format(
      " Num sat tracked: %u  Num sat in solution: %u\n", (unsigned)fields.num_sats_tracked,
      (unsigned)fields.num_sats_sol);
}

void Message_NV_OEM6_BESTPOS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 BESTPOS]\n";
  generic_dump_BESTPOS(fields, out);
}

bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m "
       "undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age "
       "num_sats_tracked num_sats_sol";
  return true;
}
bool Message_NV_OEM6_BESTPOS::getAllFieldValues(std::ostream& o) const
{
  o << mrpt::format(
      "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f "
      "%u %u",
      (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
      (unsigned)fields.solution_stat, (unsigned)fields.position_type, fields.lon, fields.lat,
      fields.hgt, fields.undulation, fields.lon_sigma, fields.lat_sigma, fields.hgt_sigma,
      fields.diff_age, fields.sol_age, (unsigned)fields.num_sats_tracked,
      (unsigned)fields.num_sats_sol);
  return true;
}

// ------------
void Message_NV_OEM6_INSPVAS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 INSPVAS]\n";
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
  out << mrpt::format(
      " INS status: `%s`\n", nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
  out << mrpt::format(
      " Longitude: %.09f deg  Latitude: %.09f deg  Height: %.03f m\n", fields.lon, fields.lat,
      fields.hgt);
  out << mrpt::format(
      " Velocities: North: %.05f  East: %.05f  Up: %.05f\n", fields.vel_north, fields.vel_east,
      fields.vel_up);
  out << mrpt::format(
      " Attitude: Roll: %.05f  Pitch: %.05f  Azimuth: %.05f\n", fields.roll, fields.pitch,
      fields.azimuth);
}

bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 "
       "vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
  return true;
}
bool Message_NV_OEM6_INSPVAS::getAllFieldValues(std::ostream& o) const
{
  o << mrpt::format(
      "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
      (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
      (unsigned)fields.ins_status, fields.lon, fields.lat, fields.hgt, fields.vel_north,
      fields.vel_east, fields.vel_up, fields.roll, fields.pitch, fields.azimuth);
  return true;
}

// ------------
void Message_NV_OEM6_INSCOVS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 INSCOVS]\n";
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
  out << mrpt::format(
      " Position cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
      "%9.03f %9.03f\n",
      fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2], fields.pos_cov[3], fields.pos_cov[4],
      fields.pos_cov[5], fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8]);
  out << mrpt::format(
      " Attitude cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
      "%9.03f %9.03f\n",
      fields.att_cov[0], fields.att_cov[1], fields.att_cov[2], fields.att_cov[3], fields.att_cov[4],
      fields.att_cov[5], fields.att_cov[6], fields.att_cov[7], fields.att_cov[8]);
  out << mrpt::format(
      " Velocity cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
      "%9.03f %9.03f\n",
      fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2], fields.vel_cov[3], fields.vel_cov[4],
      fields.vel_cov[5], fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
}

bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
  return true;
}
bool Message_NV_OEM6_INSCOVS::getAllFieldValues(std::ostream& o) const
{
  o << mrpt::format(
      "%u.%08u "
      "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f "
      "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f "
      "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f ",
      (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week), fields.pos_cov[0],
      fields.pos_cov[1], fields.pos_cov[2], fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
      fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8], fields.att_cov[0], fields.att_cov[1],
      fields.att_cov[2], fields.att_cov[3], fields.att_cov[4], fields.att_cov[5], fields.att_cov[6],
      fields.att_cov[7], fields.att_cov[8], fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
      fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5], fields.vel_cov[6], fields.vel_cov[7],
      fields.vel_cov[8]);
  return true;
}

// ------------
void Message_NV_OEM6_RANGECMP::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 RANGECMP]\n";
  out << mrpt::format(
      " Number of SAT observations: %u\n", static_cast<unsigned int>(this->num_obs));
}

void Message_NV_OEM6_RANGECMP::internal_writeToStream(mrpt::serialization::CArchive& out) const
{
  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
  out << msg_len;
  out.WriteBuffer(&header, sizeof(header));
  out << num_obs;
  ASSERT_EQUAL_(num_obs, obs_data.size());
  if (num_obs) out.WriteBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
}

void Message_NV_OEM6_RANGECMP::internal_readFromStream(mrpt::serialization::CArchive& in)
{
  uint32_t expected_msg_len;
  in >> expected_msg_len;
  in.ReadBuffer(&header, sizeof(header));
  in >> num_obs;
  ASSERT_LT_(num_obs, 2000);
  obs_data.resize(num_obs);
  if (num_obs) in.ReadBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
  fixEndianness();
}

// ------------
void Message_NV_OEM6_RXSTATUS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 RXSTATUS]\n";
  out << mrpt::format(" Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
}

// ------------
void Message_NV_OEM6_RAWEPHEM::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 RAWEPHEM]\n";
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
}

// ------------
void Message_NV_OEM6_VERSION::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 VERSION]\n";
  out << mrpt::format(" Number of components: %u\n", static_cast<unsigned int>(this->num_comps));
  for (size_t i = 0; i < components.size(); i++)
  {
    out << mrpt::format(
        " Component #%u:\n  Model: `%.*s`\n  Serial: `%.*s`\n  SW "
        "version:`%.*s`\n",
        (unsigned int)i, (int)sizeof(components[i].model), components[i].model,
        (int)sizeof(components[i].serial), components[i].serial,
        (int)sizeof(components[i].swversion), components[i].swversion);
  }
}

void Message_NV_OEM6_VERSION::internal_writeToStream(mrpt::serialization::CArchive& out) const
{
  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
  out << msg_len;
  out.WriteBuffer(&header, sizeof(header));
  out << num_comps;
  ASSERT_EQUAL_(num_comps, components.size());
  if (num_comps) out.WriteBuffer(&components[0], sizeof(components[0]) * components.size());
}

void Message_NV_OEM6_VERSION::internal_readFromStream(mrpt::serialization::CArchive& in)
{
  uint32_t expected_msg_len;
  in >> expected_msg_len;
  in.ReadBuffer(&header, sizeof(header));
  in >> num_comps;
  ASSERT_LT_(num_comps, 2000);
  components.resize(num_comps);
  if (num_comps) in.ReadBuffer(&components[0], sizeof(components[0]) * components.size());
  fixEndianness();
}

// ------------
void Message_NV_OEM6_RAWIMUS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 RAWIMUS]\n";
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
  out << mrpt::format(" Status: 0x%08lu\n", (long)fields.imu_status);
  out << mrpt::format(
      " Acel: X=%li Y=%li Z=%li\n", (long)fields.accel_x, -(long)fields.accel_y_neg,
      (long)fields.accel_z);
  out << mrpt::format(
      " Gyro: X=%li Y=%li Z=%li\n", (long)fields.gyro_x, -(long)fields.gyro_y_neg,
      (long)fields.gyro_z);
}

bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
  return true;
}
bool Message_NV_OEM6_RAWIMUS::getAllFieldValues(std::ostream& o) const
{
  o << mrpt::format(
      "%u.%08u %u %li %li %li %li %li %li", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week), (unsigned)fields.imu_status, (long)fields.accel_x,
      -(long)fields.accel_y_neg, (long)fields.accel_z, (long)fields.gyro_x,
      -(long)fields.gyro_y_neg, (long)fields.gyro_z);
  return true;
}

// ------------
void generic_dump_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& out)
{
  out << mrpt::format(" Clock status: 0x%08lu\n", (long)fields.clock_status);
  out << mrpt::format(" GPS week: %lu Seconds: %f\n", (long)fields.week, fields.week_seconds);
  out << mrpt::format(
      " Clock offset: %f  (std dev = %e)\n", fields.clock_offset, fields.clock_offset_std);
  out << mrpt::format(" UTC offset: %f\n", fields.utc_offset);
}

void Message_NV_OEM6_MARKTIME::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 MARKTIME]\n";
  generic_dump_MARKTIME(fields, out);
}

bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
  return true;
}
void generic_getFieldValues_MARKTIME(
    const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& o)
{
  o << mrpt::format(
      "%u.%08u %u %lu %f %f", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
      (unsigned)fields.clock_status, (long unsigned)fields.week, fields.week_seconds,
      fields.utc_offset);
}
bool Message_NV_OEM6_MARKTIME::getAllFieldValues(std::ostream& o) const
{
  generic_getFieldValues_MARKTIME(fields, o);
  return true;
}
// ------------
void Message_NV_OEM6_MARK2TIME::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 MARK2TIME]\n";
  generic_dump_MARKTIME(
      *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields), out);
}
bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions(std::ostream& o) const
{
  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
  return true;
}
bool Message_NV_OEM6_MARK2TIME::getAllFieldValues(std::ostream& o) const
{
  generic_getFieldValues_MARKTIME(
      *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields), o);
  return true;
}

// ------------
void Message_NV_OEM6_MARKPOS::dumpToStream(std::ostream& out) const
{
  out << "[Novatel OEM6 MARKPOSE]\n";
  generic_dump_BESTPOS(*reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields), out);
}

// ------------
void Message_NV_OEM6_IONUTC::dumpToStream(std::ostream& out) const
{
  out << "[Novatel NV_OEM6_IONUTC]\n";
  out << mrpt::format(
      " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
      (unsigned)(fields.header.ms_in_week));
  out << mrpt::format(
      " UTC ref week: %u  Tot: %u\n", (unsigned)fields.utc_wn, (unsigned)fields.tot);
  out << mrpt::format(
      " Leap seconds delta_t: %u  future: %u\n", (unsigned)fields.deltat_ls,
      (unsigned)fields.deltat_lsf);
}
