/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

// ===========================================================================
//  Program: gps2rawlog
//  Intention: Parse binary/text GPS logs, recognice all messages and save
//             as a RawLog file, easily readable by MRPT C++ programs.
//
//  Started: JLBC @ Feb-2016
// ===========================================================================

#include <mrpt/3rdparty/tclap/CmdLine.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::serialization;
using namespace mrpt::system;
using namespace std;

// Declare the supported command line switches ===========
TCLAP::CmdLine cmd("gps2rawlog", ' ', MRPT_getVersion().c_str());

TCLAP::ValueArg<std::string> arg_input_file(
    "i", "input", "Input raw file (required) (*.raw,*.gps,...)", true, "", "log.gps", cmd);
TCLAP::ValueArg<std::string> arg_output_file(
    "o", "output", "Output dataset (*.rawlog)", false, "", "dataset_out.rawlog", cmd);

TCLAP::SwitchArg arg_overwrite(
    "w", "overwrite", "Force overwrite target file without prompting.", cmd, false);

int main(int argc, char** argv)
{
  try
  {
    printf(" gps2rawlog - Part of the MRPT\n");
    printf(
        " MRPT C++ Library: %s - Sources timestamp: %s\n", MRPT_getVersion().c_str(),
        MRPT_getCompilationDate().c_str());

    // Parse arguments:
    if (!cmd.parse(argc, argv)) throw std::runtime_error("");  // should exit.

    const string input_gps_file = arg_input_file.getValue();
    string output_rawlog_file = arg_output_file.getValue();
    if (output_rawlog_file.empty())
      output_rawlog_file = mrpt::system::fileNameChangeExtension(input_gps_file, "rawlog");

    ASSERT_FILE_EXISTS_(input_gps_file);

    // Open input rawlog:
    auto fil_input = std::make_shared<CFileGZInputStream>();
    cout << "Opening for reading: '" << input_gps_file << "'...\n";
    fil_input->open(input_gps_file);
    cout << "Open OK.\n";

    // Open output:
    if (mrpt::system::fileExists(output_rawlog_file) && !arg_overwrite.isSet())
    {
      cout << "Output file already exists: `" << output_rawlog_file
           << "`, aborting. Use `-w` flag to overwrite.\n";
      return 1;
    }

    CFileGZOutputStream fil_out;
    cout << "Opening for writing: '" << output_rawlog_file << "'...\n";
    if (!fil_out.open(output_rawlog_file)) throw std::runtime_error("Error writing file!");

    // GPS object:
    CGPSInterface gps_if;
    gps_if.bindStream(fil_input);

    auto arch = archiveFrom(fil_out);

    // ------------------------------------
    //  Parse:
    // ------------------------------------
    while (!fil_input->checkEOF())
    {
      gps_if.doProcess();

      const auto lst_obs = gps_if.getObservations();

      printf(
          "%u bytes parsed, %u new observations identified...\n",
          (unsigned)fil_input->getPosition(), (unsigned)lst_obs.size());

      for (const auto& kv : lst_obs) arch << *kv.second;
    }

    // successful end of program.
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << mrpt::exception_to_str(e) << std::endl;
    return 1;
  }
}  // end of main()
