#include "init_inputs.h"

#include <iostream>
#include <fstream>

#include "type_define.h"
#include "jf_utils.hpp"

#define TIME_BUFFER 0.01

///////////////////////////////////////////////////////////
//      Pos_Atti_Ini_File
bool Pos_Atti_Ini_FileReader::parseFromString(std::string line, pos_atti_ini_RawData &pos_atti_ini_data)
{
    std::vector<std::string> list = split_string(line, ',');
    if (list.size() >= 6)
    {        
        pos_atti_ini_data.pos_ini[0] = atof(list[0].c_str());
        pos_atti_ini_data.pos_ini[1] = atof(list[1].c_str());
        pos_atti_ini_data.pos_ini[2] = atof(list[2].c_str());

        pos_atti_ini_data.atti_ini[0] = atof(list[3].c_str());
        pos_atti_ini_data.atti_ini[1] = atof(list[4].c_str());
        pos_atti_ini_data.atti_ini[2] = atof(list[5].c_str());
        return true;
    }
    return false;
}

int Pos_Atti_Ini_FileReader::loadPos_Atti_Ini_File(std::string pos_atti_ini_File, double start_time, double end_time)
{
    char buf[1024];
    std::ifstream ifs(pos_atti_ini_File);
    struct pos_atti_ini_RawData pos_atti_ini_data;

    // buffer
    start_time -= TIME_BUFFER;
    end_time += TIME_BUFFER;

    if (!ifs.is_open())
    {
        std::cout << "error when opening pos_atti_ini file, TERMINATED!!!\n"
                  << std::endl;
        throw;
    }
    while (!ifs.eof())
    {
        ifs.getline(buf, 1024);
        std::string line(buf);
        Pos_Atti_Ini_FileReader::parseFromString(line, pos_atti_ini_data);
        
        raw_data.push_back(pos_atti_ini_data);
        
    }
    ifs.close();
    return raw_data.size();
}

///////////////////////////////////////////////////////////
//      imu2base_File
bool Imu2BaseFileReader::parseFromString(std::string line, Imu2BaseRawData &Imu2BaseRaw_data)
{
    std::vector<std::string> list = split_string(line, ',');
    if (list.size() >= 3)
    {        
        Imu2BaseRaw_data.imu2base_calib[0] = atof(list[0].c_str());
        Imu2BaseRaw_data.imu2base_calib[1] = atof(list[1].c_str());
        Imu2BaseRaw_data.imu2base_calib[2] = atof(list[2].c_str());
        
        return true;
    }
    return false;
}

int Imu2BaseFileReader::loadImu2BaseFile(std::string Imu2BaseFile)
{
    char buf[1024];
    std::ifstream ifs(Imu2BaseFile);
    struct Imu2BaseRawData imu2base_data;

    if (!ifs.is_open())
    {
        std::cout << "error when opening odom file, TERMINATED!!!\n"
                  << std::endl;
        throw;
    }
    while (!ifs.eof())
    {
        ifs.getline(buf, 1024);
        std::string line(buf);
        Imu2BaseFileReader::parseFromString(line, imu2base_data);
        
        raw_data.push_back(imu2base_data);
        
    }
    ifs.close();
    return raw_data.size();
}

////////////////////////////////////////////////////////////
//      imu2base_lever_armFile
bool Imu2Base_LeverArmFileReader::parseFromString(std::string line, Imu2Base_LeverArmRawData &Imu2Base_LeverArmRaw_data)
{
    std::vector<std::string> list = split_string(line, ',');
    if (list.size() >= 3)
    {        
        Imu2Base_LeverArmRaw_data.imu2base_lever_arm[0] = atof(list[0].c_str());
        Imu2Base_LeverArmRaw_data.imu2base_lever_arm[1] = atof(list[1].c_str());
        Imu2Base_LeverArmRaw_data.imu2base_lever_arm[2] = atof(list[2].c_str());
        
        return true;
    }
    return false;
}

int Imu2Base_LeverArmFileReader::loadImu2Base_LeverArmFile(std::string Imu2BaseleverarmFile)
{
    char buf[1024];
    std::ifstream ifs(Imu2BaseleverarmFile);
    struct Imu2Base_LeverArmRawData Imu2Base_LeverArmRaw_data;

    if (!ifs.is_open())
    {
        std::cout << "error when opening odom file, TERMINATED!!!\n"
                  << std::endl;
        throw;
    }
    while (!ifs.eof())
    {
        ifs.getline(buf, 1024);
        std::string line(buf);
        Imu2Base_LeverArmFileReader::parseFromString(line, Imu2Base_LeverArmRaw_data);
        
        raw_data.push_back(Imu2Base_LeverArmRaw_data);
        
    }
    ifs.close();
    return raw_data.size();
}
////////////////////////////////////////////////////////////