#pragma once

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <string>
#include <vector>

///////////////////////////////////////////////////////////
// QRP_config File
struct QRPConfigRawData
{
    //////// Q ////////
    double sigma_accl_noise;   // [m/s^2]  (value derived from Noise Spectral Density in datasheet)
    double sigma_gyro_noise;   // [rad/s] (value derived from Noise Spectral Density in datasheet)

    //////// R ////////
    Eigen::Vector3d sigma_pos;
    Eigen::Vector3d sigma_theta; 
    Eigen::Vector3d sigma_vel;

    //////// P ////////
    double sigma_init_pos;
    double sigma_init_vel; 
    double sigma_init_dtheta; 
};

// QRP_config_auto File
struct QRPConfigAutoData
{
    //////// Q ////////
    double sigma_accl_noise;   // [m/s^2]  (value derived from Noise Spectral Density in datasheet)
    double sigma_gyro_noise;   // [rad/s] (value derived from Noise Spectral Density in datasheet)

    //////// R ////////
    Eigen::Vector4d sigma_pos_x; // r_min,r_max,a,k
    Eigen::Vector4d sigma_pos_y;
    Eigen::Vector4d sigma_pos_z;
    Eigen::Vector4d sigma_theta_roll; 
    Eigen::Vector4d sigma_theta_pitch;
    Eigen::Vector4d sigma_theta_yaw;
    Eigen::Vector4d sigma_vel_x;
    Eigen::Vector4d sigma_vel_y;
    Eigen::Vector4d sigma_vel_z;

    //////// P ////////
    double sigma_init_pos;
    double sigma_init_vel; 
    double sigma_init_dtheta; 
};

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

    int loadQRPConfigFile(FILE *QRPConfigFile);
    int loadQRPConfigAutoFile(FILE *QRPConfigFile);
    std::vector<QRPConfigRawData> raw_data;
    std::vector<QRPConfigAutoData> auto_data;   

private:
    //bool parseFromString(std::string line, QRPConfigRawData &qrp_data);
   
};