#pragma once

#include <string>
#include <vector>

#include <Eigen/Dense>

#include "inputs.h"

struct mocapData
{
    Eigen::Vector3f pos;
    Eigen::Quaternionf quat;
    int status;
    double stamp;
};

///////////////////////////////////////////////////////////
// pos_atti_ini File
struct pos_atti_ini_RawData
{    
    Eigen::Vector3f pos_ini;   
    Eigen::Vector3f atti_ini;
};

class Pos_Atti_Ini_FileReader
{
public:
    Pos_Atti_Ini_FileReader() {}
    ~Pos_Atti_Ini_FileReader() {}

    int loadPos_Atti_Ini_File(std::string pos_atti_ini_File, double start_time, double end_time);
    std::vector<pos_atti_ini_RawData> raw_data;

private:
    bool parseFromString(std::string line, pos_atti_ini_RawData &pos_atti_ini_data);    
};

///////////////////////////////////////////////////////////
// imu2base File
struct Imu2BaseRawData
{
    Eigen::Vector3f imu2base_calib;
};

class Imu2BaseFileReader
{
public:
    Imu2BaseFileReader() {}
    ~Imu2BaseFileReader() {}

    int loadImu2BaseFile(std::string Imu2BaseFile);
    std::vector<Imu2BaseRawData> raw_data;

private:
    bool parseFromString(std::string line, Imu2BaseRawData &imu2base_data);
   
};
///////////////////////////////////////////////////////////
// imu2base_lever_arm File
struct Imu2Base_LeverArmRawData
{
    Eigen::Vector3f imu2base_lever_arm;
};

class Imu2Base_LeverArmFileReader
{
public:
    Imu2Base_LeverArmFileReader() {}
    ~Imu2Base_LeverArmFileReader() {}

    int loadImu2Base_LeverArmFile(std::string Imu2BaseLeverArmFile);
    std::vector<Imu2Base_LeverArmRawData> raw_data;

private:
    bool parseFromString(std::string line, Imu2Base_LeverArmRawData &Imu2Base_LeverArm_data);
   
};