#include <string>
#include <fstream>
#include <Eigen/Dense>
#include <nlohmann/json.hpp>


Eigen::Matrix4d read_data_from_json(const std::string& file_path, std::string& info) {
    Eigen::Matrix4d mat; 
    nlohmann::json jfile;
    std::ifstream file(file_path);
    if(!file.is_open()) {
        if (info == "right") {
            info = "Cannot open right_hand file! use E(4, 4)";
        }
        else if (info == "left") {
            info = "Cannot open left_hand file! use E(4, 4)";
        }
        else {
            info = "Cannot open head file! use E(4, 4)";
        }
        mat = Eigen::Matrix4d::Identity();
    }
    else {
        file >> jfile;
        std::vector<std::vector<double>> data = jfile["T"];
        for(int i = 0; i < 4; i++) {
            for(int j = 0; j < 4; j++) {
                mat(i, j) = data[i][j];
            }
        }
        if (info == "right") {
            info = "Success read right_hand data";
        }
        else if (info == "left") {
            info = "Success read left_hand data";
        }
        else {
            info = "Success read head data";
        }
    }
    return mat;
}    
    
    
    
