sttl.cpp

#include "sttl.h"

#include <iostream>
#include <fstream>

#include <json/json.h>
#include <json/value.h>

#include <chrono>
#include <functional>

#include <cstdlib>


using namespace std;

std::string sttl::Transform::Id::toString() const{
	std::stringstream str;
	str.fill('0');
	str.width(16);
	str<<std::hex<<id[0];
	str.fill('0');
	str.width(16);
	str<<std::hex<<id[1];
	return str.str();
}
bool sttl::Transform::Id::fromString(const std::string& string){
	for(int i=0; i<2; ++i){
		std::stringstream str(string.substr(i*16,16));
		str>>std::hex>>id[i];
	}
	return true;
}



bool readJson(sttl::Transform & transform, Json::Value & node){
	transform.frameName = node["name"].asString();
	transform.referenceFrameName = node["referenceFrame"].asString();
	transform.id.fromString(node["id"].asString());


	if(node["transform"].size()!=4){
		std::cerr<<"transform does not have 4 elements!"<<std::endl;
		return false;
	}
	if(node["transform"][0].size()!=4){
		std::cerr<<"transform does not have 4x4 elements!"<<std::endl;
		return false;
	}

	for(int i=0; i<4; ++i){
		for(int j=0; j<4; ++j){
			transform.transform(i, j) = node["transform"][i][j].asDouble();
		}
	}


	transform.hasCovariance = false;
	if(node.isMember("covariance")){
		transform.hasCovariance = true;
		if(node["covariance"].size()!=7){
			std::cerr<<"covariance does not have 7 elements!"<<std::endl;
			return false;
		}
		if(node["covariance"][0].size()!=7){
			std::cerr<<"covariance does not have 7x7 elements!"<<std::endl;
			return false;
		}
		for(int i=0; i<7; ++i){
			for(int j=0; j<7; ++j){
				transform.covariance(i, j) = node["covariance"][i][j].asDouble();
			}
		}
	}

	return true;
}

bool fillJson(const sttl::Transform & transform, Json::Value & node){
	node["name"] = transform.frameName;


	Json::Value trans(Json::arrayValue);
	trans.resize(4);
		for(int i=0; i<4; ++i){
			trans[i]=Json::Value(Json::arrayValue);
			trans[i].resize(4);
			for(int k=0; k<4; ++k){
				trans[i][k] = transform.transform(i, k);
			}
		}
	node["transform"] = trans;



	if(transform.hasCovariance){
	Json::Value cov(Json::arrayValue);
	cov.resize(7);
		for(int i=0; i<7; ++i){
			cov[i]=Json::Value(Json::arrayValue);
			cov[i].resize(7);
			for(int k=0; k<7; ++k){
				cov[i][k] = transform.covariance(i, k);
			}
		}
		node["covariance"] = cov;
	}

	node["referenceFrame"] = transform.referenceFrameName;
	node["id"] = transform.id.toString();

	return true;
}


uint64_t hash_combine(uint64_t& seed, const uint64_t &hash) {
    seed ^= hash + 0x9e3773e79b4319b9 + (seed<<6) + (seed>>2);
    return seed;
}

// inline void hash_combine(std::size_t& seed, const std::size_t &hash) {
//     seed ^= hash + 0x9e3779b9 + (seed<<6) + (seed>>2);
// }

bool sttl::Transform::generateId(){
	if(!(id==Id())){
		std::cerr<<"Id was not empty when calling generateId()!"<<std::endl;
		return false;
	}

	uint64_t id_time = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	uint64_t hashNames = 0;
	hash_combine(hashNames, std::hash<std::string>()(frameName));
	hash_combine(hashNames, std::hash<std::string>()(referenceFrameName));
	hash_combine(hashNames, std::hash<std::string>()(byUser));
	hash_combine(hashNames, std::hash<std::string>()(method));
	hash_combine(hashNames, std::hash<std::string>()(program));

	uint64_t hashTransf = 0;
	for(int i=0; i<4; ++i) for(int k=0; k<4; ++k){
		hash_combine(hashTransf, std::hash<double>()(transform(i, k)));
	}

	static bool srandCalled = false;
	if(!srandCalled){
		srandCalled = true;
		std::srand(id_time);
	}
	uint64_t random = std::rand();
	hash_combine(random, std::rand());
	hash_combine(random, std::rand());
	hash_combine(random, std::rand());
	hash_combine(random, std::rand());

	id = Id(hash_combine(random, id_time), hash_combine(hashNames, hashTransf));
	return true;
}


bool sttl::saveList(const std::list<sttl::Transform> & transfs, const std::string &file ){
	Json::Value root(Json::arrayValue);
	for(std::list<sttl::Transform>::const_iterator itr = transfs.begin(); itr != transfs.end(); ++itr){
		Json::Value frame;
		fillJson(*itr, frame);
		root.append(frame);
	}
	std::fstream fs;
	fs.open (file, std::fstream::out);
    if (fs.is_open()){

		fs<<root;

		fs.close();
	}else{
		cerr<<"Unable to open file "<<file<<" for writing..."<<std::endl;
		return false;
	}

	return true;
}


bool sttl::loadList(std::list<sttl::Transform> & transfs, const std::string &file ){
	if(!transfs.empty()){
		std::cerr<<"Load list - list provided not empty!"<<std::endl;
		return false;
	}

	std::fstream fs;
	fs.open (file, std::fstream::in);
    if (!fs.is_open()){
    	std::cerr<<"Error opening file "<<file<<std::endl;
    	return false;
    }
	Json::Value root;
	fs >> root;

    for(int i = 0 ; i < root.size() ; ++i){
    	sttl::Transform trans;
    	bool ret = readJson(trans, root[i]);
    	if(!ret){
    		std::cerr<<"Error reading transform # "<<i<<std::endl;
    		return false;
    	}
    	transfs.push_back(trans);
    }
    return true;
}