#include "gtest/gtest.h"

#include <boost/bimap.hpp>

#include "opencv2/opencv.hpp"
#include "utils/utils.hpp"
#include <unordered_map>

using namespace cv;
using namespace std;

Mat formatTransformationMat2(const Mat transformation_matrix)
{
    cv::Mat m = cv::Mat::ones(2, 3, CV_64F);
    m.at<double>(0, 0) = transformation_matrix.at<double>(0, 0);
    m.at<double>(0, 1) = transformation_matrix.at<double>(0, 1);
    m.at<double>(0, 2) = transformation_matrix.at<double>(0, 2);
    m.at<double>(1, 0) = transformation_matrix.at<double>(1, 0);
    m.at<double>(1, 1) = transformation_matrix.at<double>(1, 1);
    m.at<double>(1, 2) = transformation_matrix.at<double>(1, 2);
    return m;
}

void runTheTest()
{

//    //load the image
//    //apply a big transformation to it, resize the image if you have to
//    //create a getKeyPointsFunction you can call from the c++ side
//
//    double rotation = 0;
//    double scale = 1;
//    Mat inputImage = cv::imread("./input/lennaWithGreenDots.jpg");
//    for (int j = 0; j < 4; j++){
//        for (int i = 0; i<360; i+= 1){
//            Mat transformationMartix;
//            Size newImageSize;
//            tie(transformationMartix, newImageSize) = calcTransformationMatrix(inputImage.size(), rotation+i, scale*(j+1));
//            cout << "Size: " << newImageSize << endl;
//            cout << "The output mat: " << endl;
//            cout << Mat(transformationMartix) << endl;
//
//            Mat outputImage(newImageSize.height, newImageSize.width, CV_8UC3, Scalar(0,0,0));
//            warpAffine(inputImage, outputImage, formatTransformationMat2(transformationMartix), outputImage.size());
//            imshow("output", outputImage);
//            waitKey(1);
//        }
//    }
//    waitKey();
////    //Then apply it
////    //Then get the keypoints for both images
////    //Then calc the transformation matrix and project both keypoints to both...
////    //Then calc all the stats...

}

TEST(accuracyTest, basic) {
//    runTheTest();

}

TEST(utilsTest, getTheKeypointsTest) {
//    Mat inputImage = cv::imread("./input/rick1.jpg");
//    auto keypoints = getKeypoints(inputImage);
//    drawKeypoints(keypoints, inputImage);
//    cv::imshow("here...", inputImage);
//    cv::waitKey();
//    cout << "Number of keypoints: " << keypoints.size() << endl;
}

TEST(utilsTest, fullKeypointTest) {
//    Mat inputImage = cv::imread("./input/rick1.jpg");
//    auto keypointsForImageOne = getKeypoints(inputImage);
//    convertKeypointsVectorToMat
}

TEST(utilsTest, testingTheConvertingOfKeypoints) {
//    Mat inputImage = cv::imread("./input/rick1.jpg");
//    auto keypointsForImageOne = getKeypoints(inputImage);
//    Mat res = convertKeypointsVectorToMat(keypointsForImageOne);
//    //Mat res = cv::Mat::zeros(1,1, 1);
//    cout << "the resultant matrix" << endl;
//    cout << res << endl;
//    applyTransformationMatrixToKeypointVector(keypointsForImageOne, );
}

typedef std::unordered_map<Keypoint, Keypoint> MatchingKeypointMap;

MatchingKeypointMap getMatchingKeypointsTwoWayMap(vector<Keypoint> image1Keypoints, vector<Keypoint> image2Keypoints, cv::Mat transformationMatFromImage1To2)
{
    MatchingKeypointMap result;
    vector<Keypoint> oneToTwo = applyTransformationMatrixToKeypointVector(image1Keypoints, transformationMatFromImage1To2);

    double threshold = 2.0;
    for (auto keypointFromImage2 : image2Keypoints)
    {
        //check the dist
        for (unsigned int i = 0; i < oneToTwo.size(); i++)
        {
            if (getKeypointDistance(keypointFromImage2, oneToTwo[i]) < threshold)
            {
                auto keyPointFromImage1 = image1Keypoints[i];
                result.insert({keyPointFromImage1, keypointFromImage2});
            }
        }
    }

    return result;
}

typedef std::unordered_map<Triangle, Triangle> MatchingTriangleMap;

tuple<vector<Keypoint>, vector<Keypoint>> splitKeypointMap(MatchingKeypointMap map){
    vector<Keypoint> kp1;
    vector<Keypoint> kp2;

    for (auto entry: map)
    {
        kp1.push_back(entry.first);
        kp2.push_back(entry.second);
    }
    return tuple<vector<Keypoint>, vector<Keypoint>>(kp1, kp2);
}

Triangle convertTriangleUsingMap(Triangle triangle, MatchingKeypointMap oneToTwo) {
    vector<Keypoint> result;
    for (auto kp: triangle.toKeypoints())
    {
        result.push_back(oneToTwo[kp]);
    }
    return result;
}

MatchingTriangleMap buildMatchingTriMap(vector<Triangle> tris1, vector<Triangle> tris2, MatchingKeypointMap unorderedMap) {
    MatchingTriangleMap result;
    for (auto tri1: tris1)
    {
        for (auto tri2: tris2)
        {
            Triangle tri1Convert = convertTriangleUsingMap(tri1, unorderedMap);
            if (tri1Convert == tri2)
            {
                result.insert({tri1, tri2});
                break;
            }
        }
    }

    return result;
}

MatchingTriangleMap getMatchingTriangleTwoWayMap(MatchingKeypointMap oneToTwo) {

    vector<Keypoint> image1Kp;
    vector<Keypoint> image2Kp;
    tie(image1Kp, image2Kp) = splitKeypointMap(oneToTwo);

    auto tris1 = buildTrianglesFromKeypoints(image1Kp);
    auto tris2 = buildTrianglesFromKeypoints(image2Kp);

    return buildMatchingTriMap(tris1, tris2, oneToTwo);
}

unsigned long xorshf962(void) {          //period 2^96-1
    unsigned long t;
    x ^= x << 16;
    x ^= x >> 5;
    x ^= x << 1;

    t = x;
    x = y;
    y = z;
    z = t ^ x ^ y;

    return z;
}

void dumpMathcingKeypointTestingInfoToJson(unsigned int count, unsigned int totalCount, unsigned int cout,
                                           MatchingKeypointMap map, unsigned int imagesCount, unsigned int image1Count,
                                           unsigned int image2Count, unsigned int trianglesTotalCount,
                                           unsigned int image2TrianglesTotalCount)
{
    //dump it!!!

}

TEST(utilsTest, testingTheConvertingOfKeypoints2)
{
    double rotation = 45;
    double scale = 2;
    Mat inputImage = cv::imread("./input/lennaWithGreenDots.jpg");
    Mat transformationMartix;
    Size newImageSize;
    tie(transformationMartix, newImageSize) = calcTransformationMatrix(inputImage.size(), rotation, scale);

    Mat outputImage(newImageSize.height, newImageSize.width, CV_8UC3, Scalar(0,0,0));
    warpAffine(inputImage, outputImage, formatTransformationMat2(transformationMartix), outputImage.size());

    auto keypointsImage1 = getKeypoints(inputImage);
    auto keypointsImage2 = getKeypoints(outputImage);

    vector<Keypoint> oneToTwo = applyTransformationMatrixToKeypointVector(keypointsImage1, transformationMartix);
    vector<Keypoint> twoToOne = applyTransformationMatrixToKeypointVector(keypointsImage2, transformationMartix.inv());

    drawKeypoints(keypointsImage1, inputImage);
    drawKeypoints(twoToOne, inputImage, cv::Scalar(0,255,0));
    drawKeypoints(keypointsImage2, outputImage);
    drawKeypoints(oneToTwo, outputImage, cv::Scalar(0,255,0));
    auto tempMap = getMatchingKeypointsTwoWayMap(keypointsImage1, keypointsImage2, transformationMartix);

    cout << "Number of matching keypints:   " << tempMap.size() << endl;
    cout << "Number of keypoints in image1: " << keypointsImage1.size() << " image2: " << keypointsImage2.size() << endl;
    cout << "average:   " << ((keypointsImage1.size() + keypointsImage2.size())/2) << endl;
    cout << "%average:  " << 100.0*(float)(tempMap.size())/(float)((keypointsImage1.size() + keypointsImage2.size())/2) << "%" << endl;

    for (auto entry : tempMap)
    {
        drawSingleKeypoint(entry.first, inputImage, cv::Scalar(255,0,0));
        drawSingleKeypoint(entry.second, outputImage, cv::Scalar(255,0,0));
    }

    cv::imwrite("image1_keypoints.jpg", inputImage);
    cv::imwrite("image2_keypoints.jpg", outputImage);

    MatchingTriangleMap tempTriangleMap = getMatchingTriangleTwoWayMap(tempMap);
    auto allTrisImage1 = buildTrianglesFromKeypoints(keypointsImage1);
    auto allTrisImage2 = buildTrianglesFromKeypoints(keypointsImage2);

    vector<Keypoint> image1Kp;
    vector<Keypoint> image2Kp;
    tie(image1Kp, image2Kp) = splitKeypointMap(tempMap);

    auto tris1 = buildTrianglesFromKeypoints(image1Kp);
    auto tris2 = buildTrianglesFromKeypoints(image2Kp);

    cout << "Number of matching Triangles:   " << tempTriangleMap.size() << endl;
    cout << "Number of Triangles made of matching points in image1: " << tris1.size() << " image2: " << tris2.size() << endl;
    cout << "average:  " << ((tris1.size() + tris2.size())/2) << endl;
    cout << "%average: " << 100.0*(float)(tempTriangleMap.size())/(float)((tris1.size() + tris2.size())/2) << "%" << endl;
    cout << "Number of Triangles in image1: " << allTrisImage1.size() << " image2: " << allTrisImage2.size() << endl;
    cout << "average:  " << ((allTrisImage1.size() + allTrisImage2.size())/2) << endl;
    cout << "%average: " << 100.0*(float)(tempTriangleMap.size())/(float)((allTrisImage1.size() + allTrisImage2.size())/2) << "%" << endl;

    for (auto entry: tempTriangleMap)
    {
        bool setColour = true;
        int r = (int) xorshf962();
        int g = (int) xorshf962();
        int b = (int) xorshf962();
        cv::Scalar colour(b,g,r);
        drawSingleTriangleOntoImage(entry.first, inputImage, setColour, colour);
        drawSingleTriangleOntoImage(entry.second, outputImage, setColour, colour);
    }

    //keypoints
    unsigned int matchingKeypointsCount = tempMap.size();
    unsigned int image1KeypointsTotalCount = keypointsImage1.size();
    unsigned int image2KeypointsTotalCout = keypointsImage2.size();
    auto matchingKeypoints = tempMap;

    //triangles
    unsigned int matchingTrianglesInBothImagesCount = tempTriangleMap.size();
    unsigned int trianglesMadeOfMatchingKeypointsInImage1Count = tris1.size();
    unsigned int trianglesMadeOfMatchingKeypointsInImage2Count = tris2.size();
    unsigned int image1TrianglesTotalCount = allTrisImage1.size();
    unsigned int image2TrianglesTotalCount = allTrisImage2.size();
    auto matchingTriangles = tempTriangleMap;

    dumpMathcingKeypointTestingInfoToJson(matchingKeypointsCount, image1KeypointsTotalCount, image2KeypointsTotalCout,
                                          matchingKeypoints, matchingTrianglesInBothImagesCount, trianglesMadeOfMatchingKeypointsInImage1Count,
                                          trianglesMadeOfMatchingKeypointsInImage2Count, image1TrianglesTotalCount, image2TrianglesTotalCount );

    cv::imwrite("image1_triangles.jpg", inputImage);
    cv::imwrite("image2_triangles.jpg", outputImage);
//    cv::waitKey();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_NOT_LESS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NOT_LESS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_not_less capabilities

    TODO Put description here

    @par Semantic:

    @code
    as_logical_t<T> r = is_not_less(x,y);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = !(x < y);
    @endcode

    @par Note:

    Due to existence of nan, this is not equivalent to a call to @ref is_greater_equal
    for floating types

    @par Alias:

    @c is_nlt

  **/
  const boost::dispatch::functor<tag::is_not_less_> is_not_less = {};
} }
#endif

#include <boost/simd/function/scalar/is_not_less.hpp>
#include <boost/simd/function/simd/is_not_less.hpp>

#endif


// External libraries
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/RungeKutta.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>

// Local headers
#include "write.h"
#include "State.h"
#include "modelParameters.h"
#include "globalVariables.h"
#include "adafFunctions.h"

//namespace {
inline double safeLog10( double x )	{ return x > 0.0 ? log10(x) : -100.0; }
//}
std::string dataName(std::string id) {
	return id + ".dat";
}

void generateViewScript(std::string path) {
	std::string filename = path.substr(path.find("\\") + 1);
	std::string folder = path.substr(0,path.find("\\"));
	std::ofstream file;
	file.open((folder+"/plots/plot-"+filename+".bat").c_str(), std::ios::out);
	file << "@../../plot-svg-and-view.bat " + filename;
	file.close();
}

void writeAllSpaceParam(const std::string& filename, const ParamSpaceValues& data)
{
	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);
	//const ParamSpace* a = &(data.ps);

	// a.iterate;  no me deja hacer esta operacion

	data.ps.iterate([&file, &data](const SpaceIterator& i){
		double logE = log10(i.val(DIM_E) / 1.6e-12);
		double logR = (i.val(DIM_R));
		double logT = (i.val(DIM_Rcd));
		double logQ = safeLog10(data.get(i)); //log10(salida.values(i));  // values(i));
//		salida.values(i);

		file << logE << '\t' << logR << '\t' << logT << '\t' <<
			logQ << std::endl;
			//logQ << std::endl;
	});

	file.close();
	generateViewScript(filename);
}

void writeEandRParamSpace(const std::string& filename, const ParamSpaceValues& data, int t,int vol)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	//double time = log10(data.ps[1][t]);

	data.ps.iterate([&](const SpaceIterator& i) {

		double logE = log10(i.val(DIM_E) / EV_TO_ERG);
		double r = i.val(DIM_R);
		double voll = (vol == 1) ? volume(r) : 1.0;
		double logQ = safeLog10(data.get(i)*voll);
		file << logE << '\t' << i.coord[DIM_R] << '\t' << safeLog10(r/schwRadius) << '\t' << logQ << std::endl;

	}, { -1, -1, t });

	file.close();
	generateViewScript(filename);
}

void writeRParamSpace(const std::string& filename, const ParamSpaceValues& data, int t, int s)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);
	double Emin = data.ps[DIM_E].first();
	double Emax = data.ps[DIM_E].last();
	data.ps.iterate([&](const SpaceIterator& iR) {
		double tot = integSimpson(log(Emin),log(Emax),
						[&](double loge)
						{
							double e = exp(loge);
							double nPh = data.interpolate({{DIM_E,e}},&iR.coord);
							return e*nPh;
						},100);
		file << safeLog10(iR.val(DIM_R)/schwRadius) << '\t' << safeLog10(tot) << std::endl;
	},{t,-1,s});
	file.close();
	generateViewScript(filename);
}

void writeEandTParamSpace(const std::string& filename, const ParamSpaceValues& data, int r)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	double logR = log10(data.ps[1][r]);

	file << "log(r)=" << logR << '\t' ;

	for (size_t t_ix = 0; t_ix < data.ps[2].size(); t_ix++) {
		double time = data.ps[2][t_ix];
		file << "t=" << log10(time) << '\t';
	}

	for (int E_ix = 0; E_ix < data.ps[0].size(); E_ix++) {

		file << std::endl;

		double logE = log10(data.ps[0][E_ix]/1.6e-12);

		file << logE << '\t';

		data.ps.iterate([&file, &data](const SpaceIterator& i){

			//double logR = log10(i.val(DIM_R));
			//double time = i.val(DIM_T);
			double logQ = safeLog10(data.get(i));

			file << logQ << '\t';
			;
		}, { E_ix, r, -1 });  //el -1 indica que las E se recorren, no quedan fijas
		//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )
	}
	file.close();
	generateViewScript(filename);
}

void writeRandTParamSpace(const std::string& filename, const ParamSpaceValues& data, int E)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	data.ps.iterate([&file, &data](const SpaceIterator& i){

		double r = i.val(DIM_R);
		//double theta = i.val(DIM_THETA);

		file << r << '\t' << '\t' << data.get(i) << std::endl;

	}, { E, -1, -1 });  //el -1 indica que las E se recorren, no quedan fijas
	//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )

	file.close();
	generateViewScript(filename);
}

void writeEnergyFunction(const std::string& filename, const ParamSpaceValues& data, int r, int t)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	double logR = log10(data.ps[1][r]);
	// version larga
	double logT = log10(data.ps.dimensions[2]->values[t]);

	file << "log(r)=" << logR << '\t' << "log(t)=" << logT << std::endl;
	data.ps.iterate([&file, &data](const SpaceIterator& i){

		double logE = log10(i.val(DIM_E) / 1.6e-12);
		double logQ = safeLog10(data.get(i));

		file << logE << '\t' << logQ << std::endl;
		;
	}, { -1, r, t });  //el -1 indica que las E se recorren, no quedan fijas
	//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )

	file.close();
	generateViewScript(filename);
}

void writeMatrix(const std::string& filename, Particle& p, Matrix& a)
{
	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	int nR = p.ps[DIM_R].size();  //ver el -1

	file << '\t';

	for (size_t z_j = 0; z_j < nR; z_j++) {
		const double r_j = p.ps[DIM_R][z_j];
		file << r_j << '\t' ;
	}

	file << std::endl;

	for (size_t z_i = 0; z_i < nR; z_i++) {

		const double r_i = p.ps[DIM_R][z_i];

		file << r_i << '\t';

		for (size_t z_j = 0; z_j < nR; z_j++) {
			const double r_j = p.ps[DIM_R][z_j];

			file << a[z_i][z_j] << '\t' ;
		}

		file << std::endl;
	}

	file.close();
	generateViewScript(filename);
}

void writeFields(State& st) {
	std::ofstream fields;
	fields.open("fields.dat",std::ios::out);
	fields		<< "r_in [Rs] = " << exp(logr.front()) << endl;
	fields		<< "r_out [Rs] = " << exp(logr.back()) << endl << endl;
	fields  << "rB1 [Rs]" 	<< "\t"
			<< "r [Rs]" 	<< "\t"
			<< "rB2 [Rs]" 	<< "\t"
			<< "MdotRIAF" 	<< "\t"
			<< "MdotCD" 	<< "\t"
			<< "Te"			<< "\t"
			<< "Ti"			<< "\t"
			<< "H/R"		<< "\t"
			<< "ne"			<< "\t"
			<< "v/c"		<< "\t"
			<< "B"			<< endl;

	st.photon.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		fields 	<< r/schwRadius/sqrt(paso_r) << "\t"
				<< r/schwRadius << "\t"
				<< r/schwRadius * sqrt(paso_r) << "\t"
				<< gAcc(r) << "\t"
				<< accRateColdDisk(r)/accRateOut << "\t"
				<< st.tempElectrons.get(iR) << "\t"
				<< st.tempIons.get(iR) << "\t"
				<< height_fun(r)/r << "\t"
				<< electronDensity(r) << "\t"
				<< abs(radialVel(r))/cLight <<"\t"
				<< st.magf.get(iR) << endl;
	},{0,-1,0});
	fields.close();

	std::ofstream SSD;
	SSD.open("SSD.dat",std::ios::out);
	SSD		<< "r_tr [Rs] = " << rTr/schwRadius << endl;
	SSD		<< "r_outCD [Rs] = " << rOutCD/schwRadius << endl << endl;
	SSD  	<< "rB1 [Rs]"			<< "\t"
			<< "r [Rs]" 			<< "\t"
			<< "rB2 [Rs]"			<< "\t"
			<< "MdotCD/MdotOut" 	<< endl;

	st.photon.ps.iterate([&](const SpaceIterator& iRcd) {
		double rCD = iRcd.val(DIM_Rcd);
		SSD 	<< rCD/sqrt(paso_rCD)/schwRadius << "\t"
				<< rCD/schwRadius << "\t"
				<< rCD*sqrt(paso_rCD)/schwRadius << "\t"
				<< accRateColdDisk(rCD)/accRateOut << endl;
	},{0,0,-1});
	SSD.close();
}

using namespace H5;

float *toFloatArray(const double *arr, size_t n) {
  float *ret = new float[n];
  for (int i = 0; i < n; i++) {
    ret[i] = float(arr[i]);
  }
  return ret;
}

void MultiplyVectorByScalar(Vector v, double s, Vector& w) {
	// Multiply vector v by scalar s and stores the data in w
    transform(v.begin(), v.end(), w.begin(), [s](double &x){ return x*s; });
}

void DivideVectorByVector(Vector v1, Vector v2, Vector& w) {
	// Multiply vector v by scalar s and stores the data in w
    transform(v1.begin(), v1.end(), v2.begin(), w.begin(), divides<double>());
}

void EvaluateFunctionInVector(fun1 f, Vector v, Vector& w) {
	// Evaluate f in vector v and stores the data in w
    transform(v.begin(), v.end(), w.begin(), [f](double &x){ return f(x); });
}

void myH5_write_single_float(const double x, H5File &h5f, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(y, PredType::NATIVE_FLOAT);
}

void myH5_write_single_float(const double x, Group &h5g, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(y, PredType::NATIVE_FLOAT);
}

void myH5_write_1d_float(const Vector v, H5File &h5f, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(toFloatArray(v.data(), n), PredType::NATIVE_FLOAT);
}

void myH5_write_1d_float(const Vector v, Group &h5g, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(toFloatArray(v.data(), n), PredType::NATIVE_FLOAT);
}

void myH5_write_single_double(const double x, H5File &h5f, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(y, PredType::NATIVE_DOUBLE);
}

void myH5_write_single_double(const double x, Group &h5g, H5std_string DATASET_NAME)
{
	double y[1];	*y = double(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(y, PredType::NATIVE_DOUBLE);
}

void myH5_write_1d_double(const Vector v, H5File &h5f, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(v.data(), PredType::NATIVE_DOUBLE);
}

void myH5_write_1d_double(const Vector v, Group &h5g, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(v.data(), PredType::NATIVE_DOUBLE);
}

int createH5file(const H5std_string FILE_NAME, const State& st)
{
	// Try block to detect exceptions raised by any of the calls inside it
    try {
        // Turn off the auto-printing when failure occurs so that we can
        // handle the errors appropriately
        Exception::dontPrint();

        // Create a new file using default property lists.
        H5File h5file(FILE_NAME, H5F_ACC_TRUNC);

		// HEADER ///////////////////////////////////////////////////////////////////////
		//Group *header_id = new Group(h5file->createGroup("Header"));
		Group header_id = h5file.createGroup("Header");
		myH5_write_single_float(blackHoleMass/solarMass, header_id, "M");
		myH5_write_single_float(accRateOut / (1.39e18 * blackHoleMass/solarMass), header_id, "Mdot_out");
		myH5_write_single_float(s, header_id, "sWind");
		myH5_write_single_float(delta, header_id, "delta_e");
		myH5_write_single_float(magFieldPar, header_id, "beta");
		myH5_write_single_float(alpha, header_id, "alpha");
		myH5_write_single_float(jAngMom, header_id, "jAngMom");

		//delete header_id;
		/////////////////////////////////////////////////////////////////////////////////

		// RIAF /////////////////////////////////////////////////////////////////////////

		Group RIAF_id = h5file.createGroup("RIAF");

		Vector rVec = st.photon.ps.dimensions[DIM_R]->values;
		Vector w(nR, 0.0);

		MultiplyVectorByScalar(rVec, 1.0/schwRadius, w);
		myH5_write_1d_float(w, RIAF_id, "r");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return volume(r)/P3(schwRadius); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "volume");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return accRateADAF(r)/accRateOut; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Mdot");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r)
					{ return boltzmann/electronRestEnergy * electronTemp(r); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Te");

		EvaluateFunctionInVector([](double r)
					{ return boltzmann/protonRestEnergy * ionTemp(r); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Ti");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return height_fun(r)/r; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "H_R");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector(electronDensity, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "n_e");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) {return abs(radialVel(r))/cLight; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "v_c");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector(magneticField, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "B");

		myH5_write_1d_float(redshift_to_inf, RIAF_id, "redshift_to_inf");

		RIAF_id.close();

		Group SSD_id = h5file.createGroup("SSD");
		SSD_id.close();

		Group Particles_id = h5file.createGroup("Particles");
		for (const auto &p : st.particles) {
   			Group p_id = Particles_id.createGroup(p->id);
			hsize_t dims[2] = { nR, nE };
			DataSpace dataspace(2, dims);
			p_id.createDataSet("Injection", PredType::NATIVE_FLOAT, dataspace);
			p_id.createDataSet("Distribution", PredType::NATIVE_FLOAT, dataspace);
		}
		Particles_id.close();
		h5file.close();

    } // end of try block
    // catch failure caused by the H5File operations
    catch (FileIException error) {
        error.printErrorStack();
        return -1;
    }
    // catch failure caused by the Group operations
    catch (GroupIException error) {
        error.printErrorStack();
        return -1;
    }
	catch (DataSetIException error) {
        error.printErrorStack();
        return -1;
    }
    // catch failure caused by the DataSpace operations
    catch (DataSpaceIException error) {
        error.printErrorStack();
        return -1;
    }
	return 0;
}


/*
 * Copyright (c) 2018, Sunanda Bose (Neel Basu) (neel.basu.z@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "mathematica++ Unit Test (Module::Chain)"
#include <boost/test/unit_test.hpp>
#include <string>
#include <cmath>
#include <iostream>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "mathematica++/connector.h"
#include "mathematica++/tokens.h"
#include "mathematica++/variant.h"

using namespace mathematica;

BOOST_AUTO_TEST_SUITE(chain)

BOOST_AUTO_TEST_CASE(scalers){
  connector shell;
  BOOST_CHECK(shell.connected());

  {
    value result = shell.function("Plus", 2L).integer(30).integer(50).end().pull().next(); // Plus[30, 50]
    BOOST_CHECK(result->type() == token::token_integer);
    BOOST_CHECK(result->stringify() == "80");
    BOOST_CHECK(boost::lexical_cast<int>(result) == 80);
  }

  {
    value result = shell.function("Round",  1L).function("N",  1L).function("Exp",  1L).integer(2).end().pull().next(); // Round[N[Exp[2]]]
    BOOST_CHECK(result->type() == token::token_integer);
    BOOST_CHECK(result->stringify() == "7");
    BOOST_CHECK(boost::lexical_cast<int>(result) == 7);
  }

  {
    value result = shell.function("Total", 1L).function("List", 2L).integer(3).integer(5).end().pull().next(); // Total[List[3,5]]
    BOOST_CHECK(result->type() == token::token_integer);
    BOOST_CHECK(result->stringify() == "8");
    BOOST_CHECK(boost::lexical_cast<int>(result) == 8);
  }

  {
    value result = shell.function("N",  1L).function("Sin",  1L).integer(3).end().pull().next(); // N[Sin[3]]
    BOOST_CHECK(result->type() == token::token_real);
    int v = boost::lexical_cast<double>(result->stringify())*100000.0f;
    BOOST_CHECK(v == 14112);
  }
}

BOOST_AUTO_TEST_CASE(vectors){
    connector shell;
    BOOST_CHECK(shell.connected());

    {
        value result = shell.function("Total", 1L).function("Flatten", 1L).function("List", 4L).integer(30).function("List", 2L).integer(40).integer(50).function("List", 1L).integer(60).integer(70).end().pull().next(); // Total[Flatten[List[30, List[40, 50], List[60], 70]]]
        BOOST_CHECK(result->type() == token::token_integer);
        BOOST_CHECK(result->stringify() == "250");
        BOOST_CHECK(boost::lexical_cast<int>(result) == 250);
    }

    {
        value result = shell.function("Flatten", 1L).function("List", 4L).integer(30).function("List", 2L).integer(40).integer(50).function("List", 1L).integer(60).integer(70).end().pull().next(); // Flatten[List[30, List[40, 50], List[60], 70]]
        BOOST_CHECK(result->type() == token::token_function);
        tokens::function::ptr f = boost::dynamic_pointer_cast<tokens::function>(result);
        BOOST_CHECK(f);
        BOOST_CHECK(f->name() == "List");
        BOOST_CHECK(f->nargs() == 5);
        BOOST_CHECK(f->_args.size() == 5);
        for(int i = 1; i < 5; ++i){
            value v = f->_args[i];
            BOOST_CHECK(v->type() == token::token_integer);
            int vi = boost::lexical_cast<int>(v);
            BOOST_CHECK(boost::lexical_cast<int>(f->_args[i-1]) +10 == vi);
        }
    }

    {
        value result = shell.function("List", 4L).integer(30).function("List", 2L).integer(40).integer(50).function("List", 1L).integer(60).integer(70).end().pull().next(); // List[30, List[40, 50], List[60], 70]
        BOOST_CHECK(result->type() == token::token_function);
        tokens::function::ptr f = boost::dynamic_pointer_cast<tokens::function>(result);
        BOOST_CHECK(f);
        BOOST_CHECK(f->name() == "List");
        BOOST_CHECK(f->nargs() == 4);
        BOOST_CHECK(f->_args.size() == 4);
        BOOST_CHECK(f->_args[0]->type() == token::token_integer);
        BOOST_CHECK(f->_args[1]->type() == token::token_function);
        BOOST_CHECK(f->_args[2]->type() == token::token_function);
        BOOST_CHECK(f->_args[3]->type() == token::token_integer);
        BOOST_CHECK(result->stringify() == "List[30, List[40, 50], List[60], 70]");

        {
            tokens::function::ptr f1 = boost::dynamic_pointer_cast<tokens::function>(f->_args[1]);
            BOOST_CHECK(f1->name() == "List");
            BOOST_CHECK(f1->nargs() == 2);
            BOOST_CHECK(boost::lexical_cast<int>(f1->_args[0]) +10 == boost::lexical_cast<int>(f1->_args[1]));
        }

        {
            tokens::function::ptr f2 = boost::dynamic_pointer_cast<tokens::function>(f->_args[2]);
            BOOST_CHECK(f2->name() == "List");
            BOOST_CHECK(f2->nargs() == 1);
            BOOST_CHECK(boost::lexical_cast<int>(f2->_args[0]) == 60);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2020 ZMP Inc info@zmp.co.jp
 *
 * Distributed under the MIT License (http://opensource.org/licenses/MIT)
 *
 * Written by Andrei Pak
 */
#include "rc110_panel.hpp"

#include <rc110_msgs/SetInteger.h>
#include <std_srvs/SetBool.h>
#include <topic_tools/MuxSelect.h>

#include <QStatusBar>
#include <QTimer>
#include <boost/math/constants/constants.hpp>

#include "ui_rc110_panel.h"

namespace zmp
{
namespace
{
constexpr float RAD_TO_DEG = boost::math::float_constants::radian;
constexpr float DEG_TO_RAD = boost::math::float_constants::degree;
constexpr float G_TO_MS2 = 9.8f;  // G to m/s2 conversion factor (Tokyo)

constexpr int STATUS_MESSAGE_TIME = 5000;  // ms

QString printSensor(float value, const QString& suffix, int precision = 2)
{
    return (value < 0 ? QString("%1 ").arg(value, -10, 'f', precision) : QString(" %1 ").arg(value, -9, 'f', precision)) +
           suffix;
}
}  // namespace

Rc110Panel::Rc110Panel(QWidget* parent) : Panel(parent), ui(new Ui::PanelWidget), calibrationTimer(new QTimer(this))
{
    ui->setupUi(this);
    calibrationTimer->setSingleShot(true);

    QList<QTreeWidgetItem*> items;
    for (int i = 0; i < std::size(TREE_ITEM_GROUP_NAMES); ++i) {
        items.push_back(new QTreeWidgetItem({TREE_ITEM_GROUP_NAMES[i]}));
        treeItems.insert((TreeItemGroup)i, items[i]);
    }
    ui->treeWidget->insertTopLevelItems(0, items);

    statusBar = new QStatusBar(this);
    layout()->addWidget(statusBar);

    ui->splitter->setStretchFactor(0, 1);  // expand tree widget to maximum height

    connect(ui->boardButton, &QPushButton::clicked, this, &Rc110Panel::onEnableBoard);
    connect(ui->adButton, &QPushButton::clicked, this, &Rc110Panel::onEnableAd);
    connect(ui->motorGroup,
            QOverload<QAbstractButton*>::of(&QButtonGroup::buttonClicked),
            this,
            &Rc110Panel::onSetMotorState);
    connect(ui->servoGroup,
            QOverload<QAbstractButton*>::of(&QButtonGroup::buttonClicked),
            this,
            &Rc110Panel::onSetServoState);
    connect(ui->driveSpeedEdit, &QLineEdit::editingFinished, this, &Rc110Panel::onEditingFinished);
    connect(ui->steeringEdit, &QLineEdit::editingFinished, this, &Rc110Panel::onEditingFinished);
    connect(ui->steeringOffsetEdit, &QLineEdit::editingFinished, this, &Rc110Panel::onEditingFinished);
    connect(ui->calibrateButton, &QPushButton::clicked, this, &Rc110Panel::onCalibrate);
    connect(calibrationTimer, &QTimer::timeout, this, &Rc110Panel::onFinishCalibration);

    subscribers.push_back(handle.subscribe("mux_drive/selected", 1, &Rc110Panel::onAdModeChanged, this));

    subscribers.push_back(handle.subscribe("motor_speed_goal", 1, &Rc110Panel::onMotorSpeed, this));
    subscribers.push_back(handle.subscribe("steering_angle_goal", 1, &Rc110Panel::onSteeringAngle, this));

    subscribers.push_back(handle.subscribe("baseboard_error", 1, &Rc110Panel::onError, this));
    subscribers.push_back(handle.subscribe("robot_status", 1, &Rc110Panel::onRobotStatus, this));
    subscribers.push_back(handle.subscribe("drive_status", 1, &Rc110Panel::onDriveStatus, this));
    subscribers.push_back(handle.subscribe("offsets_status", 1, &Rc110Panel::onOffsets, this));

    subscribers.push_back(handle.subscribe("odometry", 1, &Rc110Panel::onOdometry, this));
    subscribers.push_back(handle.subscribe("servo_battery", 1, &Rc110Panel::onServoBattery, this));
    subscribers.push_back(handle.subscribe("motor_battery", 1, &Rc110Panel::onMotorBattery, this));
    subscribers.push_back(handle.subscribe("baseboard_temperature", 1, &Rc110Panel::onBaseboardTemperature, this));
    subscribers.push_back(handle.subscribe("servo_temperature", 1, &Rc110Panel::onServoTemperature, this));
    subscribers.push_back(handle.subscribe("imu/data", 1, &Rc110Panel::onImu, this));
    subscribers.push_back(handle.subscribe("motor_rate", 1, &Rc110Panel::onMotorRate, this));
    subscribers.push_back(handle.subscribe("wheel_speeds", 1, &Rc110Panel::onWheelSpeeds, this));

    publishers["drive_manual"] = handle.advertise<ackermann_msgs::AckermannDriveStamped>("drive_manual", 1);
    publishers["offsets"] = handle.advertise<rc110_msgs::Offsets>("offsets", 1);

    statusBar->showMessage("");

    startTimer(100);  // ms
}

Rc110Panel::~Rc110Panel() = default;

void Rc110Panel::timerEvent(QTimerEvent* event)
{
    if (driveSpeed != 0) {
        publishDrive();  // prevent motor from stopping by timeout
    }
}

QTreeWidgetItem* Rc110Panel::getTreeItem(TreeItemGroup group, const char* name) const
{
    QTreeWidgetItem* item = nullptr;
    auto list = ui->treeWidget->findItems(name, Qt::MatchExactly | Qt::MatchRecursive);

    // if item exists, check if it is from required group
    for (auto candidate : list) {
        auto groupItem = candidate->parent();
        if (groupItem && treeItems.key(groupItem) == group) {
            item = list.front();
            break;
        }
    }
    // if it does not exist, add it
    if (!item) {
        item = new QTreeWidgetItem({name});
        treeItems[group]->addChild(item);
        treeItems[group]->setExpanded(true);
    }
    return item;
}

void Rc110Panel::onEnableBoard(bool on)
{
    std_srvs::SetBool service;
    service.request.data = uint8_t(on);
    ros::service::call("enable_board", service);

    statusBar->showMessage(service.response.success ? QString("Board was set to %1").arg(on ? "on" : "off")
                                                    : QString("Failed to set board state"),
                           STATUS_MESSAGE_TIME);
}

void Rc110Panel::onEnableAd(bool on)
{
    topic_tools::MuxSelect service;
    service.request.topic = on ? "drive_ad" : "drive_manual";

    if (ros::service::call("mux_drive/select", service)) {
        statusBar->showMessage(on ? "AD enabled" : "Joystick enabled", STATUS_MESSAGE_TIME);
    } else {
        statusBar->showMessage("Failed to switch AD mode", STATUS_MESSAGE_TIME);
    }
}

void Rc110Panel::onSetMotorState(QAbstractButton* button)
{
    int state = (button == ui->motorOnRadio)        ? rc110_msgs::Status::MOTOR_ON
                : (button == ui->motorNeutralRadio) ? rc110_msgs::Status::MOTOR_NEUTRAL
                                                    : rc110_msgs::Status::MOTOR_OFF;
    rc110_msgs::SetInteger service;
    service.request.data = state;
    ros::service::call("motor_state", service);

    statusBar->showMessage(service.response.success ? QString("Drive motor was set to %1")
                                                              .arg(state == rc110_msgs::Status::MOTOR_ON    ? "on"
                                                                   : state == rc110_msgs::Status::MOTOR_OFF ? "off"
                                                                                : "neutral")
                                                    : QString("Failed to set motor state"),
                           STATUS_MESSAGE_TIME);
}

void Rc110Panel::onSetServoState(QAbstractButton* button)
{
    int state = (button == ui->servoOnRadio)        ? rc110_msgs::Status::MOTOR_ON
                : (button == ui->servoNeutralRadio) ? rc110_msgs::Status::MOTOR_NEUTRAL
                                                    : rc110_msgs::Status::MOTOR_OFF;
    rc110_msgs::SetInteger service;
    service.request.data = state;
    ros::service::call("servo_state", service);

    statusBar->showMessage(service.response.success ? QString("Servomotor was set to %1")
                                                              .arg(state == rc110_msgs::Status::MOTOR_ON    ? "on"
                                                                   : state == rc110_msgs::Status::MOTOR_OFF ? "off"
                                                                                : "neutral")
                                                    : QString("Failed to set servo state"),
                           STATUS_MESSAGE_TIME);
}

void Rc110Panel::onEditingFinished()
{
    if (auto edit = dynamic_cast<QLineEdit*>(sender())) {
        float value = edit->text().toFloat();

        auto name = edit->objectName().toStdString();
        if (name == "driveSpeedEdit") {
            driveSpeed = value;
            publishDrive();
        } else if (name == "steeringEdit") {
            steeringAngle = value;
            publishDrive();
        } else if (name == "steeringOffsetEdit") {
            offsets.steering = ui->steeringOffsetEdit->text().toFloat();
            publishOffsets();
        }
    }
}

void Rc110Panel::onCalibrate()
{
    QApplication::setOverrideCursor(Qt::WaitCursor);
    ui->calibrateButton->setEnabled(false);

    calibrationSums.clear();
    calibrationTimer->start(3000);
}

void Rc110Panel::onFinishCalibration()
{
    const auto& motorCurrent = calibrationSums["motor current"];
    if (motorCurrent.first) {
        offsets.motor_current += motorCurrent.second / motorCurrent.first;
    }
    const auto& accelX = calibrationSums["accel x"];
    if (accelX.first) {
        offsets.accel_x += accelX.second / accelX.first;
    }
    const auto& accelY = calibrationSums["accel y"];
    if (accelY.first) {
        offsets.accel_y += accelY.second / accelY.first;
    }
    const auto& accelZ = calibrationSums["accel z"];
    if (accelZ.first) {
        offsets.accel_z += accelZ.second / accelZ.first - G_TO_MS2;
    }
    const auto& gyroYaw = calibrationSums["gyro yaw"];
    if (gyroYaw.first) {
        offsets.gyro += gyroYaw.second / gyroYaw.first;
    }
    publishOffsets();

    ui->calibrateButton->setEnabled(true);
    QApplication::restoreOverrideCursor();
}

void Rc110Panel::publishDrive()
{
    ackermann_msgs::AckermannDriveStamped message;
    message.header.stamp = ros::Time::now();
    message.drive.speed = driveSpeed;
    message.drive.steering_angle = steeringAngle * DEG_TO_RAD;
    publishers["drive_manual"].publish(message);
}

void Rc110Panel::publishOffsets()
{
    publishers["offsets"].publish(offsets);
}

void Rc110Panel::onAdModeChanged(const std_msgs::String& message)
{
    ui->adButton->setChecked(message.data == "drive_ad");
}

void Rc110Panel::onMotorSpeed(const std_msgs::Float32& message)
{
    ui->driveSpeedEdit->setText(QString::number(message.data));
    showDriveGoalStatus();
}

void Rc110Panel::onSteeringAngle(const std_msgs::Float32& message)
{
    ui->steeringEdit->setText(QString::number(message.data * RAD_TO_DEG));
    showDriveGoalStatus();
}

void Rc110Panel::showDriveGoalStatus()
{
    statusBar->showMessage(QString("Drive was updated. Speed: %1, Angle: %2")
                                   .arg(ui->driveSpeedEdit->text())
                                   .arg(ui->steeringEdit->text()),
                           STATUS_MESSAGE_TIME);
}

void Rc110Panel::onError(const rc110_msgs::BaseboardError& message)
{
    int error = message.data;
    if (error == rc110_msgs::BaseboardError::NONE) {
        ui->errorLabel->setPixmap(QPixmap(":/ok.png"));
        ui->errorLabel->setToolTip("Baseboard is ok");
    } else {
        ui->errorLabel->setPixmap(QPixmap(":/error.png"));

        QString tooltip = "Please, restart baseboard: %1";
        if (error == rc110_msgs::BaseboardError::BOARD_HEAT) {
            ui->errorLabel->setToolTip(tooltip.arg("board has too high temperature"));
        } else if (error == rc110_msgs::BaseboardError::MOTOR_HEAT) {
            ui->errorLabel->setToolTip(tooltip.arg("motor has too high temperature"));
        } else if (error == rc110_msgs::BaseboardError::MOTOR_FAILURE) {
            ui->errorLabel->setToolTip(tooltip.arg("encoder feedback and polarity of the motor"));
        } else if (error == rc110_msgs::BaseboardError::LOW_VOLTAGE) {
            ui->errorLabel->setToolTip(tooltip.arg("voltage dropped less than 6V for around 1s"));
        }
    }
}

void Rc110Panel::onRobotStatus(const rc110_msgs::Status& message)
{
    ui->boardButton->setChecked(message.board_enabled);

    switch (message.motor_state) {
        case rc110_msgs::Status::MOTOR_NEUTRAL:
            ui->motorNeutralRadio->setChecked(true);
            break;
        case rc110_msgs::Status::MOTOR_ON:
            ui->motorOnRadio->setChecked(true);
            break;
        default:
            ui->motorOffRadio->setChecked(true);
    }

    switch (message.servo_state) {
        case rc110_msgs::Status::MOTOR_NEUTRAL:
            ui->servoNeutralRadio->setChecked(true);
            break;
        case rc110_msgs::Status::MOTOR_ON:
            ui->servoOnRadio->setChecked(true);
            break;
        default:
            ui->servoOffRadio->setChecked(true);
    }
}

void Rc110Panel::onDriveStatus(const ackermann_msgs::AckermannDriveStamped& driveStatus)
{
    getTreeItem(DRIVE, "speed")->setText(1, printSensor(driveStatus.drive.speed, "m/s"));
    getTreeItem(DRIVE, "steering angle")->setText(1, printSensor(driveStatus.drive.steering_angle * RAD_TO_DEG, "°"));
    getTreeItem(DRIVE, "steering speed")
            ->setText(1, printSensor(driveStatus.drive.steering_angle_velocity * RAD_TO_DEG, "°"));
}

void Rc110Panel::onOffsets(const rc110_msgs::Offsets& message)
{
    offsets = message;
    QString text = "Motor Current:\t %1\n"
                   "Accel X:\t\t %2\n"
                   "Accel Y:\t\t %3\n"
                   "Accel Z:\t\t %4\n"
                   "Gyro Yaw:\t %5\n";
    ui->offsetsLabel->setText(text.arg(printSensor(message.motor_current * 1000, "mA", 4))
                                      .arg(printSensor(message.accel_x, "m/s²", 4))
                                      .arg(printSensor(message.accel_y, "m/s²", 4))
                                      .arg(printSensor(message.accel_z, "m/s²", 4))
                                      .arg(printSensor(message.gyro * RAD_TO_DEG, "°/s", 4)));

    ui->steeringOffsetEdit->setText(QString::number(message.steering));

    statusBar->showMessage(QString("Offsets updated."), STATUS_MESSAGE_TIME);
}

void Rc110Panel::onOdometry(const nav_msgs::Odometry& odometry)
{
    getTreeItem(DRIVE, "angular velocity")
            ->setText(1, printSensor(float(odometry.twist.twist.angular.z * RAD_TO_DEG), "°/s"));
}

void Rc110Panel::onServoBattery(const sensor_msgs::BatteryState& batteryState)
{
    getTreeItem(BATTERY, "servo voltage")->setText(1, printSensor(batteryState.voltage, "V"));
    getTreeItem(BATTERY, "servo current")->setText(1, printSensor(batteryState.current * 1000, "mA", 0));
}

void Rc110Panel::onMotorBattery(const sensor_msgs::BatteryState& batteryState)
{
    getTreeItem(BATTERY, "motor voltage")->setText(1, printSensor(batteryState.voltage, "V"));
    getTreeItem(BATTERY, "motor current")->setText(1, printSensor(batteryState.current * 1000, "mA", 0));

    float voltage = batteryState.voltage;
    if (voltage < 6.4f) {
        ui->batteryLabel->setPixmap(QPixmap(":/battery_0.png"));
    } else if (voltage < 7.0f) {
        ui->batteryLabel->setPixmap(QPixmap(":/battery_1.png"));
    } else if (voltage < 7.4f) {
        ui->batteryLabel->setPixmap(QPixmap(":/battery_2.png"));
    } else if (voltage < 7.6f) {
        ui->batteryLabel->setPixmap(QPixmap(":/battery_3.png"));
    } else {
        ui->batteryLabel->setPixmap(QPixmap(":/battery_4.png"));
    }

    if (calibrationTimer->isActive()) {
        auto& value = calibrationSums["motor current"];
        ++value.first;
        value.second += batteryState.current;
    }
}

void Rc110Panel::onBaseboardTemperature(const sensor_msgs::Temperature& temperature)
{
    getTreeItem(TEMPERATURE, "baseboard")->setText(1, printSensor(temperature.temperature, "°C", 1));
}

void Rc110Panel::onServoTemperature(const sensor_msgs::Temperature& temperature)
{
    getTreeItem(TEMPERATURE, "servo")->setText(1, printSensor(temperature.temperature, "°C", 1));
}

void Rc110Panel::onImu(const sensor_msgs::Imu& imu)
{
    getTreeItem(IMU, "accel x")->setText(1, printSensor(imu.linear_acceleration.x, "m/s²"));
    getTreeItem(IMU, "accel y")->setText(1, printSensor(imu.linear_acceleration.y, "m/s²"));
    getTreeItem(IMU, "accel z")->setText(1, printSensor(imu.linear_acceleration.z, "m/s²"));
    getTreeItem(IMU, "gyro yaw")->setText(1, printSensor(imu.angular_velocity.z * RAD_TO_DEG, "°/s"));

    if (calibrationTimer->isActive()) {
        auto& accelX = calibrationSums["accel x"];
        ++accelX.first;
        accelX.second += imu.linear_acceleration.x;
        auto& accelY = calibrationSums["accel y"];
        ++accelY.first;
        accelY.second += imu.linear_acceleration.y;
        auto& accelZ = calibrationSums["accel z"];
        ++accelZ.first;
        accelZ.second += imu.linear_acceleration.z;
        auto& gyroYaw = calibrationSums["gyro yaw"];
        ++gyroYaw.first;
        gyroYaw.second += imu.angular_velocity.z;
    }
}

void Rc110Panel::onMotorRate(const rc110_msgs::MotorRate& motorRate)
{
    getTreeItem(OTHER, "motor rate")->setText(1, printSensor(motorRate.motor_rate, "cycles/s"));
    getTreeItem(OTHER, "estimated speed")->setText(1, printSensor(motorRate.estimated_speed, "m/s"));
}

void Rc110Panel::onWheelSpeeds(const rc110_msgs::WheelSpeeds& wheelSpeeds)
{
    getTreeItem(OTHER, "wheel speed FL")->setText(1, printSensor(wheelSpeeds.speed_fl, "m/s"));
    getTreeItem(OTHER, "wheel speed FR")->setText(1, printSensor(wheelSpeeds.speed_fr, "m/s"));
    getTreeItem(OTHER, "wheel speed RL")->setText(1, printSensor(wheelSpeeds.speed_rl, "m/s"));
    getTreeItem(OTHER, "wheel speed RR")->setText(1, printSensor(wheelSpeeds.speed_rr, "m/s"));
}

}  // namespace zmp

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(zmp::Rc110Panel, rviz::Panel)


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ENUMERATE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ENUMERATE_HPP_INCLUDED

#include <boost/simd/function/make.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/detail/brigand.hpp>

namespace boost { namespace simd { namespace ext
{
  // -----------------------------------------------------------------------------------------------
  // enumerate<T>()
  BOOST_DISPATCH_OVERLOAD ( enumerate_
                          , (typename T, typename X)
                          , bd::cpu_
                          , bd::target_< bs::pack_< bd::fundamental_<T>, X > >
                          )
  {
    BOOST_FORCEINLINE typename T::type operator()(T const& ) const BOOST_NOEXCEPT
    {
      return do_( brigand::range<std::size_t,0,T::type::static_size>{} );
    }

    template<typename... N>
    static BOOST_FORCEINLINE typename T::type do_( brigand::list<N...> const& ) BOOST_NOEXCEPT
    {
      return make<typename T::type>(N::value...);
    }
  };

  // -----------------------------------------------------------------------------------------------
  // enumerate<T>(x)
  BOOST_DISPATCH_OVERLOAD ( enumerate_
                          , (typename A0, typename T, typename X)
                          , bd::cpu_
                          , bd::scalar_< bd::fundamental_<A0> >
                          , bd::target_< bs::pack_< bd::fundamental_<T>, X > >
                          )
  {
    BOOST_FORCEINLINE typename T::type operator()(A0 a0, T const& ) const BOOST_NOEXCEPT
    {
      return a0 + enumerate<typename T::type>();
    }
  };

  // -----------------------------------------------------------------------------------------------
  // enumerate<T>(a,n)
  BOOST_DISPATCH_OVERLOAD ( enumerate_
                          , (typename A0, typename A1,typename T,typename X)
                          , bd::cpu_
                          , bd::scalar_< bd::fundamental_<A0> >
                          , bd::scalar_< bd::fundamental_<A1> >
                          , bd::target_< bs::pack_< bd::fundamental_<T>, X > >
                          )
  {
    BOOST_FORCEINLINE typename T::type operator()(A0 a0, A1 a1, T const& ) const BOOST_NOEXCEPT
    {
      return a0 + a1*enumerate<typename T::type>();
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_DOWNBREAK_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_DOWNBREAK_HPP_INCLUDED

#include <boost/simd/pack.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace detail
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  template < class T>  struct downbreak
  {
    using stype =  bd::scalar_of_t<T>;
    using sdtype = bd::downgrade_t<stype>;
    using type = pack<sdtype, bs::cardinal_of<T>::value*2>;
  };
  template < class T>  struct downbreaks
  {
    using stype =  bd::scalar_of_t<T>;
    using sdtype = bd::downgrade_t<stype, signed>;
    using type = pack<sdtype, bs::cardinal_of<T>::value*2>;
  };
  template < class T>  struct downbreaku
  {
    using stype =  bd::scalar_of_t<T>;
    using sdtype = bd::downgrade_t<stype, unsigned>;
    using type = pack<sdtype, bs::cardinal_of<T>::value*2>;
  };

  template < typename T>
  using downbreak_t = typename downbreak<T>::type;
  template < typename T>
  using downbreaks_t = typename downbreaks<T>::type;
  template < typename T>
  using downbreaku_t = typename downbreaku<T>::type;
} } }

#endif


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <fstream>

using namespace TwoDLib;

BOOST_AUTO_TEST_CASE(AdapterTest)
{

pugi::xml_document doc;
std::vector<TwoDLib::Mesh> vec_vec_mesh;
std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_rev;
std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_res;
pugi::xml_parse_result result0 = doc.load_file("condee2a5ff4-0087-4d69-bae3-c0a223d03693.model");
pugi::xml_node  root0 = doc.first_child();
TwoDLib::Mesh mesh0 = TwoDLib::RetrieveMeshFromXML(root0);
std::vector<TwoDLib::Mesh> vec_mesh{ mesh0, mesh0, mesh0 };

std::vector<TwoDLib::Redistribution> vec_reversal = TwoDLib::RetrieveMappingFromXML("Reversal",root0);
std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_reversal { vec_reversal, vec_reversal, vec_reversal};
std::vector<TwoDLib::Redistribution> vec_reset = TwoDLib::RetrieveMappingFromXML("Reset",root0);
std::vector< std::vector<TwoDLib::Redistribution> > vec_vec_reset { vec_reset, vec_reset, vec_reset};

TwoDLib::Ode2DSystemGroup group(vec_mesh, vec_vec_reversal, vec_vec_reset);
group.Initialize(0,0,0);
group.Initialize(1,0,0);
group.Initialize(2,0,0);

TwoDLib::TransitionMatrix mat("condee2a5ff4-0087-4d69-bae3-c0a223d03693_0_0.05_0_0_.mat");
TwoDLib::CSRMatrix csrmat1(mat,group,0);
TwoDLib::CSRMatrix csrmat2(mat,group,1);
TwoDLib::CSRMatrix csrmat3(mat,group,2);
std::vector<TwoDLib::CSRMatrix> vecmat{csrmat1, csrmat2, csrmat3};

TwoDLib::CSRAdapter csr_adapter(group,vecmat,1e-5);

std::vector<MPILib::Rate> vecrates { 800., 900., 1000. };
std::ofstream fs("fs.dat");

 for( int i = 0; i < 10; i++)
   {
     group.Evolve();
     group.RemapReversal();
     for (MPILib::Index i_part = 0; i_part < csr_adapter.NrIterations(); i_part++ ){
       csr_adapter.ClearDerivative();
       csr_adapter.CalculateDerivative(vecrates);
       csr_adapter.AddDerivative();
     }
     group.RedistributeProbability();
     // no MapFinish() in C++ version

     fs << group.F()[0] << " " <<  group.F()[1] <<  " " << group.F()[2] << '\n';
   }

 std::ofstream dumpdata1("group1simulation.data");
 std::ofstream dumpdata2("group2simulation.data");
 std::ofstream dumpdata3("group3simulation.data");
 std::vector<std::ostream*> vec_stream { &dumpdata1, &dumpdata2, &dumpdata3 };
 group.Dump(vec_stream);
}


/**
 * @file   semantic_tsdf_integrator.cpp
 * @brief  Integrator of semantic and geometric information
 * @author Antoni Rosinol
 */

#include "kimera_semantics/semantic_tsdf_integrator.h"

#include <functional>  // For placeholders for Semantics
#include <list>
#include <memory>
#include <numeric>  // For accumulate for Semantics
#include <string>
#include <utility>

#include <Eigen/Core>

#include <voxblox/integrator/tsdf_integrator.h>
#include <voxblox/utils/timing.h>

#include "kimera_semantics/color.h"
#include "kimera_semantics/common.h"
#include "kimera_semantics/semantic_voxel.h"

namespace kimera {

std::string SemanticTsdfIntegrator::SemanticConfig::print() const {
  std::stringstream ss;
  // clang-format off
  ss << "================== Semantic Integrator Config ====================\n";
  ss << " - semantic_truncation_distance_factor:               " << semantic_truncation_distance_factor_ << "\n";
  ss << "==============================================================\n";
  // clang-format on
  return ss.str();
}

SemanticTsdfIntegrator::SemanticTsdfIntegrator(
    const Config& config,
    const SemanticConfig& semantic_config,
    vxb::Layer<SemanticVoxel>* semantic_layer,
    vxb::Layer<vxb::TsdfVoxel>* tsdf_layer)
    : MergedTsdfIntegrator(config, CHECK_NOTNULL(tsdf_layer)),
      semantic_config_(semantic_config),
      semantic_log_likelihood_(),
      semantic_layer_(CHECK_NOTNULL(semantic_layer)) {
  SemanticProbability match_probability =
      semantic_config.semantic_measurement_probability_;
  SemanticProbability non_match_probability =
      1.0 - semantic_config.semantic_measurement_probability_;
  CHECK_GT(match_probability, 0.0);
  CHECK_GT(non_match_probability, 0.0);
  CHECK_LT(match_probability, 1.0);
  CHECK_LT(non_match_probability, 1.0);
  log_match_probability_ = std::log(match_probability);
  log_non_match_probability_ = std::log(non_match_probability);
  CHECK_GT(log_match_probability_, log_non_match_probability_)
      << "Your probabilities do not make sense... The likelihood of a "
         "label, knowing that we have measured that label, should not be"
         "smaller than the likelihood of seeing another label!";
  semantic_log_likelihood_ =
      semantic_log_likelihood_.Constant(log_non_match_probability_);
  semantic_log_likelihood_.diagonal() =
      semantic_log_likelihood_.diagonal().Constant(log_match_probability_);
  // TODO(Toni): sanity checks, set as DCHECK_EQ.
  CHECK_NEAR(semantic_log_likelihood_.diagonal().sum(),
             kTotalNumberOfLabels * log_match_probability_,
             100 * vxb::kFloatEpsilon);
  CHECK_NEAR(
      semantic_log_likelihood_.sum(),
      kTotalNumberOfLabels * log_match_probability_ +
          std::pow(kTotalNumberOfLabels, 2) * log_non_match_probability_ -
          kTotalNumberOfLabels * log_non_match_probability_,
      1000 * vxb::kFloatEpsilon);
}

// Use if you don't have labels, but the info is encoded in colors.
// Otw, use integratePointCloud directly with semantic labels.
void SemanticTsdfIntegrator::integratePointCloud(
    const vxb::Transformation& T_G_C,
    const vxb::Pointcloud& points_C,
    const HashableColors& colors,
    const bool freespace_points) {
  SemanticLabels semantic_labels(colors.size());
  // TODO(Toni): parallelize with openmp
  for (size_t i = 0; i < colors.size(); i++) {
    // TODO(Toni): this will break badly if the color is not found in the
    // map, and alerting the user about that might be too costly.
    // TODO(Toni): currently simulator sends sporadic 255,255,255,255 for
    // rays traced to nowhere... map color::White() to label unknown 0u.
    // LOG(ERROR) << "RGB: " << std::to_string(colors[i].r) << ' '
    //            <<  std::to_string(colors[i].g) << ' '
    //            <<  std::to_string(colors[i].b);
    // TODO(Toni): Pointcloud recolor sets `a` field to 0. Making the
    // map lookup fail.
    const HashableColor& color = colors[i];
    HashableColor color_a = HashableColor(color.r, color.g, color.b, 255u);
    // const auto& it =
    // semantic_config_.color_to_semantic_label_map_.find(color_a); if (it !=
    // semantic_config_.color_to_semantic_label_map_.end()) {
    semantic_labels[i] =
        semantic_config_.color_to_semantic_label_map_.at(color_a);
    //} else {
    //  LOG(ERROR) << "Caught an unknown color: \n"
    //             << "RGB: " << std::to_string(color_a.r) << ' '
    //             <<  std::to_string(color_a.g) << ' '
    //              <<  std::to_string(color_a.b) << ' '
    //              <<  std::to_string(color_a.a);
    //  semantic_labels[i] = 0u; // Assign unknown label for now...
    //}
  }

  vxb::timing::Timer integrate_pcl_semantic_tsdf_timer(
      "semantic_tsdf/integrate");
  integratePointCloud(
      T_G_C, points_C, colors, semantic_labels, freespace_points);
  integrate_pcl_semantic_tsdf_timer.Stop();
}

void SemanticTsdfIntegrator::integratePointCloud(
    const vxb::Transformation& T_G_C,
    const vxb::Pointcloud& points_C,
    const HashableColors& colors,
    const SemanticLabels& semantic_labels,
    const bool freespace_points) {
  CHECK_GE(points_C.size(), 0u);
  CHECK_EQ(points_C.size(), colors.size());
  CHECK_EQ(points_C.size(), semantic_labels.size());

  // Pre-compute a list of unique voxels to end on.
  // Create a hashmap: VOXEL INDEX -> index in original cloud.
  VoxelMap voxel_map;
  // This is a hash map (same as above) to all the indices that need to be
  // cleared.
  VoxelMap clear_map;

  std::unique_ptr<vxb::ThreadSafeIndex> index_getter(
      vxb::ThreadSafeIndexFactory::get(config_.integration_order_mode,
                                       points_C));

  bundleRays(T_G_C,
             points_C,
             freespace_points,
             index_getter.get(),
             &voxel_map,
             &clear_map);

  integrateRays(T_G_C,
                points_C,
                colors,
                semantic_labels,
                config_.enable_anti_grazing,
                false,
                voxel_map,
                clear_map);

  integrateRays(T_G_C,
                points_C,
                colors,
                semantic_labels,
                config_.enable_anti_grazing,
                true,
                voxel_map,
                clear_map);
}

void SemanticTsdfIntegrator::integrateRays(
    const vxb::Transformation& T_G_C,
    const vxb::Pointcloud& points_C,
    const HashableColors& colors,
    const SemanticLabels& semantic_labels,
    const bool enable_anti_grazing,
    const bool clearing_ray,
    const VoxelMap& voxel_map,
    const VoxelMap& clear_map) {
  // if only 1 thread just do function call, otherwise spawn threads
  if (config_.integrator_threads == 1u) {
    constexpr size_t thread_idx = 0u;
    integrateVoxels(T_G_C,
                    points_C,
                    colors,
                    semantic_labels,
                    enable_anti_grazing,
                    clearing_ray,
                    voxel_map,
                    clear_map,
                    thread_idx);
  } else {
    std::list<std::thread> integration_threads;
    for (size_t i = 0u; i < config_.integrator_threads; ++i) {
      integration_threads.emplace_back(&SemanticTsdfIntegrator::integrateVoxels,
                                       this,
                                       T_G_C,
                                       points_C,
                                       colors,
                                       semantic_labels,
                                       enable_anti_grazing,
                                       clearing_ray,
                                       voxel_map,
                                       clear_map,
                                       i);
    }

    for (std::thread& thread : integration_threads) {
      thread.join();
    }
  }

  vxb::timing::Timer insertion_timer("inserting_missed_blocks");
  updateLayerWithStoredBlocks();
  updateSemanticLayerWithStoredBlocks();
  insertion_timer.Stop();
}

// NEEDS TO BE THREAD-SAFE
void SemanticTsdfIntegrator::integrateVoxels(
    const vxb::Transformation& T_G_C,
    const vxb::Pointcloud& points_C,
    const HashableColors& colors,
    const SemanticLabels& semantic_labels,
    const bool enable_anti_grazing,
    const bool clearing_ray,
    const VoxelMap& voxel_map,
    const VoxelMap& clear_map,
    const size_t thread_idx) {
  VoxelMap::const_iterator it;
  size_t map_size;
  if (clearing_ray) {
    it = clear_map.begin();
    map_size = clear_map.size();
  } else {
    it = voxel_map.begin();
    map_size = voxel_map.size();
  }
  for (size_t i = 0u; i < map_size; ++i) {
    if (((i + thread_idx + 1) % config_.integrator_threads) == 0u) {
      integrateVoxel(T_G_C,
                     points_C,
                     colors,
                     semantic_labels,
                     enable_anti_grazing,
                     clearing_ray,
                     *it,
                     voxel_map);
    }
    ++it;
  }
}

// HAS TO BE THREADSAFE!!!
void SemanticTsdfIntegrator::integrateVoxel(
    const vxb::Transformation& T_G_C,
    const vxb::Pointcloud& points_C,
    const HashableColors& colors,
    const SemanticLabels& semantic_labels,
    const bool enable_anti_grazing,
    const bool clearing_ray,
    const VoxelMapElement& global_voxel_idx_to_point_indices,
    const VoxelMap& voxel_map) {
  if (global_voxel_idx_to_point_indices.second.empty()) {
    return;
  }

  const vxb::Point& origin = T_G_C.getPosition();
  HashableColor merged_color;
  vxb::Point merged_point_C = vxb::Point::Zero();
  vxb::FloatingPoint merged_weight = 0.0f;
  // Calculate semantic labels frequencies to encode likelihood function.
  // Prefill with 0 frequency.
  SemanticProbabilities semantic_label_frequencies =
      SemanticProbabilities::Zero();

  // Loop over all point indices inside current voxel.
  // Generate merged values to propagate to other voxels:
  // - merged_point_C: point in Camera frame of reference representing the
  //                   measured depth value summarized from all depth values.
  // - merged_color: color to ray-cast through the voxels.
  // - semantic_label_frequencies: number of observed labels in the voxel.
  for (const size_t& pt_idx : global_voxel_idx_to_point_indices.second) {
    const vxb::Point& point_C = points_C[pt_idx];
    const HashableColor& color = colors[pt_idx];

    const vxb::FloatingPoint& point_weight = getVoxelWeight(point_C);
    // TODO(Toni) skip?
    // if (point_weight < kEpsilon) {
    //   continue;
    // }
    merged_point_C = (merged_point_C * merged_weight + point_C * point_weight) /
                     (merged_weight + point_weight);
    merged_color = HashableColor::blendTwoColors(
        merged_color, merged_weight, color, point_weight);
    merged_weight += point_weight;

    const SemanticLabel& semantic_label = semantic_labels[pt_idx];
    CHECK_LT(semantic_label, semantic_label_frequencies.size());
    semantic_label_frequencies[semantic_label] += 1.0f;

    // only take first point when clearing
    if (clearing_ray) {
      break;
    }
  }

  const vxb::Point merged_point_G = T_G_C * merged_point_C;
  vxb::RayCaster ray_caster(origin,
                            merged_point_G,
                            clearing_ray,
                            config_.voxel_carving_enabled,
                            config_.max_ray_length_m,
                            voxel_size_inv_,
                            config_.default_truncation_distance);

  vxb::GlobalIndex global_voxel_idx;
  // TODO(Toni): also put semantic_block
  // and tsdf_block outside while loop!!!!!! Why is merged not doing this???
  vxb::BlockIndex tsdf_block_idx;
  vxb::BlockIndex semantic_block_idx;
  vxb::Block<vxb::TsdfVoxel>::Ptr tsdf_block = nullptr;
  vxb::Block<SemanticVoxel>::Ptr semantic_block = nullptr;
  // This bool checks if the ray already updated a voxel as empty.
  bool ray_updated_voxel_empty = false;
  while (ray_caster.nextRayIndex(&global_voxel_idx)) {
    if (enable_anti_grazing) {
      // Check if this one is already the block hash map for this
      // insertion. Skip this to avoid grazing.
      if ((clearing_ray ||
           global_voxel_idx != global_voxel_idx_to_point_indices.first) &&
          voxel_map.find(global_voxel_idx) != voxel_map.end()) {
        continue;
      }
    }

    // This is supposedly thread-safe...
    vxb::TsdfVoxel* tsdf_voxel = allocateStorageAndGetVoxelPtr(
        global_voxel_idx, &tsdf_block, &tsdf_block_idx);
    CHECK(tsdf_block);
    CHECK_NOTNULL(tsdf_voxel);

    updateTsdfVoxel(origin,
                    merged_point_G,
                    global_voxel_idx,
                    merged_color,
                    merged_weight,
                    tsdf_voxel);

    // TODO(Toni): if we add the "empty" label, this should not be necessary
    // but we will be doing quite a bit more of computation...
    // If voxel carving is enabled, then only allocate the label voxels
    // within semantic_truncation_distance_factor times the truncation
    // distance from the surface.
    if (config_.voxel_carving_enabled) {
      if (std::abs(tsdf_voxel->distance) <
          semantic_config_.semantic_truncation_distance_factor_ *
              config_.default_truncation_distance) {
        // if (ray_updated_voxel_empty) {
        //  //LOG(WARNING) << "This ray has already started updating empty"
        //  //                       "... How is this possible? TSDF is not
        //  what"
        //  //                       " you want dist_z is!";
        //  continue;
        //}
        SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr(
            global_voxel_idx, &semantic_block, &semantic_block_idx);
        CHECK(semantic_block);
        CHECK_NOTNULL(semantic_voxel);
        updateSemanticVoxel(
            global_voxel_idx, semantic_label_frequencies, semantic_voxel);
      } else {
        VLOG(10) << "Not allocating voxel: too far from nearest object.";
      }
    } else {
      LOG_FIRST_N(WARNING, 1) << "Semantic voxel carving disabled.";
      // Allocate voxel no matter its distance to a surface. When we are
      // far away from a surface, we update the "empty" label.
      SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr(
          global_voxel_idx, &semantic_block, &semantic_block_idx);
      CHECK(semantic_block);
      CHECK_NOTNULL(semantic_voxel);
      if (std::abs(tsdf_voxel->distance) <
          semantic_config_.semantic_truncation_distance_factor_ *
              config_.default_truncation_distance) {
        // if (ray_updated_voxel_empty) {
        //  //LOG(WARNING) << "This ray has already started updating empty"
        //  //                       "... How is this possible? TSDF is not
        //  what"
        //  //                       " you want dist_z is!";
        //  continue;
        //}
        updateSemanticVoxel(
            global_voxel_idx, semantic_label_frequencies, semantic_voxel);
      } else {
        // Update empty.
        ray_updated_voxel_empty = true;
        // If we are far from the surface start to update the semantic
        // labels of the voxels as "empty".
        SemanticProbabilities update_empty = SemanticProbabilities::Zero();
        // Use as evidence of this voxel being empty all the points used
        // inside this merged ray.
        CHECK_EQ(update_empty.rows(), semantic_label_frequencies.rows());
        // Maybe we should use
        // global_voxel_idx_to_point_indices.second.size()?
        // TODO(Toni): the sum of semantic_label_frequencies is -inf
        // sometimes.
        update_empty[0u] = 1.0;  // * semantic_label_frequencies.sum();
        CHECK_GE(update_empty.sum(), 1.0);
        updateSemanticVoxel(global_voxel_idx, update_empty, semantic_voxel);
      }
    }
  }
}

// TODO(Toni): Complete this function!!
SemanticProbability SemanticTsdfIntegrator::computeMeasurementProbability(
    vxb::FloatingPoint ray_distance) {
  return 1.0;
}

void SemanticTsdfIntegrator::updateSemanticVoxel(
    const vxb::GlobalIndex& global_voxel_idx,
    const SemanticProbabilities& measurement_frequencies,
    SemanticVoxel* semantic_voxel) {
  CHECK_NOTNULL(semantic_voxel);
  // Lookup the mutex that is responsible for this voxel and lock it
  std::lock_guard<std::mutex> lock(mutexes_.get(global_voxel_idx));

  // Thread-safe region, you can modify the Voxel!
  CHECK_NOTNULL(semantic_voxel);

  // Calculate new probabilities given the measurement frequencies.
  updateSemanticVoxelProbabilities(measurement_frequencies,
                                   &semantic_voxel->semantic_priors);

  // Get MLE semantic label.
  calculateMaximumLikelihoodLabel(semantic_voxel->semantic_priors,
                                  &semantic_voxel->semantic_label);

  // Colorize according to current MLE semantic label.
  updateSemanticVoxelColor(semantic_voxel->semantic_label,
                           &semantic_voxel->color);
}

SemanticVoxel* SemanticTsdfIntegrator::allocateStorageAndGetSemanticVoxelPtr(
    const vxb::GlobalIndex& global_voxel_idx,
    vxb::Block<SemanticVoxel>::Ptr* last_semantic_block,
    vxb::BlockIndex* last_block_idx) {
  CHECK_NOTNULL(last_semantic_block);
  CHECK_NOTNULL(last_block_idx);

  const vxb::BlockIndex block_idx = vxb::getBlockIndexFromGlobalVoxelIndex(
      global_voxel_idx, voxels_per_side_inv_);

  // TODO(margaritaG): citing Marius: this logic makes sure that if you
  // already have the right block pointer you don't need to go looking for it
  // again, so in order to make this logic effective you need to move the
  // block ptr and block idx outside of the while loop in the function calling
  // this.
  if ((block_idx != *last_block_idx) || (*last_semantic_block == nullptr)) {
    *last_semantic_block = semantic_layer_->getBlockPtrByIndex(block_idx);
    *last_block_idx = block_idx;
  }

  // If no block at this location currently exists, we allocate a temporary
  // voxel that will be merged into the map later
  if (*last_semantic_block == nullptr) {
    // To allow temp_label_block_map_ to grow we can only let
    // one thread in at once
    std::lock_guard<std::mutex> lock(temp_semantic_block_mutex_);

    typename vxb::Layer<SemanticVoxel>::BlockHashMap::iterator it =
        temp_semantic_block_map_.find(block_idx);
    if (it != temp_semantic_block_map_.end()) {
      *last_semantic_block = it->second;
    } else {
      auto insert_status = temp_semantic_block_map_.emplace(
          block_idx,
          std::make_shared<vxb::Block<SemanticVoxel>>(
              voxels_per_side_,
              voxel_size_,
              vxb::getOriginPointFromGridIndex(block_idx, block_size_)));

      CHECK(insert_status.second) << "Block already exists when allocating at "
                                  << block_idx.transpose();

      *last_semantic_block = insert_status.first->second;
    }
  }

  // Only used if someone calls the getAllUpdatedBlocks I believe.
  // (*last_semantic_block)->updated().set(); // do this when updating to
  // master.
  (*last_semantic_block)->updated() = true;

  const vxb::VoxelIndex local_voxel_idx =
      vxb::getLocalFromGlobalVoxelIndex(global_voxel_idx, voxels_per_side_);

  return &((*last_semantic_block)->getVoxelByVoxelIndex(local_voxel_idx));
}

// NOT THREAD SAFE
void SemanticTsdfIntegrator::updateSemanticLayerWithStoredBlocks() {
  vxb::BlockIndex last_block_idx;
  vxb::Block<SemanticVoxel>::Ptr block = nullptr;
  for (const std::pair<const vxb::BlockIndex, vxb::Block<SemanticVoxel>::Ptr>&
           tmp_label_block_pair : temp_semantic_block_map_) {
    semantic_layer_->insertBlock(tmp_label_block_pair);
  }
  temp_semantic_block_map_.clear();
}

/** THREAD SAFE
 * Given a measured semantic label (semantic_label),
 * a measurement probability (measurement_probability), and
 * the set of prior semantic probabilities (semantic_prior_probability).
 * Returns: __normalized__ posterior probabilities given by the update eq.:
 *
 * posterior_i = measurement_probability * prior_i; // Iff i == measured_label
 * posterior_i = (1 - measurement_probability) * prior_i; // i !=
 *mesured_label
 *
 * Typically, one would set measurement_probability to be 0.9
 * Unless prior knowledge of the likelihood of the measurement is given.
 * For example, one may encode higher probability of measuring label floor
 * if voxel is close to ground.
 **/
// TODO(Toni): Unit Test this function!
void SemanticTsdfIntegrator::updateSemanticVoxelProbabilities(
    const SemanticProbabilities& measurement_frequencies,
    SemanticProbabilities* semantic_prior_probability) const {
  CHECK_NOTNULL(semantic_prior_probability);
  CHECK_EQ(semantic_prior_probability->size(), kTotalNumberOfLabels);
  CHECK_LE((*semantic_prior_probability)[0], 0.0);
  CHECK(std::isfinite((*semantic_prior_probability)[0]));
  CHECK(!semantic_prior_probability->hasNaN());
  CHECK_EQ(measurement_frequencies.size(), kTotalNumberOfLabels);
  CHECK_GE(measurement_frequencies.sum(), 1.0)
      << "We should at least have one measurement when calling this "
         "function.";

  // Check prior is normalized!
  // static constexpr bool kDebug = true;
  // if (kDebug) {
  //  CHECK_NEAR(semantic_prior_probability->sum(), 1.0, vxb::kFloatEpsilon);
  //}

  // A. Pre-multiply each column of
  // likelihood matrix with corresponding measurement frequency.
  // B. Compute posterior probabilities:
  // Post_i = (likelihood * meas freqs)_i + prior_i;
  *semantic_prior_probability +=
      semantic_log_likelihood_ * measurement_frequencies;
  CHECK(!semantic_prior_probability->hasNaN());

  // Normalize posterior probability.
  // TODO(Toni): no need to normalize all the time unless someone asks
  // for meaningful probabilities?
  // normalizeProbabilities(semantic_prior_probability);
}

// THREAD SAFE
void SemanticTsdfIntegrator::normalizeProbabilities(
    SemanticProbabilities* unnormalized_probs) const {
  CHECK_NOTNULL(unnormalized_probs);
  CHECK_GT(unnormalized_probs->size(), 0u);
  CHECK_LT((*unnormalized_probs)[0], 0.0) << "Are you sure you are using"
                                             "log odds?";

  // Find norm
  // Make it a double so you make sure there is no overflow??
  SemanticProbability normalization_factor = unnormalized_probs->norm();
  // Eigen's normalize :
  // "If the input vector is too small (i.e., this->norm()==0),
  // then this function returns a copy of the input."
  CHECK_GE(normalization_factor, 0.0);
  if (normalization_factor != 0.0) {
    unnormalized_probs->normalize();
  } else {
    CHECK_EQ(unnormalized_probs->size(), kTotalNumberOfLabels);
    static const SemanticProbability kUniformLogProbability =
        std::log(1 / kTotalNumberOfLabels);
    LOG(WARNING) << "Normalization Factor is " << normalization_factor
                 << ", all values are 0. Normalizing to log(1/n) = "
                 << kUniformLogProbability;
    unnormalized_probs->setConstant(kUniformLogProbability);
  }

  // TODO(Toni): REMOVE
  // Check that normalization really happened.
  static constexpr bool kDebug = true;
  if (kDebug) {
    CHECK_NEAR(unnormalized_probs->norm(), 1.0f, vxb::kFloatEpsilon);
  }
}

// THREAD SAFE
void SemanticTsdfIntegrator::calculateMaximumLikelihoodLabel(
    const SemanticProbabilities& semantic_posterior,
    SemanticLabel* semantic_label) const {
  CHECK_NOTNULL(semantic_label);
  // Return semantic label with current max probability.
  // TODO(Toni): what if there is a draw?
  // Warning:
  //    the result is undefined if semantic_posterior contains NaN.
  CHECK(!semantic_posterior.hasNaN())
      << "Eigen's maxCoeff has undefined behaviour with NaNs, fix your "
         "posteriors.";
  semantic_posterior.maxCoeff(semantic_label);
}

// THREAD SAFE
void SemanticTsdfIntegrator::updateSemanticVoxelColor(
    const SemanticLabel& semantic_label,
    HashableColor* semantic_voxel_color) const {
  CHECK_NOTNULL(semantic_voxel_color);
  // Do Not modify semantic_label_color_map_ here bcs we need to remain
  // thread-safe, and adding mutexes for coloring seems silly.
  // Precompute, for all possible SemanticLabels (255) a color.
  *semantic_voxel_color =
      semantic_config_.semantic_label_color_map_.at(semantic_label);
}

}  // Namespace kimera


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/detail/any_of.hpp>
#include <boost/hana/detail/wrong.hpp>
#include <boost/hana/integral_constant.hpp>
namespace hana = boost::hana;

template <typename I>
struct is_even {
    static constexpr bool value = I::value % 2 == 0;
};

static_assert(!hana::detail::any_of<is_even>::value, "");
static_assert(!hana::detail::any_of<is_even, hana::int_<1>>::value, "");
static_assert(!hana::detail::any_of<is_even, hana::int_<1>, hana::int_<3>>::value, "");
static_assert(!hana::detail::any_of<is_even, hana::int_<1>, hana::int_<3>, hana::int_<5>>::value, "");
static_assert(!hana::detail::any_of<is_even, hana::int_<1>, hana::int_<3>, hana::int_<5>, hana::int_<7>>::value, "");

static_assert(hana::detail::any_of<is_even, hana::int_<0>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<2>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<2>, hana::int_<4>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<2>, hana::int_<4>, hana::int_<6>>::value, "");

static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<1>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<1>, hana::int_<2>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<3>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<3>, hana::int_<4>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<1>, hana::int_<3>, hana::int_<5>, hana::int_<8>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<1>, hana::int_<8>, hana::int_<5>, hana::int_<7>>::value, "");

// Make sure we short-circuit properly
template <typename ...Dummy>
struct fail {
    static_assert(hana::detail::wrong<Dummy...>::value,
        "this must never be instantiated");
};
static_assert(hana::detail::any_of<is_even, hana::int_<1>, hana::int_<2>, fail<>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<1>, hana::int_<2>, fail<>, hana::int_<3>>::value, "");
static_assert(hana::detail::any_of<is_even, hana::int_<1>, hana::int_<2>, fail<>, hana::int_<4>>::value, "");

int main() { }