// The Lithopia project initiated by Jeong-Mo Hong for 3D Printing Community.
// Copyright (c) 2015 Jeong-Mo Hong - All Rights Reserved.
// This file is subject to the terms and conditions defined in
// file 'LICENSE.txt', which is part of this source code package.

#include "../Engine/Framework/WORLD.h"
#include "../Engine/Geometry/LITHOPHANE.h"
#include "../Engine/Geometry/SHELL_OF_REVOLUTION.h"
#include "../Engine/DataStructure/GridUniform2D.h"
#include "../Engine/CONVENTIONAL_MACROS.h"
#include <boost/date_time/gregorian/gregorian.hpp>

#include <IL/il.h>
#include <IL/ilu.h>

#define CYLINDERICAL_LITHOPHANE 0
#define SPHERICAL_PROJECTION 1
#define PLANAR_LITHOPHANE 2
#define TRIANGULAR_PRISM 3
#define RECTANGULAR_PRISM 4
#define PENTAGONAL_PRISM 5
#define HEXAGONARL_PRISM 6
#define PLANE_WITH_BLACK_HOLES 7
#define PLANE_WITH_WHITE_HOLES 8
#define PLANE_KEY_RING 9
#define PLANE_LITHOPHANE_FREE 10
#define CIRCULAR_TRUNCATED_CONE_LITHOPHANE 11
#define CURVED_CYLINDERICAL_LITHOPHANE 12
#define FREEFORM_CURVED_CYLINDERICAL_LITHOPHANE 13

//#define KEYRING_RELEASE 1

using namespace std;

string image_file_name;

void ilResizeImage(const int& ground_width, const int& ground_height);
void InitializeImageFromFile(IMAGE_2D& image, const string image_file_path, const int& ground_width, const int& ground_height);
void ilReadImageFromFile(const string image_file_path);
void CopyImageFromILImage(IMAGE_2D& image, const string image_file_path);
void PrepareForWriting(StaticTriangularSurface& surface);

int main(int argc, char *argv[])
{
	ilInit();

	const int object_type = std::atoi(argv[1]);

	string output_directory(argv[2]);

	StaticTriangularSurface surface;

#ifdef KEYRING_RELEASE
	std::cout << "This keyring maker is developed by Jeong-Mo Hong." << std::endl;

	std::string user_name = "marantz2000";
	boost::gregorian::date expiration_day(boost::gregorian::from_simple_string(std::string("2015-09-01")));

	if (object_type != PLANE_KEY_RING)
	{
		std::cout << "The first parameter should be 9. Terminating." << std::endl;
		return -1;
	}
	else
	{
		using namespace boost::gregorian;

		date today = day_clock::local_day();
//		date today(from_simple_string(std::string("2015-09-03")));

		std::cout << "This release is allowed for " << user_name << " until " << to_simple_string(expiration_day) << std::endl;

		days days_left = expiration_day - today;

		if (days_left.is_negative())
		{
			std::cout << "License has been expired. Terminating." << std::endl;

			return -1;
		}
		else
		{
			std::cout << "License is valid. OK to proceed." << std::endl;
		}

// 		date::ymd_type ymd = today.year_month_day();
// 		greg_weekday wd = today.day_of_week();
// 		std::cout << wd.as_long_string() << " " << ymd.month.as_long_string() << " " << ymd.day << ", " << ymd.year << std::endl;
	}
#endif

#ifndef KEYRING_RELEASE
	if (object_type == CYLINDERICAL_LITHOPHANE)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);

		const float inner_radius = std::atof(argv[6]);
		const float base_thickness = std::atof(argv[7]);
		const float front_thickness = std::atof(argv[8]);
		const int smooth = std::atoi(argv[9]);

		LITHOPHANE lithophane_maker;

		image.ReflectLeftRight();
		image.Rotate90();
		lithophane_maker.InitializeCylinder(image, surface, inner_radius, base_thickness, front_thickness, smooth);
	}
	else if (object_type == SPHERICAL_PROJECTION)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);

		const float print_thickness = std::atof(argv[6]);
		const float print_width = std::atof(argv[7]);
		const int smooth = std::atoi(argv[8]);

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializeExtrusionAndSphericalMapping(image, surface, print_thickness, print_width, smooth);
	}
	else if (object_type == PLANAR_LITHOPHANE)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);

		const float base_depth = std::atof(argv[6]);
		const float print_depth = std::atof(argv[7]);
		const float print_width = std::atof(argv[8]);
		const int smooth = std::atoi(argv[9]);

		LITHOPHANE lithophane_maker;
		image.ReflectLeftRight();
		lithophane_maker.InitializePlane(image, surface, base_depth, print_depth, print_width, smooth);
	}
	else if (object_type >= TRIANGULAR_PRISM && object_type <= HEXAGONARL_PRISM)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);

		const int num_sides = object_type;				// TRIANGULAR_PRISM = 3, ...
		const float inner_radius = std::atof(argv[6]);
		const float base_thickness = std::atof(argv[7]);
		const float print_thickness = std::atof(argv[8]);
		const float corner_angle = std::atof(argv[9]);		// degree
		const int smoothing = std::atoi(argv[10]);

		image.ReflectLeftRight();
		image.Rotate90();

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializePrism(image, surface, num_sides, inner_radius, base_thickness, print_thickness, corner_angle, smoothing);
	}
	else if (object_type == PLANE_WITH_BLACK_HOLES)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);
		image.ReflectLeftRight();
		image.ReflectUpDown();

		const float thickness = std::atof(argv[6]);
		const float width = std::atof(argv[7]);
		const int smoothing = std::atoi(argv[8]);

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializeExtrusion(image, surface, thickness, width, smoothing);
	}
	else if (object_type == PLANE_WITH_WHITE_HOLES)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);
		image.ReverseColors();
		image.ReflectLeftRight();
		image.ReflectUpDown();

		const float thickness = std::atof(argv[6]);
		const float width = std::atof(argv[7]);
		const int smoothing = std::atoi(argv[8]);

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializeExtrusion(image, surface, thickness, width, smoothing);
	}
	else if (object_type == PLANE_KEY_RING)
	{
		string shape_image_file_path(argv[3]);
		string word_image_file_path(argv[4]);
		string litho_image_file_path(argv[5]);

		const int ground_width = std::atoi(argv[6]);
		const int ground_height = std::atoi(argv[7]);

		const float base_thickness = std::atof(argv[8]);
		const float front_print_thickness = std::atof(argv[9]);
		const float back_print_thickness = std::atof(argv[10]);
		const float width = std::atof(argv[11]);
		const int smoothing = std::atoi(argv[12]);

		// Read Image
		IMAGE_2D shape_image, word_image, litho_image;

		InitializeImageFromFile(shape_image, shape_image_file_path, ground_width, ground_height);
		InitializeImageFromFile(word_image, word_image_file_path, ground_width, ground_height);
		InitializeImageFromFile(litho_image, litho_image_file_path, ground_width, ground_height);

		const BOX_2D<int> blue_box = shape_image.GetColorBox(BMP_RGB(0, 0, 255));
		const T scale = MIN2((T)(blue_box.i_end_ - blue_box.i_start_ + 1) / (T)litho_image.res_x_, (T)(blue_box.j_end_ - blue_box.j_start_ + 1) / (T)litho_image.res_y_);
		litho_image.ScaleBy(scale, scale);
		litho_image.ReverseColors();
		litho_image.SetMultipleAllColor(0.5f);
		litho_image.ReverseColors();

		// litho_image + word_image
		const int i_offset = MAX(0, ((blue_box.i_end_ - blue_box.i_start_ + 1) - litho_image.res_x_)) / 2;
		const int j_offset = MAX(0, ((blue_box.j_end_ - blue_box.j_start_ + 1) - litho_image.res_y_)) / 2;

		for (int j = litho_image.data_.j_start_; j <= litho_image.data_.j_end_; ++j)
		for (int i = litho_image.data_.i_start_; i <= litho_image.data_.i_end_; ++i)
		{
			int ii = i + blue_box.i_start_ + i_offset;
			int jj = j + blue_box.j_start_ + j_offset;

			word_image.data_(ii, jj) = litho_image.data_.getClamped(i, j);
		}

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializePlanarKeyring(shape_image, word_image, surface, base_thickness, front_print_thickness, back_print_thickness, width, smoothing);
	}
	else if (object_type == PLANE_LITHOPHANE_FREE)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);
		//		image.ReverseColors();	// be careful

		const float base_thickness = std::atof(argv[6]);
		const float print_thickness = std::atof(argv[7]);
		const float width = std::atof(argv[8]);
		const int smoothing = std::atoi(argv[9]);

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializeFreeFormPlanarLithophane(image, surface, base_thickness, print_thickness, width, smoothing);
	}
	else if (object_type == CIRCULAR_TRUNCATED_CONE_LITHOPHANE)
	{
		string image_file_path(argv[3]);
		const int ground_width = std::atoi(argv[4]);
		const int ground_height = std::atoi(argv[5]);

		IMAGE_2D image;
		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);

		const float upper_inner_radius = std::atof(argv[6]);
		const float bottom_inner_radius = std::atof(argv[7]);
		const float base_thickness = std::atof(argv[8]);
		const float front_thickness = std::atof(argv[9]);
		const int smooth = std::atoi(argv[10]);

		LITHOPHANE lithophane_maker;

		image.ReflectLeftRight();
		image.Rotate90();
		lithophane_maker.InitializeCircularTruncatedCone(image, surface, 0, upper_inner_radius, bottom_inner_radius, base_thickness, front_thickness, smooth);
	}
	else if (object_type == CURVED_CYLINDERICAL_LITHOPHANE)
	{
		// disabled.
// 		string image_file_path(argv[3]);
// 		const int ground_width = std::atoi(argv[4]);
// 		const int ground_height = std::atoi(argv[5]);
//
// 		IMAGE_2D image;
// 		InitializeImageFromFile(image, image_file_path, ground_width, ground_height);
//
// 		const float inner_radius = std::atof(argv[6]);
// 		const float base_thickness = std::atof(argv[7]);
// 		const float front_thickness = std::atof(argv[8]);
// 		const int smooth = std::atoi(argv[9]);
//
// 		//TODO: use input
// 		ParametricCurve curve;
// 		curve.
// 		curve.x_.InitializeZeroOneDomain(1, 0, (T)(34 * 2) / (T)(46 * 2), -0.8); // radius direction
// 		curve.y_.InitializeZeroOneDomain(0, 0, 0, 0);
// 		curve.z_.InitializeZeroOneDomain(0, 1, 1, 1);  // height
//
// 		SHELL_OF_REVOLUTION shell_maker;
// 		shell_maker.GenerateLithophane(image, surface, curve, inner_radius, base_thickness, front_thickness, smooth);
	}
	else if (object_type == FREEFORM_CURVED_CYLINDERICAL_LITHOPHANE)
	{
/*
		string shape_image_file_path(argv[3]);
		string litho_image_file_path(argv[4]);
		const int ground_width = std::atoi(argv[5]);
		const int ground_height = std::atoi(argv[6]);

		ilReadImageFromFile(shape_image_file_path);

		IMAGE_2D image;
		CopyImageFromILImage(image, shape_image_file_path);

		const BOX_2D<int> blue_box = image.GetColorBox(BMP_RGB(0, 0, 255));

//		image.ExtendOneColumn(image.res_x_ / 2, 20);

		IMAGE_2D input_image;
		ilReadImageFromFile(litho_image_file_path);
		CopyImageFromILImage(input_image, litho_image_file_path);

		const T scale = MIN2((T)(blue_box.i_end_ - blue_box.i_start_ + 1) / (T)input_image.res_x_, (T)(blue_box.j_end_ - blue_box.j_start_ + 1) / (T)input_image.res_y_);

		input_image.ScaleBy(scale, scale);

		IMAGE_2D litho_image;
		litho_image.Initialize(image.res_x_, image.res_y_);
		litho_image.SetAllColor(BMP_RGB(0, 0, 0));

		// copy image to litho height map

		// centering
		const int i_offset = MAX(0, ((blue_box.i_end_ - blue_box.i_start_ + 1) - input_image.res_x_)) / 2;
		const int j_offset = MAX(0, ((blue_box.j_end_ - blue_box.j_start_ + 1) - input_image.res_y_)) / 2;

		for (int j = input_image.data_.j_start_; j <= input_image.data_.j_end_; ++j)
		for (int i = input_image.data_.i_start_; i <= input_image.data_.i_end_; ++i)
		{
			litho_image.data_(i + blue_box.i_start_ + i_offset, j + blue_box.j_start_ + j_offset) = input_image.data_.getClamped(i, j);
		}

//		litho_image.ExtendOneColumn(litho_image.res_x_ / 2, 20);

		//TODO: auto extending
		//TODO: scaling

		const float base_thickness = std::atof(argv[7]);
		const float front_print_thickness = std::atof(argv[8]);
		const float back_print_thickness = std::atof(argv[9]);
		const float radius = std::atof(argv[10]);
		const int smoothing = std::atoi(argv[11]);

		//TODO: use input
		PARAMETRIC_CURVE_SEGMENT curve;
		curve.x_.InitializeZeroOneDomain(1, 0, 1, 0);	// radius direction
		curve.y_.InitializeZeroOneDomain(0, 0, 0, 0);
		curve.z_.InitializeZeroOneDomain(0, 1, 1, 1);		// height

		SHELL_OF_REVOLUTION shell_maker;
		shell_maker.GenerateFreefromCurvedCylindericalLithophane(image, litho_image, surface, curve, base_thickness, front_print_thickness, back_print_thickness, radius, smoothing);
*/
	}
	else
	{
		std::cout << "Object IX is invalid" << std::endl;
		exit(1);
	}

#else if
	if (object_type == PLANE_KEY_RING)
	{
		string shape_image_file_path(argv[3]);
		string word_image_file_path(argv[4]);
		string litho_image_file_path(argv[5]);

		const int ground_width = std::atoi(argv[6]);
		const int ground_height = std::atoi(argv[7]);

		const float base_thickness = std::atof(argv[8]);
		const float front_print_thickness = std::atof(argv[9]);
		const float back_print_thickness = std::atof(argv[10]);
		const float width = std::atof(argv[11]);
		const int smoothing = std::atoi(argv[12]);

		// Read Image
		IMAGE_2D shape_image, word_image, litho_image;

		InitializeImageFromFile(shape_image, shape_image_file_path, ground_width, ground_height);
		InitializeImageFromFile(word_image, word_image_file_path, ground_width, ground_height);
		InitializeImageFromFile(litho_image, litho_image_file_path, ground_width, ground_height);

		const BOX_2D<int> blue_box = shape_image.GetColorBox(BMP_RGB(0, 0, 255));
		const T scale = MIN2((T)(blue_box.i_end_ - blue_box.i_start_ + 1) / (T)litho_image.res_x_, (T)(blue_box.j_end_ - blue_box.j_start_ + 1) / (T)litho_image.res_y_);
		litho_image.ScaleBy(scale, scale);
		litho_image.ReverseColors();
		litho_image.SetMultipleAllColor(0.5f);
		litho_image.ReverseColors();

		// litho_image + word_image
		const int i_offset = MAX(0, ((blue_box.i_end_ - blue_box.i_start_ + 1) - litho_image.res_x_)) / 2;
		const int j_offset = MAX(0, ((blue_box.j_end_ - blue_box.j_start_ + 1) - litho_image.res_y_)) / 2;

		for (int j = litho_image.data_.j_start_; j <= litho_image.data_.j_end_; ++j)
		for (int i = litho_image.data_.i_start_; i <= litho_image.data_.i_end_; ++i)
		{
			int ii = i + blue_box.i_start_ + i_offset;
			int jj = j + blue_box.j_start_ + j_offset;

			word_image.data_(ii, jj) = litho_image.data_.GetClamped(i, j);
		}

		LITHOPHANE lithophane_maker;
		lithophane_maker.InitializePlanarKeyring(shape_image, word_image, surface, base_thickness, front_print_thickness, back_print_thickness, width, smoothing);
	}
#endif

	PrepareForWriting(surface);

	// Write STL file
	const string output_file_path_and_name_stl = output_directory + "/" + image_file_name + ".stl";
	surface.writeSTL(output_file_path_and_name_stl.c_str());

	// Write CTM file
	const string output_file_path_and_name_ctm = output_directory + "/" + image_file_name + ".ctm";
	surface.writeCTM(output_file_path_and_name_ctm.c_str());

	return 0;
}

void ilResizeImage(const int& ground_width, const int& ground_height)
{
	// image resizing
	int depth = ilGetInteger(IL_IMAGE_DEPTH);

	const int image_width = ilGetInteger(IL_IMAGE_WIDTH);
	const int image_height = ilGetInteger(IL_IMAGE_HEIGHT);

	const float dw = (float)ground_width / (float)image_width;
	const float dh = (float)ground_height / (float)image_height;

	const float dd = MIN2(dw, dh);

	const int final_width = (int)((float)image_width*dd);
	const int final_height = (int)((float)image_height*dd);

	iluScale(final_width, final_height, depth);
}

void ilReadImageFromFile(const string image_file_path)
{
	// Find file name from path
	if ((int)image_file_path.find("\\") > -1 || (int)image_file_path.find("/") > -1)
	{
		int ix = MAX2((int)image_file_path.find_last_of("\\"), (int)image_file_path.find_last_of("/"));
		image_file_name = image_file_path.substr(ix + 1);
		image_file_name;
	}
	else
	{
		image_file_name = image_file_path;
	}

	if (ilLoadImage(image_file_path.c_str()) == false)
	{
		std::cout << "Cannot read image file " << image_file_path.c_str() << std::endl;
		exit(1);
	}
}

void CopyImageFromILImage(IMAGE_2D& image, const string image_file_path)
{
	int width = ilGetInteger(IL_IMAGE_WIDTH);
	int height = ilGetInteger(IL_IMAGE_HEIGHT);
	int size = ilGetInteger(IL_IMAGE_SIZE_OF_DATA);
	int ch = ilGetInteger(IL_IMAGE_CHANNELS);

	if (ch < 3)
	{
		std::cout << "Image channel < 3" << std::endl;
		exit(1);
	}

	ILubyte * bytes = ilGetData();

	image.res_x_ = width;
	image.res_y_ = height;
	image.data_.initialize(0, 0, width, height, false);

	if (IL_BMP == ilTypeFromExt(image_file_path.c_str()))
	{
		// for bmp
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				int ix = (j*width + i)*ch;

				image.data_(i, j).r_ = bytes[ix + 2];
				image.data_(i, j).g_ = bytes[ix + 1];
				image.data_(i, j).b_ = bytes[ix + 0];
			}
		}
	}
	else
	{
		// for others
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				int ix = ((height - 1 - j)*width + i)*ch;

				image.data_(i, j).r_ = bytes[ix + 0];
				image.data_(i, j).g_ = bytes[ix + 1];
				image.data_(i, j).b_ = bytes[ix + 2];
			}
		}
	}
}

void InitializeImageFromFile(IMAGE_2D& image, const string image_file_path, const int& ground_width, const int& ground_height)
{
	// Find file name from path
	if ((int)image_file_path.find("\\") > -1 || (int)image_file_path.find("/") > -1)
	{
		int ix = MAX2((int)image_file_path.find_last_of("\\"), (int)image_file_path.find_last_of("/"));
		image_file_name = image_file_path.substr(ix + 1);
	}
	else
	{
		image_file_name = image_file_path;
	}

	if (ilLoadImage(image_file_path.c_str()) == false)
	{
		std::cout << "Cannot read image file " << image_file_path.c_str() << std::endl;
		exit(1);
	}

	ilResizeImage(ground_width, ground_height);

	int width = ilGetInteger(IL_IMAGE_WIDTH);
	int height = ilGetInteger(IL_IMAGE_HEIGHT);
	int size = ilGetInteger(IL_IMAGE_SIZE_OF_DATA);
	int ch = ilGetInteger(IL_IMAGE_CHANNELS);

	if (ch < 3)
	{
		std::cout << "Image channel < 3" << std::endl;
		exit(1);
	}

	ILubyte * bytes = ilGetData();

	image.res_x_ = width;
	image.res_y_ = height;
	image.data_.initialize(0, 0, width, height, false);

	if (IL_BMP == ilTypeFromExt(image_file_path.c_str()))
	{
		// for bmp
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				int ix = (j*width + i)*ch;

				image.data_(i, j).r_ = bytes[ix + 2];
				image.data_(i, j).g_ = bytes[ix + 1];
				image.data_(i, j).b_ = bytes[ix + 0];
			}
		}
	}
	else
	{
		// for others
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				int ix = ((height - 1 - j)*width + i)*ch;

				image.data_(i, j).r_ = bytes[ix + 0];
				image.data_(i, j).g_ = bytes[ix + 1];
				image.data_(i, j).b_ = bytes[ix + 2];
			}
		}
	}
}

void PrepareForWriting(StaticTriangularSurface& surface)
{
	BOX_3D<T> aabb = surface.getAABB();

	float scale = MAX3(aabb.x_max_ - aabb.x_min_, aabb.y_max_ - aabb.y_min_, aabb.z_max_ - aabb.z_min_);
	if (scale != 0.0f) scale = 1.0f / scale;

	glm::vec3 scalevec(scale, scale, scale);
	const Vector3D<float> aabb_center = aabb.GetCenter();
	const glm::vec3 center(aabb_center.x_, aabb_center.y_, aabb_center.z_);

	glm::mat4 m_matrix;
	m_matrix = glm::scale(scalevec) * glm::translate(-center);

	surface.OOBB_.Initialize(aabb);

//	std::cout << surface.vertex_positions_.num_elements_ << std::endl;

	surface.findAdjacentTrianglesOfVertices();
	surface.determineFaceAveragedVertexNormals();

	surface.translate(-aabb_center);
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//[ lazy_vector
// Defining this allows the assignment below of an expression to a double
// without writing any specific code to do so.
#include <boost/yap/expression.hpp>

#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>

template <boost::yap::expr_kind Kind, typename Tuple>
struct lazy_vector_expr;

// This transform turns a terminal of std::vector<double> into a terminal
// containing the nth double in that vector.  Think of it as turning our
// expression of vectors into an expression of scalars.
struct take_nth
{
    boost::yap::terminal<lazy_vector_expr, double>
    operator() (boost::yap::terminal<lazy_vector_expr, std::vector<double>> const & expr);

    std::size_t n;
};

// A custom expression template that defines lazy + and - operators that
// produce expressions, and an eager [] operator that returns the nth element
// of the expression.
//[ lazy_vector_decl
template <boost::yap::expr_kind Kind, typename Tuple>
struct lazy_vector_expr
{
    static const boost::yap::expr_kind kind = Kind;

    Tuple elements;

    // Note that this does not return an expression; it is greedily evaluated.
    auto operator[] (std::size_t n) const;
};

BOOST_YAP_USER_BINARY_OPERATOR(plus, lazy_vector_expr, lazy_vector_expr)
BOOST_YAP_USER_BINARY_OPERATOR(minus, lazy_vector_expr, lazy_vector_expr)
//]

template <boost::yap::expr_kind Kind, typename Tuple>
auto lazy_vector_expr<Kind, Tuple>::operator[] (std::size_t n) const
{ return boost::yap::evaluate(boost::yap::transform(*this, take_nth{n})); }

boost::yap::terminal<lazy_vector_expr, double>
take_nth::operator() (boost::yap::terminal<lazy_vector_expr, std::vector<double>> const & expr)
{
    double x = boost::yap::value(expr)[n];
    // This move is something of a hack; we're forcing Yap to take a copy of x
    // by using std::move().  The move indicates that the terminal should keep
    // the value of x (since, being an rvalue, it may be a temporary), rather
    // than a reference to x.  See the "How Expression Operands Are Treated"
    // section of the tutorial for details.
    return boost::yap::make_terminal<lazy_vector_expr, double>(std::move(x));
}

// In order to define the += operator with the semantics we want, it's
// convenient to derive a terminal type from a terminal instantiation of
// lazy_vector_expr.  Note that we could have written a template
// specialization here instead -- either one would work.  That would of course
// have required more typing.
struct lazy_vector :
    lazy_vector_expr<
        boost::yap::expr_kind::terminal,
        boost::hana::tuple<std::vector<double>>
    >
{
    lazy_vector () {}

    explicit lazy_vector (std::vector<double> && vec)
    { elements = boost::hana::tuple<std::vector<double>>(std::move(vec)); }

    template <boost::yap::expr_kind Kind, typename Tuple>
    lazy_vector & operator+= (lazy_vector_expr<Kind, Tuple> const & rhs)
    {
        std::vector<double> & this_vec = boost::yap::value(*this);
        for (int i = 0, size = (int)this_vec.size(); i < size; ++i) {
            this_vec[i] += rhs[i];
        }
        return *this;
    }
};

int main ()
{
    lazy_vector v1{std::vector<double>(4, 1.0)};
    lazy_vector v2{std::vector<double>(4, 2.0)};
    lazy_vector v3{std::vector<double>(4, 3.0)};

    double d1 = (v2 + v3)[2];
    std::cout << d1 << "\n";

    v1 += v2 - v3;
    std::cout << '{' << v1[0] << ',' << v1[1]
              << ',' << v1[2] << ',' << v1[3] << '}' << "\n";

    // This expression is disallowed because it does not conform to the
    // implicit grammar.  operator+= is only defined on terminals, not
    // arbitrary expressions.
    // (v2 + v3) += v1;

    return 0;
}
//]


#include <blitz/timer.h>

using namespace blitz;

void report(const char* name, Timer& timer, int N,
    long int iterations);
void unfused(int N);
void fused(int N);
void chunky(int N);

float* _bz_restrict a;
float* _bz_restrict b;
float* _bz_restrict c;
float* _bz_restrict d;
float* _bz_restrict e;
float* _bz_restrict f;

/*
 * Code to support the "fuse" macro
 */

int _chunk;
bool _done_chunks;
int _chunk_size = 512;

#define  fuse    _chunk = 0; _done_chunks = false;    \
                 for (; !_done_chunks; ++_chunk)

int main(int argc, char** argv)
{
    if (argc == 2)
        _chunk_size = atoi(argv[1]);

    cout << "Using chunk size " << _chunk_size << endl;

    const int N = 100000;

    a = new float[N];
    b = new float[N];
    c = new float[N];
    d = new float[N];
    e = new float[N];
    f = new float[N];

    for (int i=0; i < N; ++i)
    {
        a[i] = i;
        b[i] = i;
        c[i] = i;
        d[i] = i;
    }

    Timer timer;
    long int iterations = 100;

    timer.start();
    for (long i=0; i < iterations; ++i)
        unfused(N);
    timer.stop();

    report("Unfused", timer, N, iterations);

    timer.start();
    for (long i=0; i < iterations; ++i)
        fused(N);
    timer.stop();

    report("Fused", timer, N, iterations);

    timer.start();
    for (long i=0; i < iterations; ++i)
        chunky(N);
    timer.stop();

    report("Chunky", timer, N, iterations);

    return 0;
}

void report(const char* name, Timer& timer, int N,
    long int iterations)
{
    float flops = float(N) * iterations * 2;
    float Mflops = flops / timer.elapsedSeconds() / 1e+6;
    cout << setw(20) << name << " " << Mflops << " Mflops/s" << endl;
}

void __sink() { }

void unfused(int N)
{
    for (int i=0; i < N; ++i)
        e[i] = a[i] * b[i] + c[i] * d[i];

    __sink();

    for (int i=0; i < N; ++i)
        f[i] = c[i] * b[i] + a[i] * d[i];
}

void fused(int N)
{
    for (int i=0; i < N; ++i)
    {
        e[i] = a[i] * b[i] + c[i] * d[i];
        f[i] = c[i] * b[i] + a[i] * d[i];
    }
}

// This "chunky" routine is a simulated implementation of
// expression templates with tiling across multiple statements
// (the "chunky fusion" approach).  This code would be
// generated by:
//
//     fuse {
//         E = A*B + C*D;
//         F = C*B + A*D;
//     }

void chunky(int N)
{
    fuse {

        {   // Code generated by E = A*B + C*D;
            int lbound = _chunk * _chunk_size;
            int uboundp1 = lbound + _chunk_size;

            if (uboundp1 > N)
            {
                _done_chunks = true;
                uboundp1 = N;
            }

            for (int i=lbound; i < uboundp1; ++i)
                e[i] = a[i] * b[i] + c[i] * d[i];
        }

        __sink();

        {   // Code generated by F = C*B + A*D;
            int lbound = _chunk * _chunk_size;
            int uboundp1 = lbound + _chunk_size;

            if (uboundp1 > N)
            {
                _done_chunks = true;
                uboundp1 = N;
            }

            for (int i=lbound; i < uboundp1; ++i)
                f[i] = c[i] * b[i] + a[i] * d[i];
        }
    }
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mdivide_left_tri.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/scal/err/domain_error.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Returns the solution of the system Ax=b when A is triangular
 * @param A Triangular matrix.  Specify upper or lower with TriView
 * being Eigen::Upper or Eigen::Lower.
 * @param b Right hand side matrix or vector.
 * @return x = b A^-1, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <Eigen::UpLoType TriView, typename T1, typename T2, int R1, int C1,
          int R2, int C2>
inline Eigen::Matrix<return_type_t<T1, T2>, R1, C2> mdivide_right_tri(
    const Eigen::Matrix<T1, R1, C1> &b, const Eigen::Matrix<T2, R2, C2> &A) {
  check_square("mdivide_right_tri", "A", A);
  check_multiplicable("mdivide_right_tri", "b", b, "A", A);
  if (TriView != Eigen::Lower && TriView != Eigen::Upper) {
    domain_error("mdivide_left_tri",
                 "triangular view must be Eigen::Lower or Eigen::Upper", "",
                 "");
  }
  return promote_common<Eigen::Matrix<T1, R2, C2>, Eigen::Matrix<T2, R2, C2> >(
             A)
      .template triangularView<TriView>()
      .transpose()
      .solve(
          promote_common<Eigen::Matrix<T1, R1, C1>, Eigen::Matrix<T2, R1, C1> >(
              b)
              .transpose())
      .transpose();
}

/**
 * Returns the solution of the system Ax=b when A is triangular
 * and A and b are matrices of doubles.
 * @param A Triangular matrix.  Specify upper or lower with TriView
 * being Eigen::Upper or Eigen::Lower.
 * @param b Right hand side matrix or vector.
 * @return x = b A^-1, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <Eigen::UpLoType TriView, int R1, int C1, int R2, int C2>
inline Eigen::Matrix<double, R1, C2> mdivide_right_tri(
    const Eigen::Matrix<double, R1, C1> &b,
    const Eigen::Matrix<double, R2, C2> &A) {
  check_square("mdivide_right_tri", "A", A);
  check_multiplicable("mdivide_right_tri", "b", b, "A", A);
#ifdef STAN_OPENCL
  if (A.rows()
      >= opencl_context.tuning_opts().tri_inverse_size_worth_transfer) {
    matrix_cl<double> A_cl(A, from_eigen_uplo_type(TriView));
    matrix_cl<double> b_cl(b);
    matrix_cl<double> A_inv_cl = tri_inverse(A_cl);
    matrix_cl<double> C_cl = b_cl * A_inv_cl;
    return from_matrix_cl(C_cl);
  } else {
#endif
    return A.template triangularView<TriView>()
        .transpose()
        .solve(b.transpose())
        .transpose();
#ifdef STAN_OPENCL
  }
#endif
}

}  // namespace math
}  // namespace stan
#endif


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_NUMERIC_SAMPLER_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_SAMPLER_HPP

#include <boost/gil/extension/numeric/pixel_numeric_operations.hpp>
#include <boost/gil/extension/dynamic_image/dynamic_image_all.hpp>

namespace boost { namespace gil {

// Nearest-neighbor and bilinear image samplers.
// NOTE: The code is for example use only. It is not optimized for performance

///////////////////////////////////////////////////////////////////////////
////
////     resample_pixels: set each pixel in the destination view as the result of a sampling function over the transformed coordinates of the source view
////
///////////////////////////////////////////////////////////////////////////
/*
template <typename Sampler>
concept SamplerConcept {
    template <typename DstP,      // Models PixelConcept
              typename SrcView,    // Models RandomAccessNDImageViewConcept
              typename S_COORDS>  // Models PointNDConcept, where S_COORDS::num_dimensions == SrcView::num_dimensions
    bool sample(const Sampler& s, const SrcView& src, const S_COORDS& p, DstP result);
};
*/

/// \brief A sampler that sets the destination pixel to the closest one in the source. If outside the bounds, it doesn't change the destination
/// \ingroup ImageAlgorithms
struct nearest_neighbor_sampler {};

template <typename DstP, typename SrcView, typename F>
bool sample(nearest_neighbor_sampler, SrcView const& src, point<F> const& p, DstP& result)
{
    typename SrcView::point_t center(iround(p));
    if (center.x >= 0 && center.y >= 0 && center.x < src.width() && center.y < src.height())
    {
        result=src(center.x,center.y);
        return true;
    }
    return false;
}

struct cast_channel_fn {
    template <typename SrcChannel, typename DstChannel>
    void operator()(const SrcChannel& src, DstChannel& dst) {
        using dst_value_t = typename channel_traits<DstChannel>::value_type;
        dst = dst_value_t(src);
    }
};

template <typename SrcPixel, typename DstPixel>
void cast_pixel(const SrcPixel& src, DstPixel& dst) {
    static_for_each(src,dst,cast_channel_fn());
}

namespace detail {

template <typename Weight>
struct add_dst_mul_src_channel {
    Weight _w;
    add_dst_mul_src_channel(Weight w) : _w(w) {}

    template <typename SrcChannel, typename DstChannel>
    void operator()(const SrcChannel& src, DstChannel& dst) const {
        dst += DstChannel(src*_w);
    }
};

// dst += DST_TYPE(src * w)
template <typename SrcP,typename Weight,typename DstP>
struct add_dst_mul_src {
    void operator()(const SrcP& src, Weight weight, DstP& dst) const {
        static_for_each(src,dst, add_dst_mul_src_channel<Weight>(weight));
//        pixel_assigns_t<DstP,DstP&>()(
//            pixel_plus_t<DstP,DstP,DstP>()(
//                pixel_multiplies_scalar_t<SrcP,Weight,DstP>()(src,weight),
//                dst),
//            dst);
    }
};
} // namespace detail

/// \brief A sampler that sets the destination pixel as the bilinear interpolation of the four closest pixels from the source.
/// If outside the bounds, it doesn't change the destination
/// \ingroup ImageAlgorithms
struct bilinear_sampler {};

template <typename DstP, typename SrcView, typename F>
bool sample(bilinear_sampler, SrcView const& src, point<F> const& p, DstP& result)
{
    using SrcP = typename SrcView::value_type;
    point_t p0(ifloor(p.x), ifloor(p.y)); // the closest integer coordinate top left from p
    point<F> frac(p.x-p0.x, p.y-p0.y);

    if (p0.x < -1 || p0.y < -1 || p0.x>=src.width() || p0.y>=src.height())
    {
        return false;
    }

	pixel<F,devicen_layout_t<num_channels<SrcView>::value> > mp(0); // suboptimal
	typename SrcView::xy_locator loc=src.xy_at(p0.x,p0.y);

	if (p0.x == -1)
    {
		if (p0.y == -1)
        {
		    // the top-left corner pixel
			++loc.y();
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],  1        ,mp);
		}
        else if (p0.y+1<src.height())
        {
            // on the first column, but not the top-left nor bottom-left corner pixel
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1], (1-frac.y),mp);
			++loc.y();
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],    frac.y ,mp);
		}
        else
        {
			// the bottom-left corner pixel
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],  1        ,mp);
		}
	}
    else if (p0.x+1<src.width())
    {
		if (p0.y == -1)
        {
		    // on the first row, but not the top-left nor top-right corner pixel
			++loc.y();
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,      (1-frac.x)           ,mp);
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],   frac.x            ,mp);
		}
        else if (p0.y+1<src.height())
        {
			// most common case - inside the image, not on the frist nor last row/column
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,      (1-frac.x)*(1-frac.y),mp);
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],   frac.x *(1-frac.y),mp);
			++loc.y();
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,      (1-frac.x)*   frac.y ,mp);
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],   frac.x *   frac.y ,mp);
		}
        else
        {
			// on the last row, but not the bottom-left nor bottom-right corner pixel
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,      (1-frac.x)           ,mp);
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(loc.x()[1],   frac.x            ,mp);
		}
	}
    else
    {
        if (p0.y == -1)
        {
            // the top-right corner pixel
            ++loc.y();
            detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,  1        ,mp);
        }
        else if (p0.y+1<src.height())
        {
			// on the last column, but not the top-right nor bottom-right corner pixel
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc, (1-frac.y),mp);
			++loc.y();
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,    frac.y ,mp);
		}
        else
        {
			// the bottom-right corner pixel
			detail::add_dst_mul_src<SrcP,F,pixel<F,devicen_layout_t<num_channels<SrcView>::value> > >()(*loc,  1        ,mp);
		}
	}

	// Convert from floating point average value to the source type
	SrcP src_result;
	cast_pixel(mp,src_result);

	color_convert(src_result, result);
	return true;
}

}}  // namespace boost::gil

#endif


/**
 * \file TriangleCheckerFilter.cpp
 */

#include "TriangleCheckerFilter.h"

#include <cmath>
#include <cstdint>
#include <cstdlib>

#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

namespace
{
  template<class DataType>
  typename boost::enable_if<typename boost::is_floating_point<DataType>::type>::type compare(DataType v1, DataType v2)
  {
    if(std::abs(v1) < 1e-5 || std::abs(v2) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(v1, static_cast<DataType>(1e-5));
      BOOST_REQUIRE_SMALL(v2, static_cast<DataType>(1e-5));
    }
    else
    {
      BOOST_REQUIRE_CLOSE(v1, v2, 0.0001);
    }
  }

  template<class DataType>
  typename boost::disable_if<typename boost::is_floating_point<DataType>::type>::type compare(DataType v1, DataType v2)
  {
    BOOST_REQUIRE_EQUAL(v1, v2);
  }
}

namespace ATK
{
  template<class DataType_>
  TriangleCheckerFilter<DataType_>::TriangleCheckerFilter()
  :TypedBaseFilter<DataType_>(1, 0), state(0), ascending(true), amplitude(1), frequency(1)
  {
  }

  template<class DataType_>
  TriangleCheckerFilter<DataType_>::~TriangleCheckerFilter()
  {
  }

  template<class DataType_>
  void TriangleCheckerFilter<DataType_>::set_amplitude(DataType_ amplitude)
  {
    this->amplitude = amplitude;
  }

  template<class DataType_>
  void TriangleCheckerFilter<DataType_>::set_frequency(int frequency)
  {
    this->frequency = frequency;
  }

  template<class DataType_>
  void TriangleCheckerFilter<DataType_>::process_impl(int64_t size) const
  {
    double real_increment = 2. / input_sampling_rate * frequency;

    for(int64_t i = 0; i < size; ++i)
    {
      state += real_increment * (ascending ? 1 : -1);
      if(state >= 1)
      {
        state -= 2 * real_increment;
        ascending = !ascending;
      }
      else if(state <= -1)
      {
        state += 2 * real_increment;
        ascending = !ascending;
      }
      compare(converted_inputs[0][i], static_cast<DataType>(amplitude * state));
    }
  }

  template class TriangleCheckerFilter<std::int16_t>;
  template class TriangleCheckerFilter<std::int32_t>;
  template class TriangleCheckerFilter<int64_t>;
  template class TriangleCheckerFilter<float>;
  template class TriangleCheckerFilter<double>;
}


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

    @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_FUNCTION_CUMPROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CUMPROD_HPP_INCLUDED

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

  /*!
    @ingroup group-reduction
    This function object computes the cumulated product of the argument elements.

    @par Header <boost/simd/function/cumprod.hpp>

    @par Note

    Take care that overflow is very easy to get here especially for integral
    types.

    @see cumsum, cummin, cummax

    @par Example:

      @snippet cumprod.cpp cumprod

    @par Possible output:

      @snippet cumprod.txt cumprod

  **/
  Value cumprod(Value const& x);
} }
#endif

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

#endif


/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#pragma once

#include "probabilityHelpers.hpp"

#include <stddef.h>
#include <stdint.h>
#include <typeinfo>

#include <armadillo>

using namespace std;
using namespace arma;

class HDP_var_base
{
  public:

    HDP_var_base(uint32_t K=0, uint32_t T=0, uint32_t Nw=0)
      : mK(K), mT(T), mNw(Nw)
    {};

    void getA(Mat<double>& a)
    {
      a=mA;
    };

    void getPerplexity(Mat<double>& perp)
    {
      perp=mPerp;
    };

    bool getDocTopics(Mat<double>& pi, Mat<double>& sigPi, Mat<uint32_t>& c) const
    {
      uint32_t D=mZeta.size();

      pi.set_size(D,mT);
      sigPi.set_size(D,mT+1);
      c.set_size(D,mT);
      for (uint32_t d=0; d<D; ++d){
        Row<double> cpi(mT);
        Row<double> csigPi(mT+1);
        Row<uint32_t> cc(mT);

        betaMode(cpi,mGamma[d].col(0),mGamma[d].col(1));
        stickBreaking(csigPi,cpi);
        if ( sum(csigPi>1.0)>0)
        {
          cout<<"d="<<d<<endl;
          cout<<"gamma: "<<mGamma[d]<<endl;
          cout<<"cpi="<<cpi<<endl;
          cout<<"csigPi="<<csigPi<<endl;
          cout<<"|csigPi|="<<sum(csigPi)<<endl;
          assert(0);
        }
        for (uint32_t i=0; i<mT; ++i){
          cc[i] = multinomialMode(mZeta[d].row(i));
        }

        //getDocTopics(cpi,csigPi,cc,mGamma[d],mZeta[d]);
        pi.row(d) = cpi;
        sigPi.row(d) = csigPi;
        c.row(d) = cc;
//        cout<<csigPi.t()<<" = "<<sum(csigPi)<<endl;
//        cout<<sigPi.row(d)<<" = "<<sum(sigPi.row(d))<<endl;
      }
      return true;
    };

  /*
   * Gets the indicators for each word pointing to a doc level topic.
   * Need to give it the doc of interest, because all documents have
   * different numbers of words and hence z is different for all docs
   *
   * @param d points to the document of interest.
   */
    bool getWordTopics(Col<uint32_t>& z, uint32_t d) const {
//      cout<<"mPhi -> D="<<mPhi.size()<<endl;
      if (z.n_rows != mPhi[d].n_rows)
      {
//        cout<<"z.rows="<<z.n_rows<<" vs phi_d.rows="<<mPhi[d].n_rows<<endl;
        return false;
      }else{
      return getWordTopics(z,mPhi[d]);
      }
    };

    bool getCorpTopicProportions(Col<double>& v, Col<double>& sigV) const
    {
      return getCorpTopicProportions(v,sigV,mA);
    };

    virtual Row<double> logP_w(uint32_t d) const
    {
      cerr<<"has to be implemented in the child classes!!"<<endl;
      exit(0);
      return Row<double>(1);
    };

//    bool getWordDistr(Mat<double>& p){
//      uint32_t D=mZeta.size();
//      p.set_size(D,mNw);
////      assert(p.n_rows == D);
////      assert(p.n_cols == mNw);
////      cout<<"D="<<D<<endl;
//      for (uint32_t d=0; d<D; ++d){
//        p.row(d) = logP_w(d);
//      }
//      return true;
//    };

protected:
    Mat<double> mA; // corpus level Beta process alpha parameter for stickbreaking
    vector<Mat<double> > mZeta; // document level topic indices/pointers to corpus level topics (Multinomial)
    vector<Mat<double> > mPhi; // document level word to doc level topic assignment (Multinomial)
    vector<Mat<double> > mGamma; // document level Beta distribution alpha parameter for stickbreaking

    Col<double> mPerp; // perplexity for each document

    uint32_t mK; // Corp level truncation
    uint32_t mT; // Doc level truncation
    uint32_t mNw; // size of dictionary

    /*
     * @param a are the parameters of the beta distribution from which v is drawn
     */
    bool getCorpTopicProportions(Col<double>& v, Col<double>& sigV, const Mat<double>& a) const
    {
      uint32_t K = a.n_rows; // corp level topics

      sigV.set_size(K+1);
      v.set_size(K);

      betaMode(v, a.col(0), a.col(1));
      stickBreaking(sigV,v);
      return true;
    };

    bool getDocTopics(Col<double>& pi, Col<double>& sigPi, Col<uint32_t>& c, const Mat<double>& gamma, const Mat<double>& zeta) const
    {
      uint32_t T = gamma.n_rows; // doc level topics

      sigPi.set_size(T+1);
      pi.set_size(T);
      c.set_size(T);

      //cout<<"K="<<K<<" T="<<T<<endl;
      betaMode(pi,gamma.col(0),gamma.col(1));
      stickBreaking(sigPi,pi);
      //cout<<"pi="<<pi<<endl;
      //cout<<"sigPi="<<sigPi<<endl;
      //cout<<"mGamma="<<mGamma[d]<<endl;
      for (uint32_t i=0; i<T; ++i){
        c[i] = multinomialMode(zeta.row(i));
      }
      return true;
    };

    bool getWordTopics(Col<uint32_t>& z, const Mat<double>& phi) const {
//      cout<<phi.n_rows<<" x "<<phi.n_cols<<endl;
      z.set_size(phi.n_rows);
      for (uint32_t i=0; i<z.n_elem; ++i){
        z[i] = multinomialMode(phi.row(i));
      }
      return true;
    };
};


// Include motion capture framework
#include "optitrack_motive_2_client/motionCaptureClientFramework.h"
// Include ACL message types (https://bitbucket.org/brettlopez/acl_msgs.git)
#include "acl_msgs/ViconState.h"

// Includes for ROS
#include "ros/ros.h"

// Includes for node
#include <boost/program_options.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>

namespace po = boost::program_options;
using namespace Eigen;

// Used to convert mocap frame (NUE) to LCM NED.
// static Eigen::Matrix3d R_NUE2NED = [] {
//     Eigen::Matrix3d tmp;
//     tmp <<  1, 0, 0,
//             0, 0, 1,
//             0, -1, 0;
//     return tmp;
// }();

// Used to convert mocap frame (NUE) to ROS ENU.
static Eigen::Matrix3d R_NUE2ENU = [] {
    Eigen::Matrix3d tmp;
    tmp <<  0, 0, 1,
            1, 0, 0,
            0, 1, 0;
    return tmp;
}();

Vector3d positionConvertNUE2ENU(double* positionNUE){
  Vector3d positionNUEVector, positionENUVector;
  positionNUEVector << positionNUE[0], positionNUE[1], positionNUE[2];

  positionENUVector = R_NUE2ENU * positionNUEVector;
  return positionENUVector;
}

Quaterniond quaternionConvertNUE2ENU(double* quaternionNUE){
    Quaterniond quaternionInNUE;
    quaternionInNUE.x() = quaternionNUE[0];
    quaternionInNUE.y() = quaternionNUE[1];
    quaternionInNUE.z() = quaternionNUE[2];
    quaternionInNUE.w() = quaternionNUE[3];

    Quaterniond quaternionInENU = Quaterniond(R_NUE2ENU * quaternionInNUE.normalized().toRotationMatrix()
                              * R_NUE2ENU.transpose());
    return quaternionInENU;
}

int main(int argc, char *argv[])
{
  // Keep track of ntime offset.
  int64_t offset_between_windows_and_linux = std::numeric_limits<int64_t>::max();

  // Init ROS
  ros::init(argc, argv, "optitrack_motive_2_client_node");
  ros::NodeHandle n;

  // Get CMDline arguments for server and local IP addresses.
  std::string szMyIPAddress;
  std::string szServerIPAddress;

  try {
    po::options_description desc ("Options");
    desc.add_options()
        ("help,h", "print usage message")
        ("local",po::value<std::string>(&szMyIPAddress),"local IP Address")
        ("server",po::value<std::string>(&szServerIPAddress), "server address");

    po::variables_map vm;
    try {
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify (vm);
    }
    catch (po::error& e) {
      std::cerr << e.what() << std::endl;
      return 0;
    }
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch (...) {}

  // Init mocap framework
  agile::motionCaptureClientFramework mocap_ = agile::motionCaptureClientFramework(szMyIPAddress, szServerIPAddress);

  // Some vars to calculate twist/acceleration and dts
  // Also keeps track of the various publishers
  std::map<int, ros::Publisher> rosPublishers;
  std::map<int, acl_msgs::ViconState> pastStateMessages;

  while (true){
    // Wait for mocap packet
    mocap_.spin();

    std::vector<agile::Packet> mocap_packets = mocap_.getPackets();

    for (agile::Packet mocap_packet : mocap_packets){

      // @TODO: Make getPackets return a list.

      // Skip this rigid body if tracking is invalid
      if (!mocap_packet.tracking_valid)
        continue;

      // estimate the windows to linux constant offset by taking the minimum seen offset.
      // @TODO: Make offset a rolling average instead of a latching offset.
      int64_t offset = mocap_packet.transmit_timestamp - mocap_packet.receive_timestamp;
      if (offset < offset_between_windows_and_linux ){
        offset_between_windows_and_linux = offset;
      }
      uint64_t packet_ntime = mocap_packet.mid_exposure_timestamp - offset_between_windows_and_linux;

      // Get past state and publisher (if they exist)
      bool hasPreviousMessage = (rosPublishers.find(mocap_packet.rigid_body_id) != rosPublishers.end());
      ros::Publisher publisher;
      acl_msgs::ViconState lastState;
      acl_msgs::ViconState currentState;

      // Initialize publisher for rigid body if not exist.
      if (!hasPreviousMessage){
        std::string topic = "/" + mocap_packet.model_name + "/vicon";

        publisher = n.advertise<acl_msgs::ViconState>(topic, 1);
        rosPublishers[mocap_packet.rigid_body_id] = publisher;
      } else {
        // Get saved publisher and last state
        publisher = rosPublishers[mocap_packet.rigid_body_id];
        lastState = pastStateMessages[mocap_packet.rigid_body_id];
      }

      // Add timestamp
      currentState.header.stamp = ros::Time(packet_ntime/1e9, packet_ntime%(int64_t)1e9);

      // Convert rigid body position from NUE to ROS ENU
      Vector3d positionENUVector = positionConvertNUE2ENU(mocap_packet.pos);
      currentState.pose.position.x = positionENUVector(0);
      currentState.pose.position.y = positionENUVector(1);
      currentState.pose.position.z = positionENUVector(2);
      // Convert rigid body rotation from NUE to ROS ENU
      Quaterniond quaternionENUVector = quaternionConvertNUE2ENU(mocap_packet.orientation);
      currentState.pose.orientation.x = quaternionENUVector.x();
      currentState.pose.orientation.y = quaternionENUVector.y();
      currentState.pose.orientation.z = quaternionENUVector.z();
      currentState.pose.orientation.w = quaternionENUVector.w();
      currentState.has_pose = true;

      // Loop through markers and convert positions from NUE to ENU
      // @TODO since the state message does not understand marker locations.

      if (hasPreviousMessage){
        // Calculate twist. Requires last state message.
        int64_t dt_nsec = packet_ntime - (lastState.header.stamp.sec*1e9 + lastState.header.stamp.nsec);
        currentState.twist.linear.x = (currentState.pose.position.x - lastState.pose.position.x)/(dt_nsec * 1e9);
        currentState.twist.linear.y = (currentState.pose.position.y - lastState.pose.position.y)/(dt_nsec * 1e9);
        currentState.twist.linear.z = (currentState.pose.position.z - lastState.pose.position.z)/(dt_nsec * 1e9);

        // Calculate rotational twist
        Quaterniond lastQuaternion;
        lastQuaternion.x() = lastState.pose.orientation.x;
        lastQuaternion.y() = lastState.pose.orientation.y;
        lastQuaternion.z() = lastState.pose.orientation.z;
        lastQuaternion.w() = lastState.pose.orientation.w;

        // @TODO: calculate the angular velocity from the two quaternions in body frame.
        Quaterniond twistQuaternion = quaternionENUVector * lastQuaternion.inverse();
        Vector3d twistEulerVector = twistQuaternion.toRotationMatrix().eulerAngles(2, 1, 0);

        currentState.twist.angular.x = 0;
        currentState.twist.angular.y = 0;
        currentState.twist.angular.z = 0;

        //currentState.twist.angular.x = twistEulerVector(0);
        //currentState.twist.angular.y = twistEulerVector(1);
        //currentState.twist.angular.z = twistEulerVector(2);
        //currentState.has_twist = true;

        // Calculate accelerations. Requires last state message.
        currentState.accel.x = (currentState.twist.linear.x - lastState.twist.linear.x)/(dt_nsec * 1e9);
        currentState.accel.y = (currentState.twist.linear.y - lastState.twist.linear.y)/(dt_nsec * 1e9);
        currentState.accel.z = (currentState.twist.linear.z - lastState.twist.linear.z)/(dt_nsec * 1e9);
        currentState.has_accel = true;

        // @TODO: Calculate angular acceleration
      }

      // Save state for future acceleration and twist computations
      pastStateMessages[mocap_packet.rigid_body_id] = currentState;

      // Publish ROS state.
      publisher.publish(currentState);

    }
  }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_HYPERGEOMETRIC_0F1_HPP
#define BOOST_MATH_HYPERGEOMETRIC_0F1_HPP

#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_0F1_bessel.hpp>

namespace boost { namespace math { namespace detail {

   template <class T>
   struct hypergeometric_0F1_cf
   {
      //
      // We start this continued fraction at b on index -1
      // and treat the -1 and 0 cases as special cases.
      // We do this to avoid adding the continued fraction result
      // to 1 so that we can accurately evaluate for small results
      // as well as large ones.  See http://functions.wolfram.com/07.17.10.0002.01
      //
      T b, z;
      int k;
      hypergeometric_0F1_cf(T b_, T z_) : b(b_), z(z_), k(-2) {}
      typedef std::pair<T, T> result_type;

      result_type operator()()
      {
         ++k;
         if (k <= 0)
            return std::make_pair(z / b, 1);
         return std::make_pair(-z / ((k + 1) * (b + k)), 1 + z / ((k + 1) * (b + k)));
      }
   };

   template <class T, class Policy>
   T hypergeometric_0F1_cf_imp(T b, T z, const Policy& pol, const char* function)
   {
      hypergeometric_0F1_cf<T> evaluator(b, z);
      boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
      T cf = tools::continued_fraction_b(evaluator, policies::get_epsilon<T, Policy>(), max_iter);
      policies::check_series_iterations<T>(function, max_iter, pol);
      return cf;
   }

   template <class T, class Policy>
   inline T hypergeometric_0F1_imp(const T& b, const T& z, const Policy& pol)
   {
      const char* function = "boost::math::hypergeometric_0f1<%1%,%1%>(%1%, %1%)";
      BOOST_MATH_STD_USING

         // some special cases
         if (z == 0)
            return T(1);

      if ((b <= 0) && (b == floor(b)))
         return policies::raise_pole_error<T>(
            function,
            "Evaluation of 0f1 with nonpositive integer b = %1%.", b, pol);

      if (z < -5 && b > -5)
      {
         // Series is alternating and divergent, need to do something else here,
         // Bessel function relation is much more accurate, unless |b| is similarly
         // large to |z|, otherwise the CF formula suffers from cancellation when
         // the result would be very small.
         if (fabs(z / b) > 4)
            return hypergeometric_0F1_bessel(b, z, pol);
         return hypergeometric_0F1_cf_imp(b, z, pol, function);
      }
      // evaluation through Taylor series looks
      // more precisious than Bessel relation:
      // detail::hypergeometric_0f1_bessel(b, z, pol);
      return detail::hypergeometric_0F1_generic_series(b, z, pol);
   }

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type hypergeometric_0F1(T1 b, T2 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hypergeometric_0F1_imp<value_type>(
         static_cast<value_type>(b),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_0F1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type hypergeometric_0F1(T1 b, T2 z)
{
   return hypergeometric_0F1(b, z, policies::policy<>());
}

} } // namespace boost::math

#endif // BOOST_MATH_HYPERGEOMETRIC_HPP


/**
 * Copyright (c) 2018 Melown Technologies SE
 *
 * 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 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 HOLDER 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.
 */

#ifndef mapproxy_generator_heightfunction_hpp_included_
#define mapproxy_generator_heightfunction_hpp_included_

#include <memory>
#include <boost/any.hpp>

#include "vts-libs/storage/range.hpp"

/** Generic height function for surface manipulation.
 */
class HeightFunction {
public:
    typedef std::shared_ptr<HeightFunction> pointer;

    virtual ~HeightFunction() {}
    virtual double operator()(double h) const = 0;

    static HeightFunction::pointer parse(const boost::any &value
                                         , const std::string &key);
    static bool changed(const HeightFunction::pointer &l
                        , const HeightFunction::pointer &r);

    virtual void build(boost::any &value) const = 0;

    virtual bool changed(const HeightFunction::pointer &other) const = 0;
};

typedef vtslibs::storage::Range<double> HeightRange;
typedef vtslibs::storage::Range<double> ScaleRange;

class SuperElevation : public HeightFunction {
public:
    struct Config {
        HeightRange heightRange;
        ScaleRange scaleRange;

        bool changed(const Config &other) const;
    };

    SuperElevation(const Config &config);

    virtual ~SuperElevation() {}

    virtual double operator()(double h) const {
        if (h < config_.heightRange.min) { return limits_.min; }
        else if (h > config_.heightRange.max) { return limits_.max; }

        return apply(h);
    }

    virtual void build(boost::any &value) const;
    virtual bool changed(const HeightFunction::pointer &other) const;

private:
    double apply(double h) const {
        return h * ((h - config_.heightRange.min) * s_
                    + config_.scaleRange.min);
    }

    /** Configuration;
     */
    Config config_;

    /** Pre-computed scaling factor.
     */
    double s_;

    /** Pre-computed value for heightRange extremes.
     */
    HeightRange limits_;
};

#endif // mapproxy_generator_heightfunction_hpp_included_


/**
 * @authors     Peter Fankhauser, Simone Arreghini
 * @affiliation ANYbotics
 * @brief       Node for comparing different normal filters performances.
 */

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#pragma GCC diagnostic ignored "-Wformat"
#include <filters/filter_chain.h>
#pragma GCC diagnostic pop
#include <ros/ros.h>
#include <Eigen/Core>
#include <cmath>
#include <vector>

#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/grid_map_ros.hpp>

// Function headers
namespace grid_map {
/*!
 * Function to calculate the normal vector computation error.Error is defined as sum over the map of absolute cosines
 * between analytical and computed normals. The cosines are calculated using the dot product between two unitary vectors.
 * The error sum is then averaged between the latest 20 iteration of the functions.
 *
 * @param map: grid map to which add the layers that contains the errors.
 * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time.
 * @param directionalErrorAreaSum: Average of the summed error over the map for area method.
 * @param directionalErrorRasterSum: Average of the summed error over the map for raster method.
 */
void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum,
                             double& directionalErrorRasterSum);

/*!
 * Function to add noise to the elevation map. Noise added is white noise from a uniform distribution [-1:1] multiplied by
 * the amount of noise wanted specified by noise_on_map.
 *
 * @param map: grid map to which add the layers that contains the errors.
 * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time.
 * @param noise_on_map: Amount of noise wanted in meters, can be set as an argument in the roslaunch phase.
 */
void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map);

/*!
 * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity.
 * It has to be performed after mapAddNoise.
 *
 * @param map: grid map to which add the layers that contains the errors.
 * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time.
 * @param outlierPercentage: Amount of outliers wanted percentage, can be set as an argument in the roslaunch phase.
 */
void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage);

/*!
 * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity.
 * It has to be performed after mapAddNoise.
 *
 * @param map: grid map to which add the layers that contains the errors.
 * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time.
 * @param filterRadius: Radius of the wanted shifting average filter.
 */
void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius);
}  // namespace grid_map

int main(int argc, char** argv) {
  // Initialize node and publisher.
  ros::init(argc, argv, "normal_filter_comparison_demo");
  ros::NodeHandle nh("~");
  ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);

  // Start time.
  double begin = ros::Time::now().toSec();

  // Load filter chain, defined in grid_map_demos/config/normal_filter_comparison.yaml.
  filters::FilterChain<grid_map::GridMap> filterChain_("grid_map::GridMap");
  std::string filterChainParametersName_;
  nh.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters"));

  // Read noise amount, in meters, from parameters server.
  double noise_on_map;
  nh.param("noise_on_map", noise_on_map, 0.015);
  ROS_INFO("noise_on_map = %f", noise_on_map);

  double outliers_percentage;
  ;
  nh.param("outliers_percentage", outliers_percentage, 0.0);
  ROS_INFO("outliers_percentage = %f", outliers_percentage);

  // Configuration of chain filter.
  if (!filterChain_.configure(filterChainParametersName_, nh)) {
    ROS_ERROR("Could not configure the filter chain!");
  }

  // Parameters for grid map dimensions.
  const double mapLength = 2.5;
  const double mapWidth = 4.0;
  const double mapResolution = 0.02;

  // Grid map object creation.
  grid_map::GridMap map({"elevation", "normal_analytic_x", "normal_analytic_y", "normal_analytic_z"});
  map.setFrameId("map");
  map.setGeometry(grid_map::Length(mapLength, mapWidth), mapResolution, grid_map::Position(0.0, 0.0));
  const grid_map::Size gridMapSize = map.getSize();
  ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
           map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(),
           map.getFrameId().c_str());

  // Initialize variables for normal quality comparison.
  double directionalErrorAreaSum = 0;
  double directionalErrorRasterSum = 0;

  // Surface shape variables.
  const double surfaceSpeed = 0.5;
  const double surfaceSlope = 0.2;
  const double surfaceBias = -0.04;
  const double wavePeriod = 5.0;

  // Work with grid map in a loop. Grid map and analytic normals are continuously generated using exact functions.
  ros::Rate rate(10.0);
  while (nh.ok()) {
    ros::Time time = ros::Time::now();

    // Calculate wave shaped elevation and analytic surface normal.
    for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) {
      grid_map::Position position;
      map.getPosition(*it, position);
      map.at("elevation", *it) =
          surfaceBias + surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()) * position.x();

      // Analytic normals computation.
      grid_map::Vector3 normalAnalytic(-surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()),
                                       -position.x() * std::cos(surfaceSpeed * time.toSec() + wavePeriod * position.y()), 1.0);
      normalAnalytic.normalize();
      map.at("normal_analytic_x", *it) = normalAnalytic.x();
      map.at("normal_analytic_y", *it) = normalAnalytic.y();
      map.at("normal_analytic_z", *it) = normalAnalytic.z();
    }

    // elevation_filtered layer is used for visualization, initialize it here and if there is noise and filtering it will be updated.
    map.add("elevation_filtered", map.get("elevation"));

    // Perturb and then filter map only if noise != 0.
    if (noise_on_map != 0.0) {
      grid_map::mapAddNoise(map, gridMapSize, noise_on_map);
    }
    if (outliers_percentage != 0.0) {
      grid_map::mapAddOutliers(map, gridMapSize, outliers_percentage);
    }
    if ((noise_on_map != 0.0) || (outliers_percentage != 0.0)) {
      grid_map::mapAverageFiltering(map, gridMapSize, 0.1);
    }

    // Computation of normals using filterChain_.update function.
    if (!filterChain_.update(map, map)) {
      ROS_ERROR("Could not update the grid map filter chain!");
    }

    // Normals error computation
    grid_map::normalsErrorCalculation(map, gridMapSize, directionalErrorAreaSum, directionalErrorRasterSum);
    ROS_INFO_THROTTLE(2.0, "directionalErrorArea = %f", directionalErrorAreaSum);
    ROS_INFO_THROTTLE(2.0, "directionalErrorRaster = %f", directionalErrorRasterSum);

    // Publish grid map.
    map.setTimestamp(time.toNSec());
    grid_map_msgs::GridMap message;
    grid_map::GridMapRosConverter::toMessage(map, message);
    publisher.publish(message);
    double end = ros::Time::now().toSec();

    // Limit simulation length to 1 minute.
    if ((end - begin) > 60) {
      break;
    }
  }

  return 0;
}

namespace grid_map {
void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum,
                             double& directionalErrorRasterSum) {
  // If layers saved as matrices accessing values is faster.
  const grid_map::Matrix mapNormalAnalyticX = map["normal_analytic_x"];
  const grid_map::Matrix mapNormalAnalyticY = map["normal_analytic_y"];
  const grid_map::Matrix mapNormalAnalyticZ = map["normal_analytic_z"];
  const grid_map::Matrix mapNormalAreaX = map["normal_area_x"];
  const grid_map::Matrix mapNormalAreaY = map["normal_area_y"];
  const grid_map::Matrix mapNormalAreaZ = map["normal_area_z"];
  const grid_map::Matrix mapNormalRasterX = map["normal_raster_x"];
  const grid_map::Matrix mapNormalRasterY = map["normal_raster_y"];
  const grid_map::Matrix mapNormalRasterZ = map["normal_raster_z"];

  // Initialize vector to use eigen scalar product (Vector3 = Eigen::Vector3d).
  grid_map::Vector3 normalVectorAnalytic = Vector3::Zero();
  grid_map::Vector3 normalVectorArea = Vector3::Zero();
  grid_map::Vector3 normalVectorRaster = Vector3::Zero();

  // Add layer for possible future visualization in RViz to understand where error is.
  map.add("directionalErrorArea", grid_map::Matrix::Zero(gridMapSize(0), gridMapSize(1)));
  map.add("directionalErrorRaster", grid_map::Matrix::Zero(gridMapSize(0), gridMapSize(1)));

  // Raster normals not defined for outermost layer of cells.
  const grid_map::Index submapStartIndex(1, 1);
  const grid_map::Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
  for (grid_map::SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) {
    const grid_map::Index index(*iterator);

    normalVectorAnalytic(0) = mapNormalAnalyticX(index(0), index(1));
    normalVectorAnalytic(1) = mapNormalAnalyticY(index(0), index(1));
    normalVectorAnalytic(2) = mapNormalAnalyticZ(index(0), index(1));

    normalVectorArea(0) = mapNormalAreaX(index(0), index(1));
    normalVectorArea(1) = mapNormalAreaY(index(0), index(1));
    normalVectorArea(2) = mapNormalAreaZ(index(0), index(1));

    normalVectorRaster(0) = mapNormalRasterX(index(0), index(1));
    normalVectorRaster(1) = mapNormalRasterY(index(0), index(1));
    normalVectorRaster(2) = mapNormalRasterZ(index(0), index(1));

    // Error(alpha) = 1.0 - abs(cos(alpha)), where alpha = angle(normalVectorAnalytic, normalVectorArea)
    // Error of perfect normals will be 0.0.
    map.at("directionalErrorArea", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorArea));
    map.at("directionalErrorRaster", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorRaster));
  }

  // Directional error defined as sum of absolute cosines of normal calculated with different methods
  const double directionalErrorArea = map["directionalErrorArea"].array().sum();
  const double directionalErrorRaster = map["directionalErrorRaster"].array().sum();

  // Calculate mean value of directional error of last 20 cycles.
  directionalErrorAreaSum = (directionalErrorAreaSum * 19.0 + directionalErrorArea) / 20.0;
  directionalErrorRasterSum = (directionalErrorRasterSum * 19.0 + directionalErrorRaster) / 20.0;
}

void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map) {
  // Add noise (using Eigen operators).
  map.add("noise", noise_on_map * Matrix::Random(gridMapSize(0), gridMapSize(1)));
  map.add("elevation_noisy", map.get("elevation") + map["noise"]);
}

void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage) {
  // Adding outliers at infinite height (accessing cell by position).
  const double numberInfPoints = outlierPercentage * gridMapSize(0) * gridMapSize(1);

  for (int i = 0; i < static_cast<int>(numberInfPoints); ++i) {
    grid_map::Position randomPosition = grid_map::Position::Random();
    if (map.isInside(randomPosition)) {
      map.atPosition("elevation_noisy", randomPosition) = std::numeric_limits<float>::infinity();
    }
  }
}

void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius) {
  grid_map::Index startIndex(0, 0);
  grid_map::SubmapIterator it(map, startIndex, gridMapSize);
  // Iterate over whole map.
  for (; !it.isPastEnd(); ++it) {
    Position currentPosition;
    map.getPosition(*it, currentPosition);
    double mean = 0.0;
    double sumOfWeights = 0.0;

    // Compute weighted mean.
    for (grid_map::CircleIterator circleIt(map, currentPosition, filterRadius); !circleIt.isPastEnd(); ++circleIt) {
      if (!map.isValid(*circleIt, "elevation_noisy")) {
        continue;
      }
      grid_map::Position currentPositionInCircle;
      map.getPosition(*circleIt, currentPositionInCircle);

      // Computed weighted mean based on Euclidian distance.
      double distance = (currentPosition - currentPositionInCircle).norm();
      double weight = pow(filterRadius - distance, 2);
      mean += weight * map.at("elevation_noisy", *circleIt);
      sumOfWeights += weight;
    }
    if (sumOfWeights!=0) {
      map.at("elevation_filtered", *it) = mean / sumOfWeights;
    } else {
      map.at("elevation_filtered", *it) = std::numeric_limits<float>::infinity();
    }
  }
}
}  // namespace grid_map


#ifndef EXAMPLES_MISC_HPP
#define EXAMPLES_MISC_HPP

#include <iostream>
#include <type_traits>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <chrono>

#include <Eigen/Core>

inline void print_info() {
	std::cout << "Eigen Version: " << EIGEN_WORLD_VERSION << "." << EIGEN_MAJOR_VERSION << "." << EIGEN_MINOR_VERSION << std::endl;
	std::cout << "Eigen Threads: " << Eigen::nbThreads() << std::endl;
	std::cout << "Eigen SIMD Instruction Sets In Use: " << Eigen::SimdInstructionSetsInUse() << std::endl;

#ifdef EIGEN_USE_MKL
	std::cout << "MKL Version: " << __INTEL_MKL__ << "." << __INTEL_MKL_MINOR__ << "." << __INTEL_MKL_UPDATE__ << std::endl;
	std::cout << "MKL Threads: " << mkl_get_max_threads();
	if (MKL_Get_Dynamic())
		std::cout << " (may use less for large problems)";
	std::cout << std::endl;
#endif
}

void pca_examples();

void lr_examples();

void autoencoder_examples();

template <typename T>
typename std::enable_if<std::is_floating_point<T>::value, std::vector<std::vector<T> > >::type parse_csv(std::string file, char separator = ',') {
    std::vector<std::vector<T>> result;
    std::ifstream fin(file.c_str());

    int row_length = -1;

    for (std::string line; getline(fin, line);) {
        auto comment = line.find_first_of('#');
        if (comment != line.npos) {
            line = line.substr(0, comment);
        }

        std::vector<T> row;
        if (row_length > 0) {
            row.reserve(row_length);
        }

        std::istringstream line_stream(line);
        std::string cell;

        while (getline(line_stream, cell, separator)) {
            if (cell.find('.') == cell.npos){
                row.push_back(atoi(cell.c_str()));
            } else {
                row.push_back(atof(cell.c_str()));
            }
        }

        if (row.size() > 0) {
            result.push_back(row);
            row_length = row.size();
        }
    }

    return result;
}

template <typename T>
typename std::enable_if<std::is_integral<T>::value, std::vector<std::vector<T> > >::type parse_csv(std::string file, char separator = ',') {
    std::vector<std::vector<T>> result;
    std::ifstream fin(file.c_str());

    int row_length = -1;

    for (std::string line; getline(fin, line);) {
        auto comment = line.find_first_of('#');
        if (comment != line.npos) {
            line = line.substr(0, comment);
        }

        std::vector<T> row;
        if (row_length > 0) {
            row.reserve(row_length);
        }

        std::istringstream line_stream(line);
        std::string cell;

        while (getline(line_stream, cell, separator)) {
            row.push_back(atoi(cell.c_str()));
        }

        if (row.size() > 0) {
            result.push_back(row);
            row_length = row.size();
        }
    }

    return result;
}

template <class T = std::chrono::milliseconds, typename F>
T benchmark_single(const F& f) {
    auto t1 = std::chrono::steady_clock::now();

    f();

    auto t2 = std::chrono::steady_clock::now();

    return std::chrono::duration_cast<T>(t2 - t1);
}

template <class Model>
void benchmark(Model& model, Eigen::MatrixXd dataset, Eigen::MatrixXd target, int iters) {
	double min = std::numeric_limits<double>::max(), max = 0, total = 0;

	for (auto it = 0; it < iters; it++) {
		double elapsed = benchmark_single<std::chrono::milliseconds>([&] { model.fit(dataset, target); }).count();
		min = std::min(min, elapsed);
		max = std::max(max, elapsed);
		total += elapsed;
	}

	std::cout << "Benchmark for " << typeid(Model).name();
	std::cout << " with input ([" << dataset.rows() << "*" << dataset.cols() << "], [" << target.rows() << "*" << target.cols() << "])";
	std::cout << " for " << iters << " iterations" << std::endl;
	std::cout << "Min: " << min << "ms" << std::endl;
	std::cout << "Max: " << max << "ms" << std::endl;
	std::cout << "Avg: " << total / iters << "ms" << std::endl;
	std::cout << "Tot: " << total << "ms" << std::endl;
}

template <class Model>
void benchmark(Model& model, Eigen::MatrixXd dataset, int iters) {
	std::cout << "Benchmark for " << typeid(Model).name();
	std::cout << " with input ([" << dataset.rows() << "*" << dataset.cols() << "])";
	std::cout << " for " << iters << " iterations" << std::endl;

	double min = std::numeric_limits<double>::max(), max = 0, total = 0;

	for (auto it = 0; it < iters; it++) {
		double elapsed = benchmark_single<std::chrono::milliseconds>([&] { model.fit(dataset); }).count();
		min = std::min(min, elapsed);
		max = std::max(max, elapsed);
		total += elapsed;
	}

	std::cout << "Min: " << min << "ms" << std::endl;
	std::cout << "Max: " << max << "ms" << std::endl;
	std::cout << "Avg: " << total / iters << "ms" << std::endl;
	std::cout << "Tot: " << total << "ms" << std::endl;
}

template <class Model>
void eval_numerical_gradient(const Model& model, const Eigen::MatrixXd& params, const Eigen::MatrixXd& dataset, const Eigen::MatrixXd& target) {
	std::cout << "Numerical Gradient for " << typeid(Model).name();
	std::cout << " with input ([" << dataset.rows() << "*" << dataset.cols() << "], [" << target.rows() << "*" << target.cols() << "])" << std::endl;

	double fx = model.loss(params, dataset, target);
	Eigen::MatrixXd adfx = model.gradient(params, dataset, target);
	double fx2;
	Eigen::MatrixXd adfx2;
	std::tie(fx2, adfx2) = model.loss_and_gradient(params, dataset, target);

	std::cout << "Diffs: " << std::abs(fx - fx2) << " " << (adfx - adfx2).squaredNorm() << std::endl;

	Eigen::MatrixXd ndfx = Eigen::MatrixXd::Zero(adfx.rows(), adfx.cols());
	auto h = 0.000001;

	Eigen::MatrixXd params_copy = params;

	for (auto i = 0; i < params.rows(); i++) {
		for (auto j = 0; j < params.cols(); j++) {
			params_copy(i, j) += h;
			double fxh = model.loss(params_copy, dataset, target);
			params_copy(i, j) -= h;
			ndfx(i, j) = (fxh - fx) / h;
		}
	}

	Eigen::MatrixXd side(params.size(), 3);
	side.col(0) = mlt::utils::eigen::ravel(adfx);
	side.col(1) = mlt::utils::eigen::ravel(ndfx);
	side.col(2) = mlt::utils::eigen::ravel((ndfx - adfx).cwiseAbs().cwiseQuotient(((ndfx.cwiseAbs() + adfx.cwiseAbs()).array() + 1e-5).matrix()));
	std::cout << side << std::endl;
}

#endif