//
// Created by ivan on 25.01.2022.
//
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>

void DSO2CSV(const std::string& path, const std::string& filename){
    std::ifstream inputFile;
    std::ofstream outputFile;
    inputFile.open(path + "/" + filename);
    std::cout << "Open inpput file success! " << std::endl;
    std::cout << "I'm going to open file: " + path + "/" + filename.substr(0, filename.size() - 4) + "_converted.txt" << std::endl;
    outputFile.open(path + "/" + filename.substr(0, filename.size() - 4) + "_converted.txt");
    if (!inputFile or !outputFile){
        std::cout << "Failed to open one of the files. Exiting..." << std::endl;
        exit(EXIT_SUCCESS);
    }
    outputFile << "time[s] x y z qx qy qz qw" << std::endl;
    std::string tmp, comma = " ";

    while (!inputFile.eof()){
        inputFile >> tmp;
        tmp = std::to_string(std::stod(tmp) * 1000000000.0);
        outputFile << tmp << comma;

        for (int i=1; i<7; i++){
            inputFile >> tmp;
            outputFile << tmp << comma;
        }
        // Last step without comma
        inputFile >> tmp;
        outputFile << tmp << std::endl;
    }

    inputFile.close();
    outputFile.close();
}

int main(int argc, char **argv){
    //std::string path = "/home/ivan/ivan/git/work_drivecast2/SLAM/datasets/tum_mono_vi/dataset-corridor4_512_16/dso";
    std::string path = argv[1];
    //std::string filename = "result";
    std::string filename = argv[2];

    DSO2CSV(path, filename);
    std::cout << "Convert is success" << std::endl;
}