//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#include "visualizer.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/video.hpp>

using namespace cv;
using namespace Eigen;
using namespace std;

vector<Mat> transform_and_project(const vector<Mat>& points, const Mat& intrinsics, const Mat& pose)
{
	vector<Mat> projected_2d_points(points.size());
	transform(points.begin(), points.end(), projected_2d_points.begin(), [&pose, &intrinsics] (const auto &point)
	{
		Mat transformed = pose * point;
		Mat projected = intrinsics * transformed.rowRange(0, 3);
		projected /= projected.at<float>(2);
		return projected;
	});

	return projected_2d_points;
}

void draw_origin_axis(Mat &colored_img, const Mat& intrinsics, const Mat& pose, float basis_length = 0.1) {
	vector<Mat> axes_endpoints;
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, 0, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << basis_length, 0, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, basis_length, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, 0, basis_length, 1));

	// transform cube into camera reference frame

	vector<Mat> p2d = transform_and_project(axes_endpoints, intrinsics, pose);

	Point p0 = Point(p2d[0].at<float>(0), p2d[0].at<float>(1));
	Point px = Point(p2d[1].at<float>(0), p2d[1].at<float>(1));
	Point py = Point(p2d[2].at<float>(0), p2d[2].at<float>(1));
	Point pz = Point(p2d[3].at<float>(0), p2d[3].at<float>(1));

	line(colored_img, p0, px, cv::Scalar(0, 0, 255), 1, 8);
	line(colored_img, p0, py, cv::Scalar(0, 255, 0), 1, 8);
	line(colored_img, p0, pz, cv::Scalar(255, 0, 0), 1, 8);

	circle(colored_img, p0, 200 / 32.0, cv::Scalar(255, 255, 0), -1, 8);
}

void visualize_volume(const Mat &source_image, const Mat &intrinsics, const Mat &rotation, const Mat &translation, const Mat& llc, const Mat& upc)
{
	Mat image = source_image.clone();

	Mat pose = Mat::eye(4, 4, CV_32F);
	rotation.copyTo(pose.colRange(0, 3).rowRange(0, 3));
	translation.copyTo(pose.col(3).rowRange(0, 3));

	Point3f llc_p(llc);
	Point3f upc_p(upc);

	// Points in the world coordinates
	vector<Mat> volume_points;
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, llc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, llc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, llc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, llc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, upc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, upc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, upc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, upc_p.y, llc_p.z, 1));

	// transform cube into camera reference frame
	vector<Mat> p2d = transform_and_project(volume_points, intrinsics, pose);

	// Draw cube
	Scalar line_color = Scalar(148, 24, 248);
	// Generate edge points
	vector<pair<int, int>> edge_point_indices;
	edge_point_indices.push_back(make_pair(1, 5));
	edge_point_indices.push_back(make_pair(2, 6));
	edge_point_indices.push_back(make_pair(3, 7));
	edge_point_indices.push_back(make_pair(0, 4));

	edge_point_indices.push_back(make_pair(0, 1));
	edge_point_indices.push_back(make_pair(1, 2));
	edge_point_indices.push_back(make_pair(2, 3));
	edge_point_indices.push_back(make_pair(3, 0));

	edge_point_indices.push_back(make_pair(4, 7));
	edge_point_indices.push_back(make_pair(7, 6));
	edge_point_indices.push_back(make_pair(6, 5));
	edge_point_indices.push_back(make_pair(5, 4));

	// Draw edges

	for (const auto &point_indices : edge_point_indices)
	{
		int p0_i = point_indices.first;
		Point p0 = Point(p2d[p0_i].at<float>(0), p2d[p0_i].at<float>(1));
		int p1_i = point_indices.second;
		Point p1 = Point(p2d[p1_i].at<float>(0), p2d[p1_i].at<float>(1));
		line(image, p0, p1, line_color, 1, 8);

	}

	// Draw circles
	for (const auto& point : p2d)
	{
		Point point_to_draw = Point(point.at<float>(0), point.at<float>(1));
		circle(image, point_to_draw, 200 / 32.0, line_color, -1, 8);
	}

	draw_origin_axis(image, intrinsics, pose);

	Mat image_to_show;

	// resize if too big for showing
	if (image.cols > 800)
	{
		Size show_size = Size(image.cols / 2, image.rows / 2);
		resize(image, image_to_show, show_size);
	} else
	{
		image_to_show = image;
	}

	imshow("image", image_to_show);
}

void visualize_volume(const Mat& input_image, const Matrix3f& intrinsics, const Isometry3f& pose, const vector<float>& volume)
{
	Mat intrinsics_cv;

	eigen2cv(intrinsics, intrinsics_cv);

	Mat llc = (Mat_<float>(3, 1) << volume[0], volume[2], volume[4]);
	Mat upc = (Mat_<float>(3, 1) << volume[1], volume[3], volume[5]);

	const Matrix4f &pose_eigen = pose.matrix();
	Mat pose_cv;
	eigen2cv(pose_eigen, pose_cv);

	Mat rotation = pose_cv(Range(0, 3), Range(0, 3));
	Mat translation = pose_cv(Range(0, 3), Range(3, 4));

	visualize_volume(input_image, intrinsics_cv, rotation, translation, llc, upc);
}


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

/* Author: Leonard Bruns */

#include "ompl/base/samplers/deterministic/HaltonSequence.h"
#include "ompl/util/Console.h"
#include <iostream>
#include <cmath>
#include <map>
#include <boost/math/special_functions/prime.hpp>

namespace ompl
{
    namespace base
    {
        HaltonSequence1D::HaltonSequence1D() : i_(1), base_(2)
        {
        }

        HaltonSequence1D::HaltonSequence1D(unsigned int base) : i_(1), base_(base)
        {
        }

        void HaltonSequence1D::setBase(unsigned int base)
        {
            base_ = base;
        }

        double HaltonSequence1D::sample()
        {
            double f = 1, r = 0;
            unsigned int i = i_;

            while (i > 0)
            {
                f /= base_;
                r += f * (i % base_);
                i = std::floor(i / base_);
            }

            ++i_;
            return r;
        }

        HaltonSequence::HaltonSequence(unsigned int dimensions)
          : DeterministicSequence(dimensions), halton_sequences_1d_(dimensions)
        {
            setBasesToPrimes();
        }

        HaltonSequence::HaltonSequence(unsigned int dimensions, std::vector<unsigned int> bases)
          : DeterministicSequence(dimensions), halton_sequences_1d_(dimensions)
        {
            if (bases.size() != dimensions)
            {
                OMPL_WARN("Number of bases does not match dimensions. Using first n primes instead.");
            }
            else
            {
                int i = 0;
                for (auto base : bases)
                {
                    halton_sequences_1d_[i].setBase(base);
                    i++;
                }
            }
        }

        std::vector<double> HaltonSequence::sample()
        {
            std::vector<double> samples;
            for (auto &seq : halton_sequences_1d_)
            {
                samples.push_back(seq.sample());
            }
            return samples;
        }

        void HaltonSequence::setBasesToPrimes()
        {
            // set the base of the halton sequences to the first n prime numbers, where n is dimensions
            unsigned int current = 2;
            for (unsigned int i = 0; i < dimensions_; i++)
            {
                current = boost::math::prime(i);
                halton_sequences_1d_[i].setBase(current);
            }
        }
    }  // namespace base
}  // namespace ompl


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/linalg.h>
#include <Eigen/Eigenvalues>
#include <gauss/internal/scopedHostPtr.h>
#include <tuple>

af::array gauss::linalg::eigvalsh(const af::array &m) {
    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();
        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigvalsh(m.as(af::dtype::f64));

    return eigvalsh(m.as(af::dtype::f32));
}

af::array gauss::linalg::eigvals(const af::array &m) {
    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();
        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);

        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigvals(m.as(af::dtype::f64));

    return eigvals(m.as(af::dtype::f32));
}

std::tuple<af::array, af::array> gauss::linalg::eig(const af::array &m) {

    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();

        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcd> solver(mat);

        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXd> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXf> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eig(m.as(af::dtype::f64));

    return eig(m.as(af::dtype::f32));
}

std::tuple<af::array, af::array> gauss::linalg::eigh(const af::array &m) {

    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcf> solver(mat);
        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());

        auto eivect = solver.eigenvectors();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);

        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();

        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> solver(mat);

        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());

        auto eivect = solver.eigenvectors();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);

        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat);
        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());
        auto eigenVectors = af::array(m.dims(0), m.dims(1), solver.eigenvectors().data());
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(mat);

        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());
        auto eigenVectors = af::array(m.dims(0), m.dims(1), solver.eigenvectors().data());
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigh(m.as(af::dtype::f64));

    return eigh(m.as(af::dtype::f32));
}

af::array gauss::linalg::lls(const af::array &A, const af::array &b) {
    af::array U;
    af::array S;
    af::array VT;

    af::svd(U, S, VT, A);

    S = af::diag(S, 0, false);

    af::array S_dagger = (S != 0).as(S.type()) * af::inverse(S);

    long missingRows = static_cast<long>(A.dims(1) - S.dims(0));
    long missingColumns = static_cast<long>(A.dims(0) - S.dims(0));
    af::array toPadRows = af::constant(0, missingRows, A.dims(0), A.type());
    af::array toPadColumns = af::constant(0, A.dims(1), missingColumns, A.type());

    S_dagger = af::join(0, S_dagger, toPadRows);
    S_dagger = af::join(1, S_dagger, toPadColumns);

    af::array V = af::transpose(VT);
    af::array UT = af::transpose(U);
    af::array x = af::matmul(V, S_dagger, UT, b);

    return x;
}

af::array gauss::linalg::levinsonDurbin(af::array acv, int order) {
    af::array result = af::constant(0, order + 1, acv.dims(1), acv.type());

    for (int i = 0; i < acv.dims(1); i++) {
        af::array phi = af::constant(0, order + 1, order + 1, acv.type());
        af::array sig = af::constant(0, order + 1, acv.type());

        phi(1, 1) = acv(1, i) / acv(0, i);
        sig(1) = acv(0, i) - (phi(1, 1) * acv(1, i));

        // First iteration, to avoid problems with negative sequences with 1 element
        int k = 2;
        if (k < (order + 1)) {
            phi(k, k) = (acv(k, i) - af::dot(phi(af::seq(1, k - 1), k - 1), acv(af::seq(1, k - 1), i))) / sig(k - 1);
            for (int j = 1; j < k; j++) {
                phi(j, k) = phi(j, k - 1) - (phi(k, k) * phi(k - j, k - 1));
            }
            sig(k) = sig(k - 1) * (1.0 - phi(k, k) * phi(k, k));
        }

        // Second and subsequent iterations
        for (int l = 3; l < (order + 1); l++) {
            af::array aux = acv(af::seq(1, l - 1), i);
            phi(l, l) = (acv(l, i) - af::dot(phi(af::seq(1, l - 1), l - 1),
                                             aux(af::seq(static_cast<double>(aux.dims(0)) - 1, 0, -1)))) /
                        sig(l - 1);
            for (int j = 1; j < l; j++) {
                phi(j, l) = phi(j, l - 1) - (phi(l, l) * phi(l - j, l - 1));
            }
            sig(l) = sig(l - 1) * (1.0 - phi(l, l) * phi(l, l));
        }

        af::array pac = af::diag(phi);
        pac(0) = 1.0;
        result(af::span, i) = pac;
    }
    return result;
}


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>
#include <vector>

using namespace std;

void vector2transform(vector<double> &vec, Eigen::Matrix4d &t){
    t(0,3) = vec[0];
    t(1,3) = vec[1];
    t(2,3) = vec[2];
    Eigen::Quaterniond temp;
    temp.w() = vec[3];
    temp.x() = vec[4];
    temp.y() = vec[5];
    temp.z() = vec[6];
    t.block(0,0,3,3) = temp.toRotationMatrix();
}

int main(){
    string path_in = "/home/chenchr/ttx.txt";
    fstream file_in(path_in, ios::in);
    vector<vector<double> > all;
    for(int i=0; i<10; ++i){
        double x, y, z, qw, qx, qy, qz;
        char temp;
        file_in >> x >> temp >> y >> temp >> z >> temp >> qw >> temp >> qx >> temp >> qy >> temp >> qz;
        all.push_back({x, y, z, qw, qx, qy, qz});
    }
    vector<Eigen::Matrix4d> all_T;
    for(int i=0; i<10; ++i){
        all_T.push_back(Eigen::Matrix4d::Identity());
        vector2transform(all[i], all_T[i]);
    }
    for(int i=0; i<10; ++i){
        cout << i << ": " << endl;
        for(int j=0; j<7; ++j)
            cout << all[i][j] << " ";
        cout << endl;
        cout << all_T[i] << endl;
    }
    return 0;
}


//
// Created by joshua on 9/6/17.
//
#include <mps/planner/util/Math.h>
#include <cmath>
#include <boost/math/constants/constants.hpp>

void mps::planner::util::math::normalize_orientation(double& value) {
    // from ompl::base::SO2StateSpace::enforceBounds
    double v = fmod(value, 2.0 * boost::math::constants::pi<double>());
    if (v < -boost::math::constants::pi<double>())
        v += 2.0 * boost::math::constants::pi<double>();
    else if (v >= boost::math::constants::pi<double>())
        v -= 2.0 * boost::math::constants::pi<double>();
    value = v;
}

void mps::planner::util::math::normalize_orientation(float& value) {
    float v = fmod(value, 2.0f * boost::math::constants::pi<float>());
    if (v < -boost::math::constants::pi<float>())
        v += 2.0f * boost::math::constants::pi<float>();
    else if (v >= boost::math::constants::pi<float>())
        v -= 2.0f * boost::math::constants::pi<float>();
    value = v;
}

/**
 * Returns the shortest direction from val_1 to val_2 assuming that both values
 * are angles in radian in [-pi, pi)
 * @param val_1
 * @param val_2
 * @return the smallest change in angle da such that val_1 + da = val_2 mod 2pi
 */
float mps::planner::util::math::shortest_direction_so2(float val_1, float val_2) {
    float value = val_2 - val_1;
    if (std::abs(value) > boost::math::constants::pi<float>()) {
        if (value > 0.0f) { // val_2 > val_1
            value -= 2.0f * boost::math::constants::pi<float>();
        } else {
            value += 2.0f * boost::math::constants::pi<float>();
        }
    }
    return value;
}


#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

int main() {
    mat A {{1.0, 2.0}, {3.0, 4.0}};
    A.print("A:");
    mat B = A*A.i();
    B.print("B:");
    mat C = eye<mat>(2, 2);
    C.print("C:");
    cout << "abs. sum: " <<  accu(abs(B - C)) << endl;
    return 0;
}


#include "geo_calculator.h"
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <assert.h>

#include "cgal_predef.h"

//// CGAL
// Compute OBB
#include <CGAL/Polygon_2.h>
#include <CGAL/min_quadrilateral_2.h>
// convex hull
#include <CGAL/convex_hull_2.h>
// polyline simplification
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>
// curve reconstruction from unorganized point set
#include <CGAL/Optimal_transportation_reconstruction_2.h>
// random point generator
#include <CGAL/point_generators_2.h>

double CGeoCalculator::ComputeDistFromPoint2Plane(COpenMeshT::Point p, COpenMeshT::Point b, COpenMeshT::Point n)
{
	// assert n is a unit vector
	assert((n.norm() - 1.0) < 10e-5);

	COpenMeshT::Point v = p - b;
	return innerProduct(v, n);
}

double CGeoCalculator::ComputeLinearity(std::vector<COpenMeshT::Point> p_list)
{
	if (p_list.size() < 3)
	{
		return 1.0;
	}
	else
	{
		std::vector<std::vector<double>> eigenvectors;
		std::vector<double> lambda;
		ComputePCAwithSVD(p_list, eigenvectors, lambda);
		//ComputePCAwithEIG(p_list, eigenvectors, lambda);
		return lambda[0] / (lambda[0] + lambda[1] + lambda[2]);
	}
}

double CGeoCalculator::ComputeLineLinearity(std::vector<COpenMeshT::Point> p_list, COpenMeshT::Point ctr, COpenMeshT::Point dir)
{
	double sum_dev = 0.0;
	for (int i = 0; i < p_list.size(); i++)
	{
		double dev = (p_list[i] - ctr - innerProduct(p_list[i] - ctr, dir)*dir).norm();
		sum_dev += dev;
	}
	std::cout << "center = " << ctr << std::endl;
	std::cout << "direction = " << dir << std::endl;
	std::cout << sum_dev <<" "<< p_list.size() << std::endl;
	sum_dev /= double(p_list.size());
	return sum_dev;
}

void CGeoCalculator::FittingLineSegmentToPointSet(std::vector<COpenMeshT::Point> p_list, COpenMeshT::Point & pstart, COpenMeshT::Point & pend)
{
	// base
	COpenMeshT::Point base(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		base += p_list[i];
	}
	base = base / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		p_list[i] -= base;
	}

	// get line direction
	std::vector<double> direction;
	ComputePrincipalDirection(p_list, direction);

	std::cout << "line direction: ";
	std::copy(direction.begin(), direction.end(), std::ostream_iterator<double>(std::cout, " "));
	std::cout << std::endl;

	// project every point to direction and find the max and min parameters
	double max_t = 0, min_t = 0;
	COpenMeshT::Point v(direction[0], direction[1], direction[2]);
	for (int i = 0; i < p_list.size(); i++) {
		double t = innerProduct(v, p_list[i]);
		std::cout << "t = " << t << std::endl;
		if (t > max_t)
			max_t = t;
		if (t < min_t)
			min_t = t;
	}
	std::cout << "min_t = "<< min_t << ", max_t = " << max_t << std::endl;
	// output
	pstart = base + v*min_t;
	pend = base + v*max_t;
}

void CGeoCalculator::FittingLineSegmentToPointSet(std::vector<COpenMeshT::Point> p_list,
	COpenMeshT::Point base, COpenMeshT::Point dir,
	COpenMeshT::Point & pstart, COpenMeshT::Point & pend)
{
	// base
	for (int i = 0; i < p_list.size(); i++)
	{
		p_list[i] -= base;
	}
	// project every point to direction and find the max and min parameters
	double max_t = 0, min_t = 0;
	for (int i = 0; i < p_list.size(); i++) {
		double t = innerProduct(dir, p_list[i]);
		std::cout << "t = " << t << std::endl;
		if (t > max_t)
			max_t = t;
		if (t < min_t)
			min_t = t;
	}
	std::cout << "min_t = " << min_t << ", max_t = " << max_t << std::endl;
	// output
	pstart = base + dir*min_t;
	pend = base + dir*max_t;
}

void CGeoCalculator::FittingPlaneToPointSet(std::vector<COpenMeshT::Point> p_list, std::vector<double>& transform)
{
	transform.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(P, Eigen::ComputeFullU | Eigen::ComputeFullV);
	V = svd.matrixV();

	Eigen::Vector3d n = V.col(2);
	n.normalized();
	transform.push_back(n[0]);
	transform.push_back(n[1]);
	transform.push_back(n[2]);
}

void CGeoCalculator::ComputePrincipalDirection(std::vector<COpenMeshT::Point> p_list, std::vector<double>& transform)
{
	transform.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(P, Eigen::ComputeThinU | Eigen::ComputeThinV);
	V = svd.matrixV();

	//std::cout << "P = [" << P << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	Eigen::Vector3d n = V.col(0);
	n.normalized();
	transform.push_back(n[0]);
	transform.push_back(n[1]);
	transform.push_back(n[2]);
}

void CGeoCalculator::ComputePCAwithEIG(std::vector<COpenMeshT::Point> p_list,
	std::vector<std::vector<double>>& eigenvectors, std::vector<double>& eigenvalues)
{
	eigenvectors.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);
	Eigen::MatrixXd S(Dim, Dim);

	// construct
	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::MatrixXd Pt = P.transpose();
	Eigen::MatrixXd PtP = P.transpose() * P;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(PtP);
	S = eigen_solver.eigenvalues().asDiagonal();
	V = eigen_solver.eigenvectors();

	//std::cout << "using ComputePCAwithEIG function" << std::endl;
	//std::cout << "S = [" << S << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	// make it in the descending order
	for (int i = Dim; i > 0; i--)
	{
		std::vector<double> tmp_vector;
		Eigen::Vector3d n = V.col(i-1);
		n.normalized();
		tmp_vector.push_back(n[0]);
		tmp_vector.push_back(n[1]);
		tmp_vector.push_back(n[2]);
		eigenvectors.push_back(tmp_vector);
		eigenvalues.push_back(S(i-1, i-1));
	}
}

void CGeoCalculator::ComputePCAwithSVD(std::vector<COpenMeshT::Point> p_list,
	std::vector<std::vector<double>>& eigenvectors, std::vector<double>& eigenvalues)
{
	eigenvectors.clear();
	eigenvalues.clear();

	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);
	Eigen::VectorXd S(Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}
	Eigen::MatrixXd CovP = P.transpose() * P;
	CovP = CovP / double(p_list.size());
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(CovP, Eigen::ComputeThinU | Eigen::ComputeThinV);
	V = svd.matrixV();
	S = svd.singularValues();

	//std::cout << "using ComputePCAwithSVD function" << std::endl;
	//std::cout << "S = [" << S << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	for (int i = 0; i < 3; i++)
	{
		std::vector<double> tmp_vector;
		Eigen::Vector3d n = V.col(i);
		n.normalized();
		tmp_vector.push_back(n[0]);
		tmp_vector.push_back(n[1]);
		tmp_vector.push_back(n[2]);
		eigenvectors.push_back(tmp_vector);
		double s = sqrt(S(i));
		eigenvalues.push_back(s);
	}
}

void CGeoCalculator::ProjectPointToPlane(COpenMeshT::Point point, std::vector<double> plane, COpenMeshT::Point & projection)
{
	double d = point[0] * plane[0] + point[1] * plane[1] + point[2] * plane[2] + plane[3];
	COpenMeshT::Point v(plane[0] * d, plane[1] * d, plane[2] * d);
	projection = point - v;
}

double CGeoCalculator::ComputeDistPointToLine(COpenMeshT::Point q, COpenMeshT::Point sp, COpenMeshT::Point tp)
{
	COpenMeshT::Point dir = tp - sp;
	dir /= dir.norm();
	return (q - sp - innerProduct(q - sp, dir)*dir).norm();
}

void CGeoCalculator::givensTransform(double x, double y, std::vector<double>& g)
{
	double absx = fabs(x);
	double c, s;

	if (absx == 0.0)
	{
		c = 0.0; s = 1.0;
	}
	else {
		double nrm = Eigen::Vector2d(x, y).norm();
		c = absx / nrm;
		s = x / absx*(y / nrm);
	}
	g.push_back(c);
	g.push_back(s);
}

void CGeoCalculator::computeOBB(const std::vector<COpenMeshT::Point> & pts, std::vector<double> & out_obb)
{
	typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
	typedef K::Point_2								Point_2;
	typedef CGAL::Polygon_2<K>						Polygon_2;

	std::vector<Point_2> pointset;
	for (int i = 0; i < pts.size(); i++)
	{
		pointset.push_back(Point_2(pts[i][1], pts[i][2]));
	}

	Polygon_2 ch, obb;
	CGAL::convex_hull_2(pointset.begin(), pointset.end(), std::back_inserter(ch));
	CGAL::min_rectangle_2(ch.vertices_begin(), ch.vertices_end(), std::back_inserter(obb));
	out_obb.clear();
	out_obb.push_back(obb.bbox().xmin());
	out_obb.push_back(obb.bbox().xmax());
	out_obb.push_back(obb.bbox().ymin());
	out_obb.push_back(obb.bbox().xmax());
}

std::vector<double>  CGeoCalculator::getBernsteinBasis(int n, double step, Eigen::MatrixXd & Bmat)
{
	// parameters
	std::vector<double> t;
	double it = 0.0;
	t.push_back(it);
	while (it < 1)
	{
		it += step;
		t.push_back(it);
	}
	if (fabs(t.back() - 1.0) > 0.00001)
		t.push_back(1.0);

	//for (int i = 0; i < t.size(); i++)
	//	std::cout << t[i] << std::endl;

	Bmat.resize(t.size(), n);
	for (int j = 0; j < n; j++)
	{
		for (int i = 0; i < t.size(); i++) {
			double c = CGeoCalculator::nchoosek(n - 1, j);
			Bmat(i, j) = c*pow(t[i], j)*pow((1.0-t[i]), (n - 1 - j));
		}
	}

	return t;
}

void CGeoCalculator::getBernsteinBasis(int n, std::vector<double> t, Eigen::MatrixXd & Bmat)
{
	Bmat.resize(t.size(), n);
	Bmat.setZero();
	for (int j = 0; j < n; j++)
	{
		for (int i = 0; i < t.size(); i++) {
			double c = CGeoCalculator::nchoosek(n - 1, j);
			Bmat(i, j) = c*pow(t[i], j)*pow((1.0 - t[i]), (n - 1 - j));
		}
	}
}

void CGeoCalculator::getSampleFromBezier(
	Eigen::MatrixXd & sample_pts,
	Eigen::MatrixXd & ctrl_pts,
	double step)
{
	int numCPs = ctrl_pts.rows();
	Eigen::MatrixXd Bmat;
	getBernsteinBasis(numCPs, step, Bmat);
	sample_pts = Bmat*ctrl_pts;
}

void CGeoCalculator::getEqualArcLengthSampleFromBezier(
	std::vector<COpenMeshT::Point>& om_sample_pts,
	std::vector<double>& tparas,
	const std::vector<COpenMeshT::Point>& om_ctrl_pts, int Nsamples)
{
	om_sample_pts.clear();
	tparas.clear();

	Eigen::MatrixXd ctrl_pts;
	convertOMptToEigenMat(om_ctrl_pts, ctrl_pts);

	int numCPs = ctrl_pts.rows();
	Eigen::MatrixXd Bmat;
	double dt = 0.0001;
	std::vector<double> ts;
	ts = getBernsteinBasis(numCPs, dt, Bmat);
	// get total arc length
	Eigen::MatrixXd dense_pts;
	dense_pts = Bmat*ctrl_pts;
	double total_length = 0.0;
	for (int i = 1; i < dense_pts.rows(); i++)
	{
		total_length += (dense_pts.row(i) - dense_pts.row(i - 1)).norm();
	}
	double dD = total_length / double(Nsamples);
	double accum_length = 0.0;
	std::vector<double> t_samples;
	t_samples.push_back(0);
	for (int i = 1; i < dense_pts.rows(); i++)
	{
		if (accum_length > dD) {
			//std::cout << accum_length << ", " << dD << std::endl;
			t_samples.push_back(ts[i]);
			accum_length = 0.0;
		}
		else {
			accum_length += (dense_pts.row(i) - dense_pts.row(i - 1)).norm();
		}
	}
	t_samples.push_back(1.0);
	getBernsteinBasis(numCPs, t_samples, Bmat);
	Eigen::MatrixXd sample_pts;
	sample_pts = Bmat*ctrl_pts;

	// convert sample_pts to om_pts;
	convertEigenMatToOMpt(sample_pts, om_sample_pts);
	tparas = t_samples;
}

double CGeoCalculator::nchoosek(int n, int k)
{
	if (k < 0 || k > n) {
		std::cerr << "Error: k should be less than n" << std::endl;
		return 0;
	}
	if (k == 0) {
		return 1;
	}
	return (n * nchoosek(n - 1, k - 1)) / k;
}

void CGeoCalculator::convertOMptToEigenMat(const std::vector<COpenMeshT::Point> om_pts, Eigen::MatrixXd & Mat)
{
	std::vector<double> om_pt_vec;
	for (int i = 0; i < om_pts.size(); i++)
	{
		om_pt_vec.push_back(om_pts[i][0]);
		om_pt_vec.push_back(om_pts[i][1]);
		om_pt_vec.push_back(om_pts[i][2]);
	}
	Mat = Eigen::Map<Eigen::MatrixXd, Eigen::Unaligned>
		(om_pt_vec.data(), 3, om_pt_vec.size() / 3);
	Mat.transposeInPlace();
}

void CGeoCalculator::convertEigenMatToOMpt(const Eigen::MatrixXd & Mat, std::vector<COpenMeshT::Point>& om_pts)
{
	om_pts.clear();
	for (int i = 0; i < Mat.rows(); i++)
	{
		COpenMeshT::Point pt(Mat(i, 0), Mat(i, 1), Mat(i, 2));
		om_pts.push_back(pt);
	}
}

void CGeoCalculator::pts_sorting_alg(std::vector<COpenMeshT::Point>& pts)
{
	// use delaunay to compute the visibility graph
	double xCoord = pts[0][0];
	DelaunayT dt;
	for (int i = 0; i < pts.size(); i++)
	{
		KPoint2 kp(pts[i][1], pts[i][2]);
		dt.insert(kp);
	}

	COpenMeshT triMesh;
	std::map<DelaunayT::Vertex_handle, COpenMeshT::VertexHandle> cgal_om_vh_map;
	std::vector<std::vector<DelaunayT::Vertex_handle>> f_vhandles_cgal;
	for (DelaunayT::Finite_faces_iterator fiter = dt.finite_faces_begin();
		fiter != dt.finite_faces_end(); ++fiter)
	{
		std::vector<COpenMeshT::VertexHandle> vhandles;
		for (int i = 0; i < 3; i++) {
			DelaunayT::Vertex_handle vh_cgal = fiter->vertex(i);
			if (cgal_om_vh_map.find(vh_cgal) == cgal_om_vh_map.end()) {
				COpenMeshT::VertexHandle vh_om =
					triMesh.add_vertex(COpenMeshT::Point(0, vh_cgal->point().x(), vh_cgal->point().y()));
				cgal_om_vh_map[vh_cgal] = vh_om;
				vhandles.push_back(vh_om);
			}
			else
			{
				COpenMeshT::VertexHandle vh_om = cgal_om_vh_map[vh_cgal];
				vhandles.push_back(vh_om);
			}
		}
		triMesh.add_face(vhandles);
	}

	COpenMeshT::VertexHandle seed_vh;
	for (COpenMeshT::VertexIter viter = triMesh.vertices_begin(); viter != triMesh.vertices_end(); ++viter)
	{
		if (triMesh.is_boundary(*viter))
		{
			seed_vh = *viter;
			break;
		}
	}

	std::vector<COpenMeshT::VertexHandle> b_vertices;
	auto next_vh = seed_vh;
	do {
		b_vertices.push_back(next_vh);
		for (COpenMeshT::VertexOHalfedgeCCWIter voh_ccwiter = triMesh.voh_ccwbegin(next_vh);
			voh_ccwiter != triMesh.voh_ccwend(next_vh); ++voh_ccwiter)
		{
			if (triMesh.is_boundary(*voh_ccwiter))
			{
				next_vh = triMesh.to_vertex_handle(*voh_ccwiter);
				break;
			}
		}
	} while (next_vh != seed_vh);

	// output
	pts.clear();
	//std::cout << b_vertices.size() << std::endl;
	for (int i = 0; i < b_vertices.size(); i++)
	{
		COpenMeshT::Point p = triMesh.point(b_vertices[i]);
		p[0] = xCoord;
		pts.push_back(p);
		//std::cout << p[1] << " " << p[2] << std::endl;
	}

}

void CGeoCalculator::simplify_polygon(std::vector<COpenMeshT::Point>& pts, double dT)
{
	namespace PS = CGAL::Polyline_simplification_2;
	typedef PS::Stop_above_cost_threshold		Stop;
	typedef PS::Squared_distance_cost			Cost;

	double xCoord = pts[0][0];
	KPolygon2 polygon;
	for (int i = 0; i < pts.size(); i++)
		polygon.push_back(KPoint2(pts[i][1], pts[i][2]));

	Cost cost;
	polygon = PS::simplify(polygon, cost, Stop(dT));

	pts.clear();
	for (int i = 0; i < polygon.size(); i++) {
		pts.push_back(COpenMeshT::Point(xCoord, polygon[i].x(), polygon[i].y()));
	}
}

void CGeoCalculator::sample_polygon(std::vector<COpenMeshT::Point>& pts, double spacing, bool is_closed)
{
	if (is_closed)
		pts.push_back(pts.front());
	std::vector<COpenMeshT::Point> samples;

	// step size = ssz
	double alength = 0.0;
	for (int i = 0; i < pts.size() - 1; i++)
	{
		alength += (pts[i] - pts[i + 1]).norm();
	}
	double ssz = alength*spacing;

	// sample
	for (int i = 0; i < pts.size() - 1; i++)
	{
		double t_alength = (pts[i] - pts[i + 1]).norm();
		int cnt = 0;
		double t = 0.0;
		while (t < 1.0)
		{
			COpenMeshT::Point pt = pts[i] + t * (pts[i + 1] - pts[i]);
			if ((pt - pts[i + 1]).norm() > ssz) {
				samples.push_back(pt);
			}
			cnt++;
			t = double(cnt)*ssz / t_alength;
		}
	}

	//if (is_closed)
	//	samples.push_back(samples.front());
	pts = samples;
}

void CGeoCalculator::sample_polygon_parameters(std::vector<double> &tparas,
	const std::vector<COpenMeshT::Point> pts, double spacing, bool is_closed)
{
	tparas.clear();
	std::vector<COpenMeshT::Point> in_pts;
	if (is_closed)
		in_pts.push_back(pts.front());

	// step size = ssz
	double alength = 0.0;
	for (int i = 0; i < pts.size() - 1; i++)
	{
		alength += (pts[i] - pts[i + 1]).norm();
	}
	double ssz = alength*spacing;

	// sample
	for (int i = 0; i < pts.size() - 1; i++)
	{
		double t_alength = (pts[i] - pts[i + 1]).norm();
		int cnt = 0;
		double t = 0.0;
		while (t < 1.0)
		{
			COpenMeshT::Point pt = pts[i] + t * (pts[i + 1] - pts[i]);
			if ((pt - pts[i + 1]).norm() > ssz) {
				tparas.push_back(t);
			}
			cnt++;
			t = double(cnt)*ssz / t_alength;
		}
	}
	//tparas.push_back(1.0);
}

void CGeoCalculator::sample_polygon_with_parameters(std::vector<double> tparas,
	std::vector<COpenMeshT::Point> cs_pts, std::vector<COpenMeshT::Point> &out_pts, bool is_closed)
{
	//
	out_pts.clear();
	if (is_closed)
		cs_pts.push_back(cs_pts.front());

	// sample
	int pid = 0;
	for (int i = 0; i < tparas.size(); i++)
	{
		double t = tparas[i];
		if (t == 0.0) pid++;
		COpenMeshT::Point pt = cs_pts[pid-1] + t * (cs_pts[pid] - cs_pts[pid-1]);
		out_pts.push_back(pt);
	}
}

void CGeoCalculator::reconstruct_curve_from_pointset(std::vector<COpenMeshT::Point> &pts, float tolOMT)
{
	typedef CGAL::Optimal_transportation_reconstruction_2<K>    Otr_2;
	std::vector<KPoint2> points;
	for (int i = 0; i < pts.size(); i++)
		points.push_back(KPoint2(pts[i][1], pts[i][2]));

	std::cout << "init pts: " << points.size() << std::endl;
	if (points.size() == 0)
		return;

	std::vector<double> obb;
	computeOBB(pts, obb);
	double dl = (std::max)((obb[1] - obb[0]) / 2.,
		(obb[3] - obb[2]) / 2.);
	tolOMT = dl*tolOMT;
	std::cout << "tolOMT: " << tolOMT << std::endl;
	Otr_2 otr2(points);
	//otr2.set_random_sample_size(15);
	//otr2.set_relevance(0.3);
	std::vector<KPoint2> ptss;
	std::vector<std::size_t> isolated_vertices;
	std::vector<std::pair<std::size_t, std::size_t> > edges;
	int cntIter = 0;
	//float tolOMT = 0.6;
	otr2.run_under_wasserstein_tolerance(tolOMT);

	otr2.indexed_output(
		std::back_inserter(ptss),
		std::back_inserter(isolated_vertices),
		std::back_inserter(edges));
	std::cout << "(-------------List output---------- )" << tolOMT << " " << cntIter << std::endl;
	// points
	std::vector<KPoint2>::iterator pit;
	for (pit = ptss.begin(); pit != ptss.end(); pit++)
		std::cout << *pit << std::endl;
	// isolated vertices
	std::vector<std::size_t>::iterator vit;
	for (vit = isolated_vertices.begin(); vit != isolated_vertices.end(); vit++)
		std::cout << "1 " << *vit << std::endl;
	// edges
	std::vector<std::pair<std::size_t, std::size_t> >::iterator eit;
	for (eit = edges.begin(); eit != edges.end(); eit++)
		std::cout << "2 " << eit->first << " " << eit->second << std::endl;

	//KPoint2 orig(0.0, 0.0);
	//for (eit = edges.begin(); eit != edges.end(); eit++)
	//{
	//	KVector2 v12 = ptss[eit->second] - ptss[eit->first];
	//	KVector2 v01 = ptss[eit->first] - orig;
	//	double a = v01.x*v12*
	//}

}


#include "NewtonMethodEqNode.h"
#include <cmath>
#include <Eigen/Dense>

void NewtonEqNode::solve(EqNodePtr& root, EqNodePtr var)
{
	EqNodePtr derivRoot;
	double x1, x2, f1, tol, df1, err;
	int kmax, k;

	tol = 1e-8;
	kmax = 100;

	x1 = var->getValue();
	f1 = root->getValue();
	df1 = root->getDerValue(*var);

	for (k = 0; k < kmax; k++) {
		x2 = x1 - f1 / df1;
		err = abs(x2 - x1);

		var->v = x2;
		x1 = x2;
		if (err < tol)
			return;

		f1 = root->getValue();
		df1 = root->getDerValue(*var);
	}
}

void NewtonEqNode::NLASolve(const int& n, std::vector< EqNodePtr >& eqs,
	std::vector < EqNodePtr >& vars, const double& tol)
{
	int kmax, k;
	Eigen::MatrixXd Jac(n, n);
	Eigen::VectorXd F(n), x(n);
	Eigen::VectorXd delta(n);

	kmax = 300;
	double err = 0;

	for (const auto& var : vars)
		var->correctValueInsideBoundaries();

	for (k = 0; k < kmax; ++k) {

		for (int i = 0; i < n; ++i) {
			x[i] = vars[i]->v;
			F[i] = eqs[i]->getValue();
			for (int j = 0; j < n; ++j)
				Jac(i, j) = eqs[i]->getDerValue(*vars[j]);
		}

		// Solve linear system
		delta = Jac.fullPivLu().solve(-F);
		// Update x values
		x = x + delta;

		for (int i = 0; i < n; ++i) vars[i]->v = x[i];

		// criterion for stop
		// get max abs value of function
		err = 0.0;
		for (int i = 0; i < n; ++i) {
			err = err < std::abs(F[i]) ? abs(F[i]) : err;
			err = err < std::abs(delta[i]) ? abs(delta[i]) : err;
		}

		if (err < tol) return;

		for (auto& var : vars) var->correctValueInsideBoundaries();
	}
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <boost/numeric/odeint.hpp>
#include "helper_func.h"
#include "LongDynamics.h"
#include "EKF.h"

int main()
{
    // Define the dataset path
    const std::string dataset_path{"../data/aircraft_long_dynamics_dataset.txt"};

    // Load dataset
    const int dataset_size = 1001;
    Eigen::VectorXd elev_input(dataset_size);
    Eigen::VectorXd throttle_input(dataset_size);
    Eigen::VectorXd alpha_true(dataset_size);
    Eigen::VectorXd alpha_measured(dataset_size);
    Eigen::VectorXd pitch_rate_true(dataset_size);
    Eigen::VectorXd pitch_rate_measured(dataset_size);
    LoadDataSet(dataset_path, elev_input, throttle_input, alpha_true,
                alpha_measured, pitch_rate_true, pitch_rate_measured);

    // Create a csv file to save output data
    std::ofstream outputs_file;
    std::string filename = "outputs.csv";
    outputs_file.open(filename);
    outputs_file << "time" << "," << "true_alpha" << "," << "measured_alpha"
                << "," << "estimated_alpha" << "," << "true_q" << ","
                << "measured_q" << "," << "estimated_q" << std::endl;

    // Define dynamic system parameters
    int num_state = 4;
    int num_input = 2;
    int num_output = 2;
    const double sample_time = 0.01;
    Eigen::MatrixXd Q(num_state, num_state); // Process noise covariance matrix
    Q << 0.0001, 0.0   , 0.0     , 0.0,
         0.0   , 0.0001, 0.0     , 0.0,
         0.0   , 0.0   , 0.000001, 0.0,
         0.0   , 0.0   , 0.0     , 0.0;

    Eigen::MatrixXd R(num_output, num_output); // Measurment noise covariance matrix
    R << 0.00001, 0.0,
         0.0    , 0.00001;

    Eigen::MatrixXd P0(num_state, num_state); // Initial estimate error covariance matrix
    P0 << 0.0001, 0.0   , 0.0     , 0.0,
         0.0   , 0.0001, 0.0     , 0.0,
         0.0   , 0.0   , 0.000001, 0.0,
         0.0   , 0.0   , 0.0     , 0.0;

    // Construct EKF object
    EKF ekf(Q, R, P0, sample_time);

    // Define integration class and a stepper integrator
    state_type x(num_state);
    LongDynamics long_dyn_object;
    boost::numeric::odeint::runge_kutta4<state_type> rk4_stepper;

    // Define vectors to hold the estimated outputs and the integrator predicted state
    Eigen::VectorXd alpha_estimated(dataset_size);
    Eigen::VectorXd pitch_rate_estimated(dataset_size);
    Eigen::VectorXd predicted_state(num_state);

    // Initialize the state
    predicted_state << 74.9167 + 0.03,
                       3.5330 + 0.03,
                       0.0 +0.001,
                       2.7 * PI / 180;

    // Run the filter on the dataset of I/O
    Eigen::VectorXd y(num_output);
    Eigen::VectorXd u(num_input);
    double t = 0.0;
    std::cout << "Processing dataset..." << std::endl;
    for (int i = 0; i < dataset_size; i++)
    {
        // Run EKF for one step
        y << alpha_measured(i),
             pitch_rate_measured(i);
        u << elev_input(i),
             throttle_input(i);
        ekf.Update(y, u, predicted_state);
        alpha_estimated(i) = ekf.GetEstimatedOutput()(0);
        pitch_rate_estimated(i) = ekf.GetEstimatedOutput()(1);

        // Save outputs to the csv file
        outputs_file << sample_time * i << ","
                     << alpha_true(i) * 180.0 / PI << ","
                     << alpha_measured(i) * 180.0 / PI << ","
                     << alpha_estimated(i) * 180.0 / PI << ","
                     << pitch_rate_true(i) * 180.0 / PI << ","
                     << pitch_rate_measured(i) * 180.0 / PI << ","
                     << pitch_rate_estimated(i) * 180.0 / PI << std::endl;

        // Pass the updated state back to the integrator
        x[0] = ekf.GetState()(0);
        x[1] = ekf.GetState()(1);
        x[2] = ekf.GetState()(2);
        x[3] = ekf.GetState()(3);

        // Run the integrator for one step and get the new state
        t = sample_time * i;
        rk4_stepper.do_step(long_dyn_object, x, t, sample_time);
        predicted_state(0) = x[0];
        predicted_state(1) = x[1];
        predicted_state(2) = x[2];
        predicted_state(3) = x[3];
    }
    std::cout << "Finished processing dataset" << std::endl;

    // Close the output csv file
    outputs_file.close();

    // Compute and print root mean square error for the estimated outputs (angle of attack and pitch rate)
    std::cout << "RMSE for the estimated AoA is: " << ComputeRMSE(alpha_true, alpha_estimated) << std::endl;
    std::cout << "RMSE for the estimated pitch rate is: " << ComputeRMSE(pitch_rate_true, pitch_rate_estimated) << std::endl;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_UTILITY_FRANK_MATRIX_HPP
#define ROKKO_UTILITY_FRANK_MATRIX_HPP

#include <cmath>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <rokko/config.h>
#include <rokko/localized_matrix.hpp>
#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
# include <rokko/distributed_matrix.hpp>
#endif

namespace rokko {

class frank_matrix {
public:
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    for(int i = 0; i < n; ++i) {
      for(int j = 0; j < n; ++j) {
        mat(i,j) = n - std::max(i, j);
      }
    }
  }

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_i = mat.translate_l2g_row(local_i);
        int global_j = mat.translate_l2g_col(local_j);
        mat.set_local(local_i, local_j, mat.get_m_global() - std::max(global_i, global_j));
      }
    }
  }

  // another (slower) implementation using set_global function
  template<typename T, typename MATRIX_MAJOR>
  static void generate_global(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.m_global != mat.n_global)
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    for(int global_i=0; global_i<mat.m_global; ++global_i) {
      for(int global_j=0; global_j<mat.n_global; ++global_j) {
        mat.set_global(global_i, global_j, mat.m_global - std::max(global_i, global_j) );
      }
    }
  }
#endif

  // calculate k-th smallest eigenvalue of dim-dimensional Frank matrix (k=0...dim-1)
  static double eigenvalue(int dim, int k) {
    return 1 / (2 * (1 - std::cos(M_PI * (2 * k + 1) / (2 * dim + 1))));
  }
};

} // namespace rokko

#endif // ROKKO_UTILITY_FRANK_MATRIX_HPP


////////////////////////////////////////////////////////////////////////////////
// EigSensitivity.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  First and second derivatives of symmetric 2x2 matrix eigenvalues.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/05/2019 10:32:17
////////////////////////////////////////////////////////////////////////////////
#ifndef EIGSENSITIVITY_HH
#define EIGSENSITIVITY_HH

#include <Eigen/Dense>
#include <array>
#include <MeshFEM/EnergyDensities/Tensor.hh>

// 2x2 case only for now, also can be sped up.
template<typename Real>
struct EigSensitivity {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;

    EigSensitivity() { }

    template<typename Derived>
    EigSensitivity(const Eigen::MatrixBase<Derived> &A) { setMatrix(A); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &A) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");

        const Real a_minus_c = A(0, 0) - A(1, 1);
        const Real b = A(0, 1);
        if (std::abs(b - A(1, 0)) > 1e-15) throw std::runtime_error("Only symmetric matrices are supported");
        // d := descriminant of characteristic quadratic
        const Real sqrt_d = std::sqrt(a_minus_c * a_minus_c + 4 * b * b);
        m_Lambda << 0.5 * (A.trace() + sqrt_d),
                    0.5 * (A.trace() - sqrt_d);

        V2d q0(a_minus_c + sqrt_d, 2 * b);
        q0.normalize();
        m_Q.col(0) = q0;
        m_Q.col(1) << -q0[1], q0[0];

        if (sqrt_d < 1e-14) m_degenerate = true;
        else                m_degenerate = false;
    }

    auto      q(size_t i) const { return m_Q.col(i); }
    Real lambda(size_t i) const { return m_Lambda[i]; }

    // Access Eigendecomposition
    const M2d &     Q() const { return m_Q; }
    const V2d &Lambda() const { return m_Lambda; }

    V2d dLambda(const M2d &dA) const { return (m_Q.transpose() * dA * m_Q).diagonal(); }
    M2d dLambda(size_t i) const { return q(i) * q(i).transpose(); }
    Real dLambda(size_t i, const M2d &dA) const { return q(i).dot(dA * q(i)); }

    // Only correct for symmetric dA_b!
    V2d d2Lambda(const M2d &dA_a, const M2d &dA_b) const {
        if (m_degenerate) return V2d(0.0, 0.0);
        Real d2lambda_0 = (2.0 / (m_Lambda[0] - m_Lambda[1])) * (q(0).dot(dA_a * q(1))) * (q(0).dot(dA_b * q(1)));
        return V2d(d2lambda_0, -d2lambda_0);
    }

    // Only correct for symmetric dA!
    M2d delta_dLambda(size_t i, const M2d &dA) const {
        if (m_degenerate) return M2d::Zero();
        double sign = (i == 0) ? 1.0 : -1.0;
        M2d result = ((sign * (2.0 / (m_Lambda[0] - m_Lambda[1])) * (q(0).dot(dA * q(1)))) * q(0)) * q(1).transpose();
        symmetrize(result);
        return result;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    M2d m_Q;
    V2d m_Lambda;

    bool m_degenerate;
};

#endif /* end of include guard: EIGSENSITIVITY_HH */


#ifndef SPLX_INTERNAL_BEZIER_H
#define SPLX_INTERNAL_BEZIER_H
#include <Eigen/Dense>
#include <splx/types.hpp>
#include <splx/internal/combinatorics.hpp>

namespace splx {
namespace internal {
namespace bezier {

/*
* given a bezier curve degree and max parameter, return row r so that
* multiplying the row with control points gives the kth derivative
* of the curve at u
*
* f^k(u) = \sum_{i=0}^degree r(i) * p(i)
* return r
*/
template<typename T>
Row<T> getBasisRow(unsigned int degree, T maxParameter, T u, unsigned int k) {
    if(u < 0 || u > maxParameter) {
    throw std::domain_error(
        std::string("u is outside of the range [0, ")
        + std::to_string(maxParameter)
        + std::string("]")
    );
    }

    if(maxParameter == 0) {
        Row<T> result(degree+1);
        result.setZero();
        if(k == 0 && degree >= 0) {
            result(0) = 1.0;
        }
        return result;
    }

    Row<T> result(degree + 1);

    T oneOverA = 1/maxParameter;
    for(unsigned int i = 0; i <= degree; i++) {
    T base = 0.0;
    T mult = 1.0;
    for(unsigned int j = 0; j+k <= degree; j++, mult *= u) {
        if(j+k >= i) {
        // base += pow(oneOverA, i) * this->comb(degree-i, j+k-i)
        //       * pow(-oneOverA, j+k-i) * this->perm(j+k, k)
        //       * mult;

        base += splx::internal::comb(degree-i, j+k-i)
            * splx::internal::pow(oneOverA, j+k) * splx::internal::perm(j+k, k)
            * mult * ((j+k-i)%2 == 0 ? 1 : -1);
        }
    }
    base *= splx::internal::comb(degree, i);
    result(i) = base;
    }
    return result;
}

/*
    Coefficient matrix for the k^th derivative bernstein base functions where each row r
    contains coefficients where k^th derivative of i^th bernstein polynomial of degree d
    can be expressed as r(0) + r(1)u + r(2)u^2 + ... + r(d)u^d
*/
template<typename T>
Matrix<T> bernsteinCoefficientMatrix(unsigned int degree, T maxParameter, unsigned int k) {
    Matrix<T> bernsteinMtr(degree+1, degree+1);
    bernsteinMtr.setZero();

    if(maxParameter == 0) {
        if(k == 0 && degree >= 0) {
            bernsteinMtr(0, 0) = 1.0;
        }
        return bernsteinMtr;
    }

    unsigned int dcombi = 1;
    for(Index i = 0;

        i < degree+1;

        dcombi *= (degree-i),
        dcombi /= (i+1),
        i++) {

        unsigned int dminicombjmini = 1;
        T min1 = 1;
        T oneOverAPowj = splx::internal::pow(1/maxParameter, i);

        for(Index j = i;

            j < degree+1;

            dminicombjmini *= (degree - j),
            dminicombjmini/= (j+1-i),
            j++,
            min1 *= -1,
            oneOverAPowj *= (1/maxParameter)) {

            bernsteinMtr(i, j) = dcombi * dminicombjmini * min1 * oneOverAPowj;

        }
    }

    Matrix<T> derivative(degree+1, degree+1);
    derivative.setZero();

    unsigned int jpermk = splx::internal::fac(k);
    for(unsigned int j = k;

        j < degree+1;

        jpermk *= (j+1),
        jpermk /= (j+1-k),
        j++) {
        derivative(j, j-k) = jpermk;
    }

    return bernsteinMtr * derivative;
}

} // namespace bezier
} // namespace internal
} // namespace splx

#endif


/**
 * @file MathTools.hpp
 * @author John D. Jakeman
 * @date 31 October 2011
 * @brief Miscelaneous math functions.
 */

#ifndef MATH_TOOLS_HPP
#define MATH_TOOLS_HPP

#include "LinearAlgebra.hpp"
#include <algorithm>
#include <functional>
#include <ctime>
#include <boost/functional/hash.hpp>
#include <boost/unordered_set.hpp>

namespace Pecos {

/**
 * \brief Map a scalar index of a flat 1D array to the equivalent
 *        d-dimensional index
 *
 * Example:
 \f[
 \begin{bmatrix}
 1 & 4 & 7\\
 2 & 5 & 8\\
 3 & 6 & 9
 \end{bmatrix}
 \rightarrow
 \begin{bmatrix}
 1,1 & 1,2 & 1,3\\
 2,1 & 2,2 & 2,3\\
 3,1 & 3,2 & 3,3
 \end{bmatrix}
 \f]
 *
 * @param sizes the number of elems in each dimension. For a 2D index
 * sizes = [numRows, numCols]
 * @param index the scalar index
 * @param num_elems the total number of elements in the d-dimensional matrix
 * @return result the d-dimensional index
 */
void ind2sub( const IntVector &sizes, int index, int num_elems,
	      IntVector &result );

/**
 * \brief Map a d-dimensional index to the scalar index of the equivalent flat
 * 1D array
 *
 * Example:
 \f[
 \begin{bmatrix}
 1,1 & 1,2 & 1,3\\
 2,1 & 2,2 & 2,3\\
 3,1 & 3,2 & 3,3
 \end{bmatrix}
 \rightarrow
 \begin{bmatrix}
 1 & 4 & 7\\
 2 & 5 & 8\\
 3 & 6 & 9
 \end{bmatrix}
 \f]
 *
 * @param sizes the number of elems in each dimension. For a 2D index
 * sizes = [numRows, numCols]
 * @param multi_index the d-dimensional index
 * @return scalar_index the scalar index of the flat array
 */
int sub2ind( const IntVector &sizes, const IntVector &multi_index );

/**
 * \brief Compute the cartesian product of an arbitray number of sets.
 *
 * These sets can consist of numbers of be themselves sets of vectors
 * @param inputSets the sets to be used in the cartesian product
 * @param elem_size the size of the vectors within each set.
 * @return the cartesian product
 */
template<typename O, typename T>
void cartesian_product( const std::vector< Teuchos::SerialDenseVector<O,T> > &input_sets, Teuchos::SerialDenseMatrix<O,T> &result, int elem_size )
{
#ifdef DEBUG
  if ( input_sets.size() == 0 )
    {
      throw(std::invalid_argument("CartesianProduct() there must be at least one input set"));
    }
#endif
  int num_elems = 1;
  int num_sets = input_sets.size();
  IntVector sizes;
  sizes.resize( num_sets );
  IntVector multi_index;
  for ( int i = 0; i < num_sets; i++ )
    {
      sizes[i] = input_sets[i].length() / elem_size;
      num_elems *= sizes[i];
    }
  result.reshape( num_sets*elem_size, num_elems );
  for ( int i = 0; i < num_elems; i++ )
    {
      ind2sub( sizes, i, num_elems, multi_index );
      for ( int j = 0; j < num_sets; j++ )
	{
	  for ( int k = 0; k < elem_size; k++ )
	    {
	      result( j*elem_size+k, i ) =
		input_sets[j][multi_index[j]*elem_size+k];
	    }
	}
    }
};

/**
 * \brief Construct the outer product of an arbitray number of sets.
 *
 * Example:
 \f[ \{1,2\}\times\{3,4\}=\{1\times3, 2\times3, 1\times4, 2\times4\} =
 \{3, 6, 4, 8\} \f]
 * @param input_sets "Vector.hpp" of sets to be used in the outer product
 * @return result the outer product
 */
template<typename O, typename T>
void outer_product( const std::vector< Teuchos::SerialDenseVector<O,T> > &input_sets, Teuchos::SerialDenseVector<O,T> &result )
{
  int num_elems = 1;
  int num_sets = input_sets.size();
  IntVector sizes( num_sets, false );
  for ( int i = 0; i < num_sets; i++ )
    {
      sizes[i] = input_sets[i].numRows();
      num_elems *= sizes[i];
    }
  IntVector multi_index( num_sets, false );
  result.sizeUninitialized( num_elems );
  for ( int i = 0; i < num_elems; i++ )
    {
      result[i] = 1.0;
      ind2sub( sizes, i, num_elems, multi_index );
      for ( int j = 0; j < num_sets; j++ )
	result[i] *= input_sets[j][multi_index[j]];
    }
}

/**
 * \brief Build a vector containing all integers in [m,n) seperated by incr
 *
 * E.g Range( 1, 10, 2 ) -> [1,3,5,7,9]
 */
template<typename O, typename T>
void range( Teuchos::SerialDenseVector<O,T> &result, T m, T n, T incr = 1 )
{
  T i = m;
  O j = 0;
  O len = (O)( n - m ) / (O)incr;
  if ( (O)( n - m ) % (O)incr != 0 ) len++;
  result.sizeUninitialized( len );
  while( i < n )
    {
      result[j] = i;
      i += incr;
      j++;
    }
};

/**
 * \brief Generate a vector whose n elements are equidistantly spaced in the
 * interval [a,b].
 *
 * @param [a,b] the range
 * @param n the number of times the interval [a,b] is divided
 * @return vec n numbers equidistant is [a,b]
 */
void linspace( RealVector &result, Real a, Real b, int n );

/**
 * \brief Round a Real to the nearest integer.
 */
Real round( Real r );

/*
 * \brief Function to allow for searching for maximum absolute
 * value (Real) in a containter
 *
 * @param a first value
 * @param b second value
 * @return true if a < b, false otherwise
 */
bool abs_compare( Real a, Real b );

/**
 * \brief Return the nearest integer to x
 * @param x the value
 * return int the neartest integer to x
 */
int nearest_integer( Real x );

/**
 * \brief Calulate n!
 *
 * @param n the factorial of interest
 * @return n!
 */
Real factorial( int n );

/**
 * \brief Return the number of possible combinations of k elements that can be
 * chosen from n elements.
 *
 *\f[ { n \choose k} = \frac{n!}{k!(n-k)!}\f]
 * @param n number of elements
 * @param k the number of elements to be chosen. k<=n.
 * @return the total number of combinations.
 */
int nchoosek( int n, int k );

/**
 * \brief Transform a point x in the interval [a,b] onto the interval [0,1] and
 * vice versa.
 *
 * @param x the value to be transformed
 * @param [a,b] the new/old interval
 * @param dir specifies whether to map to [a,b] or from [a,b]. That is
 * dir=0  returns [0,1] -> [a,b]
 * dir!=0 returns [a,b]  -> [0,1]
 * @return the rescaled value
 */
Real rescale( Real x, Real a, Real b, int dir );

/**
 * \brief Transform a point x in the interval
 * \f$[a_1,b_1] \times \cdots \times [a_d,b_d] \f$ onto
 * the hypercube interval \f$[0,1]^d\f$ and vice versa.
 *
 * @param x the value to be transformed
 * @param [a,b] the new/old interval
 * @param dir specifies whether to map to [a,b] or from [a,b]. That is
 * dir=0  returns [0,1] -> [a,b]
 * dir!=0 returns [a,b]  -> [0,1]
 * @return the rescaled value
 */
void rescale( RealMatrix &x, const RealVector &domain, int dir );

/**
 *\brief transform points in domain_in to points in domain_out
 */
void hypercube_map( RealMatrix &x, const RealVector &domain_in,
		    const RealVector &domain_out,
		    RealMatrix &result );

/**
 * \brief Construct a d-dimensional mesh on a hyper-rectangle. The meshes are
 * equidistant with respect to each coordinate direction.
 *
 * @param num_pts_1d array specifying the number of meshpoints for each dimension
 * @param domain the min and max value of each dimension.
 *        That is \f$ [a_1,b_1,...,a_d,b_d]\f$
 * @return the multi-dimensional mesh coordinates. ( num_dims x num_pts ) array.
 * num_pts = num_pts_1d[0]*...*num_pts_1d[d-1]
 */
void mesh_grid( const IntVector &num_pts_1d,
		const RealVector &domain,
		RealMatrix &result );

/**
 * Get all the multi-dimensional indices of a multi-dimensional polynomial
 * basis with a specified degree sum. A cleaner interface is provided by
 * GetMultiDimensionalPolynomial_indices()
 * @return B the multi-dimensional indices
 */
void get_multi_dimensional_polynomial_subspace_indices( IntMatrix &B,
							int* elems,
							int len_elems,
							int* pos,
							int len_pos,
							int choices_made,
							int first_pos,
							int order, int &row );

/**
 * \brief Get all the multi-dimensional indices of a multi-dimensional polynomial
 * basis with a specified degree sum.
 *
 * @param num_dims the dimensionailty
 * @param degree the degree sum of the polynomial indices wanted
 * @return B the multi-dimensional polynomial_indices
 */
void get_multi_dimensional_polynomial_indices( int num_dims, int degree,
					       IntMatrix &result );

/**
 * \brief Get the smallest degree of the total degree polynomial such that the
 * number of terms in the basis is larger than or equal to num_samples
*/
int get_total_degree_from_num_samples( int num_dims, int num_samples );

/**
 * \brief Get the multi-dimensional hypercube \f$[a,b]^d\f$
 *
 * @param num_dims the dimension of the computational domain.
 * @param \f$[a,b]\f$ the one-dimensional bounds of the hypercube.
 * @return result the multi-dimensional bounds of the hypercube.
 */
void set_hypercube_domain( RealVector &result, int num_dims, Real a, Real b );

/// Perturb the columns of a matrix
void get_permutations( IntMatrix &permutations,
		       int M , int N, unsigned int seed );

void compute_hyperbolic_level_subdim_indices( int num_dims, int level,
					      int num_active_dims,
					      Real p,
					      IntMatrix &result );

void compute_hyperbolic_level_indices( int num_dims, int level, Real p,
				       IntMatrix &result );

void compute_hyperbolic_indices( int num_dims, int level, Real p,
				 IntMatrix &result );

// weights ( num_dims x 1 ) vector with values in [0,1]
void compute_anisotropic_hyperbolic_indices( int num_dims, int level, Real p,
					     RealVector &weights,
					     IntMatrix &result );

// prepend "mt_" for MathTools to avoid name clash
enum lp_norm
  {
    mt_l1_norm,
    mt_l2_norm,
    mt_linf_norm,
  };

/**
 * \brief Return the index of the element of x with the minimum value.
 *
 *\f[
 \arg \! \min_i x_i\quad i=1,\ldots,n
 \f]
*/
 template<typename T>
int argmin( int n, T* x )
{
  int amin( 0 );
  T min = x[0];
  for ( int i = 1; i < n; i++ )
    {
      if ( x[i] < min )
	{
	  min = x[i];
	  amin = i;
	}
    }
  return amin;
};

/**
 * \brief Return the index of the element of x with the maximum value.
 *
 *\f[
 \arg \! \max_i x_i \quad i=1,\ldots,n
 \f]
*/
template<typename T>
int argmax( int n, T* x )
{
  int amax( 0 );
  T max = x[0];
  for ( int i = 1; i < n; i++ )
    {
      if ( x[i] > max )
	{
	  max = x[i];
	  amax = i;
	}
    }
  return amax;
};

template<typename T>
int magnitude_argmax( int n, T* x )
{
  int amax( 0 );
  T max = std::abs( x[0] );
  for ( int i = 1; i < n; i++ )
    {
      Real abs_x = std::abs( x[i] );
      if ( abs_x > max )
	{
	  max = abs_x;
	  amax = i;
	}
    }
  return amax;
};

/**
 * \brief Return the sum of the elements in the array x.
 *
 *\f[
 \sum_{i=1}^N x_i
 \f]
*/
template<typename T>
T sum( int n, T* x )
{
  T sum = x[0];
  for ( int i = 1; i < n; i++ )
    sum += x[i];
  return sum;
};

/**
 * \brief Return the sum of the elements in the vector x.
 *
 *\f[
 \sum_{i=1}^N x_i
 \f]
 *
 * This will not return the same result as sum( int n, T* x ) if
 * the vector is a subvector of another vector. That is stride does not equal
 * the number of rows.
 */
template < typename O, typename T >
T sum( Teuchos::SerialDenseVector<O,T> &v )
{
  T tmp = Teuchos::ScalarTraits<T>::zero();
  for ( O i = 0; i < v.length(); i++ )
    tmp += v[i];
  return tmp;
};

/**
 * \brief Return the mean of the array x.
 *
 * \f[
 \bar{x} = \frac{1}{N}\sum_{i=1}^N x_i
 \f]
*/
Real mean( int n, Real *x );

/**
 * \brief Return the sample variance of the array x.
 *
 * \f[
 \sigma^2 = \frac{1}{N-\text{ddof}}\sum_{i=1}^N ( x_i-\bar{x} )^2
 \f]
 *
 * \param ddof Delta Degrees of Freedom (ddof).
 * The divisor used in calculations is \f$N - \text{ddof}\f$,
 * where \$fN\$f represents the number of elements.
 * By default ddof is one.
 */
Real variance( int n, Real *x, int ddof = 1 );

/**
 * \brief Calculate one of several different types of \f$\ell_p\f$ error norms
 * of a set of vectors.
 *
 * Each column of the reference_values matrix is considered
 * independently with the correpsonding column in the approximation_values.
 * The error between the each set of two columns tested is returned.
 * \param reference_values a matrix containing the reference values
 * \param approximate_values a matrix containig the approximate values
 * \param active_columns specifies which set of columns to consider. If empty
 *        all columns are considered
 * \pram normalise if true the error norms are normalised. The l_inf norm
 *                 is normalised by the half the range of the data in the
 *                 reference_values column. The L_2 norm is normalised by the
 *                 standard deviation of the data in the reference values column.
 */
void lp_error( RealMatrix &reference_values,
	       RealMatrix &approximate_values,
	       std::vector<lp_norm> error_norms, RealMatrix &error,
	       IntVector &active_columns, bool normalise = false );

/**
 *\brief Construct a Latin Hyperube Design (LHD)
 */
void latin_hypercube_design( int num_pts, int num_dims, RealMatrix &result,
			     int seed );

void compute_next_combination( int num_dims, int level, IntVector &index,
			       bool &extend, int &h, int &t );

void compute_combinations( int num_dims, int level, IntMatrix &result );

/**
 * \breif Return  the sign of x.
 *
 * \return 1 if the corresponding element of x is greater than zero;
 * 0 if the corresponding element of X equals zero;
 *-1 if the corresponding element of X is less than zero
 */
template<typename T>
int sgn( T x )
{
  return ( T(0) < x ) - ( x < T(0) );
}

template<typename T>
int num_non_zeros( T *data, int n )
{
  int num_non_zero_count( 0 );
  for ( int i = 0; i < n; i++ )
    {
      if ( std::abs( data[i] ) > 0 )
	num_non_zero_count++;
    }
  return num_non_zero_count;
}

/**
 *\brief return the median of a std::vector
 */
template<typename T>
double median( std::vector<T> &v )
{
  size_t n = v.size();
  typename std::vector<T>::iterator target = v.begin() + n / 2;
  std::nth_element( v.begin(), target, v.end() );
  if( n % 2 != 0 )
    {
      return (double)*target;
    }
  else
    {
      std::vector<Real>::iterator target_neighbour =
	std::max_element( v.begin(), target );
      return (double)( *target + *target_neighbour ) / 2.0;
    }
};

/**
 *\brief return the median of a RealVector.
 */
template<typename O, typename T>
double median( Teuchos::SerialDenseVector<O,T> &v )
{
  std::vector<T> tmp( v.length() );
  for ( int i = 0; i < v.length(); i++ )
    tmp[i] = v[i];
  return median( tmp );
}

template<typename O, typename T>
double pnorm( Teuchos::SerialDenseVector<O,T> &v, double p )
{
  double sum = 0.;
  for ( O i = 0; i < v.length(); i++ )
    {
      sum += std::pow( std::abs( (double)v[i] ), p );
    }
  return std::pow( sum, 1./p );
}

template<typename O, typename T>
O num_nonzeros( Teuchos::SerialDenseVector<O,T> &v )
{
  O num_nonzeros = Teuchos::ScalarTraits<O>::zero();
  for ( int d = 0; d < v.length(); d++ )
    {
      if ( v[d] != Teuchos::ScalarTraits<T>::zero() ) num_nonzeros++;
    }
  return num_nonzeros;
}

template<typename O, typename T>
void nonzero( Teuchos::SerialDenseVector<O,T> &v,
	      IntVector &result )
{
  int num_nonzeros = 0;
  result.sizeUninitialized( v.length() );
  for ( int d = 0; d < v.length(); d++ )
    {
      if ( v[d] != Teuchos::ScalarTraits<T>::zero() )
	{
	  result[num_nonzeros] = d;
	  num_nonzeros++;
	}
    }
  // chop off unwanted memory
  result.resize( num_nonzeros );
}

void lhs_indices( int num_dims, int num_pts, int duplication, int seed,
		  IntMatrix &result );

/// Construct the improved distributed hypercube sample
void ilhs( int num_dims, int num_pts, int duplication, int seed,
	   RealMatrix &result );

/// Construct basic random hypercube sample
void lhs( int num_dims, int num_pts, int seed,
	  RealMatrix &result );

template<typename T>
class index_sorter {
public:
  index_sorter(const T &values){ values_ = values; }
  bool operator()( int lhs, int rhs) const {
    return values_[lhs] < values_[rhs];
  }
private:
  T values_;
};

template<typename T>
class magnitude_index_sorter {
public:
  magnitude_index_sorter(const T &values){ values_ = values; }
  bool operator()( int lhs, int rhs) const {
    return std::abs( values_[lhs] ) > std::abs( values_[rhs] );
  }
private:
  T values_;
};

// sorts in ascending order
template<typename O, typename T>
void argsort( const Teuchos::SerialDenseVector<O,T> &values,
	      IntVector &result )
{
  std::vector<O> indices( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    indices[i] = i;

  std::sort( indices.begin(), indices.end(), index_sorter< Teuchos::SerialDenseVector<O,T> >( values ) );

  result.sizeUninitialized( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    result[i] = indices[i];
}

template<typename O, typename T>
void argsort( Teuchos::SerialDenseVector<O,T> &v,
	      Teuchos::SerialDenseVector<O,T> &result_0,
	      IntVector &result_1 )
{
  argsort( v, result_1 );
  result_0.sizeUninitialized( v.length() );
  for ( O i = 0; i < v.length(); i++ )
    result_0[i] = v[result_1[i]];
}

// sorts in descending order absolute value of entries
template<typename O, typename T>
void magnitude_argsort( const Teuchos::SerialDenseVector<O,T> &values,
			IntVector &result )
{
  std::vector<O> indices( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    indices[i] = i;

  std::sort( indices.begin(), indices.end(), magnitude_index_sorter< Teuchos::SerialDenseVector<O,T> >( values ) );

  result.sizeUninitialized( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    result[i] = indices[i];
}

/**
 * \brief find the interval containing a target value. Assumes data is
 * in ascending order
 */
template<typename O, typename T>
int binary_search( T target, Teuchos::SerialDenseVector<O,T> &data )
{
  O low = 0, high = data.length()-1, mid;
  while ( low <= high )
    {
      mid = low + ( high - low ) / 2;
      if ( target < data[mid] ) high = mid - 1;
      else if ( target > data[mid] ) low = mid + 1;
      else return mid;
    }
  if ( high < 0 ) return 0;
  else if ( high < low ) return high;
  else return low;
}

class LinearInterpolant1D
{
private:
  RealVector pts_, vals_;

public:
  LinearInterpolant1D( RealVector &pts, RealVector &vals )
  {
    pts_ = pts; vals_ = vals;
  };

  ~LinearInterpolant1D(){};

  void interpolate( RealVector &pts, RealVector &result )
  {
    int num_pts = pts.length();
    if ( result.length() != num_pts )
      result.sizeUninitialized( num_pts );
    for ( int i = 0; i < num_pts; i++ )
      {
	// enforce constant interpolation when interpolation is outside the
	// range of pts_
	if ( pts[i] <= pts_[0] )
	  result[i] = vals_[0];
	else if ( pts[i] >= pts_[pts_.length()-1] )
	  result[i] = vals_[pts_.length()-1];
	else
	  {
	    // assumes binary search returns index of the closest point in pts_
	    // to the left of pts[i]
	    int index = binary_search( pts[i], pts_ );
	    result[i] = vals_[index] + ( ( vals_[index+1] - vals_[index] ) /
					 ( pts_[index+1] - pts_[index] ) )
	      * ( pts[i] - pts_[index] );
	  }
      }
  };
};

/**
 * \brief Reverse the contents of a vector.
 *
 * Useful for changing an ordered array to/from ascending/descending order
 */
template<typename O, typename T>
void reverse( Teuchos::SerialDenseVector<O,T> &v )
{
  O n = v.length();
  Teuchos::SerialDenseVector<O,T> tmp( v );
  for ( O i = 0; i < n; i++ )
    v[i] = tmp[n-i-1];
}

template <typename O, typename T>
struct VectorEqual{
  /*
   * \brief Determine if two vectors of doubles are equal
   */
  typedef Teuchos::SerialDenseVector<O,T> VectorType;
  bool operator()( const VectorType &v1, const VectorType &v2 ) const{
    if ( v1.length() != v2.length() )
      return false;
    else{
      Real tol = std::numeric_limits<T>::epsilon();
      Real dist = 0.;
      for ( int i = 0; i < v1.length(); i++ )
	dist += ( v1[i]-v2[i] )*( v1[i]-v2[i] );
      dist = std::sqrt( dist );
      if ( dist > tol )
	return false;
    }
    return true;
  }
};

template<typename VectorType >
struct VectorHash{
  int operator()( const VectorType &v ) const{
    return (int)boost::hash_range(v.values(), v.values() + v.length() );
  }
};

/*
 * \brief Get the columns in A that are not in B
 */
template <typename O, typename T>
void set_difference_matrix_columns( const Teuchos::SerialDenseMatrix<O,T> &A,
				    const Teuchos::SerialDenseMatrix<O,T> &B,
				    IntVector &result ){
  if ( B.numCols() == 0 ){
    range( result, 0, A.numCols() );
    return;
  }

  int num_rows = A.numRows(), num_cols = A.numCols();
  if ( num_rows != B.numRows() ){
    std::string msg = "set_difference_matrix_columns: A and B are inconsistent";
    throw(std::runtime_error( msg ) );
  }
  typedef Teuchos::SerialDenseVector<O,T> VectorType;

  // Hash columns of the matrix B
  boost::unordered_set< VectorType,VectorHash<VectorType>,
			VectorEqual<O,T> > col_set;
  for ( int i = 0; i < B.numCols(); i++ ){
    VectorType col( Teuchos::View, const_cast<T*>(B[i]), num_rows );
    col_set.insert( col );
  }

  // Find columns of A not in B
  int num_result = 0;
  typename boost::unordered_set< VectorType,VectorHash<VectorType>,
				 VectorEqual<O,T> >::const_iterator it;
  result.sizeUninitialized( num_cols );
  for ( int i = 0; i < num_cols; i++ ){
    VectorType col( Teuchos::View, const_cast<T*>(A[i]), num_rows );
    it = col_set.find( col );
    if ( it == col_set.end() ){
      // Column is not in B
      result[num_result] = i;
      num_result++;
    }
  }

  // Remove unused memory
  result.resize( num_result );
}

template<typename MatrixType>
void extract_submatrix_from_column_indices( const MatrixType &A,
					    const IntVector &column_indices,
					    MatrixType &submatrix ){
  int num_rows = A.numRows(), num_indices = column_indices.length();
  reshape( submatrix, num_rows, num_indices );

  for ( int j = 0; j < num_indices; j++ )
    for ( int i = 0; i < num_rows; i++ )
      submatrix(i,j) = A(i,column_indices[j]);
}

template<typename MatrixType>
void copy_matrix( const MatrixType &source, MatrixType &dest,
	   int num_rows, int num_cols, int start_row=0, int start_col=0 ){

  MatrixType source_subset( Teuchos::View, source, num_rows, num_cols,
			    start_row, start_col );
  reshape( dest, num_rows, num_cols );
  dest.assign( source_subset );
}

template<typename VectorType>
void copy_vector( const VectorType &source, VectorType &dest,
	   int num_rows, int start_row=0 ){
  VectorType source_subset( Teuchos::View, source.values()+start_row, num_rows );
  resize( dest, num_rows );
  dest.assign( source_subset );
}

template<typename MatrixType>
void hstack( const MatrixType &source1, const MatrixType &source2,
	     MatrixType &dest ){
  if ( ( source1.numRows() != source2.numRows() ) &&
       ( ( source1.numCols()!=0 ) && ( source2.numCols()!=0 ) ) )
    throw( std::runtime_error("hstack: matrices are inconsistent") );
  int num_rows = source1.numRows() ? source1.numRows() : source2.numRows();
  reshape( dest, num_rows, source1.numCols()+source2.numCols() );
  int cntr = 0;
  for ( int j = 0; j < source1.numCols(); j++, cntr++ )
    for ( int i = 0; i < num_rows; i++ )
      dest(i,cntr) = source1(i,j);
  for ( int j = 0; j < source2.numCols(); j++, cntr++ )
    for ( int i = 0; i < num_rows; i++ )
      dest(i,cntr) = source2(i,j);
}

/**
 * Convert a static array to a "Vector.hpp" of vectors.
 * @param a array to be converted
 * @param n the number of vectors
 * @param m the size of each "Vector.hpp"
 * @return v the "Vector.hpp" of vectors
 */
template <typename O, typename T>
void convert(T a[], int m, int n, std::vector< Teuchos::SerialDenseVector<O,T> > &v)
{
  v.resize( m );
  for ( int i = 0; i < m; i++ )
    {
      Teuchos::SerialDenseVector<O,T> tmp( Teuchos::View, a + i * n, n );
      v[i] = tmp;
    }
};

/**
 * Convert a static array to a weakly ordered multiset of vectors.
 * Set allows for multiple keys with the same value.
 * @param a array to be converted
 * @param n the number of vectors
 * @param m the size of each "Vector.hpp"
 * @return s the set
 */
template <typename O, typename T>
void convert(T a[], int m, int n, std::set< Teuchos::SerialDenseVector<O,T> > &s)
{
  for ( int i = 0; i < m; i++ )
    {
      Teuchos::SerialDenseVector<O,T> v( Teuchos::View, a+i*n, n );
      s.insert( v );
    }
};

/**
 * \brief Convert a std::vector of vectors to a matrix
 *
 * Each element of the std::vector becomes a column of the matrix
 */
template <typename O, typename T>
void convert( const std::vector< Teuchos::SerialDenseVector<O,T> > &V,
	      Teuchos::SerialDenseMatrix<O,T> &M )
{
  M.shapeUninitialized( V[0].length(), (int)V.size() );
  for ( int i = 0; i < (int)V.size(); i++ )
    {
      for ( int j = 0; j < V[0].length(); j++ )
	{
	  M(j,i) = V[i][j];
	}
    };
};

/**
 * Add a set of vectors together.
 * @param vectors set of vectors to be added
 * @param result the accumualted values
 */
template< typename T, typename Operator>
void accumulate( const std::vector< std::vector<T> > &vectors,
		 std::vector<T> &result, Operator op)
{
  if( !vectors.empty() )
    {
	//invariant: all vectors are of the same size
      result = vectors[0] ;
      int N = result.size() ;
      for( int i = 1 ; i < vectors.size() ; ++i )
	{
	  #ifdef DEBUG
	  if ( vectors[i].size() != N )
	    {
	      throw(std::runtime_error("Accumulate() vectors must all have the same size"));
	    }
	  #endif
	  std::transform( result.begin(), result.end(), vectors[i].begin(),
			  result.begin(), op );
	}
    }
  else
    {
      result.clear();
    }
};

template< typename T >
bool is_nan_or_inf( T x )
{
  if ( x != x ||
       x >= std::numeric_limits<T>::infinity() ||
       x <= -std::numeric_limits<T>::infinity() ||
       x >= std::numeric_limits<T>::max() ||
       x <= -std::numeric_limits<T>::max() )
    return true;
  return false;
}

template <typename O, typename T>
bool has_nan_or_inf( const Teuchos::SerialDenseMatrix<O,T> &matrix )
{
  for ( O j = 0 ; j < matrix.numCols(); j++ )
    {
      for ( O i = 0 ; i < matrix.numRows(); i++ )
	{
	  if ( is_nan_or_inf<T>( matrix(i,j ) ) )
	    {
	      disp( matrix(i,j) );
	      return true;
	    }
	}
    }
  return false;
}

void get_column_norms( RealMatrix &A, RealVector &result );

}  // namespace Pecos

#endif