text:
/*
   Copyright 2017 Kai Huebl (kai@huebl-sgh.de)

   Lizenziert gemäß Apache Licence Version 2.0 (die „Lizenz“); Nutzung dieser
   Datei nur in Übereinstimmung mit der Lizenz erlaubt.
   Eine Kopie der Lizenz erhalten Sie auf http://www.apache.org/licenses/LICENSE-2.0.

   Sofern nicht gemäß geltendem Recht vorgeschrieben oder schriftlich vereinbart,
   erfolgt die Bereitstellung der im Rahmen der Lizenz verbreiteten Software OHNE
   GEWÄHR ODER VORBEHALTE – ganz gleich, ob ausdrücklich oder stillschweigend.

   Informationen über die jeweiligen Bedingungen für Genehmigungen und Einschränkungen
   im Rahmen der Lizenz finden Sie in der Lizenz.

   Autor: Kai Huebl (kai@huebl-sgh.de)
 */

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/lexical_cast.hpp>
#include "OpcUaStackCore/Base/Log.h"
#include "OpcUaStackCore/BuildInTypes/XmlNumber.h"

namespace OpcUaStackCore
{

	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	//
	// encoder - element
	//
	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaBoolean& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaBoolean xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaByte& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaByte xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaSByte& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaSByte xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt16& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaUInt16 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt16& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaInt16 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt32& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaUInt32 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt32& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaInt32 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt64& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaUInt64 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt64& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaInt64 xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaFloat& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "Float xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaDouble& value, const std::string& element)
	{
		boost::property_tree::ptree elementTree;
		if (!xmlEncode(elementTree, value)) {
			Log(Error, "OpcUaDouble xml encode error")
				.parameter("Element", element);
			return false;
		}
		pt.push_back(std::make_pair(element, elementTree));
		return true;
	}

	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	//
	// encoder
	//
	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaBoolean& value)
	{
		if (value) {
			pt.put_value("true");
		}
		else {
			pt.put_value("false");
		}
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaByte& value)
	{
		std::stringstream ss;
		ss << (int16_t)value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaSByte& value)
	{
		std::stringstream ss;
		ss << (int16_t)value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt16& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt16& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt32& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt32& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaUInt64& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaInt64& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaFloat& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	bool
	XmlNumber::xmlEncode(boost::property_tree::ptree& pt, OpcUaDouble& value)
	{
		std::stringstream ss;
		ss << value;
		pt.put_value(ss.str());
		return true;
	}

	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	//
	// decoder
	//
	// ------------------------------------------------------------------------
	// ------------------------------------------------------------------------
	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaBoolean& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		boost::to_upper(sourceValue);
		if (sourceValue == "TRUE") value = true;
		else value = false;
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaByte& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = (OpcUaByte)boost::lexical_cast<OpcUaUInt16>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaByte xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaSByte& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = (OpcUaSByte)boost::lexical_cast<OpcUaInt16>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaSByte xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaUInt16& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<uint16_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaUInt16 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaInt16& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<int16_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaInt16 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaUInt32& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<uint32_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaUInt32 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaInt32& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<int32_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaInt32 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaUInt64& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<uint64_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaUInt64 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaInt64& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<int64_t>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaInt64 xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaFloat& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<float>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaFloat xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

	bool
	XmlNumber::xmlDecode(boost::property_tree::ptree& pt, OpcUaDouble& value)
	{
		std::string sourceValue = pt.get_value<std::string>();
		try {
			value = boost::lexical_cast<double>(sourceValue);
		} catch(boost::bad_lexical_cast& /*e*/) {
			Log(Error, "OpcUaDouble xml decoder error - value format error")
				.parameter("Value", sourceValue);
			return false;
		}
		return true;
	}

}

max_stars_repo_name: gianricardo/OpcUaStack

max_issues_repo_name: gianricardo/OpcUaStack

max_forks_repo_name: gianricardo/OpcUaStack


text:
// ./visual_odometry -i intrinsics.yml -e extrinsics.yml -s ../data/cropped/ -o ../data/out/ ../data/img_list.xml
#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <unistd.h>
#include <sys/wait.h>

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include "opencv2/core/core.hpp"
#include "opencv2/core/operations.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/video/tracking.hpp>

#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/stereo/stereo_tracker_sim.h"
#include "../../../tracker/stereo/stereo_tracker_libviso.h"
#include "../../../tracker/stereo/stereo_tracker_bfm.h"
#include "../../../tracker/detector/feature_detector_harris_cv.h"
#include "../../../tracker/detector/feature_detector_gftt_cv.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../extern/libviso2/src/viso_stereo.h"
#include "../../extern/libviso2/src/matrix.h"
#include "../../feature_filter.h"
#include "../../math_helper.h"
#include "../../eval_helper.h"
#include "../../format_helper.h"
#include "../../cv_plotter.h"
#include "../../matches.h"
using namespace vo;

// bumblebee dataset
#define CALIB_WIDTH		 640
#define CALIB_HEIGHT		 480
#define MAX_FEATURES		 500

// Lucas Kanade tracking params
// bumblebee dataset
//#define STEREO_BOX_W		 13		// 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 21 // 21
//#define TRACK_BOX_H		 21 // 21
//#define LK_PYRAMID_LEVEL	 3
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 10

#define STEREO_BOX_W		 21		// 13
#define STEREO_BOX_H		 21		// 5 or 3
#define TRACK_BOX_W		 21 // 21
#define TRACK_BOX_H		 21 // 21
#define LK_PYRAMID_LEVEL	 3
#define BORDER_FILTER		 5
#define BORDER_MASK_SIZE	 5

// detector params
#define QUALITY_LVL		 1	  // 0.15
#define MIN_DISTANCE		 4	  // 10
#define BLOCK_SIZE		 3	  // 3
#define USE_HARRIS		 true	  // false
#define HARRIS_K         0.04	  // 0.04

// libviso dataset
//#define STEREO_BOX_W		 11		 // 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 11 // 21
//#define TRACK_BOX_H		 11 // 21
//#define LK_PYRAMID_LEVEL	 3  // 5
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 60

//TODO hardcoded - in future add movement checking through optical flow
// feature tracking params
#define SBA_FRAME_STEP		      2  // 10

// plotting settings
#define SCALE_FACTOR             0.5	       // 0.5, zoom with 2.0
#define CENTER_X		            300
#define CENTER_Y		            200
#define WINDOW_WIDTH		         900
#define WINDOW_HEIGHT		      700
//#define WINDOW_CAMERA_WIDTH	   1000
//#define WINDOW_CAMERA_HEIGHT	   800

using namespace std;
//using namespace cv;

// functions
void getCalibParams(string& intrinsic_filename, string& extrinsic_filename, Mat& P_left, Mat& P_right, Mat& Q,
                    Mat& C_left, Mat &D_left, Mat& C_right, Mat& D_right);

static int printHelp()
{
   cout << "Usage:\n ./visual_odometry -i intrinsics.yml -e extrinsics.yml -s source_folder -o output_folder <image "
           "list XML/YML file>\n" << endl;
   return 0;
}

void printPoint() {}
void printMatch() {}

void writeExtrinsicParams(deque<Mat>& Rt_params, fstream& file)
{
   // Rt for extr params (no cam pose)
   // TODO left or right coord system
   double quat[4];
   //cout << extr_params[0] << "\n -> 0-Rt\n";
   //MathHelper::matToQuat(Rt_params[0], quat);
   //cout << quat << endl;

   std::setprecision(6);

   for(size_t i = 0; i < Rt_params.size(); i++) {
      MathHelper::matToQuat(Rt_params[i], quat);
      for(int j = 0; j < 4; j++) {
         file << quat[j] << " ";
      }
      Mat& Rt = Rt_params[i];
      file << Rt.at<double>(0,3) << " " << Rt.at<double>(1,3) << " " << Rt.at<double>(2,3)<< endl;
   }
   // TODO - y
}

void writeMatRt(Mat& Rt, ofstream& fp)
{
   std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++)
         fp << Rt.at<double>(i,j) << " ";
   }
   fp << endl;
   //file << Rt.at<double>(0,3) << " " << Rt.at<double>(1,3) << " " << Rt.at<double>(2,3)<< endl;
}

void writePointProjections(fstream& file, deque<Matches>& features, deque<vector<uchar>>& track_status,
      deque<Mat>& ctw_params, VisualOdometryStereo::parameters& param)
{
   assert(features.size() == SBA_FRAME_STEP && track_status.size() == (features.size()-1));
   //unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;
   //   double rerr_left = 0.0;
   //   double rerr_right = 0.0;
   Mat C = Mat::eye(3, 3, CV_64F);
   for(size_t k = 0; k < features.size(); k++) {
      for(size_t i = 0; i < features[k].left_.size(); i++) {
         if(k > 0 && track_status[k-1][i] == 1)
            continue;

         d = max(features[k].left_[i].pt.x - features[k].right_[i].pt.x, 1.f);
         pt.at<double>(0,0) = (features[k].left_[i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features[k].left_[i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;

         for(int f = k; f >= 0; f--) {
            pt = ctw_params[f] * pt;
         }
         //	 pt = ctw_params[k] * pt;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         uint32_t cnt = 1;
         for(unsigned j = k;; j++) {
            if(j == track_status.size())
               break;
            if(track_status[j][i] == 1)
               cnt++;
            else
               break;
         }
         if(cnt == 1)
            continue;
         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << p.x << " " << p.y << " " << p.z << " ";

         file << cnt;
         //cout << c;
         for(unsigned j = k; j < (k+cnt); j++) {
            file << " " << j << " " << features[j].left_[i].pt.x << " " << features[j].left_[i].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;

         // test triangulation
         //	 cout << "start triang. test\n";
         //	 cout << "1. frame in file: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 for(uint32_t j = k; j < (k+cnt); j++) {
         //	    d = max(features[j].left_[i].pt.x - features[j].right_[i].pt.x, 1.f);
         //	    pt.at<double>(0,0) = (features[j].left_[i].pt.x - param.calib.cu) * param.base / d;
         //	    pt.at<double>(1,0) = (features[j].left_[i].pt.y - param.calib.cv) * param.base / d;
         //	    pt.at<double>(2,0) = param.calib.f * param.base / d;
         //	    pt.at<double>(3,0) = 1.0;
         //	    cout << (j-k)+1 << ". frame: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "  -->  ";
         //	    for(int32_t f = j; f >= 0; f--) {
         //	       pt = ctw_params[f] * pt;
         //	    }
         //	    cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 }
         //	 cout << "end triang. test\n";
      }
   }
   //cout << "reproj error left: " << rerr_left << "\nreproj error right: " << rerr_right << "\n";
}

void writePointProjectionsFromFristFrame(fstream& file, deque<Matches>& features, deque<vector<int32_t>>& track_idx,
                                         deque<Mat>& ctw_params, VisualOdometryStereo::parameters& param)
{
   //   cout << features_left.size() << endl;
   //   assert(features_left.size() == SBA_FRAME_STEP && features_left.size() == features_right.size() && track_status.size() == (features_left.size()-1));
   //unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;

   //   for(int32_t f = ctw_params.size()-1; f >= 0; f--) {
   //      cout << f << " frame: " << ctw_params[f] << endl;
   //   }

   Mat C = Mat::eye(3, 3, CV_64F);
   for(size_t k = 0; k <= 0; k++) {
      for(size_t i = 0; i < features[k].left_.size(); i++) {
         if(k > 0 && track_idx[k-1][i] >= 0)
            continue;
         // TODO racunat sa nenormaliziranom znacajkama zbog ciste rektifikacije...?
         d = max(features[k].left_[i].pt.x - features[k].right_[i].pt.x, 1.f);
         pt.at<double>(0,0) = (features[k].left_[i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features[k].left_[i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;
         for(int32_t f = k; f >= 0; f--) {
            pt = ctw_params[f] * pt;
         }
         //	 pt = ctw_params[k] * pt;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         //	 Point3f pt_right(p.x-0.12, p.y, p.z);
         //	 rerr_left += MathHelper::getReprojError(features_left[k][i].pt, p, C);
         //	 rerr_right += MathHelper::getReprojError(features_right[k][i].pt, pt_right, C);

         // TODO
         //	 p.x += extr_params[k][4];
         //	 p.y += extr_params[k][5];
         //	 p.z += extr_params[k][6];
         //cout << "camera system: " << pt << endl;
         //pt = extr_params[k] * pt;
         //cout << "world system: " << pt << endl;
         uint32_t cnt = 1;
         int32_t index = i;
         for(uint32_t j = k;; j++) {
            if(j == track_idx.size())
               break;
            if(track_idx[j][index] >= 0) {
               cnt++;
               index = track_idx[j][index];
            }
            else
               break;
         }
         if(cnt == 1)
            continue;
         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << p.x << " " << p.y << " " << p.z << " ";

         file << cnt;
         //cout << c;
         index = i;
         for(uint32_t j = k; j < (k+cnt); j++) {
            if(j > k)
               index = track_idx[j-1][index];
            file << " " << j << " " << features[j].left_[index].pt.x << " " << features[j].left_[index].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;

         // test triangulation
         //	 index = i;
         //	 cout << "start triang. test\n";
         //	 cout << "1. frame in file: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 for(uint32_t j = k; j < (k+cnt); j++) {
         //	    if(j > k)
         //	       index = track_idx[j-1][index];
         //
         //	    d = max(features[j].left_[index].pt.x - features[j].right_[index].pt.x, 1.f);
         //	    pt.at<double>(0,0) = (features[j].left_[index].pt.x - param.calib.cu) * param.base / d;
         //	    pt.at<double>(1,0) = (features[j].left_[index].pt.y - param.calib.cv) * param.base / d;
         //	    pt.at<double>(2,0) = param.calib.f * param.base / d;
         //	    pt.at<double>(3,0) = 1.0;
         //	    cout << (j-k)+1 << ". frame: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "  -->  ";
         //	    for(int32_t f = j; f >= 0; f--) {
         //	       pt = ctw_params[f] * pt;
         //	    }
         //	    cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 }
         //	 cout << "end triang. test\n";
      }
   }
   //cout << "reproj error left: " << rerr_left << "\nreproj error right: " << rerr_right << "\n";
}

void writePointProjections2Frames(fstream& file, deque<vector<KeyPoint>>& features_left, deque<vector<KeyPoint>>& features_right,
      deque<Mat>& extr_inv, VisualOdometryStereo::parameters& param)
{
   assert(features_left.size() >= SBA_FRAME_STEP && features_left.size() == features_right.size());
   //assert(features_left.size() == extr_params.size());
   unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;
   Mat C = Mat::eye(3, 3, CV_64F);
   for(unsigned k = start; k < (start+1); k++) {
      for(unsigned i = 0; i < features_left[k].size(); i++) {
         // TODO ovdje dodat sakupljeni pomak od prvog frejma
         // todo racunat sa nenormaliziranom znacajkama zbog ciste rektifikacije...?
         d = max(features_left[k][i].pt.x - features_right[k][i].pt.x, 1.f);
         pt.at<double>(0,0) = (features_left[k][i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features_left[k][i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         //	 Point3f pt_right(p.x-0.12, p.y, p.z);
         //	 rerr_left += MathHelper::getReprojError(features_left[k][i].pt, p, C);
         //	 rerr_right += MathHelper::getReprojError(features_right[k][i].pt, pt_right, C);

         // TODO
         //cout << "camera system: " << pt << endl;
         //pt = extr_inv[k-start] * pt;
         //cout << "world system: " << pt << endl;

         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         unsigned c = 2;
         file << c;
         //cout << c;
         for(unsigned j = k; j < (k+c); j++) {
            file << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;
      }
   }
}

// TODO - not direct motion but inverse camera motion matrix?
void readMotion(string filename, double motion_params[7])
{
   ifstream file(filename);

   string line, tmp_line;

   while(std::getline(file, tmp_line)) {
      line = tmp_line;
   }
   stringstream stream(line);
   string val;
   //cout << "LINE: " << line << endl;
   for(int i = 0; i < 7; i++) {
      stream >> val;
      motion_params[i] = std::stod(val);
      //cout << "READ: " << motion_params[i] << endl << endl;
   }

   file.close();
}

void matToKeypoints(Mat& mat, vector<KeyPoint>& keys)
{
   assert(mat.cols == keys.size() && mat.rows == 1 && mat.channels() == 2);
   for(unsigned i = 0; i < keys.size(); i++) {
      keys[i].pt.x = mat.at<Vec2d>(0,i)[0];
      keys[i].pt.y = mat.at<Vec2d>(0,i)[1];
   }
}
void keypointsToMat(vector<KeyPoint>& keys, Mat& mat)
{
   assert(mat.cols == keys.size() && mat.rows == 1 && mat.channels() == 2);
   for(unsigned i = 0; i < keys.size(); i++) {
      mat.at<Vec2d>(0,i)[0] = keys[i].pt.x;
      mat.at<Vec2d>(0,i)[1] = keys[i].pt.y;
   }
}

void cvKeysToVisoMatches(deque<vector<KeyPoint>>& keys_left, deque<vector<KeyPoint>>& keys_right,
      vector<uchar>& status, vector<Matcher::p_match>& matches)
{
   //assert(keys_left.back().size() == keys_right.back.size() && keys_left.back().size() == status.size());
   Matcher::p_match m;

   size_t last = keys_left.size() - 1;
   for(size_t i = 0; i < status.size(); i++) {
      if(status[i] == 1) {
         m.u1p = keys_left[last-1][i].pt.x;
         m.v1p = keys_left[last-1][i].pt.y;
         m.u1c = keys_left[last][i].pt.x;
         m.v1c = keys_left[last][i].pt.y;
         m.u2p = keys_right[last-1][i].pt.x;
         m.v2p = keys_right[last-1][i].pt.y;
         m.u2c = keys_right[last][i].pt.x;
         m.v2c = keys_right[last][i].pt.y;
         matches.push_back(m);
      }
   }
}

void cvKeysToVisoMatches(deque<Matches>& all_features, vector<uchar>& status, vector<Matcher::p_match>& matches)
{
   Matcher::p_match m;

   size_t last = all_features.size() - 1;
   assert(all_features[last-1].left_.size() == status.size());
   for(size_t i = 0; i < status.size(); i++) {
      if(status[i] == 1) {
         m.u1p = all_features[last-1].left_[i].pt.x;
         m.v1p = all_features[last-1].left_[i].pt.y;
         m.u1c = all_features[last].left_[i].pt.x;
         m.v1c = all_features[last].left_[i].pt.y;
         m.u2p = all_features[last-1].right_[i].pt.x;
         m.v2p = all_features[last-1].right_[i].pt.y;
         m.u2c = all_features[last].right_[i].pt.x;
         m.v2c = all_features[last].right_[i].pt.y;
         //cout << m.u1p << " " << m.v1p << endl << m.u1c << " " << m.v1c << endl;
         matches.push_back(m);
      }
   }
}

void convertAllMatchesToKeys(vector<Matcher::p_match>& matches, vector<vector<KeyPoint>>& keypoints)
{
   keypoints.resize(4);
   KeyPoint kp;
   for(size_t i = 0; i < matches.size(); i++) {
      //cout << "match " << i << endl;
      kp.pt.x = matches[i].u1p;
      kp.pt.y = matches[i].v1p;
      keypoints[0].push_back(kp);
      kp.pt.x = matches[i].u2p;
      kp.pt.y = matches[i].v2p;
      keypoints[1].push_back(kp);
      kp.pt.x = matches[i].u1c;
      kp.pt.y = matches[i].v1c;
      keypoints[2].push_back(kp);
      kp.pt.x = matches[i].u2c;
      kp.pt.y = matches[i].v2c;
      keypoints[3].push_back(kp);
   }
}
void convertInlierMatchesToKeys(vector<Matcher::p_match>& matches, vector<int32_t>& inliers, vector<vector<KeyPoint>>& keypoints)
{
   keypoints.resize(4);
   KeyPoint kp;
   for(size_t i = 0; i < inliers.size(); i++) {
      //cout << "match " << i << endl;
      int32_t idx = inliers[i];
      kp.pt.x = matches[idx].u1p;
      kp.pt.y = matches[idx].v1p;
      keypoints[0].push_back(kp);
      kp.pt.x = matches[idx].u2p;
      kp.pt.y = matches[idx].v2p;
      keypoints[1].push_back(kp);
      kp.pt.x = matches[idx].u1c;
      kp.pt.y = matches[idx].v1c;
      keypoints[2].push_back(kp);
      kp.pt.x = matches[idx].u2c;
      kp.pt.y = matches[idx].v2c;
      keypoints[3].push_back(kp);
   }
}
Mat getCameraMatrix(VisualOdometryStereo::parameters& param)
{
   Mat C = Mat::zeros(3, 4, CV_64F);
   C.at<double>(0,0) = param.calib.f;
   C.at<double>(1,1) = param.calib.f;
   C.at<double>(2,2) = 1.0;
   C.at<double>(0,2) = param.calib.cu;
   C.at<double>(1,2) = param.calib.cv;
   return C;
}

void readGpsPoints(string filename, vector<Mat> points)
{
   ifstream file(filename);
   string val;
   points.clear();
   Mat pt(2, 1, CV_64F);
   while(!file.eof()) {
      file >> val;
      pt.at<double>(0,0) = std::stod(val);
      file >> val;
      pt.at<double>(1,0) = std::stod(val);
      cout << pt << endl;
      points.push_back(pt.clone());
      for(int i = 0; i < 9; i++) file >> val;
   }
}

void visualOdometry(vector<string>& imagelist, string& cparams_file, string& source_folder, string& output_folder)
{
   vector<KeyPoint> features_full;
   vector<KeyPoint> features_left;
   vector<KeyPoint> features_right;

   deque<vector<uchar>> track_status;
   deque<Matches> all_features;
   deque<vector<KeyPoint>> features_libviso_left;
   deque<vector<KeyPoint>> features_libviso_right;

   deque<Mat> extr_params; // pose mat
   deque<Mat> Rt_params;

   double cam_params[5];
   FormatHelper::readCameraParams(cparams_file, cam_params);
   VisualOdometryStereo::parameters param;
   param.calib.f = cam_params[0];	 // focal length in pixels
   param.calib.cu = cam_params[2];	 // principal point (u-coordinate) in pixels
   param.calib.cv = cam_params[3];	 // principal point (v-coordinate) in pixels
   param.base = cam_params[4];

   // bumblebee dataset - rectified - lcd calib
   //param.calib.f  = 491.22;	    // focal length
   //param.calib.cu = 328.8657;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 249.43165;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.119744;
   ////param.base	  = 0.5;

   // old lcd
   //param.calib.f  = 491.598;	 // focal length
   //param.calib.cu = 336.437;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 249.277;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.12;

   // bumblebee - alan calib rectified - with opencv
   //param.calib.f  = 492.2473;	 // focal length in pixels
   //param.calib.cu = 333.5488;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 240.6432;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.12043;      // 386.1448 / 718.856

   // bumblebee - alan calib rectified - original from pdf - i dont have calib data, only rectif data
// param.calib.f = 425.3849;
// param.calib.cu = 325.9;
// param.calib.cv = 244.54275;
// param.base = 0.12;

   // libviso datasets: 00, 14
   //param.calib.f  = 718.856;	 // focal length in pixels
   //param.calib.cu = 607.1928;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 185.2157;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53716;      // 386.1448 / 718.856

   // libviso 11 dataset
   //param.calib.f  = 707.0912;	 // focal length in pixels
   //param.calib.cu = 601.8873;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 183.1104;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53715;      // 386.1448 / 718.856
   // optional
   //   param.inlier_threshold = 1.5;       // 1.5
   //   param.ransac_iters = 200;	       // 200
   //   param.bucket.bucket_height = 50;    // 50
   //   param.bucket.bucket_width = 50;     // 50
   //param.bucket.max_features = 6;      // 2

   //Image nullimg;
   string simdata_folder = "/home/kivan/projects/datasets/stereo_model/points_kitti_cam_nonoise_base_0.50/";
   string simdata_xml = "/home/kivan/projects/datasets/stereo_model/stereosim_viso00path.xml";
   Mat mask = imread("/home/kivan/Projects/project-vista/config_files/mask_kitti_rect.png", CV_LOAD_IMAGE_GRAYSCALE);
   FeatureDetectorBase* detector = new FeatureDetectorGFTTCV(5000, 0.0001, 1, 3, true, 0.04, mask);
   //FeatureDetectorBase* detector = new FeatureDetectorHarrisCV(10, 10, 10, 3, 1, 0.04);
   StereoTrackerBase* tracker = new StereoTrackerBFM(detector, 2000, 0.9);
   //StereoTrackerBase* tracker = new StereoTrackerSim(simdata_folder, simdata_xml);
   //StereoTrackerBase* tracker = new StereoTrackerLibviso(param);
   // init visual odometry
   VisualOdometryStereo viso(param, tracker);
   // LIBVISO end

   //Mat P_left, P_right, Q, C_left, D_left, C_right, D_right;
   //getCalibParams(intrinsic_filename, extrinsic_filename, P_left, P_right, Q, C_left, D_left, C_right, D_right);

   //	Mat descriptors_1, descriptors_2;
   Mat img_left_prev = imread(source_folder + imagelist[0], CV_LOAD_IMAGE_GRAYSCALE);
   Mat img_right_prev = imread(source_folder + imagelist[1], CV_LOAD_IMAGE_GRAYSCALE);

   //TODO try histo eq;

   Matrix pose = Matrix::eye(4);
   Matrix viso_cvtrack = Matrix::eye(4);
   //Matrix point_rt = Matrix::eye(4);
   Mat Rt_inv;
   Mat mat_I = Mat::eye(4, 4, CV_64F);
   mat_I.copyTo(Rt_inv);
   Vec<double,7> trans_vec;
   extr_params.push_back(mat_I.clone());
   Mat Rt(4, 4, CV_64F);
   Mat Rt_gt(4, 4, CV_64F);
   Mat Rt_gt_prev = Mat::eye(4, 4, CV_64F);
   Mat Rt_gt_curr = Mat::eye(4, 4, CV_64F);
   MathHelper::invTrans(Rt_inv, Rt);
   Rt_params.push_back(Rt.clone());
   cout << Rt << endl;
   cout << Rt_inv << endl;

   Mat prev_location_viso = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_viso_cvtrack = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_sba = Mat::zeros(4, 1, CV_64F);

   int32_t dims[] = {img_left_prev.cols, img_left_prev.rows, img_left_prev.cols};
   // init the tracker
   if(!viso.process(img_left_prev.data, img_right_prev.data, dims))
      cout << "init frame - no estimation" << endl;

   Matches matches(features_left, features_right);

   Mat disp_libviso = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_cvtracker = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_birch = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_sba = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera_right = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);

   imshow("libviso_orig", disp_libviso);
   //imshow("libviso_cvtracker", disp_libviso_cvtracker);
   //imshow("SBA", disp_sba);
   //moveWindow("libviso_orig", 0, 0);
   //moveWindow("libviso_cvtracker", 682, 0);

   // for SBA
   int frame = 2;
   string extr_filename = "cam_extr.txt";
   string points_filename = "cam_points.txt";
   fstream extr_file;
   fstream points_file;
   //   vector<Mat> gps_pts;
   //   readGpsPoints("gps_route.txt", gps_pts);

   Mat Rt_sba_path = Mat::eye(4, 4, CV_64F);	   // sba extrinsic params in whole scene path
   Mat Rt_sba = Mat::eye(4, 4, CV_64F);		   // sba output extrinsic params
   Mat pose_sba = Mat::eye(4, 4, CV_64F);
   Mat pose_libviso = Mat::eye(4, 4, CV_64F);   // matrix transforms points in current camera coordinate system to world coord system
   CvPlotter plotter(WINDOW_WIDTH, WINDOW_HEIGHT, SCALE_FACTOR , CENTER_X, CENTER_Y);

   ifstream groundtruth_file("/home/kivan/Projects/datasets/KITTI/poses/00.txt");
   ofstream reprojerr_file("stereo_reproj_error.txt");
   ofstream libviso_ptsfile("viso_points.txt");
   writeMatRt(pose_libviso, libviso_ptsfile);

   // -- demo start
   //string left_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/left_cam/";
   //string right_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/right_cam/";
   //std::ostringstream frame_num;
   //frame_num << std::setw(6) << std::setfill('0') << 0;
   //cout << frame_num.str() << "\n";
   //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", img_left_prev);
   //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", img_right_prev);
   // -- demo end

   for(unsigned i = 2; i < imagelist.size(); i+=(2)) {
      cout << source_folder + imagelist[i] << endl;
      Mat img_left = imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
      Mat img_right = imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
      cout << i/2 << " frame:" << endl;

      if(viso.process(img_left.data, img_right.data, dims)) {
         vector<Matcher::p_match>& libviso_features = viso.getFeatures();
         vector<int32_t>& inliers = viso.getInliers();
//         double reproj_err = MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt);
//         cout << "reproj error: " << reproj_err << endl;

         ////	 cout << "libviso features size: " << viso.p_matched.size() << endl;
         vector<vector<KeyPoint>> matches, matches_all;
         convertInlierMatchesToKeys(libviso_features, inliers, matches);
         //convertAllMatchesToKeys(libviso_features, matches_all);

         // SBA begin
         //features_libviso_left.clear();
         //features_libviso_right.clear();
         //features_libviso_left.push_back(matches[0]);
         //features_libviso_right.push_back(matches[1]);
         //features_libviso_left.push_back(matches[2]);
         //features_libviso_right.push_back(matches[3]);
         // SBA end

         Mat disp_allfeats;
         vector<uchar> viso_status(matches[0].size(), 1);
         cvtColor(img_left_prev, disp_camera, COLOR_GRAY2RGB);
         cvtColor(img_left_prev, disp_allfeats, COLOR_GRAY2RGB);
         FeatureFilter::drawOpticalFlow(disp_camera, matches[0], matches[2], viso_status, Scalar(0,0,255));
         //FeatureFilter::drawOpticalFlow(disp_allfeats, matches_all[0], matches_all[2], viso_status, Scalar(0,0,255));
         imshow("camera_left", disp_camera);
         //imshow("camera_left_all", disp_allfeats);
         //moveWindow("camera left", 400, 200);

         // save imgs for demo
         // -- demo start
         //cvtColor(img_right, disp_camera_right, COLOR_GRAY2RGB);
         //FeatureFilter::drawOpticalFlow(disp_camera_right, matches[1], matches[3], viso_status, Scalar(0,0,255));
         //frame_num.str("");
         //frame_num << std::setw(6) << std::setfill('0') << (i/2);
         //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", disp_camera);
         //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", disp_camera_right);
         // -- demo end

         // on success, update current pose
         // TODO try without inverse
         pose = pose * Matrix::inv(viso.getMotion());
         //cout << pose << endl << endl;
         MathHelper::matrixToMat(pose, pose_libviso);
         writeMatRt(pose_libviso, libviso_ptsfile);
         //viso_ptsfile << ~pose.extractCols(std::vector<int>(1,3)) << endl;

         //	 cout << "point_rt - rt * viso.getMotion:\n" << point_rt_mat << endl;
         //TODO - why Rt and point_rt not excatly the same? because of Matrix::inv?
         Matrix Rt_inv_libviso = Matrix::inv(viso.getMotion());
         Matrix Rt_libviso = viso.getMotion();
         MathHelper::matrixToMat(Rt_inv_libviso, Rt_inv);
         MathHelper::matrixToMat(Rt_libviso, Rt);
         //MathHelper::invTrans(Rt, Rt_inv);    // better
         extr_params.push_back(Rt_inv.clone());

         // output some statistics
         vector<core::Point> points_lp, points_rp, points_lc, points_rc;
         FeatureHelper::LibvisoInliersToPoints(libviso_features, inliers, points_lp, points_rp, points_lc, points_rc);
         //cout << points_lp.size() << ", " << points_rp.size() << ", " << points_lc.size() << ", " << points_rc.size() << endl;
         Mat C = getCameraMatrix(param);
         //cout << Rt_inv << "\n";

         FormatHelper::readNextMatrixKitti(groundtruth_file, Rt_gt_curr);
         Mat Rt_gt_inv = Rt_gt_prev * Rt_gt_curr;
         //cout << Rt_gt_inv << endl;
         MathHelper::invTrans(Rt_gt_inv, Rt_gt);
         //Rt_gt = Rt_gt_inv;
         cout << "Groundtruth:\n" << Rt_gt << endl;
         cout << "Odometry:\n" << Rt << endl;
         cout << "-------------------------------\n";
         MathHelper::invTrans(Rt_gt_curr, Rt_gt_prev);
         double reproj_error = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                Rt, param.base);
         double reproj_error_gt = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                   Rt_gt, param.base);
         cout << "reprojection error (libviso): " << reproj_error << "\n";
         cout << "reprojection error (groundtruth): " << reproj_error_gt << "\n";
         cout << "-----------------------------------------------------------------------\n";
         reprojerr_file << reproj_error << "\n";
         if(reproj_error > reproj_error_gt) {
            cout << "Found better for GT!!!\n";
            //return;
         }

         double num_matches = viso.getNumberOfMatches();
         double num_inliers = viso.getNumberOfInliers();
         cout << "Matches: " << num_matches;
         cout << ", Inliers: " << num_inliers << " -> " << 100.0*num_inliers/num_matches << " %" << endl;
         Mat location_viso(pose_libviso, Range(0,4), Range(3,4)); // extract 4-th column
         //cout << location_viso << endl;
         //Matrix location = ~point_rt.extractCols(std::vector<int>(1,3));
         //for(int k = 0; k < 3; k++) location.val[0][k] = - location.val[0][k];
         //cout << point_rt * ~location << endl;
         //cout << location << endl << endl;

         // drawing
         //drawLine(disp_libviso, Point(100.5,100.5), Point(500,500));
         //Point pt1 = coordToPoint(prev_location, SCALE_FACTOR);
         //Point pt2 = coordToPoint(location, SCALE_FACTOR);
         plotter.drawLine(prev_location_viso, location_viso, disp_libviso);
         imshow("libviso_orig", disp_libviso);
         waitKey(10);
         //cout << pose << endl << endl;
         //cout << endl << location.val[0][3] << endl;

         location_viso.copyTo(prev_location_viso);
      } else {
         cout << "libviso ... failed!" << endl;
         waitKey(0);
         //writeMatRt(pose_libviso, libviso_ptsfile);
         //extr_params.push_back(Rt_inv.clone());
         exit(1);
      }

      // BEGIN SBA
      //Rt_params.push_back(Rt.clone());

      //if(frame >= SBA_FRAME_STEP) {
      //   extr_file.open(extr_filename, std::ios_base::out | std::fstream::trunc);
      //   writeExtrinsicParams(Rt_params, extr_file);
      //   extr_file.close();

      //   points_file.open(points_filename, std::ios_base::out | std::fstream::trunc);
      //   deque<vector<uchar>> status_viso;
      //   vector<uchar> s_viso(features_libviso_left[0].size(), 1);
      //   status_viso.push_back(s_viso);

      //   //writePointProjectionsFromFristFrame(points_file, all_features, track_idx, extr_params, param);
      //   //writePointProjections(points_file, all_features, track_status, extr_params, param);
      //   //writePointProjections(points_file, features_libviso_left, features_libviso_right, status_viso, extr_params, param);
      //   writePointProjections2Frames(points_file, features_libviso_left, features_libviso_right, extr_params, param);
      //   //writePointProjectionsDebug(points_file, all_features_left, all_features_right, track_status, extr_params, imagelist, source_folder);
      //   points_file.close();

      //   pid_t pid = fork();
      //   if(pid >= 0) {
      //      // child
      //      if(pid == 0) {
      //         //execl("eucsbademo", "eucsbademo", "cam_extr.txt", "cam_points.txt", "C_left_cam.txt", "output.txt", (char*)NULL);
      //         //const char* camera_mat = normalized_points ? "C_normal.txt" : "C_left_cam.txt";
      //         //const char* camera_mat = "C_libviso_00.txt";
      //         const char* camera_mat = "C_bb.txt"; // TODO - why not better
      //         cout << "Using camera matrix: " << camera_mat << endl;
      //         execl("eucsbademo_mot", "eucsbademo_mot", "cam_extr.txt", "cam_points.txt", camera_mat, "output.txt", (char*)NULL);
      //      }

      //      //execl("eucsbademo", "eucsbademo", "cam_extr.txt", "cam_points.txt", "cam_intr.txt", "output.txt", (char*)NULL);
      //      // parent
      //      else
      //         wait(NULL);
      //   }
      //   else {
      //      cout << "fork failed... exiting." << endl;
      //      exit(-1);
      //   }

      //   if(frame == SBA_FRAME_STEP)
      //      plotter.drawFirstFrames("output.txt", prev_location_sba, disp_sba);

      //   double motion_params[7];
      //   readMotion("output.txt", motion_params);
      //   // TODO ovo i za libviso
      //   MathHelper::quatToMat(motion_params, Rt_sba);
      //   Rt_sba_path = Rt_sba_path * Rt_sba;
      //   //	 cout << "viso Rt:\n" << Rt << endl;
      //   //	 cout << "sba Rt:\n" << Rt_sba << endl;

      //   //Mat pose_sba = Rt_sba_path.inv();
      //   Mat pose_sba_tmp;
      //   MathHelper::invTrans(Rt_sba, pose_sba_tmp);
      //   pose_sba = pose_sba * pose_sba_tmp;
      //   Mat location_sba(4, 1, CV_64F);
      //   location_sba.at<double>(0,0) = pose_sba.at<double>(0,3);
      //   location_sba.at<double>(1,0) = pose_sba.at<double>(1,3);
      //   location_sba.at<double>(2,0) = pose_sba.at<double>(2,3);

      //   // isto kao iznad sa pose_sba?
      //   //	 location_sba.at<double>(0,0) = Rt_sba_path.at<double>(0,3);
      //   //	 location_sba.at<double>(1,0) = -Rt_sba_path.at<double>(1,3);
      //   //	 location_sba.at<double>(2,0) = -Rt_sba_path.at<double>(2,3);

      //   //	 Point pt1 = coordToPoint(prev_location_sba, SCALE_FACTOR);
      //   //	 Point pt2 = coordToPoint(location_sba, SCALE_FACTOR);
      //   plotter.drawLine(prev_location_sba, location_sba, disp_sba);
      //   //imshow("Libviso", disp_libviso);
      //   imshow("SBA", disp_sba);
      //   waitKey(10);
      //   location_sba.copyTo(prev_location_sba);
      //   // calculate and print reproj errors
      //   Mat C = getCameraMatrix(param);
      //   double err_viso_l, err_viso_r, err_sba_l, err_sba_r;
      //   MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt, err_viso_l, err_viso_r);
      //   MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt_sba, err_sba_l, err_sba_r);
      //   cout << "Reprojection error (libviso):\n" << "left: " << err_viso_l << "\nright: " << err_viso_r << endl;
      //   cout << "Reprojection error (sba):\n" << "left: " << err_sba_l << "\nright: " << err_sba_r << endl;

      //   //all_features.pop_front();
      //   //track_status.pop_front();
      //   Rt_params.pop_front();
      //   Rt_params[0] = mat_I.clone();
      //   extr_params.pop_front();
      //   //extr_params.pop_front();
      //   //extr_params.push_front(mat_I.clone());
      //   extr_params[0] = mat_I.clone();
      //}
      // END SBA
      //

      frame++;

      cv::swap(img_left_prev, img_left);
      cv::swap(img_right_prev, img_right);
      //		img_left_prev.deallocate();
      //		img_left_prev = std::move(img_left);
      //		img_right_prev.deallocate();
      //		img_right_prev = std::move(img_right);
   }
   groundtruth_file.close();
   reprojerr_file.close();
   libviso_ptsfile.close();
   waitKey(0);
}

void getCalibParams(String& intrinsic_filename, String& extrinsic_filename, Mat& P_left,
                    Mat& P_right, Mat& Q, Mat& C_left, Mat& D_left, Mat& C_right, Mat& D_right)
{
   // reading intrinsic parameters
   FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
   if(!fs.isOpened())
   {
      cout << "Failed to open file " << intrinsic_filename << endl;
      return;
   }

   //Mat M1, D1, M2, D2;
   fs["M1"] >> C_left;
   fs["D1"] >> D_left;
   fs["M2"] >> C_right;
   fs["D2"] >> D_right;
   Mat R, T, R1, R2;
   fs["R"] >> R;
   fs["T"] >> T;

   cv::Size imageSize(CALIB_WIDTH, CALIB_HEIGHT);
   stereoRectify(C_left, D_left, C_right, D_right, imageSize, R, T, R1, R2, P_left, P_right, Q, CALIB_ZERO_DISPARITY, -1,
         imageSize);
   return;
}

inline static bool readStringList(const string& filename, vector<string>& l)
{
   l.resize(0);
   FileStorage fs(filename, FileStorage::READ);
   if( !fs.isOpened() )
      return false;
   FileNode n = fs.getFirstTopLevelNode();
   if( n.type() != FileNode::SEQ )
      return false;
   FileNodeIterator it = n.begin(), it_end = n.end();
   for( ; it != it_end; ++it )
      l.push_back((string)*it);
   return true;
}

int main(int argc, char	** argv)
{
   string config_file;
   string imagelistfn;
   string cam_params_file;
   string source_folder;
   string output_folder;

   try {
      po::options_description generic("Generic options");
      generic.add_options()
         ("help", "produce help message")
         ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
         ;
      po::options_description config("Config file options");
      config.add_options()
         ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
         ("source_folder,s", po::value<string>(&source_folder), "folder with source")
         ("output_folder,o", po::value<string>(&output_folder), "folder for output")
         ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
         ;

      po::options_description cmdline_options;
      cmdline_options.add(generic).add(config);

      po::options_description config_file_options;
      config_file_options.add(config);
      po::variables_map vm;
      //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
      po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
      notify(vm);
      if(vm.count("help")) {
         cout << generic;
         cout << config;
         return 0;
      }

      ifstream ifs(config_file.c_str());
      if (!ifs) {
         cout << "can not open config file: " << config_file << "\n";
         cout << generic;
         cout << config;
         return 0;
      }
      else {
         po::store(parse_config_file(ifs, config_file_options), vm);
         notify(vm);
      }
      cout << "Configuring done, using:" << endl;

      if(vm.count("camera_params")) {
         cout << "Camera params: ";
         cout << cam_params_file << endl;
      }
      if(vm.count("source_folder")) {
         cout << "Source folder: ";
         cout << source_folder << endl;
      }
      if(vm.count("output_folder")) {
         cout << "Output folder: ";
         cout << output_folder << endl;
      }
      if(vm.count("imglist")) {
         cout << "Image list file: ";
         cout << imagelistfn << endl;
      }
   }
   catch(std::exception& e) {
      cout << e.what() << "\n";
      return 1;
   }

   if(imagelistfn == "")
   {
      cout << "error: no xml image list given." << endl;
      return printHelp();
   }
   if(output_folder == "" || source_folder == "")
   {
      cout << "error: no output or source folder given." << endl;
      return printHelp();
   }

   vector<string> imagelist;
   bool ok = readStringList(imagelistfn, imagelist);
   if(!ok || imagelist.empty())
   {
      cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
      return printHelp();
   }

   visualOdometry(imagelist, cam_params_file, source_folder, output_folder);

   return 0;
}

ext: cc


text:
// -*- C++ -*-

//---------------------------------------------------------------------
//  Created:            Thursday, September 11, 2013
//  Original author:    Alexander Samoilov
//---------------------------------------------------------------------

#include "elliptic_integral.hpp"

#if defined PRINT_ME

#include <iostream>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>

int main()
{
    unsigned nsteps = 42;
    double dx = 1.0 / nsteps;
    for (unsigned i = 1; i < nsteps; ++i) {
        double rk = i * dx;
        double f, e;
        elliptic_integral(rk, f, e);
        double bf = boost::math::ellint_1(sqrt(rk));
        double be = boost::math::ellint_2(sqrt(rk));
        std::cout << "rk: " << rk << " f: " << f << " bf: " << bf << " f/bf: " << f/bf << " e: " << e << " be: " << be << std::endl;
    }
}

#endif

ext: cpp

max_stars_repo_name: alsam/cpp-samples

max_issues_repo_name: alsam/cpp-samples

max_forks_repo_name: alsam/cpp-samples


text:
/**
 * Detect collisions between different geometry.
 * Includes continuous collision detection to compute the time of impact.
 * Supported geometry: point vs edge
 */

#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/broad_phase/hash_grid.hpp>

#include <ccd/ccd.hpp>
#include <ccd/impact.hpp>
#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

///////////////////////////////////////////////////////////////////////////////
// Broad-Phase CCD
///////////////////////////////////////////////////////////////////////////////

/// @brief Use broad-phase method to create a set of candidate collisions.
void detect_collision_candidates_linear(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    Candidates& candidates,
    DetectionMethod method,
    const double inflation_radius = 0.0);

/// @brief Use broad-phase method to create a set of candidate collisions.
void detect_collision_candidates(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates,
    DetectionMethod method,
    const double inflation_radius = 0.0);

/**
 * @brief Use a brute force method to create a set of all candidate collisions.
 *
 * @param[in] vertices     The vertices of the bodies.
 * @param[in] edges        The edges of the bodies defined as pairs of indices
 *                         into the rows of the vertices matrix. Each row is an
 *                         edge.
 * @param[in] group_ids    If two vertices share a group they are not considered
 *                         possible collisions.
 * @param[out] candidates  Vectors of candidates to build.
 */
void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& vertices,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates);

/**
 * @brief Use a hash grid method to create a set of all candidate collisions.
 *
 * @param[in] vertices_t0     The vertices of the bodies at the start of the
 *                            time-step.
 * @param[in] vertices_t1     The vertices of the bodies at the end of the
 *                            time-step.
 * @param[in] edges          The edges of the bodies defined as pairs of indices
 *                           into the rows of the vertices matrix. Each row is
 *                           an edge.
 * @param[in] group_ids      If two vertices share a group they are not
 *                           considered possible collisions.
 * @param[out] candidates    Vector of candidates to build.
 */
void detect_collision_candidates_hash_grid(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0);

/// @brief Use a BVH to create a set of all candidate collisions.
void detect_collision_candidates_linear_bvh(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0);

///////////////////////////////////////////////////////////////////////////////
// Helper functions
///////////////////////////////////////////////////////////////////////////////

inline AABB vertex_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const size_t vi,
    double inflation_radius)
{
    return AABB(
        V0.row(vi).cwiseMin(V1.row(vi)).array() - inflation_radius,
        V0.row(vi).cwiseMax(V1.row(vi)).array() + inflation_radius);
}

inline AABB edge_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::Vector2i& E,
    double inflation_radius)
{
    return AABB(
        vertex_aabb(V0, V1, E(0), inflation_radius),
        vertex_aabb(V0, V1, E(1), inflation_radius));
}

inline AABB face_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::Vector3i& F,
    double inflation_radius)
{
    return AABB(
        vertex_aabb(V0, V1, F(0), inflation_radius),
        AABB(
            vertex_aabb(V0, V1, F(1), inflation_radius),
            vertex_aabb(V0, V1, F(2), inflation_radius)));
}

void detect_collision_candidates_linear_bvh(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    const size_t bodyA_id,
    const size_t bodyB_id,
    Candidates& candidates,
    const double inflation_radius);

} // namespace ipc::rigid

ext: hpp

max_stars_repo_name: ipc-sim/rigid-ipc

max_issues_repo_name: ipc-sim/rigid-ipc

max_forks_repo_name: ipc-sim/rigid-ipc


text:
#ifndef RAY_TRACING_RENDERER_HPP
#define RAY_TRACING_RENDERER_HPP

#include <algorithm>
#include <memory>
#include <string>
#include <iomanip>
#include <iostream>
#include <utility>
#include <vector>

#include <Eigen/Core>

#include <tbb/tbb.h>
#include <tbb/parallel_for.h>

#include "camera.hpp"
#include "image.hpp"
#include "log.hpp"
#include "projection.hpp"
#include "ray.hpp"
#include "renderer.hpp"
#include "scene.hpp"
#include "viewport.hpp"

class RayTracingRenderer : public Renderer {
  public:
    RayTracingRenderer(std::shared_ptr<Viewport> viewport, std::shared_ptr<Projection> projection, std::shared_ptr<Camera> camera);
    void animate(Scene scene, double duration, double step, std::string prefix, ProjectionType projectionType);
    Image render(Scene scene, ProjectionType projectionType);

  private:
    void updateCamera(double step);
    double cameraRadius {2};
    double cameraXZAngle {M_PI_2};
    double cameraAngularVelocity {M_PI/5};

    std::string formatFrameName(std::string prefix, int frameNumber);

    Ray generateRay(int x, int y, ProjectionType projectionType);
    Ray updateRay(Scene scene, Ray ray);

    std::shared_ptr<Color> calculatePixelColor(Scene scene, Ray ray);
    bool calculateSpherePixelColor(std::shared_ptr<Sphere> sphere, Ray ray, double t);
    std::pair<std::shared_ptr<Sphere>, double> findIntersectedBlackHole(Scene scene, Ray ray);
    std::pair<std::shared_ptr<Sphere>, double> findIntersectedStar(Scene scene, Ray ray);
    std::pair<std::shared_ptr<Disk>, double> findIntersectedAccretionDisk(Scene scene, Ray ray);

    std::shared_ptr<Viewport> viewport;
    std::shared_ptr<Projection> projection;
    std::shared_ptr<Camera> camera;

    double timeStep {1};
    int numIterations {100};
};

#endif // RAY_TRACING_RENDERER_HPP

max_stars_repo_name: matheusportela/black-hole-ray-tracer

max_issues_repo_name: matheusportela/black-hole-ray-tracer

max_forks_repo_name: matheusportela/black-hole-ray-tracer


text:
#include "HecubaSession.h"
#include "ArrayDataStore.h"
#include "SpaceFillingCurve.h"
#include "IStorage.h"
#include "yaml-cpp/yaml.h"
#include "ObjSpec.h"
#include "DataModel.h"

#include <cstdlib>
#include <vector>
#include <string>
#include <sstream>
#include <bits/stdc++.h>

#include <iostream>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>

//#include "numpy/arrayobject.h" // FIXME to use the numpy constants NPY_*
#define NPY_ARRAY_C_CONTIGUOUS    0x0001
#define NPY_ARRAY_F_CONTIGUOUS    0x0002
#define NPY_ARRAY_OWNDATA         0x0004
#define NPY_ARRAY_FORCECAST       0x0010
#define NPY_ARRAY_ENSURECOPY      0x0020
#define NPY_ARRAY_ENSUREARRAY     0x0040
#define NPY_ARRAY_ELEMENTSTRIDES  0x0080
#define NPY_ARRAY_ALIGNED         0x0100
#define NPY_ARRAY_NOTSWAPPED      0x0200
#define NPY_ARRAY_WRITEABLE       0x0400
#define NPY_ARRAY_UPDATEIFCOPY    0x1000

// This code adds the decoding of YAML files to ObjSpec (NOT the other way around)
namespace YAML {
    template<>
        struct convert<DataModel::datamodel_spec> {
            static Node encode(const DataModel::datamodel_spec& obj) {
                Node node;

                return node;
            }
            static std::string mapUserType(std::string name) {
                /* Translates 'user type' to 'data model type'. numpy.ndarray --> hecuba.hnumpy.StorageNumpy */
                if ((name == "numpy.ndarray") || (name == "StorageNumpy")) {
                    return "hecuba.hnumpy.StorageNumpy";
                }
                name = ObjSpec::yaml_to_cass(name);
                return name;
            }
            static bool decode(const Node& node, DataModel::datamodel_spec& dmodel) {
                ObjSpec::valid_types obj_type;
                std::vector<std::pair<std::string, std::string>> partitionKeys;
                std::vector<std::pair<std::string, std::string>> clusteringKeys;
                std::vector<std::pair<std::string, std::string>> cols;
                std::string pythonString="";

                std::string attrName, attrType;
                if(!node.IsMap()) { return false; }

                const Node typespec = node["TypeSpec"];
                if (!typespec.IsSequence() || (typespec.size() != 2)) { return false; }
                dmodel.id=typespec[0].as<std::string>();
                pythonString="class " + dmodel.id;

                if (typespec[1].as<std::string>() == "StorageDict") {
                    pythonString="from hecuba import StorageDict\n\n" + pythonString;
                    obj_type=ObjSpec::valid_types::STORAGEDICT_TYPE;
                    const Node keyspec =  node["KeySpec"];
                    if (!keyspec.IsSequence() || (keyspec.size() == 0)) { return false; }

                    pythonString+=" (StorageDict):\n   '''\n   @TypeSpec dict <<";

                    if (!keyspec[0].IsSequence() || (keyspec[0].size() != 2)) { return false; }

                    attrName=keyspec[0][0].as<std::string>();
                    attrType=keyspec[0][1].as<std::string>();
                    partitionKeys.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                    pythonString+=attrName+":"+attrType;

                    for (uint32_t i=1; i<keyspec.size();i++) {
                        if (!keyspec[i].IsSequence() || (keyspec[i].size() != 2)) { return false; }

                        attrName=keyspec[i][0].as<std::string>();
                        attrType=keyspec[i][1].as<std::string>();
                        clusteringKeys.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                        pythonString+=","+attrName+":"+attrType;
                    }
                    pythonString+=">";
                    const Node valuespec =  node["ValueSpec"];
                    if (!valuespec.IsSequence() || (valuespec.size() == 0)) { return false; }
                    for (uint32_t i=0; i<valuespec.size();i++) {
                        if (!valuespec[i].IsSequence() || (valuespec[i].size() != 2)) { return false; }
                        attrName=valuespec[i][0].as<std::string>();
                        attrType=valuespec[i][1].as<std::string>();
                        cols.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                        pythonString+=","+attrName+":"+attrType;
                    }
                    pythonString+=">\n   '''";
                } else if (typespec[1].as<std::string>() == "StorageObject") {
                    pythonString="from hecuba import StorageObj\n\n" + pythonString;
                    pythonString=pythonString+" (StorageObj):\n   '''\n";
                    obj_type=ObjSpec::valid_types::STORAGEOBJ_TYPE;
                    const Node classfields =  node["ClassField"];
                    if (!classfields.IsSequence() || (classfields.size() == 0)) { return false; }
                    partitionKeys.push_back(std::pair<std::string,std::string>("storage_id","uuid"));

                    for (uint32_t i=0; i<classfields.size();i++) {
                        if (!classfields[i].IsSequence() || (classfields[i].size() != 2)) { return false; }
                        attrName=classfields[i][0].as<std::string>();
                        attrType=mapUserType(classfields[i][1].as<std::string>());
                        cols.push_back(std::pair<std::string,std::string>(attrName,attrType));
                        pythonString+="   @ClassField "+attrName+" "+attrType+"\n";
                        std::cout<<"7-"<<i<<": "<<pythonString<<std::endl;
                    }
                    pythonString+="   '''\n";
                    std::cout<<"8: "<<pythonString<<std::endl;

                } else return false;

                dmodel.o=ObjSpec(obj_type,partitionKeys,clusteringKeys,cols,pythonString);
                return true;
            }
        };
};

void HecubaSession::parse_environment(config_map &config) {
    const char * nodePort = std::getenv("NODE_PORT");
    if (nodePort == nullptr) {
        nodePort = "9042";
    }
    config["NODE_PORT"] = std::string(nodePort);

    const char * contactNames = std::getenv("CONTACT_NAMES");
    if (contactNames == nullptr) {
        contactNames = "127.0.0.1";
    }
    config["CONTACT_NAMES"] = std::string(contactNames);

    const char * createSchema = std::getenv("CREATE_SCHEMA");
    std::string createSchema2 ;
    if (createSchema == nullptr) {
        createSchema2 = std::string("true");
    } else {
        createSchema2 = std::string(createSchema);
        std::transform(createSchema2.begin(), createSchema2.end(), createSchema2.begin(),
                [](unsigned char c){ return std::tolower(c); });
    }
    config["CREATE_SCHEMA"] = createSchema2;

    const char * executionName = std::getenv("EXECUTION_NAME");
    if (executionName == nullptr) {
        executionName = "my_app";
    }
    config["EXECUTION_NAME"] = std::string(executionName);

    const char * timestampedWrites = std::getenv("TIMESTAMPED_WRITES");
    std::string timestampedWrites2;
    if (timestampedWrites == nullptr) {
        timestampedWrites2 = std::string("false");
    } else {
        timestampedWrites2 = std::string(timestampedWrites);
        std::transform(timestampedWrites2.begin(), timestampedWrites2.end(), timestampedWrites2.begin(),
                [](unsigned char c){ return std::tolower(c); });
    }
    config["TIMESTAMPED_WRITES"] = timestampedWrites2;

        //{"writer_buffer",      std::to_string(writer_queue)},??? == WRITE_BUFFER_SIZE?
    const char * writeBufferSize = std::getenv("WRITE_BUFFER_SIZE");
    if (writeBufferSize == nullptr) {
        writeBufferSize = "1000";
    }
    config["WRITE_BUFFER_SIZE"] = std::string(writeBufferSize);

        ///writer_par ==> 'WRITE_CALLBACKS_NUMBER'
    const char *writeCallbacksNum = std::getenv("WRITE_CALLBACKS_NUMBER");
    if (writeCallbacksNum == nullptr) {
        writeCallbacksNum = "16";
    }
    config["WRITE_CALLBACKS_NUMBER"] = std::string(writeCallbacksNum);

    const char * cacheSize = std::getenv("MAX_CACHE_SIZE");
    if (cacheSize == nullptr) {
        cacheSize = "1000";
    }
    config["MAX_CACHE_SIZE"] = std::string(cacheSize);

    const char *replicationFactor = std::getenv("REPLICA_FACTOR");
    if (replicationFactor == nullptr) {
        replicationFactor = "1";
    }
    config["REPLICA_FACTOR"] = std::string(replicationFactor);

    const char *replicationStrategy = std::getenv("REPLICATION_STRATEGY");
    if (replicationStrategy == nullptr) {
        replicationStrategy = "SimpleStrategy";
    }
    config["REPLICATION_STRATEGY"] = std::string(replicationStrategy);

    const char *replicationStrategyOptions = std::getenv("REPLICATION_STRATEGY_OPTIONS");
    if (replicationStrategyOptions == nullptr) {
        replicationStrategyOptions = "";
    }
    config["REPLICATION_STRATEGY_OPTIONS"] = replicationStrategyOptions;

    if (config["REPLICATION_STRATEGY"] == "SimpleStrategy") {
        config["REPLICATION"] = std::string("{'class' : 'SimpleStrategy', 'replication_factor': ") + config["REPLICA_FACTOR"] + "}";
    } else {
        config["REPLICATION"] = std::string("{'class' : '") + config["REPLICATION_STRATEGY"] + "', " + config["REPLICATION_STRATEGY_OPTIONS"] + "}";
    }
}

CassError HecubaSession::run_query(std::string query) const{
	CassStatement *statement = cass_statement_new(query.c_str(), 0);

    //std::cout << "DEBUG: HecubaSession.run_query : "<<query<<std::endl;
    CassFuture *result_future = cass_session_execute(const_cast<CassSession *>(storageInterface->get_session()), statement);
    cass_statement_free(statement);

    CassError rc = cass_future_error_code(result_future);
    if (rc != CASS_OK) {
        printf("Query execution error: %s - %s\n", cass_error_desc(rc), query.c_str());
    }
    cass_future_free(result_future);
    return rc;
}

void HecubaSession::decodeNumpyMetadata(HecubaSession::NumpyShape *s, void* metadata) {
    // Numpy Metadata(all unsigned): Ndims + Dim1 + Dim2 + ... + DimN  (Layout in C by default)
    unsigned* value = (unsigned*)metadata;
    s->ndims = *(value);
    value ++;
    s->dim = (unsigned *)malloc(s->ndims);
    for (unsigned i = 0; i < s->ndims; i++) {
        s->dim[i] = *(value + i);
    }
}
void HecubaSession::getMetaData(void * raw_numpy_meta, ArrayMetadata &arr_metas) {
    std::vector <uint32_t> dims;
    std::vector <uint32_t> strides;

    // decode void *metadatas
    HecubaSession:: NumpyShape * s = new HecubaSession::NumpyShape();
    decodeNumpyMetadata(s, raw_numpy_meta);
    uint32_t acum=1;
    for (uint32_t i=0; i < s->ndims; i++) {
        dims.push_back( s->dim[i]);
        acum *= s->dim[i];
    }
    for (uint32_t i=0; i < s->ndims; i++) {
        strides.push_back(acum * sizeof(double));
        acum /= s->dim[s->ndims-1-i];
    }
    uint32_t flags=NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_WRITEABLE | NPY_ARRAY_ALIGNED;

    arr_metas.dims = dims;
    arr_metas.strides = strides;
    arr_metas.elem_size = sizeof(double);
    arr_metas.flags = flags;
    arr_metas.partition_type = ZORDER_ALGORITHM;
    arr_metas.typekind = 'f';
    arr_metas.byteorder = '=';
}

void HecubaSession::registerNumpy(ArrayMetadata &numpy_meta, std::string name, uint64_t* uuid) {

    //std::cout<< "DEBUG: HecubaSession::registerNumpy BEGIN "<< name << UUID2str(uuid)<<std::endl;
    void *keys = std::malloc(sizeof(uint64_t *));
    uint64_t *c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);//new uint64_t[2];
    c_uuid[0] = *uuid;
    c_uuid[1] = *(uuid + 1);

    std::memcpy(keys, &c_uuid, sizeof(uint64_t *));

    char *c_name = (char *) std::malloc(name.length() + 1);
    std::memcpy(c_name, name.c_str(), name.length() + 1);

    //COPY VALUES
    int offset = 0;
    uint64_t size_name = strlen(c_name)+1;
    uint64_t size = 0;

    //size of the vector of dims
    size += sizeof(uint32_t) * numpy_meta.dims.size();

    //plus the other metas
    size += sizeof(numpy_meta.elem_size)
		+ sizeof(numpy_meta.partition_type)
		+ sizeof(numpy_meta.flags)
		+ sizeof(uint32_t)*numpy_meta.strides.size() // dims & strides
		+ sizeof(numpy_meta.typekind)
		+ sizeof(numpy_meta.byteorder);

    //allocate plus the bytes counter
    unsigned char *byte_array = (unsigned char *) malloc(size+ sizeof(uint64_t));
    unsigned char *name_array = (unsigned char *) malloc(size_name);

    // copy table name
    memcpy(name_array, c_name, size_name); //lgarrobe

    // Copy num bytes
    memcpy(byte_array+offset, &size, sizeof(uint64_t));
    offset += sizeof(uint64_t);

    //copy everything from the metas
	//	flags int, elem_size int, partition_type tinyint,
    //       dims list<int>, strides list<int>, typekind text, byteorder text

    memcpy(byte_array + offset, &numpy_meta.flags, sizeof(numpy_meta.flags));
    offset += sizeof(numpy_meta.flags);

    memcpy(byte_array + offset, &numpy_meta.elem_size, sizeof(numpy_meta.elem_size));
    offset += sizeof(numpy_meta.elem_size);

    memcpy(byte_array + offset, &numpy_meta.partition_type, sizeof(numpy_meta.partition_type));
    offset += sizeof(numpy_meta.partition_type);

    memcpy(byte_array + offset, &numpy_meta.typekind, sizeof(numpy_meta.typekind));
    offset +=sizeof(numpy_meta.typekind);

    memcpy(byte_array + offset, &numpy_meta.byteorder, sizeof(numpy_meta.byteorder));
    offset +=sizeof(numpy_meta.byteorder);

    memcpy(byte_array + offset, numpy_meta.dims.data(), sizeof(uint32_t) * numpy_meta.dims.size());
    offset +=sizeof(uint32_t)*numpy_meta.dims.size();

    memcpy(byte_array + offset, numpy_meta.strides.data(), sizeof(uint32_t) * numpy_meta.strides.size());
    offset +=sizeof(uint32_t)*numpy_meta.strides.size();

    //memcpy(byte_array + offset, &numpy_meta.inner_type, sizeof(numpy_meta.inner_type));
    //offset += sizeof(numpy_meta.inner_type);

    int offset_values = 0;
    char *values = (char *) malloc(sizeof(char *)*4);

    uint64_t *base_numpy = (uint64_t *) malloc(sizeof(uint64_t) * 2);//new uint64_t[2];
    memcpy(base_numpy, c_uuid, sizeof(uint64_t)*2);
    //std::cout<< "DEBUG: HecubaSession::registerNumpy &base_numpy = "<<base_numpy<<std::endl;
    std::memcpy(values, &base_numpy, sizeof(uint64_t *));  // base_numpy
    offset_values += sizeof(unsigned char *);

    char *class_name=(char*)malloc(strlen("hecuba.hnumpy.StorageNumpy")+1);
    strcpy(class_name, "hecuba.hnumpy.StorageNumpy");
    //std::cout<< "DEBUG: HecubaSession::registerNumpy &class_name = "<<class_name<<std::endl;
    memcpy(values+offset_values, &class_name, sizeof(unsigned char *)); //class_name
    offset_values += sizeof(unsigned char *);

    //std::cout<< "DEBUG: HecubaSession::registerNumpy &name = "<<name_array<<std::endl;
    memcpy(values+offset_values, &name_array, sizeof(unsigned char *)); //name
    offset_values += sizeof(unsigned char *);

    //std::cout<< "DEBUG: HecubaSession::registerNumpy &np_meta = "<<byte_array<<std::endl;
    memcpy(values+offset_values, &byte_array,  sizeof(unsigned char *)); // numpy_meta
    offset_values += sizeof(unsigned char *);

    try {
        numpyMetaWriter->write_to_cassandra(keys, values);
    }
    catch (std::exception &e) {
        std::cerr << "HecubaSession::registerNumpy: Error writing" <<std::endl;
        std::cerr << e.what();
        throw e;
    };

}

void HecubaSession::createSchema(void) {
    // Create hecuba
    std::vector<std::string> queries;
    std::string create_hecuba_keyspace = std::string(
            "CREATE KEYSPACE IF NOT EXISTS hecuba  WITH replication = ") +  config["REPLICATION"];
    queries.push_back(create_hecuba_keyspace);
    std::string create_hecuba_qmeta = std::string(
            "CREATE TYPE IF NOT EXISTS hecuba.q_meta("
            "mem_filter text,"
            "from_point frozen<list<double>>,"
            "to_point frozen<list<double>>,"
            "precision float);");
    queries.push_back(create_hecuba_qmeta);
    std::string create_hecuba_npmeta = std::string(
            "CREATE TYPE IF NOT EXISTS hecuba.np_meta ("
            "flags int, elem_size int, partition_type tinyint,"
            "dims list<int>, strides list<int>, typekind text, byteorder text)");
    queries.push_back(create_hecuba_npmeta);
    std::string create_hecuba_istorage = std::string(
            "CREATE TABLE IF NOT EXISTS hecuba.istorage"
            "(storage_id uuid,"
            "class_name text,name text,"
            "istorage_props map<text,text>,"
            "tokens list<frozen<tuple<bigint,bigint>>>,"
            "indexed_on list<text>,"
            "qbeast_random text,"
            "qbeast_meta frozen<q_meta>,"
            "numpy_meta frozen<np_meta>,"
            "block_id int,"
            "base_numpy uuid,"
            "view_serialization blob,"
            "primary_keys list<frozen<tuple<text,text>>>,"
            "columns list<frozen<tuple<text,text>>>,"
            "PRIMARY KEY(storage_id));");
    queries.push_back(create_hecuba_istorage);
    // Create keyspace EXECUTION_NAME
    std::string create_keyspace = std::string(
            "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
        std::string(" WITH replication = ") +  config["REPLICATION"];
    queries.push_back(create_keyspace);

    for(auto q: queries) {
        CassError rc = run_query(q);
        if (rc != CASS_OK) {
            std::string msg = std::string("HecubaSession:: Error Creating Schema executing: ") + q;
            throw ModuleException(msg);
        }
    }
}

/***************************
 * PUBLIC
 ***************************/

/* Constructor: Establish connection with underlying storage system */
HecubaSession::HecubaSession() : currentDataModel(NULL) {

    parse_environment(this->config);

    /* Establish connection */
    this->storageInterface = std::make_shared<StorageInterface>(stoi(config["NODE_PORT"]), config["CONTACT_NAMES"]);
    //this->storageInterface = new StorageInterface(stoi(config["NODE_PORT"]), config["CONTACT_NAMES"]);

    if (this->config["CREATE_SCHEMA"] == "true") {
        createSchema();
    }

// TODO: extend writer to support lists	std::vector<config_map> pkeystypes = { {{"name", "storage_id"}} };
// TODO: extend writer to support lists	std::vector<config_map> ckeystypes = {};
// TODO: extend writer to support lists	std::vector<config_map> colstypes = {{{"name", "class_name"}},
// TODO: extend writer to support lists							{{"name", "name"}},
// TODO: extend writer to support lists							{{"name", "tokens"}},   //list
// TODO: extend writer to support lists							{{"name", "primary_keys"}}, //list
// TODO: extend writer to support lists							{{"name", "columns"}},  //list
// TODO: extend writer to support lists							{{"name", "indexed_on"}} }; //list
// TODO: extend writer to support lists	dictMetaWriter = storageInterface->make_writer("istorage", "hecuba",
// TODO: extend writer to support lists													pkeystypes, colstypes,
// TODO: extend writer to support lists													config);
// TODO: extend writer to support lists

std::vector<config_map> pkeystypes_n = { {{"name", "storage_id"}} };
std::vector<config_map> ckeystypes_n = {};
std::vector<config_map> colstypes_n = {
						{{"name", "base_numpy"}},
                        {{"name", "class_name"}},
						{{"name", "name"}},
						{{"name", "numpy_meta"}},
						//{{"name", "block_id"}}, //NOT USED
						//{{"name", "view_serialization"}},  //Used by Python, uses pickle format. Let it be NULL and initialized at python code
};
						// TODO: extend writer to support lists {{"name", "tokens"}} }; //list

// The attributes stored in istorage for all numpys are the same, we use a single writer for the session
numpyMetaWriter = storageInterface->make_writer("istorage", "hecuba",
												pkeystypes_n, colstypes_n,
												config);

}

HecubaSession::~HecubaSession() {
    delete(currentDataModel);
    delete(numpyMetaWriter);
}

/* loadDataModel: loads a DataModel from 'model_filename' path which should be
 * a YAML file. It also generates its corresponding python generated class in
 * the same directory where the model resides or in the 'pythonSpecPath'
 * directory. */
void HecubaSession::loadDataModel(const char * model_filename, const char * pythonSpecPath=NULL) {

    if (currentDataModel != NULL) {
        std::cerr << "WARNING: HecubaSession::loadDataModel: DataModel already defined. Discarded and load again"<<std::endl;
        delete(currentDataModel);
    }

    // TODO: parse file to get model information NOW HARDCODED

    // class dataModel(StorageDict):
    //    '''
    //         @TypeSpec dict <<lat:int,ts:int>,metrics:numpy.ndarray>
    //    '''

    DataModel* d = new DataModel();

    if (model_filename == NULL) {
		throw std::runtime_error("Trying to load a NULL model file name ");
    }

    // Detect dirname and basename for 'model_filename'
    std::string pythonPath("");
    std::string baseName(model_filename);

    size_t pos = baseName.find_last_of('/');
    if (pos != std::string::npos) { // model_filename has a path
        if (pythonSpecPath != NULL) {
            pythonPath = pythonSpecPath;
        } else {
            pythonPath = baseName.substr(0,pos);
        }
        baseName = baseName.substr(pos+1, baseName.size());
    }
    // Remove .yaml extension
    pos = baseName.find_last_of('.');
    if (pos != std::string::npos) {
        baseName = baseName.substr(0, pos);
    }
    // Create moduleName
    std::string moduleName;
    if (pythonPath == "") {
        moduleName = baseName;
    } else {
        moduleName = pythonPath + "/" + baseName;
    }

    d->setModuleName(moduleName);

    // Add .py extension to moduleName
    moduleName =  moduleName + ".py";

    std::ofstream fd(moduleName);

    YAML::Node node = YAML::LoadFile(model_filename);

    assert(node.Type() == YAML::NodeType::Sequence);

    for(std::size_t i=0;i<node.size();i++) {
		DataModel::datamodel_spec x = node[i].as<DataModel::datamodel_spec>();
		d->addObjSpec(x.id, x.o);
        fd<<x.o.getPythonString();
    }
    fd.close();

    // ALWAYS Add numpy (just in case) ##############################
    std::vector<std::pair<std::string, std::string>> pkeystypes_numpy = {
                                  {"storage_id", "uuid"},
                                  {"cluster_id", "int"}
    };
    std::vector<std::pair<std::string, std::string>> ckeystypes_numpy = {{"block_id","int"}};
    std::vector<std::pair<std::string, std::string>> colstypes_numpy = {
                                  {"payload", "blob"},
    };
    d->addObjSpec(ObjSpec::valid_types::STORAGENUMPY_TYPE, "hecuba.hnumpy.StorageNumpy", pkeystypes_numpy, ckeystypes_numpy, colstypes_numpy);
    // ##############################

    currentDataModel = d;
}

IStorage* HecubaSession::createObject(const char * id_model, const char * id_object, void * metadata, void* value) {
    // Create Cassandra tables 'ksp.id_object' for object 'id_object' according to its type 'id_model' in 'model'
    // TODO: create type depending on 'model', now its only dictionary

    DataModel* model = currentDataModel;
    if (model == NULL) {
        throw ModuleException("HecubaSession::createObject No data model loaded");
    }

    IStorage * o;

    ObjSpec oType = model->getObjSpec(id_model);
    //std::cout << "DEBUG: HecubaSession::createObject '"<<id_model<< "' ==> " <<oType.debug()<<std::endl;

    std::string object_name(config["EXECUTION_NAME"] + "." + std::string(id_object));
    uint64_t *c_uuid = generateUUID5(object_name.c_str()); // UUID for the new object

    switch(oType.getType()) {
        case ObjSpec::valid_types::STORAGEOBJ_TYPE:
            {
                // StorageObj case
                //  Create table 'class_name' "CREATE TABLE ksp.class_name (storage_id UUID, nom typ, ... PRIMARY KEY (storage_id))"
                std::string query = "CREATE TABLE IF NOT EXISTS " +
                    config["EXECUTION_NAME"] + "." + id_model +
                    oType.table_attr;

                CassError rc = run_query(query);
                if (rc != CASS_OK) {
                    if (rc == CASS_ERROR_SERVER_INVALID_QUERY) { // keyspace does not exist
                        std::cout<< "HecubaSession Creating keyspace "<< config["EXECUTION_NAME"]<< std::endl;
                        std::string create_keyspace = std::string(
                                "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
                            std::string(" WITH replication = ") +  config["REPLICATION"];
                        rc = run_query(create_keyspace);
                        if (rc != CASS_OK) {
                            std::string msg = std::string("HecubaSession:: Error executing query ") + create_keyspace;
                            throw ModuleException(msg);
                        } else {
                            rc = run_query(query); // Repeat table creation after creating keyspace
                            if (rc != CASS_OK) {
                                std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                                throw ModuleException(msg);
                            }
                        }
                    } else {
                        std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                        throw ModuleException(msg);
                    }
                }
                // Table for storageobj class created
                // Add entry to ISTORAGE: TODO add the tokens attribute
                std::string name = config["EXECUTION_NAME"] + "." + id_object;
                std::string insquery = std::string("INSERT INTO ") +
                    std::string("hecuba.istorage") +
                    std::string("(storage_id, name, class_name, columns)") +
                    std::string("VALUES ") +
                    std::string("(") +
                    UUID2str(c_uuid) + std::string(", ") +
                    "'" + name + "'" + std::string(", ") +
                    "'" + model->getModuleName() + "." + id_model + "'" + std::string(", ") +
                    oType.getColsStr() +
                    std::string(")");
                run_query(insquery);

                //  Create Writer for storageobj
                std::vector<config_map>* keyNamesDict = oType.getKeysNamesDict();
                std::vector<config_map>* colNamesDict = oType.getColsNamesDict();

                Writer *writer = storageInterface->make_writer(id_model, config["EXECUTION_NAME"].c_str(),
                          *keyNamesDict, *colNamesDict,
                          config);
                delete keyNamesDict;
                delete colNamesDict;
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, writer);

            }
            break;
        case ObjSpec::valid_types::STORAGEDICT_TYPE:
            {
                // Dictionary case
                //  Create table 'name' "CREATE TABLE ksp.name (nom typ, nom typ, ... PRIMARY KEY (nom, nom))"
                bool new_element = true;
                std::string query = "CREATE TABLE " +
                    config["EXECUTION_NAME"] + "." + id_object +
                    oType.table_attr;

                CassError rc = run_query(query);
                if (rc != CASS_OK) {
                    if (rc == CASS_ERROR_SERVER_ALREADY_EXISTS ) {
                        new_element = false; //OOpps, creation failed. It is an already existent object.
                    } else if (rc == CASS_ERROR_SERVER_INVALID_QUERY) {
                        std::cout<< "HecubaSession Creating keyspace "<< config["EXECUTION_NAME"]<< std::endl;
                        std::string create_keyspace = std::string(
                                "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
                            std::string(" WITH replication = ") +  config["REPLICATION"];
                        rc = run_query(create_keyspace);
                        if (rc != CASS_OK) {
                            std::string msg = std::string("HecubaSession:: Error executing query ") + create_keyspace;
                            throw ModuleException(msg);
                        } else {
                            rc = run_query(query);
                            if (rc != CASS_OK) {
                                if (rc == CASS_ERROR_SERVER_ALREADY_EXISTS) {
                                    new_element = false; //OOpps, creation failed. It is an already existent object.
                                }  else {
                                    std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                                    throw ModuleException(msg);
                                }
                            }
                        }
                    } else {
                        std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                        throw ModuleException(msg);
                    }
                }

                if (new_element) {
                    //  Add entry to hecuba.istorage: TODO add the tokens attribute
                    std::string name = config["EXECUTION_NAME"] + "." + id_object;
                    // TODO EXPECTED:vvv NOW HARDCODED
                    //classname = id_model
                    // keys = {c_uuid}, values={name, class_name, primary_keys, columns } # no tokens, no numpy_meta, ...
                    //try {
                    //	dictMetaWriter->write_to_cassandra(keys, values);
                    //}
                    //catch (std::exception &e) {
                    //	std::cerr << "Error writing in registering" <<std::endl;
                    //	std::cerr << e.what();
                    //	throw e;
                    //}
                    // EXPECTED^^^^
                    // Example: insert into hecuba.istorage (storage_id, primary_keys) values (3dd30d5d-b0d4-45b5-a21a-c6ad313007fd, [('lat','int'),('ts','int')]);
                    std::string insquery = std::string("INSERT INTO ") +
                        std::string("hecuba.istorage") +
                        std::string("(storage_id, name, class_name, primary_keys, columns)") +
                        std::string("VALUES ") +
                        std::string("(") +
                        UUID2str(c_uuid) + std::string(", ") +
                        "'" + name + "'" + std::string(", ") +
                        "'" + id_model + "'" + std::string(", ") +
                        oType.getKeysStr() + std::string(", ") +
                        oType.getColsStr() +
                        std::string(")");
                    run_query(insquery);
                } else {
                    std::cerr << "WARNING: Object "<<id_object<<" already exists. Trying to overwrite it. It may fail if the schema does not match."<<std::endl;
                    // TODO: THIS IS NOT WORKING. We need to get the storage_id (c_uuid) from istorage DISABLE
                    // TODO: Check the schema in Cassandra matches the model
                }

                //  Create Writer for dictionary
                std::vector<config_map>* keyNamesDict = oType.getKeysNamesDict();
                std::vector<config_map>* colNamesDict = oType.getColsNamesDict();

                Writer *writer = storageInterface->make_writer(id_object, config["EXECUTION_NAME"].c_str(),
                          *keyNamesDict, *colNamesDict,
                          config);
                delete keyNamesDict;
                delete colNamesDict;
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, writer);
            }
            break;

        case ObjSpec::valid_types::STORAGENUMPY_TYPE:
            {
                // Create table
                std::string query = "CREATE TABLE IF NOT EXISTS " + config["EXECUTION_NAME"] + "." + id_object +
                    " (storage_id uuid, cluster_id int, block_id int, payload blob, "
                    "PRIMARY KEY((storage_id,cluster_id),block_id)) "
                    "WITH compaction = {'class': 'SizeTieredCompactionStrategy', 'enabled': false};";

                this->run_query(query);

                // StorageNumpy
                ArrayDataStore *array_store = new ArrayDataStore(id_object, config["EXECUTION_NAME"].c_str(),
                        this->storageInterface->get_session(), config);
                //std::cout << "DEBUG: HecubaSession::createObject After ArrayDataStore creation " <<std::endl;

                // Create entry in hecuba.istorage for the new numpy
                ArrayMetadata numpy_metas;
                getMetaData(metadata, numpy_metas); // numpy_metas = getMetaData(metadata);
                //std::cout<< "DEBUG: HecubaSession::createObject After metadata creation " <<std::endl;
                registerNumpy(numpy_metas, id_object, c_uuid);
                //std::cout<< "DEBUG: HecubaSession::createObject After REGISTER numpy into ISTORAGE" <<std::endl;

                //Create keys, values to store the numpy
                //double* tmp = (double*)value;
                //std::cout<< "DEBUG: HecubaSession::createObject BEFORE store FIRST element in NUMPY is "<< *tmp << " and second is "<<*(tmp+1)<< " 3rd " << *(tmp+2)<< std::endl;
                array_store->store_numpy_into_cas(c_uuid, numpy_metas, value);
                array_store->wait_stores();

                // TODO: el writer para pasarselo al istorage esta en la cache table del array datastor: anaydir getCache al arraydatastore
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, array_store->getWriteCache()->get_writer());

            }
            break;
        default:
            throw ModuleException("HECUBA Session: createObject Unknown type ");// + std::string(oType.objtype));
            break;
    }
    //std::cout << "DEBUG: HecubaSession::createObject DONE" << std::endl;
    return o;
}

uint64_t* HecubaSession::generateUUID(void) const {
    uint64_t *c_uuid; // UUID for the new object
    c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);

    boost::uuids::random_generator gen;
    boost::uuids::uuid u = gen();

    memcpy(c_uuid, &u, 16);

    return c_uuid;
}

uint64_t* HecubaSession::generateUUID5(const char* name) const{
    /* uses sha1 */
    uint64_t *c_uuid; // UUID for the new object
    c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);

    boost::uuids::string_generator gen1;
    boost::uuids::uuid dns_namespace_uuid = gen1("{6ba7b810-9dad-11d1-80b4-00c04fd430c8}");
    boost::uuids::name_generator gen(dns_namespace_uuid);

    // boost::uuids::name_generator_md5 gen(boost::uuids::ns::dns());

    boost::uuids::uuid u = gen(name);

    memcpy(c_uuid, &u, 16);

    return c_uuid;
}

std::string HecubaSession::UUID2str(uint64_t* c_uuid) {
    /* This MUST match with the 'cass_statement_bind_uuid' result */
    char str[37] = {};
    unsigned char* uuid = reinterpret_cast<unsigned char*>(c_uuid);
    //std::cout<< "HecubaSession: uuid2str: BEGIN "<<std::hex<<c_uuid[0]<<c_uuid[1]<<std::endl;
    sprintf(str,
        "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x",
        uuid[0], uuid[1], uuid[2], uuid[3],
        uuid[4], uuid[5],
        uuid[6], uuid[7],
        uuid[8], uuid[9],
        uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]
        );
    //std::cout<< "HecubaSession: uuid2str: "<<str<<std::endl;
    return std::string(str);
}

DataModel* HecubaSession::getDataModel() {
    return currentDataModel;
}

max_stars_repo_name: cugni/hecuba

max_issues_repo_name: cugni/hecuba

max_forks_repo_name: cugni/hecuba