/* +------------------------------------------------------------------------+
   |                     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 "nav-precomp.h"  // Precomp header
//
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <mrpt/kinematics/CVehicleVelCmd_Holo.h>
#include <mrpt/nav/reactive/CLogFileRecord.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

using namespace mrpt::nav;

IMPLEMENTS_SERIALIZABLE(CLogFileRecord, CSerializable, mrpt::nav)

uint8_t CLogFileRecord::serializeGetVersion() const { return 27; }
void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const
{
  uint32_t i, n;

  // Version 0 ---------
  n = infoPerPTG.size();
  out << n;
  for (i = 0; i < n; i++)
  {
    out << infoPerPTG[i].PTG_desc.c_str();

    uint32_t m = infoPerPTG[i].TP_Obstacles.size();
    out << m;
    if (m)
      out.WriteBuffer(
          (const void*)&(*infoPerPTG[i].TP_Obstacles.begin()),
          m * sizeof(infoPerPTG[i].TP_Obstacles[0]));

    out << infoPerPTG[i].TP_Targets;  // v8: CPoint2D -> TPoint2D. v26: vector
    out << infoPerPTG[i].TP_Robot;    // v17
    out << infoPerPTG[i].timeForTPObsTransformation
        << infoPerPTG[i].timeForHolonomicMethod;  // made double in v12
    out << infoPerPTG[i].desiredDirection << infoPerPTG[i].desiredSpeed
        << infoPerPTG[i].evaluation;  // made double in v12
    // removed in v23: out << evaluation_org << evaluation_priority; //
    // added in v21
    out << infoPerPTG[i].HLFR;

    // Version 9: Removed security distances. Added optional field with
    // PTG info.
    const bool there_is_ptg_data = infoPerPTG[i].ptg ? true : false;
    out << there_is_ptg_data;
    if (there_is_ptg_data) out << infoPerPTG[i].ptg;

    // Was: out << infoPerPTG[i].clearance.raw_clearances; // v19
    infoPerPTG[i].clearance.writeToStream(out);  // v25
  }
  out << nSelectedPTG << WS_Obstacles;
  out << WS_Obstacles_original;  // v20

  // Removed v24: out << robotOdometryPose;
  out << robotPoseLocalization << robotPoseOdometry;  // v24

  out << WS_targets_relative;  // v8, v26: vector
  // v16:
  out << ptg_index_NOP << ptg_last_k_NOP;
  out << rel_cur_pose_wrt_last_vel_cmd_NOP
      << rel_pose_PTG_origin_wrt_sense_NOP;  // v24: CPose2D->TPose2D

  // Removed: v24. out << ptg_last_curRobotVelLocal; // v17
  ptg_last_navDynState.writeToStream(out);  // v24

  if (ptg_index_NOP < 0) out << cmd_vel /*v10*/ << cmd_vel_original;  // v15

  // Previous values: REMOVED IN VERSION #6
  n = robotShape_x.size();
  out << n;
  if (n)
  {
    out.WriteBuffer((const void*)&(*robotShape_x.begin()), n * sizeof(robotShape_x[0]));
    out.WriteBuffer((const void*)&(*robotShape_y.begin()), n * sizeof(robotShape_y[0]));
  }

  // Version 1 ---------
  out << cur_vel << cur_vel_local; /*v10*/
  // out << estimatedExecutionPeriod; // removed v13

  // Version 3 ----------
  for (i = 0; i < infoPerPTG.size(); i++)
  {
    // v22: this is now a TParameters
    // (mrpt 2.1.0) Directly a std::map<>
    out << infoPerPTG[i].evalFactors;
  }

  out << nPTGs;  // v4
  // out << timestamp; // removed v13
  out << robotShape_radius;  // v11
  // out << cmd_vel_filterings; // added v12: Removed in v15

  out << values << timestamps;  // v13

  out << relPoseSense << relPoseVelCmd;  // v14, v24 changed CPose2D->TPose2D

  // v15: cmd_vel converted from std::vector<double> into CSerializable
  out << additional_debug_msgs;  // v18

  navDynState.writeToStream(out);  // v24

  out << visuals;  // v27
}

void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    case 7:
    case 8:
    case 9:
    case 10:
    case 11:
    case 12:
    case 13:
    case 14:
    case 15:
    case 16:
    case 17:
    case 18:
    case 19:
    case 20:
    case 21:
    case 22:
    case 23:
    case 24:
    case 25:
    case 26:
    case 27:
    {
      // Version 0 --------------
      uint32_t i, n;

      infoPerPTG.clear();

      in >> n;
      infoPerPTG.resize(n);
      for (i = 0; i < n; i++)
      {
        auto& ipp = infoPerPTG[i];
        in >> ipp.PTG_desc;

        int32_t m;
        in >> m;
        ipp.TP_Obstacles.resize(m);
        if (m) in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m);

        ipp.TP_Targets.clear();
        if (version >= 8)
        {
          if (version >= 26)
          {
            in >> ipp.TP_Targets;
          }
          else
          {
            mrpt::math::TPoint2D trg;
            in >> trg;
            ipp.TP_Targets.push_back(trg);
          }
        }
        else
        {
          mrpt::poses::CPoint2D pos;
          in >> pos;
          ipp.TP_Targets.emplace_back(pos.x(), pos.y());
        }
        if (version >= 17)
          in >> ipp.TP_Robot;
        else
          ipp.TP_Robot = mrpt::math::TPoint2D(0, 0);

        if (version >= 12)
        {
          in >> ipp.timeForTPObsTransformation >> ipp.timeForHolonomicMethod;
          in >> ipp.desiredDirection >> ipp.desiredSpeed >> ipp.evaluation;
        }
        else
        {
          in.ReadAsAndCastTo<float, double>(ipp.timeForTPObsTransformation);
          in.ReadAsAndCastTo<float, double>(ipp.timeForHolonomicMethod);
          in.ReadAsAndCastTo<float, double>(ipp.desiredDirection);
          in.ReadAsAndCastTo<float, double>(ipp.desiredSpeed);
          in.ReadAsAndCastTo<float, double>(ipp.evaluation);
        }
        if (version >= 21 && version < 23)
        {
          double evaluation_org, evaluation_priority;
          in >> evaluation_org >> evaluation_priority;
        }

        in >> ipp.HLFR;

        if (version >= 9)  // Extra PTG info
        {
          ipp.ptg.reset();

          bool there_is_ptg_data;
          in >> there_is_ptg_data;
          if (there_is_ptg_data)
            ipp.ptg = std::dynamic_pointer_cast<CParameterizedTrajectoryGenerator>(in.ReadObject());
        }

        if (version >= 19)
        {
          if (version < 25)
          {
            std::vector<std::map<double, double>> raw_clearances;
            in >> raw_clearances;
            ipp.clearance.resize(raw_clearances.size(), raw_clearances.size());
            for (size_t k = 0; k < raw_clearances.size(); k++)
              ipp.clearance.get_path_clearance_decimated(k) = raw_clearances[k];
          }
          else
          {
            ipp.clearance.readFromStream(in);
          }
        }
        else
        {
          ipp.clearance.clear();
        }
      }

      in >> nSelectedPTG >> WS_Obstacles;
      if (version >= 20)
      {
        in >> WS_Obstacles_original;  // v20
      }
      else
      {
        WS_Obstacles_original = WS_Obstacles;
      }

      if (version < 24)
      {
        mrpt::poses::CPose2D robotOdometryPose;
        in >> robotOdometryPose;
        robotPoseOdometry = robotOdometryPose.asTPose();
        robotPoseLocalization = robotOdometryPose.asTPose();
      }
      else
      {
        in >> robotPoseLocalization >> robotPoseOdometry;  // v24
      }

      WS_targets_relative.clear();
      if (version >= 8)
      {
        if (version >= 26)
        {
          in >> WS_targets_relative;
        }
        else
        {
          mrpt::math::TPoint2D trg;
          in >> trg;
          WS_targets_relative.emplace_back(trg);
        }
      }
      else
      {
        mrpt::poses::CPoint2D pos;
        in >> pos;
        WS_targets_relative.emplace_back(mrpt::math::TPoint2D(pos.x(), pos.y()));
      }

      if (version >= 16)
      {
        in >> ptg_index_NOP >> ptg_last_k_NOP;
        if (version >= 24)
        {
          in >> rel_cur_pose_wrt_last_vel_cmd_NOP >> rel_pose_PTG_origin_wrt_sense_NOP;
        }
        else
        {
          mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP,
              crel_pose_PTG_origin_wrt_sense_NOP;
          in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP;
          rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose();
          rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP.asTPose();
        }
      }
      else
      {
        ptg_index_NOP = -1;
      }
      if (version >= 17 && version < 24)
      {
        in >> ptg_last_navDynState.curVelLocal;
      }
      if (version >= 24)
      {
        ptg_last_navDynState.readFromStream(in);
      }

      if (version >= 10)
      {
        if (version >= 15)
        {
          if (ptg_index_NOP < 0) in >> cmd_vel;
        }
        else
        {
          std::vector<double> vel;
          in >> vel;
          if (vel.size() == 2)
            cmd_vel = mrpt::kinematics::CVehicleVelCmd::Ptr(
                new mrpt::kinematics::CVehicleVelCmd_DiffDriven);
          else
            cmd_vel =
                mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_Holo);
          for (size_t k = 0; k < cmd_vel->getVelCmdLength(); k++)
            cmd_vel->setVelCmdElement(i, vel[k]);
        }
      }
      else
      {
        float v, w;
        in >> v >> w;
        cmd_vel =
            mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven);
        cmd_vel->setVelCmdElement(0, v);
        cmd_vel->setVelCmdElement(0, w);
      }

      if (version >= 15 && ptg_index_NOP < 0) in >> cmd_vel_original;

      if (version < 13)
      {
        float old_exec_time;
        in >> old_exec_time;
        values["executionTime"] = old_exec_time;
      }

      if (version < 6)
      {
        mrpt::math::CVectorFloat prevV, prevW, prevSelPTG;

        // Previous values: (Removed in version 6)
        in >> n;
        prevV.resize(n);
        if (n) in.ReadBufferFixEndianness(&(*prevV.begin()), n);

        in >> n;
        prevW.resize(n);
        if (n) in.ReadBufferFixEndianness(&(*prevW.begin()), n);

        in >> n;
        prevSelPTG.resize(n);
        if (n) in.ReadBufferFixEndianness(&(*prevSelPTG.begin()), n);
      }

      in >> n;
      robotShape_x.resize(n);
      robotShape_y.resize(n);
      if (n)
      {
        in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n);
        in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n);
      }

      if (version > 0)
      {  // Version 1 --------------
        if (version >= 10)
        {
          in >> cur_vel >> cur_vel_local;
        }
        else
        {
          float actual_v, actual_w;
          in >> actual_v >> actual_w;
          cur_vel = mrpt::math::TTwist2D(0, 0, 0);
          cur_vel_local = mrpt::math::TTwist2D(actual_v, .0, actual_w);
        }
      }
      else
      {  // Default values for old versions:
        cur_vel = mrpt::math::TTwist2D(0, 0, 0);
      }

      if (version < 13 && version > 1)
      {
        float old_estim_period;
        in >> old_estim_period;
        values["estimatedExecutionPeriod"] = old_estim_period;
      }

      for (i = 0; i < infoPerPTG.size(); i++)
      {
        infoPerPTG[i].evalFactors.clear();
      }
      if (version > 2)
      {
        // Version 3..22 ----------
        for (i = 0; i < infoPerPTG.size(); i++)
        {
          if (version < 22)
          {
            in >> n;
            for (unsigned int j = 0; j < n; j++)
            {
              float f;
              in >> f;
              infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] = f;
            }
          }
          else
          {
            in >> infoPerPTG[i].evalFactors;
          }
        }
      }

      if (version > 3)
      {
        // Version 4 ----------
        in >> nPTGs;
        if (version < 9)  // Old "securityDistances", now unused.
        {
          in >> n;
          float dummy;
          for (i = 0; i < n; i++) in >> dummy;
        }
      }
      else
      {
        nPTGs = infoPerPTG.size();
      }

      if (version > 4)
      {
        if (version < 10)
        {
          int32_t navigatorBehavior;  // removed in v10
          in >> navigatorBehavior;
        }

        if (version < 6)  // Removed in version 6:
        {
          mrpt::poses::CPoint2D doorCrossing_P1, doorCrossing_P2;
          in >> doorCrossing_P1 >> doorCrossing_P2;
        }
      }

      if (version > 6 && version < 13)
      {
        mrpt::system::TTimeStamp tt;
        in >> tt;
        timestamps["tim_start_iteration"] = tt;
      }

      if (version >= 11)
      {
        in >> robotShape_radius;
      }
      else
      {
        robotShape_radius = 0.5;
      }

      if (version >= 12 && version < 15)
      {
        std::vector<std::vector<double>> dummy_cmd_vel_filterings;
        in >> dummy_cmd_vel_filterings;
      }

      if (version >= 13)
      {
        in >> values >> timestamps;
      }
      else
      {
        values.clear();
        timestamps.clear();
      }

      if (version >= 14)
      {
        if (version >= 24)
        {
          in >> relPoseSense >> relPoseVelCmd;
        }
        else
        {
          mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd;
          in >> crelPoseSense >> crelPoseVelCmd;
          relPoseSense = crelPoseSense.asTPose();
          relPoseVelCmd = crelPoseVelCmd.asTPose();
        }
      }
      else
      {
        relPoseSense = relPoseVelCmd = mrpt::math::TPose2D(0, 0, 0);
      }

      if (version >= 18)
        in >> additional_debug_msgs;
      else
        additional_debug_msgs.clear();

      if (version >= 24)
        navDynState.readFromStream(in);
      else
      {
        navDynState = CParameterizedTrajectoryGenerator::TNavDynamicState();
        navDynState.curVelLocal = cur_vel_local;
        if (!WS_targets_relative.empty()) navDynState.relTarget = WS_targets_relative[0];
      }

      if (version >= 27)
      {  // read:
        in >> visuals;
      }
      else
        visuals.clear();
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}
