/*
 * mask.hpp
 *
 *  Created on: Jun 12, 2013
 *      Author: kthierbach
 */

#ifndef MASK_HPP_
#define MASK_HPP_

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <functional>
#include <limits>
#include <memory>
#include <stdlib.h>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include "circularLinkedList.hpp"
#include "glm/glm.hpp"
#include "image.hpp"

namespace elib{

template <class Point>
class Mask
{
	public:
		Mask() noexcept
		{
		}
		Mask(int rank, const std::vector<int> &dimensions) : rank(rank)
		{
			this->dimensions = std::vector<int>(dimensions.begin(), dimensions.end());
		}
		Mask(const Mask &other) : rank(other.rank), dimensions(other.dimensions), points(other.points)
		{
			if(other.mask != nullptr)
			{
				mask = std::unique_ptr<boost::numeric::ublas::compressed_matrix<int>>(new boost::numeric::ublas::compressed_matrix<int>(*other.mask));
			}
			if(other.bounding_box != nullptr)
			{
				bounding_box = std::unique_ptr<BoundingBox<Point>>(new BoundingBox<Point>(*other.bounding_box));
			}
		}
		Mask(Mask&& other) : rank(other.rank), dimensions(std::move(other.dimensions)), points(std::move(other.points)),
				mask(std::move(other.mask)), bounding_box(std::move(other.bounding_box))
		{
		}
		virtual ~Mask()
		{
		}
		Mask& operator=(const Mask &other)
		{
			rank = other.rank;
			dimensions = other.dimensions;
			points = other.points;
			if(other.mask != nullptr)
			{
				mask = std::unique_ptr<boost::numeric::ublas::compressed_matrix<int>>(new boost::numeric::ublas::compressed_matrix<int>(*other.mask));
			}
			if(other.bounding_box != nullptr)
			{
				bounding_box = std::unique_ptr<BoundingBox<Point>>(new BoundingBox<Point>(*bounding_box));
			}
			return *this;
		}
		Mask& operator=(Mask &&other)
		{
			rank = other.rank;
			dimensions = std::move(other.dimensions);
			points = std::move(other.points);
			mask = std::move(other.mask);
			bounding_box = std::move(other.bounding_box);
			return *this;
		}
		bool overlap(Mask &other)
		{
			namespace ub = boost::numeric::ublas;

			if(this->bounding_box == nullptr)
			{
				this->setBox(this->bounding_box);
			}
			if(other.bounding_box == nullptr)
			{
				other.setBox(other.bounding_box);
			}
			if(this->bounding_box->overlap(*other.bounding_box))
			{
				if(this->mask == nullptr)
				{
					this->createSparseRepresentation();
				}
				if(other.mask == nullptr)
				{
					other.createSparseRepresentation();
				}
				Mask mask(this->getRank(), *(this->getDimensions()));
				boost::numeric::ublas::compressed_matrix<int> result;
				try
				{
					result = boost::numeric::ublas::element_prod(*this->mask, *other.mask);
				}
				catch(const std::exception &e)
				{
					std::cout << "Can't get element product: " << e.what() << std::endl;
					abort();
				}
				if(mask.getRank()==2)
				{
					for(auto i = result.begin1(); i!= result.end1(); ++i)
					{
						for(auto j= i.begin(); j != i.end(); ++j)
							mask.addPoint(Point(j.index1(), j.index2()));
					}
				}
				return bool(mask.getSize());
			}
			return false;
		}
		void addPoint(Point p)
		{
			points.push_back(p);
			mask.reset();
			bounding_box.reset();
		}
		void deleteSparseRepresentation()
		{
			mask.reset();
		}
		const std::vector<Point>* getMask() const
		{
			return &points;
		}
		Image<int> toImage()
		{
			Image<int> image;
			int *image_data;
			int pixel;
			bool add_point;

			image = Image<int>(rank, dimensions, bit_depth, 1);
			image_data = image.getData();
			for(auto it=points.begin(); it!=points.end(); ++it)
			{
				add_point=true;
				for(int i=0; i<rank; ++i)
				{
					if((*it)[i] < 0 || (*it)[i]>=dimensions[i])
					{
						add_point=false;
					}
				}
				if(add_point)
				{
					pixel = Mask::getPixel(*it);
					image_data[pixel] = 1;
				}
			}
			return image;
		}
		BoundingBox<Point> getBoundingBox()
		{
			if(bounding_box == nullptr)
			{
				setBox(bounding_box);
			}
			return *bounding_box;
		}
		std::string getBoxMask()
		{
			if(bounding_box == nullptr)
			{
				setBox(bounding_box);
			}
			return boxMask(bounding_box);
		}
		const std::vector<Point>* getPoints() const
		{
			return &points;
		}
		int getSize() const
		{
			return int(points.size());
		}
		void getOutline(std::vector<Point> &polygon, Point &centroid) const
		{
			outline(polygon, centroid);
		}
		const std::vector<int>* getDimensions() const
		{
			return &dimensions;
		}
		int getPixel(Point p)
		{
			return pixel(p);
		}
		int getRank() const
		{
			return rank;
		}

		void setDimensions(const std::vector<int> &dimensions)
		{
			this->dimensions = std::vector<int>(dimensions.begin(), dimensions.end());
		}
		void setRank(int rank)
		{
			this->rank = rank;
		}
		int getWidth() const
		{
			if(rank>0)
				return dimensions[0];
			else
				return 0;
		}
		int getHeight() const
		{
			if(rank>1)
				return dimensions[1];
			else
				return 0;
		}
		int getDepth() const
		{
			if(rank>2)
				return dimensions[2];
			else
				return 1;
		}
		void setOrigin(Point origin)
		{
			for(auto i = points.begin(); i != points.end(); ++i)
			{
				*i += origin;
			}
		}

	private:
		int rank = 0;
		const static int bit_depth = 16;
		std::vector<int> dimensions;
		std::vector<Point> points;
		std::unique_ptr<boost::numeric::ublas::compressed_matrix<int>> mask = nullptr;
		std::unique_ptr<BoundingBox<Point>> bounding_box = nullptr;

		inline int pixel(glm::ivec2 p)
		{
			return p[1]*dimensions[0]+p[0];
		}
		inline int pixel(glm::ivec3 p)
		{
			return p[2]*dimensions[0]*dimensions[1] + p[1]*dimensions[0] + p[0];
		}
		void createSparseRepresentation()
		{
			namespace ub = boost::numeric::ublas;
			mask = std::unique_ptr<ub::compressed_matrix<int>>(new ub::compressed_matrix<int>(dimensions[0], dimensions[1], points.size()));
			if(!mask)
			{
				std::cerr << "Creation of sparse representation failed!";
				abort();
			}
			for(auto i=points.begin(); i!=points.end(); ++i)
			{
				mask->insert_element((*i).x, (*i).y, 1);
			}
		}
		void setBox(std::unique_ptr<BoundingBox<glm::ivec2>> &box)
		{
			int minX = std::numeric_limits<int>::max(),
				minY = std::numeric_limits<int>::max(),
				maxX = 0,
				maxY = 0;
			for(auto it=points.begin(); it!=points.end(); ++it)
			{
				minX = std::min(minX, it->x);
				minY = std::min(minY, it->y);
				maxX = std::max(maxX, it->x);
				maxY = std::max(maxY, it->y);
			}
			box = std::unique_ptr<BoundingBox<glm::ivec2>>(new BoundingBox<glm::ivec2>(glm::ivec2(minX, minY), glm::ivec2(maxX+1, maxY+1)));
		}
		void setBox(std::unique_ptr<BoundingBox<glm::ivec3>> &box)
		{
			int minX = std::numeric_limits<int32_t>::max(),
				minY = std::numeric_limits<int32_t>::max(),
				minZ = std::numeric_limits<int32_t>::max(),
				maxX = 0,
				maxY = 0,
				maxZ = 0;
			for(auto it=points.begin(); it!=points.end(); ++it)
			{
				minX = std::min(minX, it->x);
				minY = std::min(minY, it->y);
				minZ = std::min(minZ, it->z);
				maxX = std::max(maxX, it->x);
				maxY = std::max(maxY, it->y);
				maxZ = std::max(maxZ, it->z);
			}
			std::unique_ptr<BoundingBox<glm::ivec3>>(new BoundingBox<glm::ivec3>(glm::ivec3(minX, minY, minZ), glm::ivec3(maxX+1, maxY+1, maxZ+1)));
		}
		std::string boxMask(const std::unique_ptr<BoundingBox<glm::ivec2>> &boundingBox)
		{
			glm::ivec2 p, p2;
			std::vector<char> tmp;
			std::vector<char>::iterator cit;
			std::vector<glm::ivec2>::iterator it;
			int pixel;
			std::string mask = "";

			p = boundingBox->getBottomRight() - boundingBox->getUpperLeft();
			dimensions[0] = p[0];
			dimensions[1] = p[1];
			tmp = std::vector<char>(p[0]*p[1], '0');
			for(it=points.begin(); it!=points.end(); ++it)
			{
				pixel = getPixel((*it)-boundingBox->getUpperLeft());
				tmp[pixel] = '1';
			}
			for(cit=tmp.begin(); cit!=tmp.end(); ++cit)
			{
				mask += *cit;
			}
			return mask;
		}
		void outline(std::vector<glm::ivec2> &outline, glm::ivec2 &centroid) const
		{
//			std::sort(points.begin(), points.end(), VectorSortComparator);
			if (!points.empty())
			{
				if(points.size() == 1)
				{
					//prevents function from infinite loop when the mask only contains one pixel
					centroid += points.front();
					outline.push_back(points.front());
					return;
				}
				std::unordered_set<glm::ivec2, VectorHashFunctor, VectorEqualFunctor> set;
				CircularLinkedList<glm::ivec2> neighbours;
				glm::ivec2 b, c, b0, b1, tmp, c1, *previous, *current;

				set.insert(points.begin(), points.end());

				neighbours.addLast(glm::ivec2(-1, 0));
				neighbours.addLast(glm::ivec2(-1, -1));
				neighbours.addLast(glm::ivec2(0, -1));
				neighbours.addLast(glm::ivec2(1, -1));
				neighbours.addLast(glm::ivec2(1, 0));
				neighbours.addLast(glm::ivec2(1, 1));
				neighbours.addLast(glm::ivec2(0, 1));
				neighbours.addLast(glm::ivec2(-1, 1));
				neighbours.setActualElement(glm::ivec2(-1, 0));

				b0 = points[0];
				for (int i = 1; i < neighbours.size(); i++)
				{
					previous = neighbours.getActualElementData();
					current = neighbours.getNext();
					tmp = glm::ivec2(b0.x + current->x, b0.y + current->y);
					if (set.find(tmp) != set.end())
					{
						b1 = tmp;
						c1 = glm::ivec2(b0.x + previous->x, b0.y + previous->y);
						outline.push_back(glm::ivec2(b0.x, b0.y));
						outline.push_back(glm::ivec2(b1.x, b1.y));
						break;
					}
				}
				b = b1;
				c = c1;
				while (true)
				{
					neighbours.setActualElement(glm::ivec2(c.x - b.x, c.y - b.y));
					for (int i = 1; i < 8; i++)
					{
						previous = neighbours.getActualElementData();
						current = neighbours.getNext();
						tmp = glm::ivec2(b.x + current->x, b.y + current->y);
						if (set.find(tmp) != set.end())
						{
							c = glm::ivec2(b.x + previous->x, b.y + previous->y);
							b = tmp;
							outline.push_back(glm::ivec2(b.x, b.y));
							break;
						}
					}
					if (b == b0)
						break;
				}
				for (unsigned int i = 0; i < outline.size(); ++i)
				{
					centroid += outline[i];
				}
				centroid /= outline.size();
			}
		}
		struct VectorHashFunctor
		{
			size_t operator()(const glm::ivec2 &v) const
			{
				return std::hash<int>()(v.x) ^ std::hash<int>()(v.y);
			}
		};
		struct VectorEqualFunctor
		{
			bool operator()(const glm::ivec2 &v1, const glm::ivec2 &v2) const
			{
				return (v1.x==v2.x && v1.y==v2.y);
			}
		};
};

} /* namespace elib */

#endif /* MASK_HPP_ */


#pragma once

#include <crest/geometry/biscale_mesh.hpp>

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

namespace crest
{
    namespace detail
    {
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> construct_saddle_point_problem(
                const Eigen::SparseMatrix<Scalar> & A,
                const Eigen::SparseMatrix<Scalar> & I_H)
        {
            // Define the matrix
            //
            // C = [ A      I_H^T ]
            //     [ I_H      0   ]
            //
            // and the right-hand side
            //
            // c = [ b ]
            //     [ 0 ]
            //
            // Solving the system C [ x, kappa ] = c then gives the corrector weights x.
            // kappa merely corresponds to a Lagrange multiplier and can be discarded.

            // Constructing the C matrix with triplets is not the most efficient way, but it is
            // fairly simple and probably still relatively efficient.
            std::vector<Eigen::Triplet<Scalar>> triplets;
            for (int i = 0; i < A.outerSize(); ++i)
            {
                for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it(A, i); it; ++it)
                {
                    triplets.push_back(Eigen::Triplet<Scalar>(it.row(), it.col(), it.value()));
                }
            }

            for (int i = 0; i < I_H.outerSize(); ++i)
            {
                for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it(I_H, i); it; ++it)
                {
                    const auto row = it.row() + A.rows();
                    const auto col = it.col();
                    triplets.push_back(Eigen::Triplet<Scalar>(row, col, it.value()));
                    // Also add the transposed element, to bring I_H^T to the top-right corner
                    triplets.push_back(Eigen::Triplet<Scalar>(col, row, it.value()));
                }
            }

            Eigen::SparseMatrix<Scalar> C(A.rows() + I_H.rows(), A.cols() + I_H.rows());
            C.setFromTriplets(triplets.cbegin(), triplets.cend());

            return C;
        };

        template <typename Scalar>
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> construct_dense_saddle_point_problem(
                const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> & A,
                const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> & I_H)
        {
            const auto n = A.rows();
            const auto m = I_H.rows();

            assert(n == I_H.cols());
            assert(n == A.cols());

            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> C(m + n, m + n);
            C.topLeftCorner(n, n) = A;
            C.bottomLeftCorner(m, n) = I_H;
            C.topRightCorner(n, m) = I_H.transpose();
            C.bottomRightCorner(m, m) = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(m, m);
            return C;
        }

        /**
         * Computes the weights of the standard coarse Lagrangian basis functions in the fine space
         * @param coarse
         * @param fine
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> standard_coarse_basis_in_fine_space(const BiscaleMesh<Scalar, int> & mesh)
        {
            // A complicating matter here is that a vertex in the fine mesh can reside in multiple
            // coarse elements, and so it's easy to count them twice.

            // The approach demonstrated here is horribly inefficient and inelegant.
            // It is meant as a simple stop-gap solution.
            // TODO: Improve this

            std::set<std::pair<int, int>> visited;
            std::vector<Eigen::Triplet<Scalar>> triplets;

            for (int coarse_index = 0; coarse_index < mesh.coarse_mesh().num_elements(); ++coarse_index)
            {
                const auto coarse_element = mesh.coarse_mesh().elements()[coarse_index];
                const auto triangle = mesh.coarse_mesh().triangle_for(coarse_index);
                const auto coeff = detail::basis_coefficients_for_triangle(triangle);

                for (const auto fine_index : mesh.descendants_for(coarse_index))
                    for (int i = 0; i < 3; ++i)
                    {
                        const auto fine_element = mesh.fine_mesh().elements()[fine_index];
                        const auto v_index = fine_element.vertex_indices[i];
                        const auto v = mesh.fine_mesh().vertices()[v_index];

                        for (int j = 0; j < 3; ++j)
                        {
                            const auto coarse_index = coarse_element.vertex_indices[j];
                            const auto key = std::make_pair(coarse_index, v_index);
                            if (visited.count(key) == 0)
                            {
                                const auto a = coeff(0, j);
                                const auto b = coeff(1, j);
                                const auto c = coeff(2, j);

                                const auto v_value = a * v.x + b * v.y + c;

                                triplets.push_back(Eigen::Triplet<Scalar>(coarse_index, v_index, v_value));
                                visited.insert(key);
                            }
                        }
                    }
            }

            Eigen::SparseMatrix<Scalar> basis(mesh.coarse_mesh().num_vertices(), mesh.fine_mesh().num_vertices());
            basis.setFromTriplets(triplets.begin(), triplets.end());
            return basis;
        }
    }
}


#include <learning/independences/continuous/mutual_information.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <algorithm>

#include <iomanip>

namespace learning::independences::continuous {

double mi_pair(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    VectorXi nv1(df->num_rows());
    VectorXi nv2(df->num_rows());

    auto raw_values1 = df.data<arrow::FloatType>(0);
    auto raw_values2 = df.data<arrow::FloatType>(1);
    for (int i = 0, rows = static_cast<int>(df->num_rows()); i < rows; ++i) {
        auto eps_i = static_cast<int>(eps(i));

        auto v1 = static_cast<int>(raw_values1[i]);
        auto v2 = static_cast<int>(raw_values2[i]);

        nv1(i) = std::min(1 + v1, eps_i) + std::min(rows - v1, eps_i) - 1;
        nv2(i) = std::min(1 + v2, eps_i) + std::min(rows - v2, eps_i) - 1;
    }

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res -= boost::math::digamma(nv1(i)) + boost::math::digamma(nv2(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k) + boost::math::digamma(df->num_rows());

    return res;
}

double mi_triple(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    VectorXi n_xz = VectorXi::Zero(df->num_rows());
    VectorXi n_yz = VectorXi::Zero(df->num_rows());
    VectorXi n_z(df->num_rows());

    auto raw_x = df.data<arrow::FloatType>(0);
    auto raw_y = df.data<arrow::FloatType>(1);
    auto raw_z = df.data<arrow::FloatType>(2);

    IndexComparator comp_z(raw_z);
    std::vector<size_t> sort_z(df->num_rows());
    std::iota(sort_z.begin(), sort_z.end(), 0);
    std::sort(sort_z.begin(), sort_z.end(), comp_z);

    for (int i = 0, rows = static_cast<int>(df->num_rows()); i < rows; ++i) {
        auto eps_i = static_cast<int>(eps(i));
        auto x_i = static_cast<int>(raw_x[i]);
        auto y_i = static_cast<int>(raw_y[i]);
        auto z_i = static_cast<int>(raw_z[i]);

        n_z(i) = std::min(1 + z_i, eps_i) + std::min(rows - z_i, eps_i) - 1;

        if (z_i < eps_i) {
            for (int j = 0, end = z_i + eps_i; j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        } else if (z_i > (rows - eps_i)) {
            for (int j = z_i - eps_i + 1, end = df->num_rows(); j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        } else {
            for (int j = z_i - eps_i + 1, end = z_i + eps_i; j < end; ++j) {
                auto index = sort_z[j];
                auto x_value = raw_x[index];
                auto y_value = raw_y[index];
                if (std::abs(x_i - x_value) < eps_i) ++n_xz(i);
                if (std::abs(y_i - y_value) < eps_i) ++n_yz(i);
            }
        }
    }

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res += boost::math::digamma(n_z(i)) - boost::math::digamma(n_xz(i)) - boost::math::digamma(n_yz(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k);

    return res;
}

double mi_general(const DataFrame& df, int k) {
    KDTree kdtree(df);
    auto knn_results = kdtree.query(df, k + 1, std::numeric_limits<double>::infinity());

    VectorXd eps(df->num_rows());
    for (auto i = 0; i < df->num_rows(); ++i) {
        eps(i) = knn_results[i].first(k);
    }

    std::vector<size_t> indices(df->num_columns() - 2);
    std::iota(indices.begin(), indices.end(), 2);
    auto z_df = df.loc(indices);
    KDTree ztree(z_df);
    auto [n_xz, n_yz, n_z] = ztree.count_ball_subspaces(z_df, df.col(0), df.col(1), eps);

    double res = 0;
    for (int i = 0; i < df->num_rows(); ++i) {
        res += boost::math::digamma(n_z(i)) - boost::math::digamma(n_xz(i)) - boost::math::digamma(n_yz(i));
    }

    res /= df->num_rows();
    res += boost::math::digamma(k);

    return res;
}

double KMutualInformation::mi(const std::string& x, const std::string& y) const {
    auto subset_df = m_ranked_df.loc(x, y);
    return mi_pair(subset_df, m_k);
}

double KMutualInformation::mi(const std::string& x, const std::string& y, const std::string& z) const {
    auto subset_df = m_ranked_df.loc(x, y, z);
    return mi_triple(subset_df, m_k);
}

double KMutualInformation::mi(const std::string& x, const std::string& y, const std::vector<std::string>& z) const {
    auto subset_df = m_ranked_df.loc(x, y, z);
    return mi_general(subset_df, m_k);
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y) const {
    auto value = mi(x, y);

    auto shuffled_df = m_ranked_df.loc(Copy(x), y);

    auto x_begin = shuffled_df.template mutable_data<arrow::FloatType>(0);
    auto x_end = x_begin + shuffled_df->num_rows();
    std::mt19937 rng{m_seed};

    int count_greater = 0;
    for (int i = 0; i < m_samples; ++i) {
        std::shuffle(x_begin, x_end, rng);
        auto shuffled_value = mi_pair(shuffled_df, m_k);

        if (shuffled_value >= value) ++count_greater;
    }

    return static_cast<double>(count_greater) / m_samples;
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y, const std::string& z) const {
    auto original_mi = mi(x, y, z);
    auto z_df = m_df.loc(z);
    auto shuffled_df = m_ranked_df.loc(Copy(x), y, z);
    auto original_rank_x = m_ranked_df.template data<arrow::FloatType>(x);

    return shuffled_pvalue(original_mi, original_rank_x, z_df, shuffled_df, MITriple{});
}

double KMutualInformation::pvalue(const std::string& x, const std::string& y, const std::vector<std::string>& z) const {
    auto original_mi = mi(x, y, z);
    auto z_df = m_df.loc(z);
    auto shuffled_df = m_ranked_df.loc(Copy(x), y, z);
    auto original_rank_x = m_ranked_df.template data<arrow::FloatType>(x);

    return shuffled_pvalue(original_mi, original_rank_x, z_df, shuffled_df, MIGeneral{});
}

}  // namespace learning::independences::continuous


#include "TetWild.h"
#include "wmtk/ExecutionScheduler.hpp"

#include <Eigen/src/Core/util/Constants.h>
#include <igl/Timer.h>
#include <wmtk/utils/AMIPS.h>
#include <array>
#include <wmtk/utils/Logger.hpp>
#include <wmtk/utils/TetraQualityUtils.hpp>

#include <limits>
#include <optional>
bool tetwild::TetWild::smooth_before(const Tuple& t)
{
    if (!m_vertex_attribute[t.vid(*this)].on_bbox_faces.empty()) return false;
    if (m_vertex_attribute[t.vid(*this)].m_is_rounded) return true;
    // try to round.
    return round(t); // Note: no need to roll back.
}

bool tetwild::TetWild::smooth_after(const Tuple& t)
{
    // Newton iterations are encapsulated here.
    wmtk::logger().trace("Newton iteration for vertex smoothing.");
    auto vid = t.vid(*this);

    auto locs = get_one_ring_tets_for_vertex(t);
    auto max_quality = 0.;
    for (auto& tet : locs) {
        max_quality = std::max(max_quality, m_tet_attribute[tet.tid(*this)].m_quality);
    }

    assert(locs.size() > 0);
    std::vector<std::array<double, 12>> assembles(locs.size());
    auto loc_id = 0;

    for (auto& loc : locs) {
        auto& T = assembles[loc_id];
        auto t_id = loc.tid(*this);

        assert(!is_inverted(loc));
        auto local_tuples = oriented_tet_vertices(loc);
        std::array<size_t, 4> local_verts;
        for (auto i = 0; i < 4; i++) {
            local_verts[i] = local_tuples[i].vid(*this);
        }

        local_verts = wmtk::orient_preserve_tet_reorder(local_verts, vid);

        for (auto i = 0; i < 4; i++) {
            for (auto j = 0; j < 3; j++) {
                T[i * 3 + j] = m_vertex_attribute[local_verts[i]].m_posf[j];
            }
        }
        loc_id++;
    }

    auto old_pos = m_vertex_attribute[vid].m_posf;
    auto old_asssembles = assembles;
    m_vertex_attribute[vid].m_posf = wmtk::newton_method_from_stack(
        assembles,
        wmtk::AMIPS_energy,
        wmtk::AMIPS_jacobian,
        wmtk::AMIPS_hessian);
    wmtk::logger().trace(
        "old pos {} -> new pos {}",
        old_pos.transpose(),
        m_vertex_attribute[vid].m_posf.transpose());

    if (m_vertex_attribute[vid].m_is_on_surface) {
        auto project = wmtk::try_project(m_vertex_attribute[vid].m_posf, old_asssembles);
        if (project) {
            m_vertex_attribute[vid].m_posf = project.value();

            auto max_after_quality = 0.;
            for (auto& loc : locs) {
                auto t_id = loc.tid(*this);
                m_tet_attribute[t_id].m_quality = get_quality(loc);
                max_after_quality = std::max(max_after_quality, m_tet_attribute[t_id].m_quality);
            }
            if (max_after_quality > max_quality) return false;
        }
        for (auto& t : locs) {
            for (auto j = 0; j < 4; j++) {
                auto f_t = tuple_from_face(t.tid(*this), j);
                auto fid = f_t.fid(*this);
                if (m_face_attribute[fid].m_is_surface_fs) {
                    auto vs = get_face_vertices(f_t);
                    bool is_out = m_envelope.is_outside(
                        {{m_vertex_attribute[vs[0].vid(*this)].m_posf,
                          m_vertex_attribute[vs[1].vid(*this)].m_posf,
                          m_vertex_attribute[vs[2].vid(*this)].m_posf}});
                    if (is_out) return false;
                }
            }
        }
    }

    for (auto& loc : locs) {
        auto t_id = loc.tid(*this);
        m_tet_attribute[t_id].m_quality = get_quality(loc);
    }

    m_vertex_attribute[vid].m_pos = tetwild::to_rational(m_vertex_attribute[vid].m_posf);

    return true;
}

void tetwild::TetWild::smooth_all_vertices()
{
    igl::Timer timer;
    double time;
    timer.start();
    auto executor = wmtk::ExecutePass<tetwild::TetWild>();
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_vertices()) {
        collect_all_ops.emplace_back("vertex_smooth", loc);
    }
    time = timer.getElapsedTime();
    wmtk::logger().info("vertex smoothing prepare time: {}s", time);
    wmtk::logger().debug("Num verts {}", collect_all_ops.size());
    if (NUM_THREADS > 0) {
        timer.start();
        auto executor = wmtk::ExecutePass<TetWild, wmtk::ExecutionPolicy::kPartition>();
        executor.lock_vertices = [](auto& m, const auto& e, int task_id) -> bool {
            return m.try_set_vertex_mutex_one_ring(e, task_id);
        };
        executor.num_threads = NUM_THREADS;
        executor(*this, collect_all_ops);
        time = timer.getElapsedTime();
        wmtk::logger().info("vertex smoothing operation time parallel: {}s", time);
    } else {
        timer.start();
        auto executor = wmtk::ExecutePass<TetWild, wmtk::ExecutionPolicy::kSeq>();
        executor(*this, collect_all_ops);
        time = timer.getElapsedTime();
        wmtk::logger().info("vertex smoothing operation time serial: {}s", time);
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_MULTIPLIES_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mul_pd(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mul_ps(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                      ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0,signed>::type  type;
      type mask = integral_constant<type,0x00FF>();

      return _mm_or_si128 ( _mm_and_si128(mask, _mm_mullo_epi16(a0,a1))
                          , _mm_slli_epi16
                            ( _mm_and_si128 ( mask
                                            , _mm_mullo_epi16
                                              ( _mm_srli_epi16(a0(), 8)
                                              , _mm_srli_epi16(a1(), 8)
                                              )
                                            )
                            , 8
                            )
                          );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                      ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mullo_epi16(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                      ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wshadow"
#endif // __clang__

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_or_si128(
                    _mm_and_si128(
                       _mm_mul_epu32(a0,a1),
                       _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                    )
                  , _mm_slli_si128(
                        _mm_and_si128(
                            _mm_mul_epu32( _mm_srli_si128(a0,4)
                                         , _mm_srli_si128(a1,4)
                                         )
                          , _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                        )
                      , 4
                    )
             );
    }

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

  };
} } }

#endif
#endif


/*
 * Copyright (C) 2015 Hamza Merzić
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include "model_parser.h"

#include <cmath>
#include <PQP/PQP.h>
#include <Eigen/Dense>
#include <fstream>
#include <memory>
#include <algorithm>

typedef Eigen::Vector3f EVector3f;

double PointDistanceToAxis(const EVector3f& point, const EVector3f& axis) {
  double l = point.dot(axis);
  return (point - axis * l).norm();
}

double PointDistanceToVector(const EVector3f& point, const EVector3f& vect) {
  double point_a_distance(point.squaredNorm());
  EVector3f d = point - vect;
  double point_b_distance(d.squaredNorm());
  EVector3f u = -vect;
  u.normalize();
  if (point_a_distance < point_b_distance) {
    d = point;
    d.normalize();
    double c = d.dot((vect).normalized());
    return (c > 0.0)
        ? sqrt(point_a_distance * (1.0 - c * c)) :
          sqrt(point_a_distance);
  } else {
    d.normalize();
    double c = d.dot((-vect).normalized());
    return (c > 0.0)
        ? sqrt(point_b_distance * (1.0 - c * c)) :
          sqrt(point_b_distance);
  }
}

PQP_Model* ModelParser::GetTransformModel(const std::string& model_file,
                                          const EMatrix& R,
                                          const EVector3f& T,
                                          const EVector3f& axis,
                                          double* axis_length,
                                          double* radius) {
  std::ifstream input_file (model_file.c_str(), std::ios::binary);
  try {
    std::unique_ptr<PQP_Model> model (new PQP_Model);

    char header[80] = "";         // Reads STL binary header
    input_file.read(header, 80);

    unsigned num_tris = 0;  // Reads number of triangles
    input_file.read(reinterpret_cast<char*>(&num_tris), sizeof(num_tris));

    EVector3f vertex[3];  // Triangle represented as three vertices
    float surf_vec[3];

    model->BeginModel();

    int16_t temp = 0;  // Used for taking two bits of data after a triangle
    uint64_t counter = 0;

    *axis_length = 0.0;
    *radius = 0.0;
    while (counter < num_tris) {
      input_file.read(reinterpret_cast<char*>(surf_vec), sizeof(surf_vec));

      for (unsigned i = 0; i < 3; ++i) {
        input_file.read(reinterpret_cast<char*>(&vertex[i]), sizeof(vertex[i]));
        vertex[i] = R * vertex[i] + T;
        *axis_length = std::max(*axis_length, double(vertex[i].dot(axis)));
        *radius = std::max(*radius,
            PointDistanceToVector(vertex[i], axis * *axis_length));
      }
      model->AddTri(vertex[0].data(), vertex[1].data(), vertex[2].data(),
        counter);

      input_file.read(reinterpret_cast<char*>(&temp), sizeof(temp));
      ++counter;
    }

    model->EndModel();
    return model.release();
  }
  catch(...) {
    throw "File " + model_file + " error!";
  }
}

// TODO(hamza): Try to get rid of code repetition
PQP_Model* ModelParser::GetModel(const std::string& model_file) {
  std::ifstream input_file (model_file.c_str(), std::ios::binary);
  try {
    std::unique_ptr<PQP_Model> model (new PQP_Model);

    char header[80] = "";         // Reads STL binary header
    input_file.read(header, 80);

    unsigned num_tris = 0;  // Reads number of triangles
    input_file.read(reinterpret_cast<char*>(&num_tris), sizeof(num_tris));

    float vertex[3][3];  // Triangle represented as three vertices
    float surf_vec[3];

    model->BeginModel();

    int16_t temp (0);  // Used for taking two bits of data after a triangle
    uint64_t counter = 0;
    while (counter < num_tris) {
      input_file.read(reinterpret_cast<char*>(surf_vec), sizeof(surf_vec));

      input_file.read(reinterpret_cast<char*>(&vertex[0]), sizeof(vertex[0]));
      input_file.read(reinterpret_cast<char*>(&vertex[1]), sizeof(vertex[1]));
      input_file.read(reinterpret_cast<char*>(&vertex[2]), sizeof(vertex[2]));
      model->AddTri(vertex[0], vertex[1], vertex[2], counter);

      input_file.read(reinterpret_cast<char*>(&temp), sizeof(temp));
      ++counter;
    }

    model->EndModel();
    return model.release();
  }
  catch(...) {
    throw "File " + model_file + " error!";
  }
}


#include "solver/BICGSTAB.h"

#include "utility/DataPtr.h"
#include "volume/Volume.h"

#include "cuda/Volume.cuh"
#include "cuda/LinearSys.cuh"
#include "cuda/CudaUtility.h"

#include "cuda/BICGSTAB.cuh"

#include "solver/LinearSystemDevice.h"
#include "solver/LinearSystemHost.h"

#include <Eigen/IterativeLinearSolvers>

#include "utility/Timer.h"

#include <iostream>
#include <fstream>

namespace fast {

	template <typename T>
	FAST_EXPORT BICGSTAB<T>::BICGSTAB(bool verbose) : Solver<T>(verbose)
	{

	}

	template <typename T>
	FAST_EXPORT typename Solver<T>::Output fast::BICGSTAB<T>::solve(LinearSystem * linearSys, const typename Solver<T>::SolveParams & solveParams)
	{

		assert(linearSys->type() == primitiveTypeof<T>());

		typename Solver<T>::Output out;
		LinearSystemDevice<T> * devicePtr = dynamic_cast<LinearSystemDevice<T>*>(linearSys);
		if (devicePtr) {

			//Try to allocate solver state
			BICGState<T> state;
			try {
				state.resize(devicePtr->NNZ);
			}
			catch (thrust::system_error & er) {
				std::cerr << er.what() << std::endl;
				return out;
			}

			bool res = BICG_Solve<T>(
				*devicePtr,
				state,
				solveParams.tolerance,
				solveParams.maxIter,
				&out.error,
				&out.iterations,
				Solver<T>::isVerbose()
				);

			out.status = res ? Solver<T>::Status::SOLVER_STATUS_SUCCESS : Solver<T>::Status::SOLVER_STATUS_NO_CONVERGENCE;
			return out;
		}

		auto hostPtr = dynamic_cast<LinearSystemHost<T>*>(linearSys);
		if (hostPtr) {

			int threadNum = omp_get_num_procs();
			if (solveParams.cpuThreads > 0) {
				threadNum = solveParams.cpuThreads;
			}
			omp_set_num_threads(threadNum);
			Eigen::setNbThreads(threadNum);

			Eigen::BiCGSTAB<Eigen::SparseMatrix<T, Eigen::RowMajor>> _solver;

			_solver.compute(hostPtr->A);
			_solver.setTolerance(solveParams.tolerance);
			_solver.setMaxIterations(solveParams.maxIter);

			hostPtr->x = _solver.solveWithGuess(hostPtr->b, hostPtr->x);

			out.error = _solver.error();
			if (_solver.info() == Eigen::ComputationInfo::Success)
				out.status = Solver<T>::Status::SOLVER_STATUS_SUCCESS;
			else
				out.status = Solver<T>::Status::SOLVER_STATUS_NO_CONVERGENCE;

			out.iterations = _solver.iterations();
			//copyHostToDevice(_x.data(), sys.x);

			return out;
		}

		//Linear system derivation not implemented
		assert(false);
		return out;
	}

	template <typename T>
	FAST_EXPORT	size_t fast::BICGSTAB<T>::requiredMemory(size_t nnz) const
	{
		return BICGState<T>::requiredBytesize(nnz);
	}

	template class BICGSTAB<double>;
	template class BICGSTAB<float>;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

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

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/param_herm_x.h>
#include <tomographer/densedm/param_rho_a.h>

#define SQRT2  boost::math::constants::root_two<double>()
#define INVSQRT2  boost::math::constants::half_root_two<double>()

// -----------------------------------------------------------------------------
// fixture(s)

#include "test_densedm_param_common.h"

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_densedm_param_herm_x)

BOOST_FIXTURE_TEST_CASE(test_param_x_1, TestFixtureQubitIdent)
{
  test_param_x(dmt, rho);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_1b, TestFixtureQubitIdent)
{
  typename DMTypes::VectorParamType x = Tomographer::DenseDM::ParamX<DMTypes>(dmt).HermToX(rho);
  BOOST_CHECK_CLOSE(x(0), 0.5, tol_percent);
  BOOST_CHECK_CLOSE(x(1), 0.5, tol_percent);
  BOOST_CHECK_SMALL(x(2), tol_percent);
  BOOST_CHECK_SMALL(x(3), tol_percent);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_1c, TestFixtureQuditPure0<2>)
{
  Eigen::Matrix2cd rho2;
  rho2 << 0.5, 0.5,
          0.5, 0.5;
  typename DMTypes::VectorParamType x = Tomographer::DenseDM::ParamX<DMTypes>(dmt).HermToX(rho2);
  BOOST_CHECK_CLOSE(x(0), 0.5, tol_percent);
  BOOST_CHECK_CLOSE(x(1), 0.5, tol_percent);
  BOOST_CHECK_CLOSE(x(2), std::sqrt(2.0)/2.0, tol_percent);
  BOOST_CHECK_SMALL(x(3), tol_percent);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_2, TestFixtureTritExample)
{
  test_param_x(dmt, rho);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_3, TestFixtureQuditPure0<4>)
{
  test_param_x(dmt, rho);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_3b, TestFixtureQuditPure0<5>)
{
  test_param_x(dmt, rho);
}
BOOST_FIXTURE_TEST_CASE(test_param_x_4, TestFixtureQuditRand<5>)
{
  test_param_x(dmt, rho);
}

BOOST_AUTO_TEST_SUITE_END()


// created by Mark O. Brown
#include "stdafx.h"
#include "Segment.h"
#include <boost/lexical_cast.hpp>

void Segment::calculateSegVariations( std::vector<parameterType>& variables, unsigned totalNumVariations){
	// handle more complicated things.
	if ( input.ramp.isRamp ){
		input.ramp.start.internalEvaluate (variables, totalNumVariations);
		input.ramp.end.internalEvaluate (variables, totalNumVariations);
		input.ramp.start.internalEvaluate( variables, totalNumVariations );
		input.ramp.end.internalEvaluate (variables, totalNumVariations);
	}
	else if ( input.pulse.isPulse )	{
		input.pulse.vOffset.internalEvaluate ( variables, totalNumVariations );
		input.pulse.amplitude.internalEvaluate ( variables, totalNumVariations );
		input.pulse.tOffset.internalEvaluate ( variables, totalNumVariations );
		input.pulse.width.internalEvaluate ( variables, totalNumVariations );
	}
	else{
		input.holdVal.internalEvaluate ( variables, totalNumVariations );
	}

	if ( input.mod.modulationIsOn )	{
		input.mod.frequency.internalEvaluate ( variables, totalNumVariations );
		input.mod.phase.internalEvaluate ( variables, totalNumVariations );
	}
	input.time.internalEvaluate( variables, totalNumVariations);
	if ( input.time.getValue(0)/1e3 < 1e-9 ){
		thrower ( "ERROR: agilent segment set to have zero time! Agilent can't handle zero-length segments." );
	}
	input.repeatNum.internalEvaluate (variables, totalNumVariations);
	//if (input.continuationType == SegmentEnd::type::repeat)	{
	//	// in which case you need a number of times to repeat.);
	//	input.repeatNum.internalEvaluate( variables, totalNumVariations );
	//}
}

void Segment::calculateSegVariations()
{
	std::vector<parameterType> variables = std::vector<parameterType>();
	calculateSegVariations(variables);
}

void Segment::storeInput( segmentInfo inputToSet ){
	input = inputToSet;
}

segmentInfo Segment::getInput(){
	return input;
}

/**
* This function takes ramp-related information as an input and returns the "position" in the ramp
* (i.e. the amount to add to the initial value due to ramping) that the waveform should be at.
*/
double Segment::rampCalc( int totalSamples, int sample, rampInfo ramp, unsigned varnum){
	auto rampSize = ramp.end.getValue (varnum) - ramp.start.getValue (varnum);
	if (ramp.type == "lin"){
		return sample * rampSize / totalSamples;
	}
	else if (ramp.type == "nr"){
		return 0;
	}
	else if (ramp.type == "tanh"){
		return rampSize * (tanh( -4 + 8 * (double)sample / totalSamples ) + 1) / 2;
	}
	else{
		if (ramp.isFileRamp) {
			if (ramp.rampFileVals.size () != totalSamples) {
				thrower ("ramp file vals not same size as total samples in agilent waveform! ramp size was "
					+ str (ramp.rampFileVals.size ()) + ", total sample number was " + str(totalSamples));
			}
			return ramp.rampFileVals[sample] * rampSize;
		}
		return 0;
	}
}

double Segment::modCalc( segModInfo mod, int iteration, long size, double pulseLength, unsigned varNum ){
	if ( !mod.modulationIsOn ){
		return 1.0;
	}
	double t = double(iteration) / size * pulseLength;
	double result;
	// mult by 1e6 to convert from MHz to Hz.
	result = sin( 2 * PI * mod.frequency.getValue(varNum) * 1e6 * t + mod.phase.getValue (varNum));
	return result;
}

double Segment::pulseCalc( segPulseInfo pulse, int iteration, long size, double pulseLength, double center,
	unsigned varNum){
	auto widthV = pulse.width.getValue (varNum) / 1000.0;
	auto ampV = pulse.amplitude.getValue (varNum);
	if ( pulse.type == "__NONE__" ){
		return 0;
	}
	else if ( pulse.type == "gaussian" ){
		// in this case, the width is the sigma of the gaussian. Note that this definition the width = tau from
		// adam's thesis because this will give the intensity of one of the beams, and the rabi rate~ sqrt(I1*I2) for
		// the two beams.

		double x = pulseLength * iteration / size;
		double result = ampV * exp( -(center - x) * (center - x) / (widthV * widthV) );
		return result;
	}
	else if ( pulse.type == "lorentzian" ){
		// in this case, the width is the standard lorentzian full width half max.
		double FWHM = widthV;
		//double center = pulseLength / 2.0;
		double x = pulseLength * iteration / size;
		// see definition: http://mathworld.wolfram.com/LorentzianFunction.html
		return ampV * (FWHM / (2.0 * PI)) / ((x - center)*(x - center) + (FWHM / 2) * (FWHM / 2));

	}
	else if ( pulse.type == "sech" ){
		// in this case, the width is just the scaling factor for the length
		//double center = pulseLength / 2.0;
		double x = pulseLength * iteration / size;
		// see definition: http://mathworld.wolfram.com/HyperbolicSecant.html
		return ampV * 1.0 / cosh( (x - center) / widthV);
	}
	else{
		thrower ( "ERROR: pulse type " + pulse.type + " is unrecognized.\r\n" );
		return 0;
	}
}

/*
 * This function uses the initial and final points along with the ramp and time of the segment to calculate all of the data points. This should be used so
 * as to, after this function, you have all of the powers that you want (not voltages), and then call the voltage converter afterwards.
 */
void Segment::calcData( unsigned long sampleRate, unsigned varNum){
	// calculate the size of the waveform.
	double numDataPointsf = input.time.getValue(varNum)/1e3 * sampleRate;
	int numDataPoints = (int)round (numDataPointsf);
	// test if good time.
	if (fabs( numDataPointsf - round( numDataPointsf ) ) > 1e-6){
		thrower ( "ERROR: Bad time entered for the time of an intensity sequence segment. The time was "
				 + str(input.time.getValue (varNum) / 1e3) + ", and the sample rate is " + str( sampleRate ) + ". The product of these"
				 " is the number of samples in the segment, and this must be an integer." );
	}
	if (input.ramp.isRamp && (input.ramp.type != "lin" && input.ramp.type != "tanh" && input.ramp.type != "nr")) {
		Segment::analyzeRampFile (input.ramp, numDataPoints);
	}
	// resize to zero. This is a complete reset of the data points in the class.
	dataArray.resize( 0 );
	// writebtn the data points. These are all in powers. These are converted to voltages later.
	for ( int dataInc = 0; dataInc < numDataPoints; dataInc++ ){
		double point;
		if ( input.ramp.isRamp ){
			point = input.ramp.start.getValue (varNum) + rampCalc( numDataPoints, dataInc, input.ramp, varNum );
		}
		else if (input.pulse.isPulse ){
			point = input.pulse.vOffset.getValue (varNum) + pulseCalc(input.pulse, dataInc, numDataPoints,
				input.time.getValue (varNum) / 1e3, input.pulse.tOffset.getValue (varNum)/1000.0, varNum)
				* modCalc(input.mod, dataInc, numDataPoints, input.time.getValue (varNum) / 1e3, varNum);
		}
		else{
			point = input.holdVal.getValue (varNum);
		}
		dataArray.push_back( point );
	}
}

unsigned Segment::returnDataSize(){
	return dataArray.size();
}

double Segment::returnDataVal( long dataNum ){
	return dataArray[dataNum];
}

void Segment::assignDataVal( int dataNum, double val ){
	dataArray[dataNum] = val;
}

void Segment::analyzeRampFile (rampInfo& ramp, long totalSamples) {
	std::ifstream rampFile (RAMP_LOCATION + ramp.type + ".txt");
	if (!rampFile.is_open ()) {
		thrower ("ERROR: ramp type " + ramp.type + " is unrecognized.\r\n");
	}
	ramp.rampFileName = ramp.type;
	ramp.rampFileVals.clear ();
	std::string word;
	while (rampFile >> word) {
		try {
			ramp.rampFileVals.push_back (boost::lexical_cast<double>(word));
		}
		catch (boost::bad_lexical_cast &) {
			thrower ("Failed to convert ramp file to doubles!");
		}
	}
	if (ramp.rampFileVals.size () != totalSamples) {
		thrower ("ramp file vals not same size as total samples! ramp size was "
			+ str (ramp.rampFileVals.size ()) + ", total sample number was " + str (totalSamples));
	}
	ramp.isFileRamp = true;
}


// This file is part of the dune-mlmc project:
//   http://users.dune-project.org/projects/dune-mlmc
// Copyright Holders: Rene Milk
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include <config.h>

#include "msfem.hh"

#include <dune/multiscale/common/main_init.hh>
#include <dune/multiscale/msfem/localsolution_proxy.hh>
#include <dune/multiscale/msfem/localproblems/localgridlist.hh>
#include <dune/multiscale/problems/base.hh>
#include <dune/multiscale/common/grid_creation.hh>
#include <dune/multiscale/msfem/msfem_solver.hh>
#include <dune/multiscale/msfem/fem_solver.hh>
#include <dune/multiscale/msfem/msfem_traits.hh>
#include <dune/multiscale/common/heterogenous.hh>

#if DUNE_MULTISCALE_WITH_DUNE_FEM
#include <dune/fem/mpimanager.hh>
#endif

#include <dune/xt/common/ranges.hh>
#include <dune/stuff/grid/walker/functors.hh>
#include <dune/stuff/grid/walker/apply-on.hh>
#include <dune/xt/common/timings.hh>
#include <dune/xt/common/logging.hh>
#include <dune/xt/common/configuration.hh>
#include <dune/xt/common/signals.hh>
#include <dune/xt/common/memory.hh>

#include <dune/gdt/products/h1.hh>
#include <dune/gdt/products/weightedl2.hh>
#include <dune/gdt/products/boundaryl2.hh>

#include <boost/filesystem.hpp>
#include <tbb/task_scheduler_init.h>

double surface_flow_gdt(const Dune::Multiscale::CommonTraits::GridType &grid,
                    const Dune::Multiscale::CommonTraits::ConstDiscreteFunctionType& solution,
                        const DMP::ProblemContainer& problem) {
  using namespace Dune::Multiscale;
  const auto gv = grid.leafGridView();

  // Constants and types
  constexpr auto dim = CommonTraits::world_dim;
  typedef double REAL; //TODO read from input
  typedef typename Dune::FieldVector<REAL,dim> FV;   // point on cell
  typedef typename Dune::FieldMatrix<REAL,dim,dim> FM;   // point on cell
  typedef typename Dune::FieldMatrix<REAL,1,dim> Grad;   // point on cell
  typedef typename Dune::QuadratureRule<REAL,dim-1> QR;
  typedef typename Dune::QuadratureRules<REAL,dim-1> QRS;

  const auto& diffusion = problem.getDiffusion();

  // Quadrature rule
  auto iCell = gv.template begin< 0,Dune::Interior_Partition >();
  auto iFace = gv.ibegin(*iCell);
  const QR& rule = QRS::rule(iFace->geometry().type(),2); // TODO order as para

  // Loop over cells
  REAL localFlux(0);
  for(iCell = gv.template begin< 0,Dune::Interior_Partition >();
      iCell != gv.template end< 0,Dune::Interior_Partition >(); ++iCell) {
    // Loop over interfaces
    const auto local_solution = solution.local_function(*iCell);
    for(iFace = gv.ibegin(*iCell); iFace != gv.iend(*iCell); ++iFace) {
      if(iFace->boundary() && abs(iFace->geometry().center()[0]) < 1e-10) {
        double area = iFace->geometry().volume();
        // Loop over gauss points
        for(auto iGauss = rule.begin(); iGauss != rule.end(); ++iGauss) {
          FV pos = iFace->geometry().global(iGauss->position());
          Grad grad;
          FM diff;
          diffusion.evaluate(pos, diff);
          local_solution->jacobian(pos, grad);
          localFlux -= iGauss->weight() * area * diff[0][0] * grad[0][0];
        }
      }
    }
  }
  localFlux = grid.comm().sum(localFlux);
  return localFlux;
}

void MultiLevelMonteCarlo::MsCgFemDifference::init(Dune::MPIHelper::MPICommunicator global, Dune::MPIHelper::MPICommunicator local) {
  // inits perm field only, no create()
  local_comm_ = local;
  if(init_called_)
    return;

  problem_ = Dune::XT::Common::make_unique<DMP::ProblemContainer>(global, local, DXTC_CONFIG);
  assert(problem_);
  init_called_ = true;
}

double MultiLevelMonteCarlo::MsCgFemDifference::compute_inflow_difference(const Dune::Multiscale::CommonTraits::GridType& coarse_grid,
                                                                          Dune::Multiscale::LocalsolutionProxy &msfem_solution,
                                                                          const std::shared_ptr<Dune::Multiscale::CommonTraits::GridType> fine_grid,
                                                                          const Dune::Multiscale::CommonTraits::ConstDiscreteFunctionType* fine_function) {
  using namespace Dune;
  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                         coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));
  Multiscale::CommonTraits::DiscreteFunctionType projected_msfem_solution(
        coarse_space, "MsFEM_Solution");
  Multiscale::MsFEMProjection::project(
        msfem_solution, projected_msfem_solution);
  const auto coarse_flow = surface_flow_gdt(coarse_grid, projected_msfem_solution, *problem_);

  if(fine_function && fine_grid) {
    const auto fine_flow = surface_flow_gdt(*fine_grid, *fine_function, *problem_);

    //fine_function->visualize("fine_sol");
    //projected_msfem_solution.visualize("proj_msfem_sol");

    return fine_flow - coarse_flow;
  }

  return coarse_flow;
}

double MultiLevelMonteCarlo::MsCgFemDifference::eval() {
  using namespace Dune;
  Dune::XT::Common::OutputScopedTiming tm("mlmc.difference_cg-msfem", DXTC_LOG_INFO_0);
  assert(init_called_);
  assert(problem_);
  auto coarse_grid = Multiscale::make_coarse_grid(*problem_, local_comm_);
  // create() new perm field
  problem_->getMutableModelData().prepare_new_evaluation(*problem_);
  auto fine_grid = Multiscale::make_fine_grid(*problem_, coarse_grid, true,local_comm_);

  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                           *coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));

  std::unique_ptr<Multiscale::LocalsolutionProxy> msfem_solution(nullptr);

  Multiscale::LocalGridList localgrid_list(*problem_, coarse_space);
  Dune::XT::Common::timings().start("mlmc.difference_cg-msfem.msfem-solve");
  Multiscale::Elliptic_MsFEM_Solver().apply(*problem_, coarse_space, msfem_solution,
                                            localgrid_list);
  Dune::XT::Common::timings().stop("mlmc.difference_cg-msfem.msfem-solve");

  Dune::XT::Common::timings().start("mlmc.difference_cg-msfem.cgfem-solve");
  Multiscale::Elliptic_FEM_Solver fem(*problem_, fine_grid);
  const auto &fine_fem_solution = fem.solve();
  Dune::XT::Common::timings().stop("mlmc.difference_cg-msfem.cgfem-solve");

  Dune::XT::Common::OutputScopedTiming tmd("mlmc.difference_cg-msfem.compute_inflow_difference", DXTC_LOG_INFO_0);
  return compute_inflow_difference(*coarse_grid, *msfem_solution,
                                   fine_grid, &fine_fem_solution);
}

double MultiLevelMonteCarlo::MsFemSingleDifference::eval() {
  using namespace Dune;
  Dune::XT::Common::OutputScopedTiming tm("mlmc.single_msfem", DXTC_LOG_INFO_0);
  assert(problem_);
//  assert(init_called_);
  auto coarse_grid = Multiscale::make_coarse_grid(*problem_, local_comm_);
  // create() new perm field
  problem_->getMutableModelData().prepare_new_evaluation(*problem_);
  typedef Multiscale::CommonTraits::SpaceChooserType::PartViewType
      PartViewType;
  const Multiscale::CommonTraits::SpaceType coarse_space(PartViewType::create(
                                                           *coarse_grid, Multiscale::CommonTraits::st_gdt_grid_level));
  std::unique_ptr<Multiscale::LocalsolutionProxy> msfem_solution(nullptr);

  Multiscale::LocalGridList localgrid_list(*problem_, coarse_space);
  Dune::XT::Common::timings().start("mlmc.single_msfem.msfem-solve");
  Multiscale::Elliptic_MsFEM_Solver().apply(*problem_, coarse_space, msfem_solution,
                                            localgrid_list);
  Dune::XT::Common::timings().stop("mlmc.single_msfem.msfem-solve");

  Dune::XT::Common::OutputScopedTiming tmd("mlmc.single_msfem.compute_inflow_difference", DXTC_LOG_INFO_0);
  return compute_inflow_difference(*coarse_grid, *msfem_solution);
}

//! workaround for https://github.com/wwu-numerik/dune-stuff/issues/42
void set_config_values(const std::vector<std::string> &keys,
                       const std::vector<std::string> &values) {
  assert(keys.size() == values.size());
  for (const auto i : Dune::XT::Common::value_range(keys.size()))
    DXTC_CONFIG.set(keys[i], values[i], true);

  // should just be
  // Dune::XT::Common::Config().add(Dune::XT::Common::Configuration(keys, values), "", true);
}

void handle_sigterm(int signal) {
  DXTC_TIMINGS.stop();
  DXTC_TIMINGS.output_per_rank("profiler");
  std::exit(signal);
}

void MultiLevelMonteCarlo::msfem_init(int argc, char **argv) {
  using namespace std;
#if DUNE_MULTISCALE_WITH_DUNE_FEM
  Dune::Fem::MPIManager::initialize(argc, argv);
#endif
  auto &helper = Dune::MPIHelper::instance(argc, argv);
  if (helper.size() > 1 &&
      !(Dune::Capabilities::isParallel<
        Dune::Multiscale::CommonTraits::GridType>::v)) {
    DUNE_THROW(Dune::InvalidStateException,
               "mpi enabled + serial grid = bad idea");
  }

  // config defaults defaults
  const vector<string> keys{"grids.macro_cells_per_dim",
                            "grids.micro_cells_per_macrocell_dim",
                            "msfem.oversampling_layers", "problem.name"};
  const vector<string> values{"8", "12", "1", "Random"};
  set_config_values(keys, values);

  //if (argc > 1 && boost::filesystem::is_regular_file(argv[1]))
  //  Dune::XT::Common::Config().read_command_line(argc, argv);

  if (argc > 1 ) {
      Dune::XT::Common::Config().read_command_line(argc, argv);
      Dune::Stuff::Common::Config().read_command_line(argc, argv);
  }
  Dune::XT::Common::test_create_directory(DXTC_CONFIG_GET("global.datadir", "data/"));

  // LOG_NONE = 1, LOG_ERROR = 2, LOG_INFO = 4,LOG_DEBUG = 8,LOG_CONSOLE =
  // 16,LOG_FILE = 32
  // --> LOG_ERROR | LOG_INFO | LOG_DEBUG | LOG_CONSOLE | LOG_FILE = 62
  Dune::XT::Common::Logger().create(DXTC_CONFIG_GET("logging.level", 62),
                       DXTC_CONFIG_GET("logging.file", std::string(argv[0]) + ".log"),
                       DXTC_CONFIG_GET("global.datadir", "data"),
                       DXTC_CONFIG_GET("logging.dir", "log" /*path below datadir*/));
  DXTC_TIMINGS.set_outputdir(DXTC_CONFIG_GET("global.datadir", "data"));
  Dune::XT::Common::install_signal_handler(SIGTERM, handle_sigterm);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>

//[runtime_conversion_factor_snippet_1

using boost::units::base_dimension;
using boost::units::base_unit;

static const long currency_base = 1;

struct currency_base_dimension : base_dimension<currency_base_dimension, 1> {};

typedef currency_base_dimension::dimension_type currency_type;

template<long N>
struct currency_base_unit :
    base_unit<currency_base_unit<N>, currency_type, currency_base + N> {};

typedef currency_base_unit<0> us_dollar_base_unit;
typedef currency_base_unit<1> euro_base_unit;

typedef us_dollar_base_unit::unit_type us_dollar;
typedef euro_base_unit::unit_type euro;

// an array of all possible conversions
double conversion_factors[2][2] = {
    {1.0, 1.0},
    {1.0, 1.0}
};

double get_conversion_factor(long from, long to) {
    return(conversion_factors[from][to]);
}

void set_conversion_factor(long from, long to, double value) {
    conversion_factors[from][to] = value;
    conversion_factors[to][from] = 1.0 / value;
}

BOOST_UNITS_DEFINE_CONVERSION_FACTOR_TEMPLATE((long N1)(long N2),
    currency_base_unit<N1>,
    currency_base_unit<N2>,
    double, get_conversion_factor(N1, N2));

//]

int main() {
    boost::units::quantity<us_dollar> dollars = 2.00 * us_dollar();
    boost::units::quantity<euro> euros(dollars);
    set_conversion_factor(0, 1, 2.0);
    dollars = static_cast<boost::units::quantity<us_dollar> >(euros);
    set_conversion_factor(0, 1, .5);
    euros = static_cast<boost::units::quantity<euro> >(dollars);
    double value = euros.value(); // = .5
    if(value != .5) {
        return(1);
    } else {
        return(0);
    }
}


#include <boost/cstdint.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <nt2/sdk/aligned/is_power_of_2.hpp>

using nt2::meta::is_power_of_2;

int main()
{
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<2> >::type ));
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<4> >::type ));
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<8> >::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2< boost::mpl::int_<0> >::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2< boost::mpl::int_<10> >::type ));
}


#include <mi/Kdtree.hpp>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
int main ( int argc, char** argv )
{

        std::vector<Eigen::Vector3d> p;
        for( int i = 0 ; i < 10000 ; i++ ) {
                double x = ( rand() % 2001 - 1000 ) * 0.001;
                double y = ( rand() % 2001 - 1000 ) * 0.001;
                double z = ( rand() % 2001 - 1000 ) * 0.001;
                p.push_back( Eigen::Vector3d( x,y,z ) );
        }

        mi::Kdtree<Eigen::Vector3d> kdtree( p ) ;

        Eigen::Vector3d v( 0.1, 0.1, 0.1 ) ;
        std::list<Eigen::Vector3d> result;

        kdtree.find( v, 0.1, result, true );
        for( std::list<Eigen::Vector3d>::iterator iter = result.begin(); iter != result.end() ; ++iter ) {
                Eigen::Vector3d& v0 = *iter;
                Eigen::Vector3d d = *iter - v;
                double sqrtDist = d.x()*d.x()+d.y()*d.y()+d.z()*d.z();
                std::cerr<<v0.x()<<" "<<v0.y()<<" "<<v0.z()<<" "<<sqrtDist<<std::endl;
        }
        return 0;
}


#ifndef RKTK_OPTIMIZER_DRIVER_HPP_INCLUDED
#define RKTK_OPTIMIZER_DRIVER_HPP_INCLUDED

// C++ standard library headers
#include <algorithm> // for std::generate
#include <array>
#include <chrono>
#include <cstdint> // for std::uint64_t
#include <cstdlib>    // for std::exit
#include <fstream> // for std::ifstream, std::ofstream
#include <functional> // for std::ref
#include <iomanip> // for std::setw, std::setfill
#include <ios> // for std::dec, std::hex
#include <iostream> // for std::cout
#include <limits> // for std::numeric_limits
#include <random> // for std::mt19937 et al.
#include <sstream> // for std::istringstream and std::ostringstream
#include <string>

// Eigen linear algebra library headers
#include <Eigen/Core>

// Project-specific headers
#include "OrderConditionEvaluator.hpp"
#include <dznl/BFGSOptimizer.hpp>

namespace rktk {

    template <typename T>
    class OptimizerDriver {

    private: // ========================================== INTERNAL TYPE ALIASES

        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXT;

        typedef std::mt19937 random_generator_t;

        typedef std::array<
                random_generator_t::result_type,
                random_generator_t::state_size> random_state_t;

    private: // =============================================== MEMBER VARIABLES

        const std::size_t num_vars;
        OrderConditionEvaluator<T> evaluator;
        const std::function<T(const T *)> stdfun;
        const std::function<void(T *, const T *)> stdgrad;
        dznl::BFGSOptimizer<T> optimizer;
        std::uint64_t uuid[5];
        random_generator_t prng;

    private: // ===================================== CONSTRUCTOR HELPER METHODS

        static random_generator_t properly_seeded_random_generator() {
            random_state_t seed_data;
            std::random_device nondet_random_source;
            std::generate(seed_data.begin(), seed_data.end(),
                          std::ref(nondet_random_source));
            seed_data[0] = static_cast<random_generator_t::result_type>(
                    std::chrono::duration_cast<std::chrono::nanoseconds>(
                            std::chrono::system_clock::now().time_since_epoch()
                    ).count() % std::numeric_limits<
                            random_generator_t::result_type>::max());
            std::seed_seq seed_sequence(seed_data.begin(), seed_data.end());
            random_generator_t random_generator(seed_sequence);
            return random_generator;
        }

        void randomize_uuid() {
            std::uniform_int_distribution<std::uint64_t> unif;
            uuid[0] = unif(prng) & 0xFFFFFFFF;
            uuid[1] = unif(prng) & 0xFFFF;
            uuid[2] = unif(prng) & 0xFFFF;
            uuid[3] = unif(prng) & 0xFFFF;
            uuid[4] = unif(prng) & 0xFFFFFFFFFFFF;
        }

    public: // ===================================================== CONSTRUCTOR

        OptimizerDriver(int order, std::size_t num_stages)
            : num_vars(num_stages * (num_stages + 1) / 2),
              evaluator(order, num_stages),
              stdfun([&](const T *x) {
                  return evaluator.objective_function(x);
              }),
              stdgrad([&](T *g, const T *x) {
                  evaluator.objective_gradient(g, x);
              }),
              optimizer(num_vars, stdfun, stdgrad),
              prng(properly_seeded_random_generator()) {
            randomize_uuid();
        }

    public: // ==================================================== INITIALIZERS

        void initialize_random() {
            std::uniform_real_distribution<double> unif(0.0, 1.0);
            VectorXT x(num_vars);
            for (std::size_t i = 0; i < num_vars; ++i) {
                x[i] = static_cast<T>(unif(prng));
            }
            optimizer.set_current_point(x);
            optimizer.set_iteration_count(0);
            randomize_uuid();
        }

        void initialize_from_file(const std::string &filename) {
            std::cout << "Opening input file \""
                      << filename << "\"..." << std::endl;
            std::ifstream input_file(filename);
            if (!input_file.good()) {
                std::cout << "ERROR: could not open input file." << std::endl;
                std::exit(EXIT_FAILURE);
            }
            std::cout << "Successfully opened input file. Reading..."
                      << std::endl;
            VectorXT x(num_vars);
            for (std::size_t i = 0; i < num_vars; ++i) {
                input_file >> x[i];
                if (input_file.fail()) {
                    std::cout << "ERROR: Could not read input file entry "
                                 "at index " << i << "." << std::endl;
                    std::exit(EXIT_FAILURE);
                }
            }
            std::cout << "Successfully read input file." << std::endl;
            optimizer.set_current_point(x);
            if (is_rktk_filename(filename)) {
                optimizer.set_iteration_count(
                        dec_substr_to_uint(filename, 52, 64));
                uuid[0] = hex_substr_to_uint(filename, 15, 23);
                uuid[1] = hex_substr_to_uint(filename, 24, 28);
                uuid[2] = hex_substr_to_uint(filename, 29, 33);
                uuid[3] = hex_substr_to_uint(filename, 34, 38);
                uuid[4] = hex_substr_to_uint(filename, 39, 51);
            } else {
                optimizer.set_iteration_count(0);
                randomize_uuid();
            }
        }

    public: // ======================================================= ACCESSORS

        std::size_t get_iteration_count() {
            return optimizer.get_iteration_count();
        }

    public: // ======================================================== MUTATORS

        void set_step_size(const T &h) { optimizer.set_step_size(h); }

    public: // ============================================ OPTIMIZATION METHODS

        bool step() {
            bool result = optimizer.step();
            if (!result) {
                std::cout << "NOTICE: Optimal step size reduced to zero. "
                          << "BFGS iteration has converged "
                          << "to the requested precision." << std::endl;
            }
            return result;
        }

    public: // ========================================== CONSOLE OUTPUT METHODS

        void print(int print_precision) {
            std::ios_base::fmtflags old_flags = std::cout.flags();
            std::cout << std::setfill('0') << std::setw(12)
                      << optimizer.get_iteration_count();
            std::cout << std::showpos << std::scientific;
            std::cout << std::setprecision(
                    print_precision > 0 ? print_precision
                                        : std::numeric_limits<T>::max_digits10);
            std::cout << " | " << optimizer.get_current_objective_value()
                      << " | " << optimizer.get_current_gradient().norm()
                      << " | " << optimizer.get_last_step_size()
                      << " | " << optimizer.get_current_point().norm()
                      << " | ";
            switch (optimizer.get_last_step_type()) {
                case dznl::StepType::NONE:
                    std::cout << "NONE";
                    break;
                case dznl::StepType::GRAD:
                    std::cout << "GRAD";
                    break;
                case dznl::StepType::BFGS:
                    std::cout << "BFGS";
                    break;
            }
            std::cout << std::endl;
            std::cout.flags(old_flags);
        }

    public: // ============================================= FILE OUTPUT METHODS

        std::string current_filename() {
            using std::log10;
            auto f_score = static_cast<int>(T(-100) * log10(
                    optimizer.get_current_objective_value()));
            if (f_score < 0) { f_score = 0; }
            if (f_score > 9999) { f_score = 9999; }
            auto g_score = static_cast<int>(T(-100) * log10(
                    optimizer.get_current_gradient().norm()));
            if (g_score < 0) { g_score = 0; }
            if (g_score > 9999) { g_score = 9999; }
            std::ostringstream filename;
            filename << std::setfill('0') << std::dec;
            filename << std::setw(4) << f_score << '-';
            filename << std::setw(4) << g_score << "-RKTK-";
            filename << std::hex << std::uppercase;
            filename << std::setw(8) << uuid[0] << '-';
            filename << std::setw(4) << uuid[1] << '-';
            filename << std::setw(4) << uuid[2] << '-';
            filename << std::setw(4) << uuid[3] << '-';
            filename << std::setw(12) << uuid[4] << '-';
            filename << std::dec;
            filename << std::setw(12)
                     << optimizer.get_iteration_count() << ".txt";
            return filename.str();
        }

        void write_to_file(std::string filename = "") {
            if (filename.empty()) { filename = current_filename(); }
            std::ofstream output_file(filename);
            output_file << std::showpos << std::scientific << std::setprecision(
                    std::numeric_limits<T>::max_digits10);
            const VectorXT &x = optimizer.get_current_point();
            for (std::size_t i = 0; i < num_vars; ++i) {
                output_file << x[i] << '\n';
            }
            output_file << '\n';
            output_file << "Objective function value: "
                        << optimizer.get_current_objective_value() << '\n';
            output_file << "Objective gradient norm:  "
                        << optimizer.get_current_gradient().norm() << '\n';
            output_file << "Most recent step size:    "
                        << optimizer.get_last_step_size() << '\n';
            output_file << "Distance from origin:     "
                        << optimizer.get_current_point().norm() << '\n';
        }

    private: //========================== STATIC FILENAME PARSING HELPER METHODS

        static bool is_dec_digit(char c) {
            return ('0' <= c) && (c <= '9');
        }

        static bool is_hex_digit(char c) {
            return (('0' <= c) && (c <= '9'))
                   || (('a' <= c) && (c <= 'f'))
                   || (('A' <= c) && (c <= 'F'));
        }

        static bool is_dec_substr(const std::string &str,
                                  std::string::size_type begin,
                                  std::string::size_type end) {
            for (auto i = begin; i < end; ++i) {
                if (!is_dec_digit(str[i])) { return false; }
            }
            return true;
        }

        static bool is_hex_substr(const std::string &str,
                                  std::string::size_type begin,
                                  std::string::size_type end) {
            for (auto i = begin; i < end; ++i) {
                if (!is_hex_digit(str[i])) { return false; }
            }
            return true;
        }

    private: // ================================ STATIC FILENAME PARSING METHODS

        static bool is_rktk_filename(const std::string &filename) {
            if (filename.size() != 68) { return false; }
            if (!is_dec_substr(filename, 0, 4)) { return false; }
            if (filename[4] != '-') { return false; }
            if (!is_dec_substr(filename, 5, 9)) { return false; }
            if (filename[9] != '-') { return false; }
            if (filename[10] != 'R') { return false; }
            if (filename[11] != 'K') { return false; }
            if (filename[12] != 'T') { return false; }
            if (filename[13] != 'K') { return false; }
            if (filename[14] != '-') { return false; }
            if (!is_hex_substr(filename, 15, 23)) { return false; }
            if (filename[23] != '-') { return false; }
            if (!is_hex_substr(filename, 24, 28)) { return false; }
            if (filename[28] != '-') { return false; }
            if (!is_hex_substr(filename, 29, 33)) { return false; }
            if (filename[33] != '-') { return false; }
            if (!is_hex_substr(filename, 34, 38)) { return false; }
            if (filename[38] != '-') { return false; }
            if (!is_hex_substr(filename, 39, 51)) { return false; }
            if (filename[51] != '-') { return false; }
            if (!is_dec_substr(filename, 52, 64)) { return false; }
            if (filename[64] != '.') { return false; }
            if (filename[65] != 't') { return false; }
            if (filename[66] != 'x') { return false; }
            return (filename[67] == 't');
        }

        static std::uint64_t dec_substr_to_uint(const std::string &str,
                                                std::string::size_type begin,
                                                std::string::size_type end) {
            std::istringstream substr_stream(str.substr(begin, end - begin));
            std::uint64_t result;
            substr_stream >> std::dec >> result;
            return result;
        }

        static std::uint64_t hex_substr_to_uint(const std::string &str,
                                                std::string::size_type begin,
                                                std::string::size_type end) {
            std::istringstream substr_stream(str.substr(begin, end - begin));
            std::uint64_t result;
            substr_stream >> std::hex >> result;
            return result;
        }

    }; // class OptimizerDriver

} // namespace rktk

#endif // RKTK_OPTIMIZER_DRIVER_HPP_INCLUDED


/*
 * @Description:
 * @Author: Ren Qian
 * @Date: 2020-03-01 18:35:19
 */

#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_

#include <string>
#include <deque>
#include <Eigen/Dense>

namespace lidar_localization {
class InterfaceGraphOptimizer {
  public:
    virtual ~InterfaceGraphOptimizer() {}
    // 优化
    virtual bool Optimize() = 0;
    // 输入、输出数据
    virtual bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) = 0;
    virtual int GetNodeNum() = 0;
    // 添加节点、边、鲁棒核
    virtual void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size) = 0;
    virtual void AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) = 0;
    virtual void AddSe3Edge(int vertex_index1,
                            int vertex_index2,
                            const Eigen::Isometry3d &relative_pose,
                            const Eigen::VectorXd noise) = 0;
    virtual void AddSe3PriorXYZEdge(int se3_vertex_index,
                                    const Eigen::Vector3d &xyz,
                                    Eigen::VectorXd noise) = 0;
    virtual void AddSe3PriorQuaternionEdge(int se3_vertex_index,
                                           const Eigen::Quaterniond &quat,
                                           Eigen::VectorXd noise) = 0;
    // 设置优化参数
    void SetMaxIterationsNum(int max_iterations_num);

  protected:
    int max_iterations_num_ = 512;
};
} // namespace lidar_localization
#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2015 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "alpha_complex_3d"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <cmath>  // float comparison
#include <limits>
#include <string>
#include <vector>
#include <random>
#include <cstddef>  // for std::size_t

#include <gudhi/Alpha_complex_3d.h>
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Unitary_tests_utils.h>
// to construct Alpha_complex from a OFF file of points
#include <gudhi/Points_3D_off_io.h>

#include <CGAL/Random.h>
#include <CGAL/point_generators_3.h>

using Fast_periodic_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, false, true>;
using Safe_periodic_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, false, true>;
using Exact_periodic_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, false, true>;

#ifdef GUDHI_DEBUG
typedef boost::mpl::list<Fast_periodic_alpha_complex_3d, Safe_periodic_alpha_complex_3d,
                         Exact_periodic_alpha_complex_3d>
    periodic_variants_type_list;

BOOST_AUTO_TEST_CASE_TEMPLATE(Alpha_complex_periodic_throw, Periodic_alpha_complex_3d, periodic_variants_type_list) {
  std::clog << "Periodic alpha complex 3d exception throw" << std::endl;
  using Bare_point_3 = typename Periodic_alpha_complex_3d::Bare_point_3;
  std::vector<Bare_point_3> p_points;

  // Not important, this is not what we want to check
  p_points.push_back(Bare_point_3(0.0, 0.0, 0.0));

  std::clog << "Check exception throw in debug mode" << std::endl;
  // Check it throws an exception when the cuboid is not iso
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 0.9, 1., 1.),
                    std::invalid_argument);
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 1., 0.9, 1.),
                    std::invalid_argument);
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 1., 1., 0.9),
                    std::invalid_argument);
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 1.1, 1., 1.),
                    std::invalid_argument);
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 1., 1.1, 1.),
                    std::invalid_argument);
  BOOST_CHECK_THROW(Periodic_alpha_complex_3d periodic_alpha_complex(p_points, 0., 0., 0., 1., 1., 1.1),
                    std::invalid_argument);
}
#endif

BOOST_AUTO_TEST_CASE(Alpha_complex_periodic) {
  // ---------------------
  // Fast periodic version
  // ---------------------
  std::clog << "Fast periodic alpha complex 3d" << std::endl;

  using Creator = CGAL::Creator_uniform_3<double, Fast_periodic_alpha_complex_3d::Bare_point_3>;
  CGAL::Random random(7);
  CGAL::Random_points_in_cube_3<Fast_periodic_alpha_complex_3d::Bare_point_3, Creator> in_cube(1, random);
  std::vector<Fast_periodic_alpha_complex_3d::Bare_point_3> p_points;

  for (int i = 0; i < 50; i++) {
    Fast_periodic_alpha_complex_3d::Bare_point_3 p = *in_cube++;
    p_points.push_back(p);
  }

  Fast_periodic_alpha_complex_3d periodic_alpha_complex(p_points, -1., -1., -1., 1., 1., 1.);

  Gudhi::Simplex_tree<> stree;
  periodic_alpha_complex.create_complex(stree);

  for (std::size_t index = 0; index < p_points.size(); index++) {
    bool found = false;
    Fast_periodic_alpha_complex_3d::Bare_point_3 ap = periodic_alpha_complex.get_point(index);
    for (auto point : p_points) {
      if ((point.x() == ap.x()) && (point.y() == ap.y()) && (point.z() == ap.z())) {
        found = true;
        break;
      }
    }
    // Check all points from alpha complex are found in the input point cloud
    BOOST_CHECK(found);
  }
  // Exception if we go out of range
  BOOST_CHECK_THROW(periodic_alpha_complex.get_point(p_points.size()), std::out_of_range);

  // ----------------------
  // Exact periodic version
  // ----------------------
  std::clog << "Exact periodic alpha complex 3d" << std::endl;

  std::vector<Exact_periodic_alpha_complex_3d::Bare_point_3> e_p_points;

  for (auto p : p_points) {
    e_p_points.push_back(Exact_periodic_alpha_complex_3d::Bare_point_3(p[0], p[1], p[2]));
  }

  Exact_periodic_alpha_complex_3d exact_alpha_complex(e_p_points, -1., -1., -1., 1., 1., 1.);

  Gudhi::Simplex_tree<> exact_stree;
  exact_alpha_complex.create_complex(exact_stree);

  // ---------------------
  // Compare both versions
  // ---------------------
  std::clog << "Exact periodic alpha complex 3d is of dimension " << exact_stree.dimension() << " - Non exact is "
            << stree.dimension() << std::endl;
  BOOST_CHECK(exact_stree.dimension() == stree.dimension());
  std::clog << "Exact periodic alpha complex 3d num_simplices " << exact_stree.num_simplices() << " - Non exact is "
            << stree.num_simplices() << std::endl;
  BOOST_CHECK(exact_stree.num_simplices() == stree.num_simplices());
  std::clog << "Exact periodic alpha complex 3d num_vertices " << exact_stree.num_vertices() << " - Non exact is "
            << stree.num_vertices() << std::endl;
  BOOST_CHECK(exact_stree.num_vertices() == stree.num_vertices());

  // We cannot compare as objects from dispatcher on the alpha shape is not deterministic.
  // cf. https://github.com/CGAL/cgal/issues/3346
  auto sh = stree.filtration_simplex_range().begin();
  auto sh_exact = exact_stree.filtration_simplex_range().begin();

  while (sh != stree.filtration_simplex_range().end() || sh_exact != exact_stree.filtration_simplex_range().end()) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(stree.filtration(*sh), exact_stree.filtration(*sh_exact), 1e-14);

    std::vector<int> vh(stree.simplex_vertex_range(*sh).begin(), stree.simplex_vertex_range(*sh).end());
    std::vector<int> exact_vh(exact_stree.simplex_vertex_range(*sh_exact).begin(),
                              exact_stree.simplex_vertex_range(*sh_exact).end());

    BOOST_CHECK(vh.size() == exact_vh.size());
    ++sh;
    ++sh_exact;
  }

  BOOST_CHECK(sh == stree.filtration_simplex_range().end());
  BOOST_CHECK(sh_exact == exact_stree.filtration_simplex_range().end());

  // ----------------------
  // Safe periodic version
  // ----------------------
  std::clog << "Safe periodic alpha complex 3d" << std::endl;

  std::vector<Safe_periodic_alpha_complex_3d::Bare_point_3> s_p_points;

  for (auto p : p_points) {
    s_p_points.push_back(Safe_periodic_alpha_complex_3d::Bare_point_3(p[0], p[1], p[2]));
  }

  Safe_periodic_alpha_complex_3d safe_alpha_complex(s_p_points, -1., -1., -1., 1., 1., 1.);

  Gudhi::Simplex_tree<> safe_stree;
  safe_alpha_complex.create_complex(safe_stree);

  // ---------------------
  // Compare both versions
  // ---------------------
  // We cannot compare as objects from dispatcher on the alpha shape is not deterministic.
  // cf. https://github.com/CGAL/cgal/issues/3346
  sh = stree.filtration_simplex_range().begin();
  auto sh_safe = safe_stree.filtration_simplex_range().begin();

  while (sh != stree.filtration_simplex_range().end() || sh_safe != safe_stree.filtration_simplex_range().end()) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(stree.filtration(*sh), safe_stree.filtration(*sh_safe), 1e-14);

    std::vector<int> vh(stree.simplex_vertex_range(*sh).begin(), stree.simplex_vertex_range(*sh).end());
    std::vector<int> safe_vh(safe_stree.simplex_vertex_range(*sh_safe).begin(),
                             safe_stree.simplex_vertex_range(*sh_safe).end());

    BOOST_CHECK(vh.size() == safe_vh.size());
    ++sh;
    ++sh_safe;
  }

  BOOST_CHECK(sh == stree.filtration_simplex_range().end());
  BOOST_CHECK(sh_safe == safe_stree.filtration_simplex_range().end());
}


// Copyright (C) 2017 Minhyuk Sung <mhsung@cs.stanford.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
//

#include "LibiglMeshRendererT.h"

#include <iostream>
#include <Eigen/Geometry>
#include <igl/frustum.h>
#include <utils/google_tools.h>
#include <utils/utils.h>

DEFINE_double(fovy_deg, 45.0, "fovy angle degree.");
DEFINE_double(camera_distance, 3.0, "camera distance.");
DEFINE_double(point_radius, 0.01, "point radius.");
DEFINE_bool(auto_adjust_camera, true, "auto-adjust camera.");

LibiglMeshRendererT::LibiglMeshRendererT(const int _width, const int _height)
  : width_(_width),
    height_(_height),
    center_(Vector3f::Zero()),
    radius_(1.0f),
    point_radius_(FLAGS_point_radius) {
}

bool LibiglMeshRendererT::read_projection(const std::string& _filename) {
  Matrix4f projection;
  if (!Utils::read_eigen_matrix_from_file(_filename, &projection)) {
    return false;
  }
  set_projection(projection);
  return true;
}

bool LibiglMeshRendererT::read_modelview(const std::string& _filename) {
  Matrix4f modelview;
  if (!Utils::read_eigen_matrix_from_file(_filename, &modelview)) {
    return false;
  }
  set_modelview(modelview);
  return true;
}

bool LibiglMeshRendererT::write_projection(const std::string& _filename) const {
  if (!Utils::write_eigen_matrix_to_file(_filename, get_projection())) {
    return false;
  }
  return true;
}

bool LibiglMeshRendererT::write_modelview(const std::string& _filename) const {
  if (!Utils::write_eigen_matrix_to_file(_filename, get_modelview())) {
    return false;
  }
  return true;
}

void LibiglMeshRendererT::reset_projection() {
  const float aspect_ratio = (float)width_ / (float)height_;
  const float znear = 0.01f * radius_;
  const float zfar = 100.0f * radius_;
  const float ymax = znear * tanf(FLAGS_fovy_deg * M_PI / 360.0);
  const float xmax = ymax * aspect_ratio;
  Matrix4f projection;
  igl::frustum(-xmax, +xmax, -ymax, +ymax, znear, zfar, projection);
  set_projection(projection);
}

void LibiglMeshRendererT::reset_modelview() {
  Matrix4f modelview = Matrix4f::Identity();
  modelview(0, 3) = -center_[0];
  modelview(1, 3) = -center_[1];
  //modelview(2, 3) = -center_[2] - FLAGS_camera_distance * radius_;
  modelview(2, 3) = -center_[2] - FLAGS_camera_distance;
  set_modelview(modelview);
}

void LibiglMeshRendererT::set_window_size(
    const int _width, const int _height) {
	width_ = _width;
	height_ = _height;
	reset_projection();
}

void LibiglMeshRendererT::set_scene_pos(
    const Vector3f& _center, const float _radius) {
	center_ = _center;
	radius_ = _radius;
	reset_projection();
	reset_modelview();
}

Vector3f LibiglMeshRendererT::get_camera_params() const {
  const ::Matrix4f modelview = get_modelview();

  // Default rotation R_d.
  // R = R_z R_x R_y R_d.
  const AngleAxisf default_rotation(
      -0.5 * M_PI, Vector3f::UnitY());

  // R R_d^T = R_z R_x R_y.
  const Matrix3f rotation =
      modelview.block(0, 0, 3, 3) *
      default_rotation.toRotationMatrix().transpose();

  // ZXY Euler angles.
  Vector3f angles = rotation.eulerAngles(2, 0, 1);

  Vector3f camera_params;
  // Y-axis (Azimuth, [R_y | 0])
  // Note: Change the sign.
  camera_params[0] = (-angles[2] / M_PI * 180.0) + 90;

  // X-axis (Elevation, [R_x | 0])
  camera_params[1] = +angles[1] / M_PI * 180.0;

  // Z-axis (Theta, [R_z | 0])
  // Note: Change the sign.
  camera_params[2] = -angles[0] / M_PI * 180.0;

  // Elevation must be in range [-90, 90].
  while(camera_params[1] < -90.0) camera_params[1] += 360.0;

  if (camera_params[1] > 270.0) {
    camera_params[1] = 360.0 - camera_params[1];
  } else if (camera_params[1] > 90.0) {
    camera_params[1] = 180.0 - camera_params[1];
    camera_params[0] += 180.0;
    camera_params[2] += 180.0;
  }

  // Azimuth and theta must be in range [0, 360).
  while(camera_params[0] >= 360.0) camera_params[0] -= 360.0;
  while(camera_params[0] < 0.0) camera_params[0] += 360.0;

  while(camera_params[2] >= 360.0) camera_params[2] -= 360.0;
  while(camera_params[2] < 0.0) camera_params[2] += 360.0;

  return camera_params;
}

void LibiglMeshRendererT::set_camera_params(const Vector3f& _camera_params,
    const Vector3f& _center, const float _radius) {
  const float azimuth_deg = _camera_params[0] - 90;
  const float elevation_deg = _camera_params[1];
  const float theta_deg = _camera_params[2];

  Affine3f modelview(Affine3f::Identity());

  if (FLAGS_auto_adjust_camera) {
    modelview.pretranslate(-_center);
  }

  // Default transformation.
  modelview.prerotate(AngleAxisf(-0.5 * M_PI, Vector3f::UnitY()));

  // Y-axis (Azimuth)
  // Note: Change the sign.
  const double azimuth = -azimuth_deg / 180.0 * M_PI;
  modelview.prerotate(AngleAxisf(azimuth, Vector3f::UnitY()));

  // X-axis (Elevation)
  const double elevation = elevation_deg / 180.0 * M_PI;
  modelview.prerotate(AngleAxisf(elevation, Vector3f::UnitX()));

  // Translation
  if (FLAGS_auto_adjust_camera) {
    modelview.pretranslate(Vector3f(0, 0, -FLAGS_camera_distance * _radius));
  } else {
    modelview.pretranslate(Vector3f(0, 0, -FLAGS_camera_distance));
  }

  // Z-axis (Theta)
  // Note: Change the sign.
  const double theta = -theta_deg / 180.0 * M_PI;
  modelview.prerotate(AngleAxisf(theta, Vector3f::UnitZ()));

  set_modelview(modelview.matrix());
}

//=============================================================================