#include 
#include 
#include 
#include "opensfm.hpp"
#include "point_io.hpp"
#include "cv_utils.hpp"
#include "tensor_math.hpp"

namespace fs = std::filesystem;

using json = nlohmann::json;
using namespace torch::indexing;

namespace osfm{

void from_json(const json& j, Cam &c){
    j.at("projection_type").get_to(c.projectionType);
    if (j.contains("width")) j.at("width").get_to(c.width);
    if (j.contains("height")) j.at("height").get_to(c.height);
    if (j.contains("focal_x")) j.at("focal_x").get_to(c.fx);
    if (j.contains("focal_y")) j.at("focal_y").get_to(c.fy);
    if (j.contains("focal")){
        j.at("focal").get_to(c.fx);
        j.at("focal").get_to(c.fy);
    }
    if (j.contains("c_x")) j.at("c_x").get_to(c.cx);
    if (j.contains("c_y")) j.at("c_y").get_to(c.cy);
    if (j.contains("k1")) j.at("k1").get_to(c.k1);
    if (j.contains("k2")) j.at("k2").get_to(c.k2);
    if (j.contains("p1")) j.at("p1").get_to(c.p1);
    if (j.contains("p2")) j.at("p2").get_to(c.p2);
    if (j.contains("k3")) j.at("k3").get_to(c.k3);
}

void from_json(const json& j, Shot &s){
    j.at("rotation").get_to(s.rotation);
    j.at("translation").get_to(s.translation);
    j.at("camera").get_to(s.camera);
}    

void from_json(const json& j, Point &p){
    j.at("coordinates").get_to(p.coordinates);
    j.at("color").get_to(p.color);
}

void from_json(const json& j, Reconstruction &r){
    j.at("cameras").get_to(r.cameras);
    j.at("shots").get_to(r.shots);
    j.at("points").get_to(r.points);
    
}

InputData inputDataFromOpenSfM(const std::string &projectRoot){
    InputData ret;
    fs::path nsRoot(projectRoot);
    fs::path reconstructionPath = nsRoot / "reconstruction.json";
    fs::path imageListPath = nsRoot / "image_list.txt";
    
    if (!fs::exists(reconstructionPath)) throw std::runtime_error(reconstructionPath.string() + " does not exist");
    if (!fs::exists(imageListPath)) throw std::runtime_error(imageListPath.string() + " does not exist");

    std::ifstream f(reconstructionPath.string());
    json data = json::parse(f);
    f.close();

    std::unordered_map images;
    f.open(imageListPath.string());
    std::string line;
    while(std::getline(f, line)){
        fs::path p(line);
        if (p.is_absolute()) images[p.filename().string()] = line;
        else images[p.filename().string()] = fs::absolute(nsRoot / p).string();
    }
    f.close();
    auto reconstructions = data.template get();

    if (reconstructions.size() == 0) throw std::runtime_error("No reconstructions found");
    if (reconstructions.size() > 1) std::cout