#include <functional>
#include <iomanip>
#include <iostream>
#include <string>
#include <tuple>
#include <utility/include_all.h>
#include <utility/iterator.h>
#include <utility/math.h>
#include <utility/unit_math.h>
#include <vector>

auto generateHexGrid(float h, float r, bool center = true, float scale = 1.0f) {
	float H = h * kernelSize();
	auto gen_position = [&](auto r, int32_t i, int32_t j, int32_t k) {
		float4 initial{ 2.0f * i + ((j + k) % 2), sqrt(3.f) * (j + 1.0f / 3.0f * (k % 2)), 2.0f * sqrt(6.0f) / 3.0f * k, h / r };
		return initial * r;
	};
	int32_t requiredSlices_x = (int32_t)math::ceilf(scale * H / r);
	int32_t requiredSlices_y = (int32_t)math::ceilf(scale * H / (sqrt(3.0f) * r));
	int32_t requiredSlices_z = (int32_t)math::ceilf(scale * H / r * 3.0f / (sqrt(6.0f) * 2.0f));

	std::vector<float4> positions;
	for (int32_t x_it = -requiredSlices_x; x_it <= requiredSlices_x; x_it++)
		for (int32_t y_it = -requiredSlices_y; y_it <= requiredSlices_y; y_it++)
			for (int32_t z_it = -requiredSlices_z; z_it <= requiredSlices_z; z_it++)
				if (center || (!center && (x_it != 0 || y_it != 0 || z_it != 0)))
					positions.push_back(gen_position(r, x_it, y_it, z_it));
	return positions;
}
#define CALC_CONSTANTS
#ifdef CALC_CONSTANTS
constexpr auto volume = 1.f;
auto radius = powf(volume, 1.f / 3.f) * PI4O3_1;
auto h = support_from_volume(volume);
auto H = h * kernelSize();
auto getPacking() {
	int32_t it = 0;
	auto spacing = math::brentsMethod(
		[&](auto r) {
		auto positions = generateHexGrid(h, r, true, 1.0f);
		auto positionsL = generateHexGrid(h, r, true, 2.0f);
		float error = 0.0f;
		for (const auto& pos : positions) {
			float density = -1.0f;
			for (const auto& posL : positionsL)
				density += volume * spline4_kernel(posL, pos);
			error += density;
		}
		std::cout << r << "[" << it++ << "] -> " << error << std::endl;
		return error;
	},
		radius * 0.75f, radius * 8.0f, 1e-5f, 100);
	return spacing;
}
auto spacing = getPacking();
#else
constexpr auto H = 0x1.2487b0p+1f;
constexpr auto h = 0x1.407358p+0f;
constexpr auto r = 0x1.e8ec8ap-3f;
constexpr auto V = 0x1.000000p+0f;
constexpr auto s = 0x1.1ece3cp-1f;
#endif

constexpr auto lutSize = 1024;
constexpr auto integralSize = 16*1024;

template<typename C>
auto generateLUT(C&& func) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;// decltype(func(c, std::declval<float4>()));
	std::array<res_t, lutSize> LUT;
	float dd = 2.f * H / ((float)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		auto n = integralSize;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);

		res_t integral = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integral)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float) xl + 0.5f * (float)dh, 0.f, 0.f, h };

			double hl = math::clamp(xl - d, 0.f, 2.f * xl);
			double hh = math::clamp(xh - d, 0.f, 2.f * xh);

			double Vl = CUDART_PI_F * hl * hl / 3.f * (3.f * xl - hl);
			double Vh = CUDART_PI_F * hh * hh / 3.f * (3.f * xh - hh);

			double dV = Vh - Vl;

			integral += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d) / H));
		}
		LUT[di] = integral;
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

constexpr float t = 0.0001f;
template<typename C>
auto gradientLUT(C&& func) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;// decltype(func(c, std::declval<float4>()));
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		constexpr auto n = integralSize;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integralp)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float) xl + 0.5f * (float) dh, 0.f, 0.f, h };

			double hl = math::clamp(xl - d + t, 0., 2. * xl);
			double hh = math::clamp(xh - d + t, 0., 2. * xh);

			double Vl = CUDART_PI * hl * hl / 3. * (3. * xl - hl);
			double Vh = CUDART_PI * hh * hh / 3. * (3. * xh - hh);

			double dV = Vh - Vl;

			integralp += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d - t) / H));
		}
		res_t integraln = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for reduction(+ : integraln)
		for (auto ni = 0; ni < n; ++ni) {
			double xl = dh * (double)ni;
			double xh = dh * (double)ni + dh;

			float4 p{ (float)xl + 0.5f * (float)dh , 0.f, 0.f, h };

			double hl = math::clamp(xl - d - t, 0., 2. * xl);
			double hh = math::clamp(xh - d - t, 0., 2. * xh);

			double Vl = CUDART_PI_F * hl * hl / 3. * (3. * xl - hl);
			double Vh = CUDART_PI_F * hh * hh / 3. * (3. * xh - hh);

			double dV = Vh - Vl;

			integraln += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (p.x - d + t) / H));
		}
		LUT[di] = (integralp - integraln) / (2.0 * t);
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}
template<typename T>
auto lookup (const std::array<T, lutSize> LUT, float x) {
	auto xRel = ((x + H) / (2.f * H)) * ((float)lutSize - 1.f);
	auto xL = math::floorf(xRel);
	auto xH = math::ceilf(xRel);
	auto xD = xRel - xL;
	int32_t xLi = math::clamp(static_cast<int32_t>(xL), 0, lutSize - 1);
	int32_t xHi = math::clamp(static_cast<int32_t>(xH), 0, lutSize - 1);
	auto lL = LUT[xLi];
	auto lH = LUT[xHi];
	return lL * xD + (1.f - xD) * lH;
};

#include <config/config.h>
#include <fstream>
#ifdef _WIN32
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
#endif

auto writeLUT(const std::string& name, const std::string& type, const std::array<double, lutSize>& LUT) {
	fs::path bin_dir(sourceDirectory);
	auto file = bin_dir / "cfg" / name;
	file.replace_extension("lut");

	if (fs::exists(file)) {
		if (fs::exists(__FILE__)) {
			auto input_ts = fs::last_write_time(__FILE__);
			auto output_ts = fs::last_write_time(file);
			if (input_ts <= output_ts) {
				return;
			}
		}
	}
	std::cout << "Writing " << file.string() << std::endl;

	std::ofstream output(file.string());
	//output << "std::vector<" << type << "> " << name << "{ ";
	int32_t ctr = 0;
	for (auto v : LUT) {
		output << std::scientific << std::setprecision(std::numeric_limits<float>::digits10 + 1)
			<< static_cast<float>(v) << " ";
	}
	//output << "};" << std::endl;
	output.close();
}

auto sphericalGradientIntegral(const std::function<double(float4, float4, float)>& func, int32_t phiSlices, int32_t thetaSlices, int32_t radiusSteps) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	for (auto di = 0; di < lutSize; ++di) {
		auto n = radiusSteps;
		double dh = H / ((double)n);
		double d = H - dd * (double)(di);
		double dTheta = (2.0 * CUDART_PI) / (double)thetaSlices;
		double dPhi = (CUDART_PI) / (double)phiSlices;

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float) xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0.0; iTheta < 2.0 * CUDART_PI; iTheta += dTheta) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0.0; iPhi < CUDART_PI; iPhi += dPhi) {
					double theta = iTheta + dTheta * 0.5;
					double phi = iPhi + dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d + t)
						continue;
					float4 p{ (float)x, (float)y, (float)z, h };
					phiSum += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, d + t));
				}
				thetaSum += phiSum;
			}
			integralp += thetaSum;
		}
		res_t integraln = vector_t<double, math::dimension_v<res_t>>::zero();
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float)xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0.0; iTheta < 2.0 * CUDART_PI; iTheta += dTheta) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0.0; iPhi < CUDART_PI; iPhi += dPhi) {
					double theta = iTheta + dTheta * 0.5;
					double phi = iPhi + dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d - t)
						continue;
					float4 p{ (float)x, (float)y, (float)z, h };
					phiSum += dV * math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, d - t));
				}
				thetaSum += phiSum;
			}
			integraln += thetaSum;
		}
		LUT[di] = (integralp - integraln) / (2.0 * t);
	}
	std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

void progressBar(int32_t frame, int32_t frameTarget, float progress) {
	std::ios cout_state(nullptr);
	cout_state.copyfmt(std::cout);
	static auto startOverall = std::chrono::high_resolution_clock::now();
	static auto startFrame = startOverall;
	static auto lastTime = startOverall;
	static int32_t lastFrame = frame;
	//if (frame != lastFrame) {
	//	lastFrame = frame;
	if(frame == 0)
		startFrame = std::chrono::high_resolution_clock::now();
	//}
	auto now = std::chrono::high_resolution_clock::now();
	lastTime = now;
	int barWidth = 128;
	std::cout << "Generating " << std::setw(4) << frame;
	if (frameTarget != -1)
		std::cout << "/" << std::setw(4) << frameTarget;
	std::cout << " [";
	int pos = barWidth * progress;
	for (int i = 0; i < barWidth; ++i) {
		if (i < pos) std::cout << "=";
		else if (i == pos) std::cout << ">";
		else std::cout << " ";
	}
	std::cout << "] " << std::setw(3) << int(progress * 100.0) << " ";
	auto dur = std::chrono::duration_cast<std::chrono::milliseconds>(now - startFrame);
	if (dur.count() < 100 || progress < 1e-3f) {
		std::cout << " ---/---s  ";
	}
	else {
		auto totalTime = ((float)std::chrono::duration_cast<std::chrono::microseconds>(now - startFrame).count()) / 1000.f / 1000.f;
		std::cout << std::fixed << std::setprecision(0) << " " << std::setw(3) << totalTime << "/" << std::setw(3) << (totalTime / progress) << "s  ";
	}
	std::cout << "\r";
	std::cout.flush();
	std::cout.copyfmt(cout_state);
}

auto sphericalIntegral(const std::function<double(float4, float4, float)>& func, int32_t phiSlices, int32_t thetaSlices, int32_t radiusSteps) {
	float4 c{ 0.f,0.f,0.f,h };
	using res_t = double;
	std::array<res_t, lutSize> LUT;
	double dd = 2.f * H / ((double)lutSize - 1);
	//std::vector<double> thetaSum(thetaSlices);
	//std::vector<double> phiSum(phiSlices);
	for (auto di = 0; di < lutSize; ++di) {
		progressBar(di, lutSize, (double)di / (double)lutSize);
		auto n = radiusSteps;
		double dh = H / ((double)n);
		double d = -H + dd * (double)(di);
		double dTheta = (2.0 * CUDART_PI) / (double)thetaSlices;
		double dPhi = (CUDART_PI) / (double)phiSlices;

		res_t integralp = vector_t<double, math::dimension_v<res_t>>::zero();
		double dVSum = 0.0;
#pragma omp parallel for
		for (auto iR = 0; iR < radiusSteps; ++iR) {
			double xl = dh * (double)iR;
			double xh = dh * (double)iR + dh;
			float r = (float)xl + 0.5f * (float)dh;
			double Vl = 4.0 / 3.0 * CUDART_PI * xl * xl * xl;
			double Vh = 4.0 / 3.0 * CUDART_PI * xh * xh * xh;

			double dV = (Vh - Vl) / (double)thetaSlices / (double)phiSlices;
			res_t thetaSum = vector_t<double, math::dimension_v<res_t>>::zero();
			for (auto iTheta = 0; iTheta < thetaSlices; iTheta++) {
				res_t phiSum = vector_t<double, math::dimension_v<res_t>>::zero();
				for (auto iPhi = 0; iPhi < phiSlices; iPhi++) {
					double theta = ((double)iTheta) * dTheta;// +dTheta * 0.5;
					double phi = ((double)iPhi) * dPhi;// +dPhi * 0.5;
					double x = r * cos(theta) * sin(phi);
					double y = r * sin(theta) * sin(phi);
					double z = r * cos(phi);
					if (x < d)
						phiSum += 0.0;
					else {
						float4 p{ (float)x, (float)y, (float)z, h };
						phiSum += math::castTo<typename vector_t<double, math::dimension_v<res_t>>::type>(func(c, p, (x - d) / (1.0 * H)));
					}
				}
				//std::sort(phiSum.begin(), phiSum.end());
				thetaSum += phiSum;
			}
			//std::sort(thetaSum.begin(), thetaSum.end());
			//integralp += dV * std::reduce(thetaSum.begin(), thetaSum.end());
			integralp += dV * thetaSum;
		}
		LUT[di] = integralp;
		//break;
	}
	std::cout << std::endl;
	//std::reverse(LUT.begin(), LUT.end());
	return LUT;
}

auto approximateGradient(const std::array<double, lutSize>& LUT, const std::array<double, lutSize>& vLUT) {
	std::array<double, lutSize> gLUT;
	for (auto& e : gLUT) e = 0.0;
	for (int32_t i = 0; i < lutSize - 1; ++i) {
		auto vi1 = 1.f; //vLUT[i + 1];
		auto vi = 1.f; //vLUT[i];
		vi1 = vi1 < 1e-5f ? vi : vi1;

		gLUT[i] = (LUT[i + 1] / vi1 - LUT[i] / vi) / (2.0 * H / (double)lutSize) * vi;
	}
	gLUT[lutSize - 1] = gLUT[lutSize - 2];
	return gLUT;
}

auto smoothLUT(const std::array<double, lutSize>& LUT) {
	std::array<double, lutSize> gLUT;
	gLUT = LUT;
	for (int32_t i = 1; i < lutSize - 1; ++i) {
		if(LUT[i-1] < LUT[i])
			gLUT[i] = (LUT[i - 1] + LUT[i + 1])*0.5;
	}
	return gLUT;
}

#include <omp.h>
int main(int32_t argc, char** argv) {
	omp_set_num_threads(12);
#ifdef CALC_CONSTANTS
	std::ios cout_state(nullptr);
	cout_state.copyfmt(std::cout);
	std::cout << std::hexfloat << "H = " << H << std::endl;
	std::cout << std::hexfloat << "h = " << h << std::endl;
	std::cout << std::hexfloat << "radius = " << radius << std::endl;
	std::cout << std::hexfloat << "volume = " << volume << std::endl;
	std::cout << std::hexfloat << "spacing = " << spacing << std::endl;
	std::cout.copyfmt(cout_state);
	std::cout << "H = " << H << std::endl;
	std::cout << "h = " << h << std::endl;
	std::cout << "radius = " << radius << std::endl;
	std::cout << "volume = " << volume << std::endl;
	std::cout << "spacing = " << spacing << std::endl;
	std::cout.copyfmt(cout_state);
#endif
	std::cout << "Running LUT generation code." << std::endl;
//#ifdef WIN32
//	HWND console = GetConsoleWindow();
//	RECT _r;
//	GetWindowRect(console, &_r);
//	MoveWindow(console, 0, 0, 1920, 1200, TRUE);
//#endif
	//std::cout << "H = " << H << std::endl;
	//std::cout << "h = " << h << std::endl;
	//std::cout << "radius = " << r << std::endl;
	//std::cout << "volume = " << V << std::endl;
	//std::cout << "spacing = " << s << std::endl;

	//std::cout << "Spherical Integral: " <<  << std::endl;
	//std::cout << "Analytical: " << CUDART_PI * 4.0 / 3.0 * H * H * H << std::endl;

	//std::array densityLUT = sphericalIntegral([](float4 c, float4 p, float d) {return kernel(c, p); }, 128, 128, 128);
	//std::cout << "Generated density LUT" << std::endl;
	float dFactor = 0.f;// 0.f;// 1.5f;
	std::array densityLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * kernel(c, p); });
	writeLUT("density", "float", densityLUT);

	auto lookup = [&](auto x) {
		float xRel = ((x + 1.f) / 2.f)* ((float)lutSize - 1.f);
		auto xL = math::floorf(xRel);
		auto xH = math::ceilf(xRel);
		auto xD = xRel - xL;
		int32_t xLi = math::clamp(static_cast<int32_t>(xL), 0, lutSize - 1);
		int32_t xHi = math::clamp(static_cast<int32_t>(xH), 0, lutSize - 1);
		auto lL = densityLUT[xLi];
		auto lH = densityLUT[xHi];
		auto val = lL * xD + (1.f - xD) * lH;
		return val;
	};

	auto findX = [&](auto x) {
		float f = -1.f;
		int32_t n = 8;
		for (int32_t n = 1; n < 11; ++n) {
			auto fx = lookup(f);
			//std::cout << "Starting at " << f << " : " << fx << " with dx = " << powf(0.5f, (float)n) << std::endl;
			while (n % 2 == 1 ? fx > x + 0.001f : fx < x - 0.001f){
				f += (n % 2 == 1 ? 1.f : -1.f) * powf(0.5f, (float)n);
				fx = lookup(f);
				//std::cout << f << " -> " << fx << std::endl;
			}
		}
		//std::cout << f << " - " << lookup(f) << " <-> " << x << std::endl;
		return f;
	};
	std::array<double, lutSize> offsetLUT;
	for (int32_t i = 0; i < lutSize; ++i) {
		float f = (float) i / (float)lutSize;
		offsetLUT[i] = /*0.24509788f*/ -0.f* findX(f);
	}
	//std::reverse(offsetLUT.begin(), offsetLUT.end());
	writeLUT("offsetLUT", "float", offsetLUT);
	//findX(0.5f);

	std::array spline2LUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * (1.f + dFactor * d) *  math::dot3(gradient(c, p), gradient(c, p)); });
	writeLUT("spline2", "float", spline2LUT);
	std::array spikyLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) * PressureKernel<kernel_kind::spline4>::value(c,p); });
	writeLUT("spiky", "float", spikyLUT);
	std::array splineLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  kernel(c, p); });
	writeLUT("spline", "float", splineLUT);
	std::array cohesionLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  Kernel<kernel_kind::cohesion>::value(c, p).x; });
	for (int32_t i = 1; i < cohesionLUT.size(); ++i) {
		cohesionLUT[i] = abs(cohesionLUT[i]) < 1e-12 ? cohesionLUT[i - 1] : cohesionLUT[i];
	}
	writeLUT("cohesion", "float", cohesionLUT);
	std::array adhesionLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d) *  Kernel<kernel_kind::adhesion>::value(c, p).x; });
	for (int32_t i = 1; i < adhesionLUT.size(); ++i) {
		adhesionLUT[i] = abs(adhesionLUT[i]) < 1e-12 ? adhesionLUT[i - 1] : adhesionLUT[i];
	}
	writeLUT("adhesion", "float", adhesionLUT);

	std::array volumeLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f + dFactor * d); });
	//for (auto& v : volumeLUT)
	//	v = 1.f;
	writeLUT("volume", "float", volumeLUT);

	////std::cout << adhesion0  << std::endl;
	//std::array spikyGradientLUT = generateLUT([&](float4 c, float4 p, float d) {
	//	return -((1.f + dFactor * d) *  SpikyKernel<kernel_kind::spline4>::gradient(c, p).x); });

	std::array spikyGradientLUT = approximateGradient(spikyLUT, volumeLUT);
	for (int32_t i = 1; i < spikyGradientLUT.size(); ++i) {
		//spikyGradientLUT[i] = fabsf(spikyGradientLUT[i]) < 1e-12f ? spikyGradientLUT[i - 1] : spikyGradientLUT[i];
	}
	//std::array spikyGradientLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f) * SpikyKernel<kernel_kind::spline4>::gradient(c, p).x ; });
	//for (auto& v : spikyGradientLUT)
		//v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	writeLUT("spikyGradient", "float", spikyGradientLUT);
	std::array splineGradientLUT = approximateGradient(splineLUT, volumeLUT);
	for (int32_t i = 1; i < splineGradientLUT.size(); ++i) {
		//splineGradientLUT[i] = fabsf(splineGradientLUT[i]) < 1e-12f ? splineGradientLUT[i - 1] : splineGradientLUT[i];
	}
	//std::array splineGradientLUT = generateLUT([&](float4 c, float4 p, float d) { return (1.f ) * gradient(c, p).x;  });
	//for (auto& v : splineGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	writeLUT("splineGradient", "float", splineGradientLUT);

	//std::array densityLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return kernel(c, p); }, 127, 127, 127);
	auto chi = [&](auto d) {
		return 1.f + dFactor * d;
		//return math::clamp(1.0 / lookup(densityLUT, -d * HforV1) + d,1.0,4.0);
	};
	//densityLUT = smoothLUT(densityLUT);
	//std::cout << "Generated density LUT" << std::endl;
	//writeLUT("density", "float", densityLUT);

	//std::array splineLUTN = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * kernel(c, p); }, 127, 127, 127);
	//splineLUTN = smoothLUT(splineLUTN);
	//std::cout << "Generated spline LUT" << std::endl;
	//writeLUT("splinePolar", "float", splineLUTN);
	//std::array splineGradientLUTN = approximateGradient(splineLUTN);
	//std::cout << "Generated spline Gradient LUT" << std::endl;
	//writeLUT("splineGradientPolar", "float", splineGradientLUTN);

	//std::array spikyLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * SpikyKernel<kernel_kind::spline4>::value(c, p); }, 128, 128, 128);
	//spikyLUT = smoothLUT(spikyLUT);
	//std::cout << "Generated spiky LUT" << std::endl;
	//writeLUT("spiky", "float", spikyLUT);
	//std::array spikyGradientLUT = approximateGradient(spikyLUT);
	//std::cout << "Generated spiky Gradient LUT" << std::endl;
	//writeLUT("spikyGradient", "float", spikyGradientLUT);
	//std::array cohesionLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * Kernel<kernel_kind::cohesion>::value(c, p).x; }, 128, 128, 128);
	//std::cout << "Generated cohesion LUT" << std::endl;
	//writeLUT("cohesion", "float", cohesionLUT);
	//std::array adhesionLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return chi(d) * Kernel<kernel_kind::adhesion>::value(c, p).x; }, 128, 128, 128);
	//auto adhesion0 = lookup(adhesionLUT, -HforV1);
	//for (auto& v : adhesionLUT)
	//	v /= adhesion0;
	//std::cout << "Generated adhesion LUT" << std::endl;
	//writeLUT("adhesion", "float", adhesionLUT);
	//std::array volumeLUT = sphericalIntegral([&](float4 c, float4 p, float d) {return 1.0; }, 128, 128, 128);
	//std::cout << "Generated volume LUT" << std::endl;
	//writeLUT("volume", "float", volumeLUT);

	//float dh = 0.001f;
	//float4 dx{ dh,0.f,0.f,0.f };
	//float4 dy{ 0.f,dh,0.f,0.f };
	//float4 dz{ 0.f,0.f,dh,0.f };

	//std::array spikyLUT = generateLUT([&](float4 c, float4 p) { return SpikyKernel<kernel_kind::spline4>::value(c, p); });
	//std::array splineLUT = generateLUT([&](float4 c, float4 p) { return kernel(c, p); });
	//std::array cohesionLUT = generateLUT([&](float4 c, float4 p) { return Kernel<kernel_kind::cohesion>::value(c, p).x; });
	//std::array adhesionLUT = generateLUT([&](float4 c, float4 p) { return Kernel<kernel_kind::adhesion>::value(c, p).x; });
	//std::array spikyGradientLUT = gradientLUT([&](float4 c, float4 p) { return SpikyKernel<kernel_kind::spline4>::value(c, p); });
	//std::array splineGradientLUT = gradientLUT([&](float4 c, float4 p) { return kernel(c, p); });
	//std::array volumeLUT = gradientLUT([&](float4 c, float4 p) { return 1.f; });

	//for (int32_t i = 0; i < lutSize; ++i) {
	//	std::cout << i << "\t" << splineLUT[i] << " - " << splineLUT2[i] << " -> " << splineLUT[i] / splineLUT2[i] << std::endl;
	//}

	//for (float x = -H; x <= H; x += H / 16.f) {
	//	std::cout << x << "\t" << lookup(splineLUT, x) << " @ " << lookup(splineLUT2, x) << std::endl;
	//}

	//for (float x = -H; x <= H; x += H / 16.f) {
	//	std::cout << x << "\t" << lookup(splineLUT, x) << " @ " << lookup(splineGradientLUT, x) << " -> " << lookup(splineLUT, x + dh) << " : " << lookup(splineLUT, x) + dh * lookup(splineGradientLUT, x) << std::endl;
	//}

	//for (auto& v : splineGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	//for (auto& v : spikyGradientLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	//for (auto& v : cohesionLUT)
	//	v = -v;// math::max(std::decay_t<decltype(v)>{0.f}, v);
	////std::cout << adhesion0  << std::endl;

	//auto x0 = lookup(splineLUT, 0.0f);
	//auto xp = lookup(splineLUT, 0.0f + t);
	//auto xn = lookup(splineLUT, 0.0f - t);
	//auto xnum = (xp - xn) / (2.f * t);
	//auto xint = lookup(splineGradientLUT, 0.f);
	//std::cout << x0 << " -> " << xp << std::endl;
	//std::cout << xnum << " <-> " << xint << std::endl;

	//float evalP = 0.f * H;
	//std::cout << "Spherical integral" << std::endl;
	//std::cout << "Kernel:   " << lookup(splineLUT, evalP) << std::endl;
	//std::cout << "Gradient: " << lookup(splineGradientLUT, evalP) << std::endl;

	//float4 c{ 0.f,0.f,0.f,h };
	//double sumd = 0.0;
	//double4 gradSumd{ 0.0,0.0,0.0,0.0 };
	//double volume = 0.f;
	//constexpr auto trapz = 512;
	//constexpr auto dt = 2.0 * (double) H / ((double)trapz);
	//constexpr auto dV = dt * dt * dt;
	//auto trapH = support_from_volume(dV);
	//for (int32_t xi = -trapz / 2; xi <= trapz / 2; ++xi) {
	//	for (int32_t yi = -trapz / 2; yi <= trapz / 2; ++yi) {
	//		for (int32_t zi = -trapz / 2; zi <= trapz / 2; ++zi) {
	//			float4 p{ (float)dt * (float)xi, (float)dt * (float)yi, (float)dt * (float)zi, h };
	//			if (p.x > evalP)
	//				continue;
	//			sumd += (double) kernel(c, p) * dV;
	//			gradSumd += math::castTo<double4>(gradient(c, p)) * dV;
	//			volume += kernel(c, p) > 0.f ? dV : 0.f;
	//		}
	//	}
	//}
	//std::cout << "Trapz" << std::endl;
	//std::cout << "Kernel:   " << sumd << std::endl;
	//std::cout << "Gradient: " << gradSumd << std::endl;

	//getchar();
}


/*
	Copyright (C) 2003-2014 by David White <davewx7@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#pragma once

#include <boost/math/special_functions/round.hpp>

#include "geometry.hpp"

template<typename T>
geometry::Point<T> rotate_point_around_origin(T x1, T y1, float alpha, bool round)
{
	geometry::Point<T> beta;

	/*   //we actually don't need the initial theta and radius.  This is why:
	x2 = R * (cos(theta) * cos(alpha) + sin(theta) * sin(alpha))
	y2 = R * (sin(theta) * cos(alpha) + cos(theta) * sin(alpha));
	but
	R * (cos(theta)) = x1
	R * (sin(theta)) = x2
	this collapses the above to:  */

	float c1 = x1 * cos(alpha) - y1 * sin(alpha);
	float c2 = y1 * cos(alpha) + x1 * sin(alpha);

	beta.x = static_cast<T>(round ? boost::math::round(c1) : c1);
	beta.y = static_cast<T>(round ? boost::math::round(c2) : c2);

	return beta;
}

template<typename T>
geometry::Point<T> rotate_point_around_origin_with_offset(T x1, T y1, float alpha, T u1, T v1, bool round=true)
{
	geometry::Point<T> beta = rotate_point_around_origin(x1 - u1, y1 - v1, alpha, round);

	beta.x += u1;
	beta.y += v1;

	return beta;
}

void rotate_rect(short center_x, short center_y, float rotation, short* rect_vertexes);
void rotate_rect(float center_x, float center_y, float rotation, float* rect_vertexes);
void rotate_rect(const rect& r, float angle, short* output);


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3M.cpp
 * @brief   Rotation (internal: 3*3 matrix representation*)
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifndef GTSAM_USE_QUATERNIONS

#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

static const Matrix3 I3 = Matrix3::Identity();

/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}

/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
  rot_.col(0) = col1.vector();
  rot_.col(1) = col2.vector();
  rot_.col(2) = col3.vector();
}

/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13,
    double R21, double R22, double R23,
    double R31, double R32, double R33) {
    rot_ << R11, R12, R13,
        R21, R22, R23,
        R31, R32, R33;
}

/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) {
  rot_ = R;
}

/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) {
  if (R.rows()!=3 || R.cols()!=3)
    throw invalid_argument("Rot3 constructor expects 3*3 matrix");
  rot_ = R;
}

///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}

/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}

/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      1,  0,  0,
      0, ct,-st,
      0, st, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Ry(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct, 0, st,
      0, 1,  0,
      -st, 0, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Rz(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct,-st, 0,
      st, ct, 0,
      0,  0, 1);
}

/* ************************************************************************* */
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) {
  double cx=cos(x),sx=sin(x);
  double cy=cos(y),sy=sin(y);
  double cz=cos(z),sz=sin(z);
  double ss_ = sx * sy;
  double cs_ = cx * sy;
  double sc_ = sx * cy;
  double cc_ = cx * cy;
  double c_s = cx * sz;
  double s_s = sx * sz;
  double _cs = cy * sz;
  double _cc = cy * cz;
  double s_c = sx * cz;
  double c_c = cx * cz;
  double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
  return Rot3(
      _cc,- c_s + ssc,  s_s + csc,
      _cs,  c_c + sss, -s_c + css,
      -sy,        sc_,        cc_
  );
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
  // get components of axis \omega
  double wx = w(0), wy=w(1), wz=w(2);
  double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG
  double l_n = wwTxx + wwTyy + wwTzz;
  if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif

  double c = cos(theta), s = sin(theta), c_1 = 1 - c;

  double swx = wx * s, swy = wy * s, swz = wz * s;
  double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
  double                  C11 = c_1*wwTyy, C12 = c_1*wy*wz;
  double                                   C22 = c_1*wwTzz;

  return Rot3(
        c + C00, -swz + C01,  swy + C02,
      swz + C01,    c + C11, -swx + C12,
     -swy + C02,  swx + C12,    c + C22);
}

/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  if (H1) *H1 = R2.transpose();
  if (H2) *H2 = I3;
  return *this * R2;
}

/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
  return Rot3(Matrix3(rot_*R2.rot_));
}

/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
  if (H1) *H1 = -rot_;
  return Rot3(Matrix3(rot_.transpose()));
}

/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  if (H1) *H1 = -(R2.transpose()*rot_);
  if (H2) *H2 = I3;
  return Rot3(Matrix3(rot_.transpose()*R2.rot_));
}

/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
  if (H1 || H2) {
      if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
      if (H2) *H2 = rot_;
    }
  return Point3(rot_ * p.vector());
}

/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {

  static const double PI = boost::math::constants::pi<double>();

  const Matrix3& rot = R.rot_;
  // Get trace(R)
  double tr = rot.trace();

  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
  // we do something special
  if (std::abs(tr+1.0) < 1e-10) {
    if(std::abs(rot(2,2)+1.0) > 1e-10)
      return (PI / sqrt(2.0+2.0*rot(2,2) )) *
          Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
    else if(std::abs(rot(1,1)+1.0) > 1e-10)
      return (PI / sqrt(2.0+2.0*rot(1,1))) *
          Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
    else // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
      return (PI / sqrt(2.0+2.0*rot(0,0))) *
          Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
  } else {
    double magnitude;
    double tr_3 = tr-3.0; // always negative
    if (tr_3<-1e-7) {
      double theta = acos((tr-1.0)/2.0);
      magnitude = theta/(2.0*sin(theta));
    } else {
      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
      // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
      magnitude = 0.5 - tr_3*tr_3/12.0;
    }
    return magnitude*Vector3(
        rot(2,1)-rot(1,2),
        rot(0,2)-rot(2,0),
        rot(1,0)-rot(0,1));
  }
}

/* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const {
  const double x = omega(0), y = omega(1), z = omega(2);
  const double x2 = x * x, y2 = y * y, z2 = z * z;
  const double xy = x * y, xz = x * z, yz = y * z;
  const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
  return (*this)
      * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
          (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
          (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
}

/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
  if(mode == Rot3::EXPMAP) {
    return (*this)*Expmap(omega);
  } else if(mode == Rot3::CAYLEY) {
    return retractCayley(omega);
  } else if(mode == Rot3::SLOW_CAYLEY) {
    Matrix Omega = skewSymmetric(omega);
    return (*this)*CayleyFixed<3>(-Omega/2);
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
  if(mode == Rot3::EXPMAP) {
    return Logmap(between(T));
  } else if(mode == Rot3::CAYLEY) {
    // Create a fixed-size matrix
    Eigen::Matrix3d A(between(T).matrix());
    // Mathematica closed form optimization (procrastination?) gone wild:
    const double a=A(0,0),b=A(0,1),c=A(0,2);
    const double d=A(1,0),e=A(1,1),f=A(1,2);
    const double g=A(2,0),h=A(2,1),i=A(2,2);
    const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
    const double M = 1 + e - f*h + i + e*i;
    const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
    const double x = (a * f - cd + f) * K;
    const double y = (b * f - ce - c) * K;
    const double z = (fg - di - d) * K;
    return -2 * Vector3(x, y, z);
  } else if(mode == Rot3::SLOW_CAYLEY) {
    // Create a fixed-size matrix
    Eigen::Matrix3d A(between(T).matrix());
    // using templated version of Cayley
    Eigen::Matrix3d Omega = CayleyFixed<3>(A);
    return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Matrix3 Rot3::matrix() const {
  return rot_;
}

/* ************************************************************************* */
Matrix3 Rot3::transpose() const {
  return rot_.transpose();
}

/* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }

/* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }

/* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }

/* ************************************************************************* */
Quaternion Rot3::toQuaternion() const {
  return Quaternion(rot_);
}

/* ************************************************************************* */

} // namespace gtsam

#endif


//==================================================================================================
/*!
  @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_COTPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COTPI_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the cotangent of input in
    \f$\pi\f$ multiples: \f$\cos(\pi x)/sin(\pi x)\f$.

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

    @par Note

      As most other trigonometric function cotd can be called
      with a second optional parameter  which is a tag on speed
      and accuracy (see @ref cos for further details)

    @see cos, sin, tan, cot, cotpi

    @par Example:

      @snippet cotpi.cpp cotpi

    @par Possible output:

      @snippet cotpi.txt cotpi

  **/
  IEEEValue cotpi(IEEEValue const& x);
} }
#endif

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

#endif


#include "mex.h"

#include <Eigen/Dense>
#include <cmath>
#include <limits>

#define FIXNAN() unaryExpr(std::ptr_fun(fixNaN))

double fixNaN(double x) {

    return std::isnan(x) ? 0 : x;
}

double fixLogInf(double x) {

    return std::isinf(x) ? -500 : x;
}

void printMatrix(char *s, Eigen::MatrixXd M) {

  mexPrintf("Matrix: %s\n", s);

  for (int i = 0; i < M.rows(); i++) {

    for (int j = 0; j < M.cols(); j++) {
      mexPrintf("\t%f", M(i, j));
    }
    mexPrintf("\n");
  }
  mexPrintf("\n\n");
}

Eigen::VectorXd logsumexp(Eigen::MatrixXd x) {

  Eigen::VectorXd y = x.colwise().maxCoeff();

  Eigen::VectorXd s = (x.rowwise() - y.transpose()).array().exp().colwise().sum().log();

  return y + s;
}

double logdet(Eigen::MatrixXd M) {

  Eigen::LLT<Eigen::MatrixXd> llt(M);

  // D = 2 * sum(log(diag(chol(M))));

  Eigen::VectorXd res = ((Eigen::MatrixXd) llt.matrixU()) // Calc chol())
          .diagonal().array().log() // Log of diagonal
          .colwise().sum(); // Calc sum of logs

  return 2 * res[0];
}

static inline double max(double a, double b) {
  return a < b ? b : a;
}

static inline double log_relative_Gauss(double z, double &e, int &exit_flag) {

  double logphi, logPhi;

  if (z < -6) {

    e = 1;
    logPhi = -1.0e12;
    exit_flag = -1;

  } else if (z > 6) {

    e = 0;
    logPhi = 0;
    exit_flag = 1;

  } else {

    logphi = -0.5 * (z * z + log(M_PI * 2)); // Const function call gets optimized away
    logPhi = log(0.5 * erfc(-z * M_SQRT1_2));
    e = exp(logphi - logPhi);
    exit_flag = 0;
  }
  return logPhi;
}

static void lt_factor(int s, int l, Eigen::VectorXd M, Eigen::MatrixXd V, double mp, double p, double gam,
        Eigen::VectorXd &Mnew, Eigen::MatrixXd &Vnew, double &pnew, double &mpnew, double &logS, double &d) {

  // rank 1 projected cavity parameters
  Eigen::VectorXd Vc = (V.col(l) - V.col(s)) * M_SQRT1_2;
  double cVc = (V(l, l) - 2 * V(s, l) + V(s, s)) / 2;
  double cM = (M(l) - M(s)) * M_SQRT1_2;

  double cVnic = max(0, cVc / (1 - p * cVc));

  double cmni = cM + cVnic * (p * cM - mp);

  // rank 1 calculation: step factor
  double z = cmni / sqrt(cVnic);

  double e;
  int exit_flag;
  double lP = log_relative_Gauss(z, e, exit_flag);

  double alpha, beta, r, dp, dmp;

  switch (exit_flag) {

    case 0:

      alpha = e / sqrt(cVnic);
      beta = alpha * (alpha * cVnic + cmni);
      r = beta / (1 - beta);

      // new message
      pnew = r / cVnic;
      mpnew = r * (alpha + cmni / cVnic) + alpha;

      // update terms
      dp = max(-p + DBL_EPSILON, gam * (pnew - p));
      dmp = max(-mp + DBL_EPSILON, gam * (mpnew - mp));
      d = max(dmp, dp); // for convergence measures

      pnew = p + dp;
      mpnew = mp + dmp;

      // project out to marginal
      Vnew = V - dp / (1 + dp * cVc) * (Vc * Vc.transpose());
      Mnew = M + (dmp - cM * dp) / (1 + dp * cVc) * Vc;

      // normalization constant
      //logS  = lP - 0.5 * (log(beta) - log(pnew)) + (alpha * alpha) / (2*beta);

      // there is a problem here, when z is very large
      logS = lP - 0.5 * (log(beta) - log(pnew) - log(cVnic)) + (alpha * alpha) / (2 * beta) * cVnic;

      break;

    case -1: // impossible combination

      d = NAN;

      //Mnew = 0;
      //Vnew = 0;

      pnew = 0;
      mpnew = 0;
      logS = -INFINITY;
      break;

    case 1: // uninformative message

      pnew = 0;
      mpnew = 0;

      // update terms
      dp = -p; // at worst, remove message
      dmp = -mp;
      d = max(dmp, dp); // for convergence measures

      // project out to marginal
      Vnew = V - dp / (1 + dp * cVc) * (Vc * Vc.transpose());
      Mnew = M + (dmp - cM * dp) / (1 + dp * cVc) * Vc;

      logS = 0;
      break;
  }
}

double min_factor(Eigen::VectorXd Mu, Eigen::MatrixXd Sigma, int k, double gam,
        Eigen::VectorXd &dlogZdMu, Eigen::VectorXd &dlogZdSigma, Eigen::MatrixXd &dlogZdMudMu) {

  int D = Mu.size();

  double logZ;

  // messages (in natural parameters)
  Eigen::VectorXd logS = Eigen::VectorXd::Zero(D - 1); // normalization constant (determines zeroth moment)
  Eigen::VectorXd MP = Eigen::VectorXd::Zero(D - 1); // mean times precision (determines first moment)
  Eigen::VectorXd P = Eigen::VectorXd::Zero(D - 1); // precision (determines second moment)

  // TODO: check if copy is really necessary here
  // marginal:
  Eigen::VectorXd M(Mu);
  Eigen::MatrixXd V(Sigma);

  double mpm;
  double s;
  double rSr;
  double dts;

  //Eigen::VectorXd dMdMu;
  //Eigen::VectorXd dMdSigma;
  //Eigen::VectorXd dVdSigma;
  Eigen::MatrixXd _dlogZdSigma;

  Eigen::MatrixXd R;
  Eigen::VectorXd r;

  Eigen::MatrixXd IRSR;
  Eigen::MatrixXd A;
  Eigen::MatrixXd A_;
  Eigen::VectorXd b;
  Eigen::VectorXd Ab;

  Eigen::VectorXd btA;

  Eigen::MatrixXd C;

  double pnew;
  double mpnew;

  double Diff = 0, diff = 0;

  int l, count = 0;

  // mvmin = Eigen::VectorXd(2);

  while (true) {

    count++;

    Diff = 0;

    for (int i = 0; i < D - 1; i++) {

      if (i < k)
        l = i;
      else
        l = i + 1;

      lt_factor(k, l, M, V, MP[i], P[i], gam, // IN
              M, V, pnew, mpnew, logS[i], diff); // OUT

      // Write back vector elements
      P[i] = pnew;
      MP[i] = mpnew;

      if (std::isnan(diff))
        goto done; // found impossible combination

      Diff = Diff + std::abs(diff);
    }

    if (count > 50) {
      mexPrintf("EP iteration ran over iteration limit. Stopped.\n");
      goto done;
    }
    if (Diff < 1.0e-3) {
      goto done;
    }
  }

done:

  if (std::isnan(diff)) {

    logZ = -INFINITY;
    dlogZdMu = Eigen::VectorXd::Zero(D);
    dlogZdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));
    dlogZdMudMu = Eigen::MatrixXd::Zero(D, D);
    //mvmin << Mu(k), Sigma(k, k);
    //dMdMu = Eigen::VectorXd::Zero(D);
    //dMdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));
    //dVdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));

  } else {

    // evaluate log Z:

    // C = eye(D) ./ sqrt(2); C(k,:) = -1/sqrt(2); C(:,k) = [];
    C = Eigen::MatrixXd::Zero(D, D - 1);
    for (int i = 0; i < D - 1; i++) {

      C(i + (i >= k), i) = M_SQRT1_2;
      C(k, i) = -M_SQRT1_2;
    }

    R = C.array().rowwise() * P.transpose().array().sqrt();
    r = (C.array().rowwise() * MP.transpose().array()).rowwise().sum();
    mpm = (MP.array() * MP.array() / P.array()).FIXNAN().sum();
    s = logS.sum();

    IRSR = R.transpose() * Sigma * R;
    IRSR.diagonal().array() += 1; // Add eye()

    rSr = r.dot(Sigma * r);

    A_ = R * IRSR.llt().solve(R.transpose());
    A = 0.5 * (A_.transpose() + A_); // ensure symmetry.

    b = (Mu + Sigma * r);
    Ab = A * b;

    dts = logdet(IRSR);
    logZ = 0.5 * (rSr - b.dot(Ab) - dts) + Mu.dot(r) + s - 0.5 * mpm;

    if (true /*TODO: needs derivative? */) {

      dlogZdSigma = Eigen::VectorXd(0.5 * (D * (D + 1)));

      btA = b.transpose() * A;

      dlogZdMu = r - Ab;
      dlogZdMudMu = -A;

      _dlogZdSigma = -A - 2 * r * Ab.transpose() + r * r.transpose() + btA * Ab.transpose();

      Eigen::MatrixXd diag = _dlogZdSigma.diagonal().asDiagonal();

      _dlogZdSigma = 0.5 * (_dlogZdSigma + _dlogZdSigma.transpose() - diag);

      // dlogZdSigma = dlogZdSigma(logical(triu(ones(D,D))));
      for (int x = 0, i = 0; x < D; x++) {

        for (int y = 0; y <= x; y++) {
          dlogZdSigma[i++] = _dlogZdSigma(y, x);
        }
      }
    }
  }

  return logZ;
}

void joint_min(Eigen::VectorXd Mu, Eigen::MatrixXd Sigma,
        Eigen::VectorXd &logP, Eigen::MatrixXd &dlogPdMu, Eigen::MatrixXd &dlogPdSigma, Eigen::MatrixXd **dlogPdMudMu) {

  Eigen::VectorXd dlPdM;
  Eigen::VectorXd dlPdS;
  Eigen::MatrixXd dlPdMdM;

  double gam = 1;
  int D = Mu.size();

  Eigen::MatrixXd gg = Eigen::MatrixXd(D, D);

  logP = Eigen::VectorXd(D);

  dlogPdMu = Eigen::MatrixXd(D, D);
  dlogPdSigma = Eigen::MatrixXd(D, D * (D + 1) / 2);
  *dlogPdMudMu = new Eigen::MatrixXd[D]; // Create an array of matrizes

  for (int k = 0; k < D; k++) {

#ifdef DEBUG_PRINTF
    if (k % 10 == 0)
      DEBUG_PRINTF('#');
#endif

    logP(k) = min_factor(Mu, Sigma, k, gam, // IN
            dlPdM, dlPdS, dlPdMdM); // OUT

    dlogPdMu.row(k) = dlPdM;
    dlogPdSigma.row(k) = dlPdS;

    (*dlogPdMudMu)[k] = dlPdMdM;
  }

  // Sanity check for INF values
  logP = logP.unaryExpr(std::ptr_fun(fixLogInf));

  // re-normalize at the end, to smooth out numerical imbalances:
  double Z = logP.array().exp().sum();

  Eigen::VectorXd Zm = (dlogPdMu.array().colwise() * logP.array().exp()).colwise().sum() /Z;
  Eigen::VectorXd Zs = (dlogPdSigma.array().colwise() * logP.array().exp()).colwise().sum() /Z;

  Eigen::MatrixXd Zij = Zm * Zm.transpose();

  for (int i = 0; i < D; i++) {

    for (int j = i; j < D; j++) {

      Eigen::MatrixXd Mj = (*dlogPdMudMu)[j];

      for (int k = 0; k < D; k++) {
        gg(i, j) -= (dlogPdMu(k, i) * dlogPdMu(k, j) + Mj(k, i)) * exp(logP(k));
      }
      gg(j, i) = // Hesse Matrix is symmetric
              gg(i, j) = gg(i, j) / Z + Zij(i, j);
    }
  }

  for (int i = 0; i < D; i++) {
    (*dlogPdMudMu)[i].array() *= gg.array();
  }

  dlogPdMu = dlogPdMu.array().rowwise() - Zm.transpose().array();
  dlogPdSigma = dlogPdSigma.array().rowwise() - Zs.transpose().array();

  logP = logP.array() - logsumexp(logP)(0, 0);
}

static void copyMatrix(Eigen::MatrixXd from, mxArray **out) {

  int r = from.rows();
  int c = from.cols();

  *out = mxCreateDoubleMatrix(r, c, mxREAL);
  double *write = mxGetPr(*out);
  for (int i = 0; i < r; i++) {

    for (int j = 0; j < c; j++) {
      write[j * r + i] = from(i, j);
    }
  }
}

static void copyCube(Eigen::MatrixXd *from, int elms, mxArray **out) {

  // They're all of the same dimension
  int r = from->rows();
  int c = from->cols();

  mwSize dims[3];
  dims[0] = r;
  dims[1] = c;
  dims[2] = elms;

  *out = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
  double *write = mxGetPr(*out);
  for (int k = 0; k < elms; k++) {

    for (int i = 0; i < r; i++) {

      for (int j = 0; j < c; j++) {
        write[k * r * c + i * c + j] = from[k](i, j);
      }
    }
  }
}

void mexFunction(int nlhs, mxArray * plhs[],
        int nrhs, const mxArray * prhs[]) {

  if (nrhs != 2)
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumInputs",
          "Two inputs required.");

  if (nlhs != 4)
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumOutputs",
          "Four outputs required.");

  if (mxGetM(prhs[0]) != 1 && mxGetN(prhs[0]) != 1) {
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumOutputs",
            "Vector for param 1 required");
  }

  // Input vars
  Eigen::Map<Eigen::VectorXd> Mu(mxGetPr(prhs[0]), mxGetM(prhs[0]) == 1 ? mxGetN(prhs[0]) : mxGetM(prhs[0]));
  Eigen::Map<Eigen::MatrixXd> Sigma(mxGetPr(prhs[1]), mxGetM(prhs[1]), mxGetN(prhs[1]));

  // Output vars
  Eigen::VectorXd logP;
  Eigen::MatrixXd dlogPdMu;
  Eigen::MatrixXd dlogPdSigma;
  Eigen::MatrixXd *dlogPdMudMu;

  // Do the heavy work
  joint_min(Mu, Sigma,
          logP, dlogPdMu, dlogPdSigma, &dlogPdMudMu);

  // Output results
  copyMatrix(logP, &(plhs[0]));
  copyMatrix(dlogPdMu, &(plhs[1]));
  copyMatrix(dlogPdSigma, &(plhs[2]));
  copyMatrix(dlogPdSigma, &(plhs[3]));
  copyCube(dlogPdMudMu, Mu.size(), &(plhs[3]));

  // Endpoint, delete array
  delete[] dlogPdMudMu;
}


///\file shooting-method.cpp
///\author Ethan Knox
///\date 8/2/2020.

#include <iostream>
#include <fstream>
#include <limits>
#include <iomanip>
#include <functional>

#include <boost/numeric/odeint.hpp>
#include <boost/math/tools/roots.hpp>

#include "potentials.h"
#include "determine_domain_from_potential.h"

using namespace std::placeholders;

const double m = 1.0;
const double k = 1.0;
const double hbar = 1.0;
const double omega = 1.0;

typedef std::vector<double> state_t;
typedef boost::numeric::odeint::runge_kutta_fehlberg78<state_t> stepper_rkf78_t;
typedef boost::numeric::odeint::runge_kutta4<state_t> rk4;

void TISE(state_t &psi, state_t &dpsi_dx, double x, std::function<double(double)> V, double E) {
    dpsi_dx[0] = psi[1];
    dpsi_dx[1] = (2.0 * m / pow(hbar, 2.0)) * (V(x) - E) * psi[0];
}

int eigenE_shooting(state_t& psi, double boundary_condition, std::string Vfunc, double L, double h_0, double eps_rel, double eps_abs, double E)
{
    // Potential
    std::function<double(double)> V;
    std::function<double(double)> dV;
    if (Vfunc == "QHO") {
        V = quantum_harmonic_oscillator;
        dV = d_quantum_harmonic_oscillator;
    }
    else { // Vfunc == "ISW"
        V = infinite_square_well;
        dV = d_infinite_square_well;
    }

    // Domain
    std::pair<double, double> domain = determine_domain_from_potential(Vfunc, L);
    const double xi = domain.first;
    const double xf = domain.second;
    double x = xi;

    // Initial Condition
    state_t psi_copy(psi);
    state_t dpsi_dx(2);
    double h_0 = 1.0e-4;

    std::function<double(double)> defect = [x, xf, h_0, psi_copy, &V](double E) {
        boost::numeric::odeint::integrate_const(rk4(), std::bind(TISE, _1, _2, _3, V, E), psi_copy, x, xf, h_0);
        return psi_copy[0];
    };
    boost::math::tools::eps_tolerance<double> tol;
    std::pair<double, double> r = boost::math::tools::bisect(defect, 0.0, 10.0, tol);
    return r.first + (r.second - r.first) / 2;
}


#ifndef SE3_OPS_HPP
#define SE3_OPS_HPP

#include <tuple>
#include <vector>
#include <Eigen/StdVector>
#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <math.h>
#include <opencv2/core.hpp>
#include <opencv2/hdf.hpp>
#include <opencv2/core/eigen.hpp>

#include "math_utils.hpp"

namespace orcvio
{

template<typename T>
  using vector_eigen = std::vector<T, Eigen::aligned_allocator<T>>;

/**
 * @brief converts vector to skew symmetric matrix in batch
 *
 * @param a: size n x 3, input vector
 *
 * @return : size n x 3 x 3, skew symmetric matrix
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 3, 3> > skew(const vector_eigen<Eigen::Matrix<Scalar, 3, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 3, 3> > S;

    for (const auto& w : a)
    {
        Eigen::Matrix<Scalar, 3, 3> w_x = skewSymmetric(w);
        S.push_back(w_x);
    }

    return S;

}

/**
 * @brief converts 6-vector to 4x4 hat form in se(3) in batch
 *
 * @param x: size n x 6, n se3 elements
 *
 * @return : size n x 4 x 4, n elements of se(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > axangle2twist(const vector_eigen<Eigen::Matrix<Scalar, 6, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > T;

    for (auto v : a)
    {
        Eigen::Matrix<Scalar, 4, 4> v_x = Eigen::Matrix4d::Zero();
        v_x(0, 1) = -v(5);
        v_x(0, 2) = v(4);
        v_x(0, 3) = v(0);
        v_x(1, 0) = v(5);
        v_x(1, 2) = -v(3);
        v_x(1, 3) = v(1);
        v_x(2, 0) = -v(4);
        v_x(2, 1) = v(3);
        v_x(2, 3) = v(2);

        T.push_back(v_x);
    }

    return T;

}

/**
 * @brief converts se3 element to SE3 in batch
 *
 * @param x: size n x 6, n se3 elements
 *
 * @return : size n x 4 x 4, n elements of SE(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > axangle2pose(const vector_eigen<Eigen::Matrix<Scalar, 6, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > T;

    for (auto x : a)
    {

        Sophus::SE3d T_temp = Sophus::SE3d::exp(x);
        T.push_back(T_temp.matrix());

    }

    return T;

}

/**
 * @brief converts axis angle to SO3 in batch
 *
 * @param a = n x 3 = n axis-angle elements
 *
 * @return : R = n x 3 x 3 = n elements of SO(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 3, 3> > axangle2rot(const vector_eigen<Eigen::Matrix<Scalar, 3, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 3, 3> > R;

    for (auto x : a)
    {

        Sophus::SO3d R_temp = Sophus::SO3d::exp(x);
        R.push_back(R_temp.matrix());

    }

    return R;

}

/**
 * @brief performs batch inverse of transform matrix
 *
 * @param T: size n x 4 x 4, n elements of SE(3)
 *
 * @return : size n x 4 x 4, inverse of T
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > inversePose(const vector_eigen<Eigen::Matrix<Scalar, 4, 4> > &T)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > iT;

    for (auto T_temp : T)
    {
        Eigen::Matrix<Scalar, 4, 4> iT_temp = Eigen::Matrix4d::Zero();

        iT_temp(0, 0) = T_temp(0, 0);
        iT_temp(0, 1) = T_temp(1, 0);
        iT_temp(0, 2) = T_temp(2, 0);

        iT_temp(1, 0) = T_temp(0, 1);
        iT_temp(1, 1) = T_temp(1, 1);
        iT_temp(1, 2) = T_temp(2, 1);

        iT_temp(2, 0) = T_temp(0, 2);
        iT_temp(2, 1) = T_temp(1, 2);
        iT_temp(2, 2) = T_temp(2, 2);

        iT_temp.block(0, 3, 3, 1) = -1 * iT_temp.block(0, 0, 3, 3) * T_temp.block(0, 3, 3, 1);

        iT_temp.block(3, 0, 1, 4) = T_temp.block(3, 0, 1, 4);

        iT.push_back(iT_temp);

    }

    return iT;

}

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
 @f}
 *
 * @param ph = 4 = point in homogeneous coordinates
 *
 * @return : odot(ph) = 4 x 6
 */
template<typename Derived>
 Eigen::Matrix<typename Derived::Scalar, 4, 6> odotOperator(const Eigen::MatrixBase<Derived>& x) {
     assert(x.rows() == 4);
     assert(x.cols() == 1);

   Eigen::Matrix<typename Derived::Scalar, 4, 6> temp;
   temp.setZero();

   temp.block(0, 3, 3, 3) = -1 * skewSymmetric(x.template head<3>());

   temp(0, 0) = x(3);
   temp(1, 1) = x(3);
   temp(2, 2) = x(3);
   return temp;
 }

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
  @f}
 *
 * @param ph = n x 4 = points in homogeneous coordinates
 *
 * @return : odot(ph) = n x 4 x 6
 */
 template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 6> > odotOperator(const vector_eigen<Eigen::Matrix<Scalar, 4, 1> > &ph)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 6> > zz;

    for (const auto& x : ph)
    {
      zz.emplace_back(odotOperator(x));
    }
    return zz;
}

/**
  * @brief circle dot operator
  *
  * @param ph = 4 = points in homogeneous coordinates
  *
  * @return : circledCirc(ph) = 6 x 4
  */
template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 6, 4> circledCirc(const Eigen::MatrixBase<Derived>& x) {
    static_assert(Derived::RowsAtCompileTime == 4, "x is not 4D vector");
    static_assert(Derived::ColsAtCompileTime == 1, "x is not a vector");
    Eigen::Matrix<typename Derived::Scalar, 6, 4> temp;
    temp.setZero();

    temp.block(3, 0, 3, 3) = -1 * skewSymmetric(x.template block<3, 1>(0, 0));

    temp.block(0, 3, 3, 1) = x.template block<3, 1>(0, 0);
    return temp;
}

/**
 * @brief circle dot operator
 *
 * @param ph = n x 4 = points in homogeneous coordinates
 *
 * @return : circledCirc(ph) = n x 6 x 4
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 6, 4> > circledCirc(const vector_eigen<Eigen::Matrix<Scalar, 4, 1> > &ph)
{

    vector_eigen<Eigen::Matrix<Scalar, 6, 4>  > zz;

    for (const auto& x : ph)
    {
      zz.push_back(circledCirc(x));
    }

    return zz;

}

/**
 * @brief only keep yaw and zero z
 *
 * @param T_SE3: 4 x 4
 *
 * @return T_SE2: 4 x 4
 */

// template<typename Scalar>
// Eigen::Matrix<Scalar, 4, 4> poseSE32SE2(const Eigen::Matrix<Scalar, 4, 4> &T_SE3)
// {

//     Eigen::Matrix<Scalar, 4, 4> T_SE2 = Eigen::Matrix4d::Identity();

//     // yaw: alpha=arctan(r21/r11)
//     Scalar yaw = M_PI/atan2(T_SE3(1, 0), T_SE3(0, 0));

//     // deal with the case when yaw is nan
//     if (!std::isfinite(yaw))
//       yaw = 0;

//     T_SE2(0, 0) = cos(yaw);
//     T_SE2(0, 1) = -sin(yaw);
//     T_SE2(0, 3) = T_SE3(0, 3);

//     T_SE2(1, 0) = sin(yaw);
//     T_SE2(1, 1) = cos(yaw);
//     T_SE2(1, 3) = T_SE3(1, 3);

//     return T_SE2;

// }

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 2, Eigen::Dynamic>
project_image(const Eigen::MatrixBase<Derived>& uv_hom) {
  auto den = uv_hom.template bottomRows<1>().array().template replicate<2, 1>();
  auto num = uv_hom.template topRows<2>().array();
  return ( num / den ).matrix();
}

template<typename Scalar>
using Matrix23 = Eigen::Matrix<Scalar, 2, 3>;

/**
 * @brief Differentiate \f$ \pi([x, y, z]^\top) = [x/z, y/z] \f$
 *
 * \f{align*}{
 * \frac{\partial \pi([x, y, z]^\top)}{\partial \mathbf{x}}
 * = [1/z, 0,   -x/z^2]
 *   [0,   1/z, -y/z^2]
 * @f}

 * @param x
 * @return Jacobian
 */
template <typename Derived>
Matrix23<typename Derived::Scalar>
  project_image_df(const Eigen::MatrixBase<Derived>& x)
{
    static_assert(Derived::RowsAtCompileTime == 3, " need 3 x 1 vector");
    static_assert(Derived::ColsAtCompileTime == 1, " need 3 x 1 vector");
    typedef typename Derived::Scalar Scalar;
   Scalar z = x(2, 0);
  Scalar zsq = z * z;
  Matrix23<typename Derived::Scalar> df;
  df <<
    1/z,   0, -x(0, 0)/zsq,
    0  , 1/z, -x(1, 0)/zsq;
  return df;
}

/**
 * @brief project_object_points
 *
 * @param P   : Camera projection matrix (3 x 4)
 * @param wTo : Object to world transform (4 x 4)
 * @param points_w : Points (n x 4), note this is points in object frame with homo coord
 * @return Points (n x 2)
 */
template <typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 2>
project_object_points(const Eigen::MatrixBase<D1>& P,
                   const Eigen::MatrixBase<D2>& wTo, const Eigen::MatrixBase<D3>& points_w) {
  auto uv_hom = P * (wTo * points_w.transpose());
  return project_image(uv_hom).transpose();
}

/**
 * @brief Computes the derivative of projection operation wrt object pose
 *
 * \f[
 * \frac{\partial \pi(K ^CT_w(\chi) x_w)}{\partial \xi} = \frac{\partial \pi(K T x_w)}{\partial T} @ \frac{^cT_w(\chi)}{\partial \xi}
 * \f]
 *
 * @param [in] P   : Camera projection matrix (3 x 4)
 * @param [in] wTo : World transform (4 x 4)
 * @param [in] points_o : Points in object frame (n x 4)
 *
 * @return Jacobians (n x 2 x 6)
 */
template<typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6>
  project_object_points_df_object(const Eigen::MatrixBase<D1>& P, const Eigen::MatrixBase<D2>& wTo,
                           const Eigen::MatrixBase<D3>& points_o,
                           const bool use_left_perturbation_flag) {
  auto X_o = points_o.transpose();
  Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6> jacobians(2*points_o.rows(), 6);
  for (int i = 0; i < points_o.rows(); ++i) {

    auto dpibydx = project_image_df(P * wTo * X_o.col(i).template topLeftCorner<4, 1>());

    Eigen::MatrixXd jac;
    if (use_left_perturbation_flag)
    {
      // using left perturbation
      jac = dpibydx * P * odotOperator(wTo * X_o.col(i));
    }
    else
    {
      // using right perturbation
      jac = dpibydx * P * wTo * odotOperator(X_o.col(i));
    }

    assert(jac.rows() == 2 && jac.cols() == 6);
    jacobians.template block<2, 6>(2*i, 0) = jac.template block<2, 6>(0,0);

  }

  return jacobians;
}

/**
 * @brief Computes the derivative of projection operation wrt camera pose
 * @param [in] P   : Camera projection matrix (3 x 4)
 * @param [in] wTo : Object frame to World frame transformation (4 x 4)
 * @param [in] cTw : world frame to camera frame transformation (4 x 4)
 * @param [in] points_o : Points in object frame (n x 4)
 *
 * @return Jacobians ((nk x 2) x 6)
 */
template<typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6>
  project_object_points_df_camera(const Eigen::MatrixBase<D1>& P,
                           const Eigen::MatrixBase<D2>& wTo,
                           const Eigen::MatrixBase<D2>& cTw,
                           const Eigen::MatrixBase<D3>& points_o,
                           const bool use_left_perturbation_flag) {

  auto X_o = points_o.transpose();
  Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6> jacobians(2*points_o.rows(), 6);

  Eigen::MatrixXd ps_puline_s = Eigen::Matrix<double, 3, 4>::Zero();
  ps_puline_s.block<3,3>(0,0) = Eigen::Matrix3d::Identity();

  for (int i = 0; i < points_o.rows(); ++i) {

    auto dpibydx = project_image_df(P * wTo * X_o.col(i).template topLeftCorner<4, 1>());

    Eigen::MatrixXd jac;
    if (use_left_perturbation_flag)
    {
      // using left perturbation
      jac = -1 * dpibydx * ps_puline_s * cTw * odotOperator(wTo * X_o.col(i));
      // or equivalently
      // jac = -1 * dpibydx * P * odotOperator(wTo * X_o.col(i));
      // std::cerr << "jac equivalent " << odotOperator(wTo * X_o.col(i)) << "\n";
    }
    else
    {
      // using right perturbation
      jac = -1 * dpibydx * ps_puline_s * odotOperator(cTw * wTo * X_o.col(i));
    }

    assert(jac.rows() == 2 && jac.cols() == 6);
    jacobians.template block<2, 6>(2*i, 0) = jac.template block<2, 6>(0,0);

    // for debugging
    // std::cerr << "jac " << jac << "\n";

  }

  return jacobians;
}

/**
 * @brief Read eigen matrices from hdfio object
 *
 * @param h5io
 * @param name
 * @return
 */
template<typename T = cv::Ptr<cv::hdf::HDF5>>
Eigen::MatrixXd
dsread(const T& h5io, const std::string& name) {
  cv::Mat m;
  if (! h5io->hlexists(name))
    throw std::runtime_error("Unable to find dataset " + name);
  h5io->dsread( m, name );
  if (m.dims > 2)
    throw std::runtime_error("Cannot handle more than 2 dims, found " + std::to_string(m.dims));
  Eigen::MatrixXd m_e;
  cv::cv2eigen(m, m_e);
  return m_e;
}

/**
 * @brief Computes the distance between two SE3 transforms
 *
 * @param T1: size 4 x 4, input transform
 * @param T2: size 4 x 4, input transform
 *
 * @return : (3-tr(R))/2, |t₁ - t₂|₂
 */
template <typename D1, typename D2>
std::tuple<typename D1::Scalar, typename D1::Scalar>
displacement(const Eigen::MatrixBase<D1>& T1, const Eigen::MatrixBase<D2>& T2)
{
  using Scalar = typename D1::Scalar;
  // tr(R) = 1 + 2 cos θ
  // 1 - cos θ = 3/2 - tr(R)/2 ∈ [0, 2]
  auto R1 = T1.template block<3,3>(0,0);
  auto R2 = T2.template block<3,3>(0,0);
  Scalar dispR = (3 - (R1.transpose() * R2).trace()) / 2;
  Scalar dispt = (T1.template topRightCorner<3,1>() - T2.template topRightCorner<3,1>()).norm();
  return std::make_tuple(dispR, dispt);
}

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
 @f}
 *
 * @param ph = 4 = point in homogeneous coordinates
 *
 * @return : odot(ph) = 4 x 6
 */
inline Eigen::Matrix<double, 4, 6> odotOperator(const Eigen::Vector4d& x) {
  Eigen::Matrix<double, 4, 6> temp;
  temp.setZero();
  temp.block(0, 3, 3, 3) = -1 * skewSymmetric(x.head(3));

  temp(0, 0) = x(3);
  temp(1, 1) = x(3);
  temp(2, 2) = x(3);
  return temp;
}

/**
 * @brief Computes the derivative of camera se3 wrt IMU se3
 * @param [in] R_b2c : rotation of body frame to camera frame
 * @param [in] t_c_b : position of camera frame in body frame
 * @param [in] R_w2c : rotation of world frame to camera frame
 * @param [in] t_b_w : position of body frame in world frame
 * @param [in] use_left_perturbation_flag : which perturbation to use
 *
 * @return Jacobians (6 x 6)
 */
inline Eigen::Matrix<double, 6, 6> get_cam_wrt_imu_se3_jacobian(const Eigen::Matrix3d& R_b2c, const Eigen::Vector3d& t_c_b, const Eigen::Matrix3d& R_w2c, const Eigen::Vector3d& t_b_w, const bool use_left_perturbation_flag)
{
  Eigen::Matrix<double, 6, 6> p_cxi_p_ixi = Eigen::Matrix<double, 6, 6>::Zero();
  if (use_left_perturbation_flag)
  {

    p_cxi_p_ixi.block<3, 3>(0, 0) = skewSymmetric(t_b_w);
    p_cxi_p_ixi.block<3, 3>(3, 0) = Eigen::Matrix<double, 3, 3>::Identity();
    p_cxi_p_ixi.block<3, 3>(0, 3) = Eigen::Matrix<double, 3, 3>::Identity();

  }
  else
  {

    p_cxi_p_ixi.block<3, 3>(0, 0) = -1 * R_b2c * skewSymmetric(t_c_b);
    p_cxi_p_ixi.block<3, 3>(3, 0) = R_b2c;
    p_cxi_p_ixi.block<3, 3>(0, 3) = R_w2c;

  }

  return p_cxi_p_ixi;
}

} // namespace orcvio
#endif // SE3_OPS_HPP


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_POSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_POSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/numeric/bindings/traits/detail/symm_herm_traits.hpp>
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A symmetric or Hermitian positive definite matrix
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * posv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric or Hermitian positive
     * definite matrix and X and B are N-by-NRHS matrices.
     *
     * The Cholesky decomposition is used to factor A as
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is a lower triangular
     * matrix. The factored form of A is then used to solve the system of
     * equations A * X = B.
     *
     * If UPLO = 'U', the leading N-by-N upper triangular part of A
     * contains the upper triangular part of the matrix A, and the
     * strictly lower triangular part of A is not referenced.
     * If UPLO = 'L', the leading N-by-N lower triangular part of A
     * contains the lower triangular part of the matrix A, and the
     * strictly upper triangular part of A is not referenced.
     */

    namespace detail {

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 float* a, int const lda,
                 float* b, int const ldb, int* info)
      {
        LAPACK_SPOSV (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 double* a, int const lda,
                 double* b, int const ldb, int* info)
      {
        LAPACK_DPOSV (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* a, int const lda,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPOSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* a, int const lda,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPOSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmMatrA, typename MatrB>
      inline
      int posv (char const uplo, SymmMatrA& a, MatrB& b) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        int info;
        posv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int posv (char const uplo, SymmMatrA& a, MatrB& b) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::posv (uplo, a, b);
    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int posv (SymmMatrA& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT( (traits::detail::symm_herm_compatible< val_t, typename matraits::matrix_structure >::value ) ) ;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::posv (uplo, a, b);
    }

    /*
     * potrf() computes the Cholesky factorization of a symmetric
     * or Hermitian positive definite matrix A. The factorization has
     * the form
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is lower triangular.
     */

    namespace detail {

      inline
      void potrf (char const uplo, int const n,
                  float* a, int const lda, int* info)
      {
        LAPACK_SPOTRF (&uplo, &n, a, &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  double* a, int const lda, int* info)
      {
        LAPACK_DPOTRF (&uplo, &n, a, &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  traits::complex_f* a, int const lda, int* info)
      {
        LAPACK_CPOTRF (&uplo, &n, traits::complex_ptr (a), &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  traits::complex_d* a, int const lda, int* info)
      {
        LAPACK_ZPOTRF (&uplo, &n, traits::complex_ptr (a), &lda, info);
      }

      template <typename SymmMatrA>
      inline
      int potrf (char const uplo, SymmMatrA& a) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        int info;
        potrf (uplo, n, traits::matrix_storage (a),
               traits::leading_dimension (a), &info);
        return info;
      }

    }

    template <typename SymmMatrA>
    inline
    int potrf (char const uplo, SymmMatrA& a) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::potrf (uplo, a);
    }

    template <typename SymmMatrA>
    inline
    int potrf (SymmMatrA& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename matraits::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::potrf (uplo, a);
    }

    /*
     * potrs() solves a system of linear equations A*X = B with
     * a symmetric or Hermitian positive definite matrix A using
     * the Cholesky factorization computed by potrf().
     */

    namespace detail {

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  float const* a, int const lda,
                  float* b, int const ldb, int* info)
      {
        LAPACK_SPOTRS (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  double const* a, int const lda,
                  double* b, int const ldb, int* info)
      {
        LAPACK_DPOTRS (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* a, int const lda,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPOTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* a, int const lda,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPOTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmMatrA, typename MatrB>
      inline
      int potrs (char const uplo, SymmMatrA const& a, MatrB& b) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        int info;
        potrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
               traits::matrix_storage (b),
               traits::leading_dimension (b),
               &info);
        return info;
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int potrs (char const uplo, SymmMatrA const& a, MatrB& b) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::potrs (uplo, a, b);
    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int potrs (SymmMatrA const& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef traits::matrix_traits<MatrB> mbtraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename matraits::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename mbtraits::matrix_structure, traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::potrs (uplo, a, b);
    }

    // TO DO: potri()

  }

}}}

#endif


/*=================================================================
*
* compute various kinds of Laplacian weight
*
* usage:
		L = perform_mesh_weight(verts, faces, type, options);
* inputs:
		verts: 3*nverts
		faces: 3*faces
		type: type is either
			%       0: 'combinatorial' or 'graph': W(i,j)=1 is vertex i is conntected to vertex j.
			%       1: 'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
			%           i and j.
			%       2: 'spring': W(i,j) = 1/d_ij where d_ij is distance between vertex
			%           i and j.
			%       3: 'conformal' or 'dcp': W(i,j) = (cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j). (do not offer W(i,j) = cot(alpha_ij)+cot(beta_ij) anymore)
			%           Refer to Computing discrete minimal surfaces and their conjugates_93,
			%           Lemma 2 of On the convergence of metric and geometric properties of polyhedral surfaces_06 and
			%           Characterizing Shape Using Conformal Factors_08.
			%           Refer to Skeleton Extraction by Mesh Extraction_08, and Intrinsic Parameterizations of Surface Meshes_02.
			%       4: 'Mean_curvature' or 'Laplace-Beltrami': W(i,j) = (1/area_i)*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j), area_i is the area of vertex i's Voroni vicinity.
			%           Refer to Discrete Differential-Geometry Operators_for triangulated 2-manifolds_02
			%       5: 'Manifold-harmonic': W(i,j) = (1/sqrt(area_i*area_j))*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j).
			%           Refer to Spectral Geometry Processing with Manifold Harmonics_08
			%       6: 'mvc': W(i,j) = [tan(/_kij/2)+tan(/_jil/2)]/d_ij where /_kij and /_jil are angles at i
		options.?:
*
*           4 or 5 can be built on 3 in matlab.
*           just 0 & 3 are handled here. 1, 2 and 6 are left as todo.
*
* JJCAO, 2013
*
*=================================================================*/

#include <mex.h>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <vector>

using namespace Eigen;
using namespace std;

typedef Triplet<double> T;
typedef SparseMatrix<double>::Index Index;
typedef SparseMatrix<double>::Scalar Scalar;

/// Return cotangent of (P,Q,R) corner (ie cotan of QP,QR angle).
double cotangent(const Vector3d& P,
                    const Vector3d& Q,
                    const Vector3d& R)
{

    Vector3d u = P - Q;
    Vector3d v = R - Q;
    // (u . v)/((u x v).len)
    double dot = u.dot(v);
    Vector3d cross_vector = u.cross(v);
    double cross_norm = std::sqrt(cross_vector.dot(cross_vector));
    if(cross_norm != 0.0)
        return (dot/cross_norm);
    else
        return 0.0; // undefined
}

////                                                  -> ->
///// Return tangent of (P,Q,R) corner (ie tangent of QP,QR angle).
//double tangent(const Point_3& P,
//                const Point_3& Q,
//                const Point_3& R)
//{
//    Vector_3 u = P - Q;
//    Vector_3 v = R - Q;
//    // (u . v)/((u x v).len)
//    double dot = (u*v);
//    CGAL_surface_mesh_parameterization_assertion(dot != 0.0);
//    Vector_3 cross_vector = CGAL::cross_product(u,v);
//    double cross_norm = std::sqrt(cross_vector*cross_vector);
//    if(dot != 0.0)
//        return (cross_norm/dot);
//    else
//        return 0.0; // undefined
//}

void compute_dcp_weight(double* verts, int* faces, int nverts, int nfaces, SparseMatrix<double>& sm)
{
	int i,j,m;
	double dtmp;
	std::vector<T> coef(nverts*6);
	for (int k = 0; k < nfaces; ++k)
	{
		int tmp = 3*k;
		int face[3] = {faces[tmp],faces[tmp+1],faces[tmp+2]};
		for (int l = 0; l < 3; ++l)
		{
			i = face[l]; j = face[(l+1)%3]; m = face[(l+2)%3];
			Vector3d p(verts[3*i],verts[3*i+1],verts[3*i+2]);
			Vector3d q(verts[3*m],verts[3*m+1],verts[3*m+2]);
			Vector3d r(verts[3*j],verts[3*j+1],verts[3*j+2]);
			dtmp = 0.5*cotangent(p, q, r);
			coef.push_back(T(i,j,dtmp));
		}
	}
	{
		SparseMatrix<double> smtmp(nverts, nverts);
		smtmp.setFromTriplets(coef.begin(), coef.end());
		sm = SparseMatrix<double>(smtmp.transpose()) + smtmp;
	}
}

void perform_mesh_weight(double* verts, int nverts, int *faces, int nfaces, int type, double *vert_areas, SparseMatrix<double>& sm)
{
	switch(type)
	{
	case 0: //'combinatorial' or 'graph'
		// ���ܴ�������
		{
		int i,j;
		std::vector<T> coef(nverts*6);
		for (int k = 0; k < nfaces; ++k)
		{
			int tmp = 3*k;
			int face[3] = {faces[tmp],faces[tmp+1],faces[tmp+2]};
			for (int l = 0; l < 3; ++l)
			{
				i = face[l]; j = face[(l+1)%3];
				coef.push_back(T(i,j,1));
			}
		}
		sm.setFromTriplets(coef.begin(), coef.end());
		//sm.coeffRef(1,1) = 2;sm.coeffRef(2,2) = 3;//sm.coeffRef(0,0) = 1;
		}
		break;
	case 3: //'conformal' or 'dcp'
		compute_dcp_weight(verts, faces, nverts, nfaces, sm);
		break;
	//case 4: // 'Mean_curvature' or 'Laplace-Beltrami'
	//	//if (vert_areas == 0)
	//	//{
	//	//	stringstream ss("options.vert_areas is not offered! ");
	//	//	mexErrMsgTxt(ss.str().c_str());
	//	//}
	//	compute_dcp_weight(verts, faces, nverts, nfaces, sm);
	//	//{
	//	//	for ( int i = 0; i < sm.rows(); ++i)
	//	//	{
	//	//		sm.row(i) = sm.row(i) * (1.0/vert_areas[i]);
	//	//	}
	//	//}
	//	break;
	default:
		stringstream ss("type: ");
		ss << type << " is not supported!";
		mexErrMsgTxt(ss.str().c_str());
	}

	sm.makeCompressed();
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	///////////// Error Check
	if ( nrhs < 3)
		mexErrMsgTxt("Number of input should be > 2");
	if (1 != nlhs)
		mexErrMsgTxt("Number of output should be 1");

	///////////// input & output arguments
	// input 0: verts: 3*nverts
	int row = mxGetM(prhs[0]);
	int nverts = mxGetN(prhs[0]);
	if(row != 3)
		mexErrMsgTxt("The mesh must be triangle mesh! it is excepted to be 3*n");

	double *verts = mxGetPr(prhs[0]);

	// input 1: faces: 3*nfaces
	row = mxGetM(prhs[1]);
	int nfaces = mxGetN(prhs[1]);
	if(row != 3)
		mexErrMsgTxt("The mesh must be triangle mesh! it is excepted to be 3*n");

	double* dfaces = mxGetPr(prhs[1]);
    int* faces = new int[nfaces*3];
    for(int i = 0; i < nfaces*3; ++i)
    {
        faces[i] = int(dfaces[i]);
		--faces[i];
    }

	// input 3: type
	double* type = mxGetPr(prhs[2]);

	// input 4: options
	double* vert_areas(0);
	if ( nrhs > 3)
	{
		mxArray* tmp;
		const mxArray *options = prhs[3];
		if ( mxSTRUCT_CLASS != mxGetClassID(options))
			mexErrMsgTxt("4th arguments is not a structure!");
		else
		{
			// options.vert_areas: 1*nverts
			tmp = mxGetField(options,0,"vert_areas");
			if (tmp)
				vert_areas = mxGetPr(tmp);// not used!
			//mexPrintf("%f, %f, %f\n", vert_areas[0],vert_areas[1],vert_areas[2]);
		}
	}

	///////////////////////////////////////////////
	SparseMatrix<double> sm(nverts, nverts);
	perform_mesh_weight(verts, nverts, faces, nfaces, *type, vert_areas, sm);

	SparseMatrix<double>::StorageIndex* innerInd = sm.innerIndexPtr();
	SparseMatrix<double>::StorageIndex* outerInd = sm.outerIndexPtr();
	Scalar* valuePtr = sm.valuePtr();

	///////////////////////////////////////////////
	// output 0
	//plhs[0] = mxCreateDoubleMatrix( nverts, nverts, mxREAL);
    mwSize nzmax= sm.nonZeros();
	plhs[0] = mxCreateSparse( nverts, nverts, nzmax, mxREAL);
	double *L = mxGetPr(plhs[0]);
	mwIndex *irs = mxGetIr(plhs[0]);//row index
    mwIndex *jcs = mxGetJc(plhs[0]);//column index

	for (int k = 0; k<nzmax; ++k)
	{
		//double dtmp = valuePtr[k];
		//long ltmp = innerInd[k];
		L[k] = valuePtr[k];
		irs[k] = innerInd[k];
		//mexPrintf("%f %f\n", L[k], irs[k]);
	}

	for (int k = 0; k<nverts; ++k)
	{
		//long ltmp = outerInd[k];
		jcs[k] = outerInd[k];
	}
	//long ltmp = outerInd[nverts];
	jcs[nverts]=outerInd[nverts];

    delete[] faces;
}


// Copyright (c) 2020, Viktor Larsson
// 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 <COPYRIGHT HOLDER> 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.

#include "univariate.h"

#include <Eigen/Eigen>
#include <complex>

namespace poselib {
namespace univariate {
/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
void solve_quadratic(double a, double b, double c, std::complex<double> roots[2]) {

    std::complex<double> b2m4ac = b * b - 4 * a * c;
    std::complex<double> sq = std::sqrt(b2m4ac);

    // Choose sign to avoid cancellations
    roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
    roots[1] = c / (a * roots[0]);
}

/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
int solve_quadratic_real(double a, double b, double c, double roots[2]) {

    double b2m4ac = b * b - 4 * a * c;
    if (b2m4ac < 0)
        return 0;

    double sq = std::sqrt(b2m4ac);

    // Choose sign to avoid cancellations
    roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
    roots[1] = c / (a * roots[0]);

    return 2;
}

/* Sign of component with largest magnitude */
inline double sign2(const std::complex<double> z) {
    if (std::abs(z.real()) > std::abs(z.imag()))
        return z.real() < 0 ? -1.0 : 1.0;
    else
        return z.imag() < 0 ? -1.0 : 1.0;
}

/* Sign of component with largest magnitude */
inline double sign(const double z) { return z < 0 ? -1.0 : 1.0; }

void solve_cubic_single_real(double c2, double c1, double c0, double &root) {
    double a = c1 - c2 * c2 / 3.0;
    double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0;
    double c = b * b / 4.0 + a * a * a / 27.0;
    if (c > 0) {
        c = std::sqrt(c);
        b *= -0.5;
        root = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0;
    } else {
        c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a);
        root = 2.0 * std::sqrt(-a / 3.0) * std::cos(std::acos(c) / 3.0) - c2 / 3.0;
    }
}

int solve_cubic_real(double c2, double c1, double c0, double roots[3]) {
    double a = c1 - c2 * c2 / 3.0;
    double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0;
    double c = b * b / 4.0 + a * a * a / 27.0;
    int n_roots;
    if (c > 0) {
        c = std::sqrt(c);
        b *= -0.5;
        roots[0] = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0;
        n_roots = 1;
    } else {
        c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a);
        double d = 2.0 * std::sqrt(-a / 3.0);
        roots[0] = d * std::cos(std::acos(c) / 3.0) - c2 / 3.0;
        roots[1] = d * std::cos(std::acos(c) / 3.0 - 2.09439510239319526263557236234192) - c2 / 3.0; // 2*pi/3
        roots[2] = d * std::cos(std::acos(c) / 3.0 - 4.18879020478639052527114472468384) - c2 / 3.0; // 4*pi/3
        n_roots = 3;
    }

    // single newton iteration
    for (int i = 0; i < n_roots; ++i) {
        double x = roots[i];
        double x2 = x * x;
        double x3 = x * x2;
        double dx = -(x3 + c2 * x2 + c1 * x + c0) / (3 * x2 + 2 * c2 * x + c1);
        roots[i] += dx;
    }
    return n_roots;
}

/* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */
void solve_quartic(double b, double c, double d, double e, std::complex<double> roots[4]) {

    // Find depressed quartic
    std::complex<double> p = c - 3.0 * b * b / 8.0;
    std::complex<double> q = b * b * b / 8.0 - 0.5 * b * c + d;
    std::complex<double> r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0;

    // Resolvent cubic is now
    // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2
    std::complex<double> bb = 2.0 * p;
    std::complex<double> cc = p * p - 4.0 * r;
    std::complex<double> dd = -q * q;

    // Solve resolvent cubic
    std::complex<double> d0 = bb * bb - 3.0 * cc;
    std::complex<double> d1 = 2.0 * bb * bb * bb - 9.0 * bb * cc + 27.0 * dd;

    std::complex<double> C3 = (d1.real() < 0) ? (d1 - sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0
                                              : (d1 + sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0;

    std::complex<double> C;
    if (C3.real() < 0)
        C = -std::pow(-C3, 1.0 / 3);
    else
        C = std::pow(C3, 1.0 / 3);

    std::complex<double> u2 = (bb + C + d0 / C) / -3.0;

    // std::complex<double> db = u2 * u2 * u2 + bb * u2 * u2 + cc * u2 + dd;

    std::complex<double> u = sqrt(u2);

    std::complex<double> s = -u;
    std::complex<double> t = (p + u * u + q / u) / 2.0;
    std::complex<double> v = (p + u * u - q / u) / 2.0;

    roots[0] = (-u - sign2(u) * sqrt(u * u - 4.0 * v)) / 2.0;
    roots[1] = v / roots[0];
    roots[2] = (-s - sign2(s) * sqrt(s * s - 4.0 * t)) / 2.0;
    roots[3] = t / roots[2];

    for (int i = 0; i < 4; i++) {
        roots[i] = roots[i] - b / 4.0;

        // do one step of newton refinement
        std::complex<double> x = roots[i];
        std::complex<double> x2 = x * x;
        std::complex<double> x3 = x * x2;
        std::complex<double> dx =
            -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d);
        roots[i] = x + dx;
    }
}

/* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */
int solve_quartic_real(double b, double c, double d, double e, double roots[4]) {

    // Find depressed quartic
    double p = c - 3.0 * b * b / 8.0;
    double q = b * b * b / 8.0 - 0.5 * b * c + d;
    double r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0;

    // Resolvent cubic is now
    // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2
    double bb = 2.0 * p;
    double cc = p * p - 4.0 * r;
    double dd = -q * q;

    // Solve resolvent cubic
    double u2;
    solve_cubic_single_real(bb, cc, dd, u2);

    if (u2 < 0)
        return 0;

    double u = sqrt(u2);

    double s = -u;
    double t = (p + u * u + q / u) / 2.0;
    double v = (p + u * u - q / u) / 2.0;

    int sols = 0;
    double disc = u * u - 4.0 * v;
    if (disc > 0) {
        roots[0] = (-u - sign(u) * std::sqrt(disc)) / 2.0;
        roots[1] = v / roots[0];
        sols += 2;
    }
    disc = s * s - 4.0 * t;
    if (disc > 0) {
        roots[sols] = (-s - sign(s) * std::sqrt(disc)) / 2.0;
        roots[sols + 1] = t / roots[sols];
        sols += 2;
    }

    for (int i = 0; i < sols; i++) {
        roots[i] = roots[i] - b / 4.0;

        // do one step of newton refinement
        double x = roots[i];
        double x2 = x * x;
        double x3 = x * x2;
        double dx = -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d);
        roots[i] = x + dx;
    }
    return sols;
}

}; // namespace univariate
}; // namespace poselib


#pragma once

#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace polyfem {

	// Show some stats about the matrix M: det, singular values, condition number, etc
	void show_matrix_stats(const Eigen::MatrixXd &M);

	template<typename T>
	T determinant(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0, 3, 3> &mat)
	{
		assert(mat.rows() == mat.cols());

		if(mat.rows() == 1)
			return mat(0);
		else if(mat.rows() == 2)
			return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
		else if(mat.rows() == 3)
			return mat(0,0)*(mat(1,1)*mat(2,2)-mat(1,2)*mat(2,1))-mat(0,1)*(mat(1,0)*mat(2,2)-mat(1,2)*mat(2,0))+mat(0,2)*(mat(1,0)*mat(2,1)-mat(1,1)*mat(2,0));

		assert(false);
		return T(0);
	}

    template<typename T>
	void read_matrix(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	Eigen::Vector4d compute_specturm(const StiffnessMatrix &mat);

} // namespace polyfem