#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
#include <chrono>
#include "Core/Utilities/QProgInfo/Visualization/QVisualization.h"
#include "QAlg/Base_QCircuit/AmplitudeEncode.h"

USING_QPANDA
using namespace std;
using namespace chrono;

#define PRINT_TRACE 0
#if PRINT_TRACE
#define PTrace printf
#define PTraceMat(mat) (std::cout << (mat) << endl)
#define PTraceCircuit(cir) (std::cout << cir << endl)
#else
#define PTrace
#define PTraceMat(mat)
#define PTraceCircuit(cir)
#endif

#define MAX_MATRIX_PRECISION 1e-10

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator &entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

    return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto &units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

    return;
}

static void controller(MatrixSequence &sequence, const EigenMatrix2c U2, EigenMatrixXc &matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto &val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
    return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        recursive_partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }

    return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, cdx) && (idx == opts - 1)))
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto entry = column[opt].first;
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

            units[units.size() - 1 - idx] = (units.end() == iter) ?
                MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

            column.emplace_back(make_pair(entry + path, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }

    return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
 	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

    return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit &circuit)
{
	std::sort(qubits.begin(), qubits.end(), [&](Qubit *a, Qubit *b)
	{
		return a->getPhysicalQubitPtr()->getQubitAddr()
			 < b->getPhysicalQubitPtr()->getQubitAddr();
	});

	auto rank = qubits.size();
	for (auto &val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					    << U4(val.second, qubits[qdx]).control(control).dagger()
						<< cir;

				break;
			}
		}
	}
}

/*******************************************************************
*                      class DiagonalMatrixDecompose
********************************************************************/
class DiagonalMatrixDecompose
{
public:
	DiagonalMatrixDecompose() {}
	~DiagonalMatrixDecompose() {}

	QCircuit decompose(const QVec& qubits, const QStat& src_mat)
	{
		//check param
		if (!is_unitary_matrix(src_mat))
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, the input matrix is not a unitary-matrix.");
		}

		const auto mat_dimension = sqrt(src_mat.size());
		const auto need_qubits_num = ceil(log2(mat_dimension));
		if (need_qubits_num > qubits.size())
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, no enough qubits.");
		}

		QCircuit decompose_result_cir;
		m_qubits = qubits;
		QVec controlqvec = qubits;
		controlqvec.pop_back();
		QStat tmp_mat22; //2*2 unitary matrix
		const size_t tmp_base_unitary_cnt = mat_dimension / 2;
		long pre_index = -1;
		for (size_t i = 0; i < tmp_base_unitary_cnt; ++i)
		{
			if (0 == i)
			{
				QCircuit index_cir_zero = index_to_circuit(0, controlqvec);
				decompose_result_cir << index_cir_zero;
			}
			else
			{
				QCircuit index_cir = index_to_merge_circuit(i, pre_index, controlqvec);
				decompose_result_cir << index_cir;
			}

			tmp_mat22.clear();
			const size_t tmp_row = (2 * i * mat_dimension) + (2 * i);
			tmp_mat22.push_back(src_mat[tmp_row]);
			tmp_mat22.push_back(src_mat[tmp_row + 1]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension + 1]);
			QGate tmp_u4 = U4(tmp_mat22, qubits.back()).control(controlqvec);
			QGATE_SPACE::U4* p_gate = dynamic_cast<QGATE_SPACE::U4*>(tmp_u4.getQGate());
			if ((abs(p_gate->getAlpha()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getBeta()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getGamma()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getDelta()) > MAX_MATRIX_PRECISION))
			{
				decompose_result_cir << tmp_u4;
			}

			pre_index = i;
		}

		return decompose_result_cir;
	}

protected:
	QCircuit index_to_circuit(size_t index, QVec& controlqvec)
	{
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if (0 == index % 2)
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t pre_index = index - 1;
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
			pre_index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, long pre_index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t tmp_pre_index = pre_index;
		if (pre_index < 0)
		{
			tmp_pre_index = 1;
		}

		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (tmp_pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;

			if (pre_index > 0)
			{
				tmp_pre_index /= 2;
			}
		}

		return ret_cir;
	}

private:
	QVec m_qubits;
};

/*******************************************************************
*                      public interface
********************************************************************/
QCircuit QPanda::matrix_decompose_qr(QVec qubits, const QStat& src_mat)
{
	auto order = std::sqrt(src_mat.size());
	EigenMatrixXc tmp_mat = EigenMatrixXc::Map(&src_mat[0], order, order);

    return matrix_decompose_qr(qubits, tmp_mat);
}

QCircuit QPanda::matrix_decompose_qr(QVec qubits, EigenMatrixXc& src_mat)
{
	if (!src_mat.isUnitary(MAX_MATRIX_PRECISION))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error or the input matrix is not a 2^n-dimensional matrix.");
	}

	QCircuit output_circuit;
    //QR decompose
    std::vector<SingleGateUnit> cir_units;
    general_scheme(src_mat, cir_units);
    circuit_insert(qubits, cir_units, output_circuit);

	return output_circuit;
}

QCircuit QPanda::diagonal_matrix_decompose(const QVec& qubits, const QStat& src_mat)
{
	return DiagonalMatrixDecompose().decompose(qubits, src_mat);
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

// Jonathan Driedger, Thomas Prätzlich, and Meinard Müller
// Let It Bee — Towards NMF-Inspired Audio Mosaicing
// Proceedings of ISMIR 2015

#pragma once

#include "STFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>

namespace fluid {
namespace algorithm {

using _impl::asEigen;
using _impl::asFluid;
using Eigen::Array;
using Eigen::ArrayXd;
using Eigen::ArrayXXd;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;

class NMFCross
{

public:
  // pass iteration number; returns true if able to continue (i.e. not
  // cancelled)
  using ProgressCallback = std::function<bool(index)>;

  NMFCross(index nIterations) : mIterations(nIterations) {}

  static void synthesize(const RealMatrixView h, const ComplexMatrixView w,
                         ComplexMatrixView out)
  {
    using namespace Eigen;
    using namespace _impl;
    MatrixXd  H = asEigen<Matrix>(h);
    MatrixXcd W = asEigen<Matrix>(w);
    MatrixXcd V = H * W;
    out <<= asFluid(V);
  }

  void process(const RealMatrixView X, RealMatrixView H1, RealMatrixView W0,
               index r, index p, index c) const
  {
    index nFrames = X.extent(0);
    index nBins = X.extent(1);
    index rank = W0.extent(0);
    nBins = W0.extent(1);
    MatrixXd W = asEigen<Matrix>(W0).transpose();
    MatrixXd H;
    H = MatrixXd::Random(rank, nFrames) * 0.5 +
        MatrixXd::Constant(rank, nFrames, 0.5);
    MatrixXd V = asEigen<Matrix>(X).transpose();
    multiplicativeUpdates(V, W, H, r, p, c);
    MatrixXd HT = H.transpose();
    H1 <<= asFluid(HT);
  }

  void addProgressCallback(ProgressCallback&& callback)
  {
    mCallbacks.emplace_back(std::move(callback));
  }

private:
  index                         mIterations;
  std::vector<ProgressCallback> mCallbacks;

  std::vector<index> topC(Eigen::VectorXd vec, index c) const
  {
    using namespace std;
    vector<double> stdVec(vec.data(), vec.data() + vec.size());
    sort(stdVec.begin(), stdVec.end());
    vector<index> idx(asUnsigned(vec.size()));
    iota(idx.begin(), idx.end(), 0);
    sort(idx.begin(), idx.end(),
         [&vec](index i1, index i2) { return vec[i1] > vec[i2]; });
    auto result = std::vector<index>(idx.begin(), idx.begin() + c);
    return result;
  }

  Eigen::MatrixXd promoteContinuity(MatrixXd& H, index size) const
  {
    index    halfSize = (size - 1) / 2;
    MatrixXd kernel = MatrixXd::Identity(size, size);
    MatrixXd padded = MatrixXd::Zero(H.rows() + size, H.cols() + size);
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    padded.block(halfSize, halfSize, H.rows(), H.cols()) = H;
    for (index i = 0; i < H.rows(); i++)
    {
      for (index j = 0; j < H.cols(); j++)
      {
        output(i, j) =
            padded.block(i, j, size, size).cwiseProduct(kernel).sum();
      }
    }
    return output;
  }

  Eigen::MatrixXd enforceTemporalSparseness(MatrixXd& H, index size,
                                            index iteration) const
  {
    index    halfSize = (size - 1) / 2;
    MatrixXd padded = MatrixXd::Zero(H.rows(), H.cols() + size);
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    padded.block(0, halfSize, H.rows(), H.cols()) = H;
    for (index i = 0; i < H.rows(); i++)
    {
      for (index j = 0; j < H.cols(); j++)
      {
        VectorXd        neighborhood = padded.row(i).segment(j, size);
        VectorXd::Index maxIndex{0};
        neighborhood.maxCoeff(&maxIndex);
        if (int(maxIndex) != halfSize)
        { output(i, j) = H(i, j) * (1 - ((iteration + 1) / mIterations)); }
        else
        {
          output(i, j) = H(i, j);
        }
      }
    }
    return output;
  }

  Eigen::MatrixXd restrictPolyphony(MatrixXd& H, ArrayXd& energyInW, index size,
                                    index iteration) const
  {
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    for (index k = 0; k < H.cols(); k++)
    {
      ArrayXd wCol = H.col(k).array() * energyInW.array();
      output.col(k) = H.col(k) * (1 - ((iteration + 1) / mIterations));
      auto top = topC(wCol, size);
      for (auto t : top) { output(t, k) = H(t, k); }
    }
    return output;
  }
  void multiplicativeUpdates(MatrixXd& V, MatrixXd& W, MatrixXd& H, index r,
                             index p, index c) const
  {
    using namespace std;
    using namespace Eigen;
    double const epsilon = std::numeric_limits<double>::epsilon();
    MatrixXd     ones = MatrixXd::Ones(V.rows(), V.cols());
    W = W.array().max(epsilon).matrix();
    // ArrayXd wNorm = W.colwise().sum();
    // W.array().rowwise() /= wNorm.transpose());
    ArrayXd energyInW = W.array().square().colwise().sum();
    for (index i = 0; i < mIterations; i++)
    {
      if ((i % 1) == 0)
      { // TODO: original version seems to work better with one in 5 iterations
        H = enforceTemporalSparseness(H, r, i);
        H = restrictPolyphony(H, energyInW, p, i);
        H = promoteContinuity(H, c);
      }
      ArrayXXd V2 = (W * H).array().max(epsilon);
      ArrayXXd hnum = (W.transpose() * (V.array() / V2).matrix()).array();
      ArrayXXd hden = (W.transpose() * ones).array();
      H = (H.array() * hnum / hden.max(epsilon)).matrix();
      // MatrixXd R = W * H;
      // R = R.cwiseMax(epsilon);
      // double divergence = (V.cwiseProduct(V.cwiseQuotient(R)) - V + R).sum();
      for (auto& cb : mCallbacks)
        if (!cb(i + 1)) return;
    }
    V = W * H;
  }
};
} // namespace algorithm
} // namespace fluid


#ifndef HAMILTONIANS_XXXXMG_HPP
#define HAMILTONIANS_XXXXMG_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class XXXMG
{
private:
	int n_;

public:

	XXXMG(int n)
		: n_(n)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "XXXXMG"},
			{"n", n_},
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		constexpr double J1 = 1.0;
		constexpr double J2 = 0.5;
		typename State::Scalar s = 0.0;

		{
			double yysign = -smp.sigmaAt(0)*smp.sigmaAt(1);
			s += -J1/2*yysign; //zz
			s += J1/2*(1.0+yysign)*smp.ratio(0, 1); //xx+yy
		}
		{
			double yysign = -smp.sigmaAt(n_-2)*smp.sigmaAt(n_-1);
			s += -J1/2*yysign; //zz
			s += J1/2*(1.0+yysign)*smp.ratio(n_-2, n_-1); //xx+yy
		}
		//Nearest-neighbor
		for(int i = 1; i < (n_-3); i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt(i+1);
			s += -J1*yysign; //zz
			s += J1*(1.0+yysign)*smp.ratio(i, i+1); //xx+yy
		}
		//Next-nearest-neighbor
		for(int i = 0; i < n_-3; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt(i+2);
			s += -J2*yysign; //zz
			s += J2*(1.0+yysign)*smp.ratio(i, i+2); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		constexpr double J1 = 1.0;
		constexpr double J2 = 0.5;
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col ^ x] += J1*(1.0 - sgn*1.0);
			m[col] += J1*sgn;
		}
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+2)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+2)%(n_)));
			m[col ^ x] += J2*(1.0 - sgn*1.0);
			m[col] += J2*sgn;
		}
		return m;
	}
};
#endif//HAMILTONIANS_XXXXMG_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_gaussian_copula_policy_hpp
#define quantlib_gaussian_copula_policy_hpp

#include <vector>
#include <numeric>
#include <algorithm>

#include <boost/bind.hpp>

#include <ql/utilities/disposable.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

namespace QuantLib {

    /*! Gaussian Latent Model's copula policy. Its simplicity is a result of
      the convolution stability of the Gaussian distribution.
    */
    /* This is the only case that would have allowed the policy to be static,
    but other copulas will need parameters and initialization.*/
    struct GaussianCopulaPolicy {

        typedef int initTraits;

        explicit GaussianCopulaPolicy(
            const std::vector<std::vector<Real> >& factorWeights =
                std::vector<std::vector<Real> >(),
            const initTraits& dummy = int())
        : numFactors_(factorWeights.size() + factorWeights[0].size())
        {
            /* check factors in LM are normalized. */
            for(Size iLVar=0; iLVar<factorWeights.size(); iLVar++) {
                Real factorsNorm =
                    std::inner_product(factorWeights[iLVar].begin(),
                        factorWeights[iLVar].end(),
                        factorWeights[iLVar].begin(), 0.);
                QL_REQUIRE(factorsNorm < 1.,
                    "Non normal random factor combination.");
            }
            /* check factor matrix is squared .......... */
        }

        /*! Number of independent random factors.
        This is the only methos that ould stop the class from being static, it
        is needed for the MC generator construction.
        */
        Size numFactors() const {
            return numFactors_;
        }

        //! returns a copy of the initialization arguments
        initTraits getInitTraits() const {
            return initTraits();
        }

        /*! Cumulative probability of the indexed latent variable
            @param iVariable The index of the latent variable requested.
        */
        Probability cumulativeY(Real val, Size iVariable) const {
            return cumulative_(val);
        }
        //! Cumulative probability of the idiosyncratic factors (all the same)
        Probability cumulativeZ(Real z) const {
            return cumulative_(z);
        }
        /*! Probability density of a given realization of values of the systemic
          factors (remember they are independent). In the normal case, since
          they all follow the same law it is just a trivial product of the same
          density.
          Intended to be used in numerical integration of an arbitrary function
          depending on those values.
        */
        Probability density(const std::vector<Real>& m) const {
            return std::accumulate(m.begin(), m.end(), 1.,
                boost::bind(std::multiplies<Real>(), _1,
                    boost::bind(density_, _2)));
        }
        /*! Returns the inverse of the cumulative distribution of the (modelled)
          latent variable (as indexed by iVariable). The normal stability avoids
          the convolution of the factors' distributions
        */
        Real inverseCumulativeY(Probability p, Size iVariable) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        /*! Returns the inverse of the cumulative distribution of the
        idiosyncratic factor (identically distributed for all latent variables)
        */
        Real inverseCumulativeZ(Probability p) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        /*! Returns the inverse of the cumulative distribution of the
          systemic factor iFactor.
        */
        Real inverseCumulativeDensity(Probability p, Size iFactor) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        //!
        //to use this (by default) version, the generator must be a uniform one.
        Disposable<std::vector<Real> >
            allFactorCumulInverter(const std::vector<Real>& probs) const {
            std::vector<Real> result;
            result.resize(probs.size());
            std::transform(probs.begin(), probs.end(), result.begin(),
                boost::bind(&InverseCumulativeNormal::standard_value, _1));
            return result;
        }
    private:
        mutable Size numFactors_;
        // no op =
        static const NormalDistribution density_;
        static const CumulativeNormalDistribution cumulative_;
    };

}

#endif


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE AutoDiff test
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// Eigen
#include <Eigen/Core>
#include <unsupported/Eigen/AutoDiff>

// AutoDiff
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> derivative_t;
typedef Eigen::AutoDiffScalar<derivative_t> scalar_t;

/*
namespace std
{
scalar_t sin(const scalar_t& t)
{
  return Eigen::sin(t);
}
scalar_t cos(const scalar_t& t)
{
  return Eigen::cos(t);
}
}
*/

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>
namespace sva
{
template<>
inline Matrix3<scalar_t> RotZ<scalar_t>(scalar_t theta)
{
  using namespace Eigen;

  Matrix3<scalar_t> ret;
  scalar_t z(0., derivative_t::Zero(theta.derivatives().rows()));
  scalar_t o(1., derivative_t::Zero(theta.derivatives().rows()));
  scalar_t s = sin(theta), c = cos(theta);

  ret << c, s, z, -s, c, z, z, z, o;

  return ret;
}
} // namespace sva

using boost::math::constants::pi;

BOOST_AUTO_TEST_CASE(PTransformVsPTransform)
{
  using namespace sva;
  using namespace Eigen;

  {
    Vector3<scalar_t> pt1T(0., 1., 0.);
    pt1T[0].derivatives().setZero(2);
    pt1T[1].derivatives().setZero(2);
    pt1T[1].derivatives()(0) = 1.;
    pt1T[2].derivatives().setZero(2);

    PTransform<scalar_t> pt1(pt1T);
    PTransformd pt2(Vector3d(1., 0., 0.));

    PTransform<scalar_t> res1 = pt1 * pt1;
    PTransform<scalar_t> res2 = pt1 * PTransform<scalar_t>{pt2};
    BOOST_CHECK(PTransform<scalar_t>{pt2} == pt2.cast<scalar_t>());

    std::cout << res1.translation()[0].derivatives().transpose() << std::endl;
    std::cout << res1.translation()[1].derivatives().transpose() << std::endl;
    std::cout << res1.translation()[2].derivatives().transpose() << std::endl;

    std::cout << std::endl;

    std::cout << res2.translation()[0].derivatives().transpose() << std::endl;
    std::cout << res2.translation()[1].derivatives().transpose() << std::endl;
    std::cout << res2.translation()[2].derivatives().transpose() << std::endl;

    std::cout << std::endl;
  }

  {
    Vector3<scalar_t> prismY(0., 1., 0.);
    prismY[0].derivatives().setZero(2, 1);
    prismY[1].derivatives().setZero(2, 1);
    prismY[1].derivatives()(0) = 1.;
    prismY[2].derivatives().setZero(2, 1);

    scalar_t rotZ = pi<double>() / 4.;
    rotZ.derivatives().setZero(2, 1);
    rotZ.derivatives()(1) = 1.;

    // PTransform<scalar_t> Rot(RotZ(rotZ));
    PTransform<scalar_t> Rot(AngleAxis<scalar_t>(-rotZ, Vector3<scalar_t>::UnitZ()).matrix());
    PTransform<scalar_t> Prism(prismY);

    PTransform<scalar_t> res = Prism * Rot;

    std::cout << res.translation()[0].derivatives().transpose() << std::endl;
    std::cout << res.translation()[1].derivatives().transpose() << std::endl;
    std::cout << res.translation()[2].derivatives().transpose() << std::endl;

    std::cout << std::endl;
  }
}


#include "stat_scheduler.h"

#include <boost/optional.hpp>

#include <cmath>

namespace {

// 10%
const long double DEFAULT_TOLERANCE = 0.1;

long double CalcMean(long double curMean, size_t numItems, long double newItem) {
    long double newMean = (curMean * numItems + newItem) / (numItems + 1);
    return newMean;
}

bool IsDiffAllowed(long double diff, long double mean,
                   long double tolerance = DEFAULT_TOLERANCE) {
    // 10% burst is allowed
    if (fabs(diff / mean) <= tolerance) {
        return true;
    }

    return false;
}

} // ns

namespace Meteor {

StatTracker::StatTracker()
    : ResourcesTracker(1)
{
}

bool StatTracker::FindFreeNode(const Job& job, Node& node) {
    ResourceQuantity bestDiff = std::numeric_limits<ResourceQuantity>::max();
    boost::optional<Node> bestNode;
    for (const auto& it : State) {
        ResourceQuantity resAvailable = it.second.Current[0];
        if (job.ResQuantity <= resAvailable) {
            ResourceQuantity diff = resAvailable - job.ResQuantity;
            if (diff <= bestDiff) {
                bestDiff = diff;
                bestNode = it.first;
            }
        }
    }

    if (bestNode) {
        node = *bestNode;
        Acquire(node, 0, job.ResQuantity);
        return true;
    }

    return false;
}

StatScheduler::StatScheduler(StatTracker& tracker)
    : Tracker(tracker)
    , DurationMean(0.0)
    , ResourceMean(0.0)
    , NumScheduled(0)
{
}

EventList StatScheduler::Schedule(const Job& job) {
    EventList events = ClearFinished(Tracker);

    bool diffIsAllowed = true;
    if (NumScheduled > 0) {
        long double durationDiff = job.Dur - DurationMean;
        long double resourceDiff = job.ResQuantity - ResourceMean;

        diffIsAllowed = (IsDiffAllowed(durationDiff, DurationMean)
                         && IsDiffAllowed(resourceDiff, ResourceMean));
    }

    Node node;
    if (diffIsAllowed && Tracker.FindFreeNode(job, node)) {
        // Resource is consumed here
        ScheduledJob scheduledJob(job, node);
        Jobs.insert(Timeline::value_type(CurTime + job.Dur, scheduledJob));
        events.push_back(Event(EventType::Scheduled, CurTime, node));

        DurationMean = CalcMean(DurationMean, NumScheduled, job.Dur);
        ResourceMean = CalcMean(ResourceMean, NumScheduled, job.ResQuantity);

        ++NumScheduled;
    } else {
        events.push_back(Event(EventType::Rejected, CurTime, Node()));
    }

    ++CurTime;

    return events;
}

} // Meteor


#ifndef STAN_MATH_PRIM_SCAL_FUN_OWENS_T_HPP
#define STAN_MATH_PRIM_SCAL_FUN_OWENS_T_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/owens_t.hpp>

namespace stan {
namespace math {

/**
 * Return the result of applying Owen's T function to the
 * specified arguments.
 *
 * Used to compute the cumulative density function for the skew normal
 * distribution.
 *
   \f[
   \mbox{owens\_t}(h, a) =
   \begin{cases}
     \mbox{owens\_t}(h, a) & \mbox{if } -\infty\leq h, a \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } h = \textrm{NaN or } a = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial h} =
   \begin{cases}
     \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial h} & \mbox{if }
 -\infty\leq h, a\leq \infty \\[6pt] \textrm{NaN} & \mbox{if } h = \textrm{NaN
 or } a = \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial a} =
   \begin{cases}
     \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial a} & \mbox{if }
 -\infty\leq h, a\leq \infty \\[6pt] \textrm{NaN} & \mbox{if } h = \textrm{NaN
 or } a = \textrm{NaN} \end{cases} \f]

   \f[
   \mbox{owens\_t}(h, a) = \frac{1}{2\pi} \int_0^a
 \frac{\exp(-\frac{1}{2}h^2(1+x^2))}{1+x^2}dx \f]

   \f[
   \frac{\partial \, \mbox{owens\_t}(h, a)}{\partial h} =
 -\frac{1}{2\sqrt{2\pi}} \operatorname{erf}\left(\frac{ha}{\sqrt{2}}\right)
   \exp\left(-\frac{h^2}{2}\right)
   \f]

   \f[
   \frac{\partial \, \mbox{owens\_t}(h, a)}{\partial a} =
 \frac{\exp\left(-\frac{1}{2}h^2(1+a^2)\right)}{2\pi (1+a^2)} \f]
 *
 * @param h First argument
 * @param a Second argument
 * @return Owen's T function applied to the arguments.
 */
inline double owens_t(double h, double a) { return boost::math::owens_t(h, a); }
}  // namespace math
}  // namespace stan

#endif


#include "rational.h"
#include <Eigen/Eigen>

#define TOL     (1e-20f)

namespace Rational
{

int gcd (int a, int b)
{
  ASSERT(a >= 0, "gcd arg 1 is not positive: " << a);
  ASSERT(b >= 0, "gcd arg 2 is not positive: " << b);
  ASSERT(a % 1 == 0, "gcd arg 1 is not an integer: " << a);
  ASSERT(b % 1 == 0, "gcd arg 2 is not an integer: " << b);

  if (b > a) std::swap(a, b);
  if (b == 0) return 1; // gcd(0, anything) = 0

  while (true) {
    a %= b;
    if (a == 0) return b;
    b %= a;
    if (b == 0) return a;
  }
}

std::vector<Number> ball_of_radius (float radius)
{
  std::vector<Number> result;
  for (int i = 1; i < radius; ++i) {
    for (int j = 1; i * i + j * j <= radius * radius; ++j) {
      if (gcd(i, j) == 1) {
        result.push_back(Number(i, j));
      }
    }
  }
  std::sort(result.begin(), result.end());
  return result;
}

//----( harmony )-------------------------------------------------------------

Harmony::Harmony (
    float max_radius,
    float prior_sec, // TODO what is this used for
    float acuity,
    float sustain_sec,
    float attack_sec,
    float randomize_rate)

  : m_attack(attack_sec / DEFAULT_AUDIO_FRAMERATE),
    m_sustain(sustain_sec / DEFAULT_AUDIO_FRAMERATE),
    m_randomize_rate(randomize_rate / sqrtf(DEFAULT_AUDIO_FRAMERATE)),
    m_points(ball_of_radius(max_radius)),

    m_pitch(m_points.size()),
    m_energy_matrix(* new MatrixXf(m_points.size(), m_points.size())),
    m_mass_vector(* new VectorXf(m_points.size())),
    m_prior_vector(* new VectorXf(m_points.size())),
    m_mass(m_points.size(), m_mass_vector.data()),
    m_prior(m_points.size(), m_prior_vector.data()),
    m_analysis(m_points.size()),
    m_dmass(m_points.size()),

    m_anal(m_points, acuity),
    m_synth(m_points)
{
  size_t size = m_points.size();
  LOG("Building Harmony with " << size << " points");

  for (size_t i = 0; i < size; ++i) {
    m_pitch[i] = m_points[i].to_float();
  }

  for (size_t i = 0; i < size; ++i) {
    for (size_t j = 0; j < size; ++j) {
      m_energy_matrix(i, j) = m_points[i].dissonance(m_points[j]) / acuity;
    }
  }

  ASSERT(size % 2, "Harmony does not have an odd number of points");
  m_mass.zero();
  m_mass[(size - 1) / 2] = 1; // start with all mass at center
  m_analysis.zero();
  m_dmass.zero();
}

Harmony::~Harmony ()
{
  delete & m_energy_matrix;
  delete & m_mass_vector;
  delete & m_prior_vector;
}

static const float BOGUS_MIN_FREQ = 0.1f;
static const float BOGUS_MAX_FREQ = 10.0f;

Harmony::Analyzer::Analyzer (
    const std::vector<Number> & points,
    float acuity)
  : Synchronized::FourierBank2(Bank(
        points.size(),
        BOGUS_MIN_FREQ,
        BOGUS_MAX_FREQ,
        acuity))
{
  // adapted from Synchronized::Bank::init_decay_transform

  ASSERT_EQ(points.size(), size);
  const float omega0 = 2 * M_PI * MIDDLE_C_HZ / DEFAULT_SAMPLE_RATE;
  const float order = 2;
  const float damp_factor = pow(2, 1.0 / order) - 1;
  const float dpitch = log(2) / acuity;
  const float min_timescale = DEFAULT_FRAMES_PER_BUFFER;

  for (size_t i = 0, I = size; i < I; ++i) {
    double freq = omega0 * points[i].to_float();
    double dfreq = 1 / (1 / fabs(dpitch * freq) + min_timescale);
    std::complex<double> omega(-damp_factor * dfreq, freq);
    std::complex<double> trans = exp(omega);

    m_trans_real[i] = trans.real();
    m_trans_imag[i] = trans.imag();
    m_rescale[i] = pow(dfreq, order); // = 1 / E(w,0)
  }
}

Harmony::Synthesizer::Synthesizer (const std::vector<Number> & points)
  : Synchronized::SimpleBank(Bank(
        points.size(),
        BOGUS_MIN_FREQ,
        BOGUS_MAX_FREQ))
{
  // adapted from Synchronized::Bank::init_transform

  ASSERT_EQ(points.size(), size);
  float omega0 = 2 * M_PI * MIDDLE_C_HZ / DEFAULT_SAMPLE_RATE;
  for (size_t i = 0; i < size; ++i) {
    float omega = omega0 * points[i].to_float();

    m_frequency[i] = tan(omega);
  }
}

void Harmony::compute_prior ()
{
  const size_t F = m_points.size();

  float radius_scale = 1.0f / sum(m_mass);
  m_prior_vector.noalias() = m_energy_matrix * m_mass_vector;
  m_prior_vector *= radius_scale;

  float prior_total = 0;
  for (size_t i = 0; i < F; ++i) {
    prior_total += m_prior[i] = expf(-m_prior[i]);
  }
  float prior_scale = prior_total > 0 ? 1 / prior_total : 0.0f;
  for (size_t i = 0; i < F; ++i) {
    m_prior[i] *= prior_scale * m_prior[i];
  }
}

void Harmony::centralize_pitch ()
{
  const size_t F = m_points.size();

  float pitch_pitch = 0;
  float dmass_pitch = 0;
  for (size_t i = 0; i < F; ++i) {
    float p = m_pitch[i];
    float m = max(TOL, m_mass[i] + m_dmass[i]);
    float metric = 1.0f / max(TOL, sqr(m));
    pitch_pitch += p * metric * p;
    dmass_pitch += p * metric * m;
  }

  const float dmass_coeff = -dmass_pitch / max(TOL, pitch_pitch);
  for (size_t i = 0; i < F; ++i) {
    m_dmass[i] += dmass_coeff * m_pitch[i];
    m_dmass[i] = max(m_dmass[i], TOL - m_mass[i]);
  }
}

void Harmony::analyze (const Vector<complex> & sound_in)
{
  m_anal.sample(sound_in, m_analysis);
}

void Harmony::sample (Vector<complex> & sound_accum)
{
  const size_t F = m_points.size();

  compute_prior();

  for (size_t i = 0; i < F; ++i) {
    m_dmass[i] = m_attack * m_analysis[i]
               + (1.0f - m_sustain) * (m_prior[i] - m_mass[i])
               + m_mass[i] * (expf(m_randomize_rate * random_std()) - 1);
    m_dmass[i] = max(m_dmass[i], TOL - m_mass[i]);
  }

  centralize_pitch();

  m_synth.sample_accum(m_mass, m_dmass, sound_accum);
  m_mass += m_dmass;
}

} // namespace Rational


// Copyright (c) 2019-2020 The Dash Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "test/test_pivx.h"
#include "bls/bls_ies.h"
#include "bls/key_io.h"
#include "bls/bls_worker.h"
#include "bls/bls_wrapper.h"
#include "random.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(bls_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(bls_sig_tests)
{
    CBLSSecretKey sk1, sk2;
    sk1.MakeNewKey();
    sk2.MakeNewKey();

    uint256 msgHash1 = uint256S("0000000000000000000000000000000000000000000000000000000000000001");
    uint256 msgHash2 = uint256S("0000000000000000000000000000000000000000000000000000000000000002");

    auto sig1 = sk1.Sign(msgHash1);
    auto sig2 = sk2.Sign(msgHash1);

    BOOST_CHECK(sig1.VerifyInsecure(sk1.GetPublicKey(), msgHash1));
    BOOST_CHECK(!sig1.VerifyInsecure(sk1.GetPublicKey(), msgHash2));

    BOOST_CHECK(sig2.VerifyInsecure(sk2.GetPublicKey(), msgHash1));
    BOOST_CHECK(!sig2.VerifyInsecure(sk2.GetPublicKey(), msgHash2));

    BOOST_CHECK(!sig1.VerifyInsecure(sk2.GetPublicKey(), msgHash1));
    BOOST_CHECK(!sig1.VerifyInsecure(sk2.GetPublicKey(), msgHash2));
    BOOST_CHECK(!sig2.VerifyInsecure(sk1.GetPublicKey(), msgHash1));
    BOOST_CHECK(!sig2.VerifyInsecure(sk1.GetPublicKey(), msgHash2));
}

static BLSIdVector GetRandomBLSIds(size_t n)
{
    BLSIdVector v;
    for (size_t i = 0; i < n; i++) {
        v.emplace_back(GetRandHash());
    }
    return v;
}

std::vector<size_t> GetRandomElements(size_t m, size_t n)
{
    assert(m <= n);
    std::vector<size_t> idxs;
    for (size_t i = 0; i < n; i++) {
        idxs.emplace_back(i);
    }
    Shuffle(idxs.begin(), idxs.end(), FastRandomContext());
    return std::vector<size_t>(idxs.begin(), idxs.begin() + m);
}

struct Member
{
    CBLSId id;
    BLSVerificationVectorPtr vecP;
    CBLSIESMultiRecipientObjects<CBLSSecretKey> contributions;
    CBLSSecretKey skShare;

    // member (operator) keys for encryption/decryption of contributions
    CBLSSecretKey sk;
    CBLSPublicKey pk;

    Member(const CBLSId& _id): id(_id)
    {
        sk.MakeNewKey();
        pk = sk.GetPublicKey();
    }
};

BOOST_AUTO_TEST_CASE(dkg)
{
    CBLSWorker worker;
    const size_t N = 40;     // quorum size
    const size_t M = 30;     // threshold

    worker.Start();

    // Create N Members first
    const BLSIdVector& ids = GetRandomBLSIds(N);
    std::vector<Member> quorum;
    for (const auto& id : ids) {
        quorum.emplace_back(Member(id));
    }

    // Then generate contributions for each one
    for (Member& m : quorum) {
        // Generate contributions (plain text)
        BLSSecretKeyVector pt_contributions;
        worker.GenerateContributions((int)M, ids, m.vecP, pt_contributions);
        BOOST_CHECK_EQUAL(m.vecP->size(), M);
        BOOST_CHECK_EQUAL(pt_contributions.size(), N);
        // Init encrypted multi-recipient object
        m.contributions.InitEncrypt(N);
        for (size_t j = 0; j < N; j++) {
            const CBLSSecretKey& plaintext = pt_contributions[j];
            // Verify contribution against verification vector
            BOOST_CHECK(worker.VerifyContributionShare(ids[j], m.vecP, plaintext));
            // Encrypt each contribution with the recipient pk
            BOOST_CHECK(m.contributions.Encrypt(j, quorum[j].pk, plaintext, PROTOCOL_VERSION));
        }
    }

    // Aggregate received contributions for each Member to produce key shares
    for (size_t i = 0; i < N; i++) {
        Member& m = quorum[i];
        // Decrypt contributions received by m with m's secret key
        BLSSecretKeyVector rcvSkContributions;
        for (size_t j = 0; j < N; j++) {
            CBLSSecretKey contribution;
            BOOST_CHECK(quorum[j].contributions.Decrypt(i, m.sk, contribution, PROTOCOL_VERSION));
            rcvSkContributions.emplace_back(std::move(contribution));
        }
        m.skShare = worker.AggregateSecretKeys(rcvSkContributions);
        // Recover public key share for m, and check against the secret key share
        BLSPublicKeyVector rcvPkContributions;
        for (size_t j = 0; j < N; j++) {
            CBLSPublicKey pkContribution = worker.BuildPubKeyShare(quorum[j].vecP, m.id);
            // This is implied by VerifyContributionShare, but let's double check
            BOOST_CHECK(rcvSkContributions[j].GetPublicKey() == pkContribution);
            rcvPkContributions.emplace_back(pkContribution);
        }
        CBLSPublicKey pkShare = worker.AggregatePublicKeys(rcvPkContributions);
        BOOST_CHECK(m.skShare.GetPublicKey() == pkShare);
    }

    // Each member signs a message with its key share producing a signature share
    const uint256& msg = GetRandHash();
    BLSSignatureVector allSigShares;
    for (const Member& m : quorum) {
        allSigShares.emplace_back(m.skShare.Sign(msg));
    }

    // Pick M (random) key shares and recover threshold secret/public key
    const auto& idxs = GetRandomElements(M, N);
    BLSSecretKeyVector skShares;
    BLSIdVector random_ids;
    for (size_t i : idxs) {
        skShares.emplace_back(quorum[i].skShare);
        random_ids.emplace_back(quorum[i].id);
    }
    CBLSSecretKey thresholdSk;
    BOOST_CHECK(thresholdSk.Recover(skShares, random_ids));
    const CBLSPublicKey& thresholdPk = thresholdSk.GetPublicKey();

    // Check that the recovered threshold public key equals the verification
    // vector free coefficient
    std::vector<BLSVerificationVectorPtr> v;
    for (const Member& m : quorum) v.emplace_back(m.vecP);
    CBLSPublicKey pk = worker.BuildQuorumVerificationVector(v)->at(0);
    BOOST_CHECK(pk == thresholdPk);

    // Pick M (random, different BLSids than before) signature shares, and recover
    // the threshold signature
    const auto& idxs2 = GetRandomElements(M, N);
    BLSSignatureVector sigShares;
    BLSIdVector random_ids2;
    for (size_t i : idxs2) {
        sigShares.emplace_back(allSigShares[i]);
        random_ids2.emplace_back(quorum[i].id);
    }
    CBLSSignature thresholdSig;
    BOOST_CHECK(thresholdSig.Recover(sigShares, random_ids2));

    // Verify threshold signature against threshold public key
    BOOST_CHECK(thresholdSig.VerifyInsecure(thresholdPk, msg));

    // Now replace a signature share with an invalid signature, recover the threshold
    // signature again, and check that verification fails with the threshold public key
    CBLSSecretKey dummy_sk;
    dummy_sk.MakeNewKey();
    CBLSSignature dummy_sig = dummy_sk.Sign(msg);
    BOOST_CHECK(dummy_sig != sigShares[0]);
    sigShares[0] = dummy_sig;
    BOOST_CHECK(thresholdSig.Recover(sigShares, random_ids2));
    BOOST_CHECK(!thresholdSig.VerifyInsecure(thresholdPk, msg));

    worker.Stop();
}

BOOST_AUTO_TEST_CASE(bls_ies_tests)
{
    // Test basic encryption and decryption of the BLS Integrated Encryption Scheme.
    CBLSSecretKey aliceSk;
    aliceSk.MakeNewKey();
    const CBLSPublicKey alicePk = aliceSk.GetPublicKey();
    BOOST_CHECK(aliceSk.IsValid());

    CBLSSecretKey bobSk;
    bobSk.MakeNewKey();
    const CBLSPublicKey bobPk = bobSk.GetPublicKey();
    BOOST_CHECK(bobSk.IsValid());

    // Encrypt a std::string object
    CBLSIESEncryptedObject<std::string> iesEnc;

    // Since no pad is allowed, serialized length must be a multiple of AES_BLOCKSIZE (16)
    BOOST_CHECK(!iesEnc.Encrypt(bobPk, "message of length 20", PROTOCOL_VERSION));

    // Message of valid length (15 + 1 byte for the total len in serialization)
    std::string message = ".mess of len 15";
    BOOST_CHECK(iesEnc.Encrypt(bobPk, message, PROTOCOL_VERSION));

    // valid decryption.
    std::string decrypted_message;
    BOOST_CHECK(iesEnc.Decrypt(bobSk, decrypted_message, PROTOCOL_VERSION));
    BOOST_CHECK_EQUAL(decrypted_message, message);

    // Invalid decryption sk
    std::string decrypted_message2;
    iesEnc.Decrypt(aliceSk, decrypted_message2, PROTOCOL_VERSION);
    BOOST_CHECK(decrypted_message2 != message);

    // Invalid ephemeral pubkey
    decrypted_message2.clear();
    auto iesEphemeralPk = iesEnc.ephemeralPubKey;
    iesEnc.ephemeralPubKey = alicePk;
    iesEnc.Decrypt(bobSk, decrypted_message2, PROTOCOL_VERSION);
    BOOST_CHECK(decrypted_message2 != message);
    iesEnc.ephemeralPubKey = iesEphemeralPk;

    // Invalid iv
    decrypted_message2.clear();
    GetRandBytes(iesEnc.iv, sizeof(iesEnc.iv));
    iesEnc.Decrypt(bobSk, decrypted_message2, PROTOCOL_VERSION);
    BOOST_CHECK(decrypted_message2 != message);
}

template<typename BLSKey>
BLSKey FromHex(const std::string& str)
{
    BLSKey k;
    k.SetByteVector(ParseHex(str));
    return k;
}

BOOST_AUTO_TEST_CASE(bls_sk_io_tests)
{
    const auto& params = Params();

    CBLSSecretKey sk = FromHex<CBLSSecretKey>("2eb071f4c520b3102e8cb9f520783da252d33993dba0313b501d69d113af9d39");
    BOOST_ASSERT(sk.IsValid());

    // Basic encoding-decoding roundtrip
    std::string encodedSk = bls::EncodeSecret(params, sk);
    auto opSk2 = bls::DecodeSecret(params, encodedSk);
    BOOST_CHECK(opSk2 != nullopt);
    CBLSSecretKey sk2 = *opSk2;
    BOOST_CHECK(sk == sk2);

    // Invalid sk, one extra char
    encodedSk.push_back('f');
    auto opSk3 = bls::DecodeSecret(params, encodedSk);
    BOOST_CHECK(opSk3 == nullopt);

    // Invalid sk, one less char
    encodedSk.pop_back();
    encodedSk.pop_back();
    auto opSk4 = bls::DecodeSecret(params, encodedSk);
    BOOST_CHECK(opSk4 == nullopt);
}

BOOST_AUTO_TEST_CASE(bls_pk_io_tests)
{
    const auto& params = Params();

    CBLSPublicKey pk = FromHex<CBLSPublicKey>("901138a12a352c7e30408c071b1ec097f32ab735a12c8dbb43c637612a3f805668a6bb73894982366d287cf0b02aaf5b");
    BOOST_ASSERT(pk.IsValid());

    // Basic encoding-decoding roundtrip
    std::string encodedPk = bls::EncodePublic(params, pk);
    auto opPk2 = bls::DecodePublic(params, encodedPk);
    BOOST_CHECK(opPk2 != nullopt);
    CBLSPublicKey pk2 = *opPk2;
    BOOST_CHECK(pk == pk2);

    // Invalid pk, one extra char
    encodedPk.push_back('f');
    auto oppk3 = bls::DecodePublic(params, encodedPk);
    BOOST_CHECK(oppk3 == nullopt);

    // Invalid pk, one less char
    encodedPk.pop_back();
    encodedPk.pop_back();
    auto oppk4 = bls::DecodePublic(params, encodedPk);
    BOOST_CHECK(oppk4 == nullopt);
}

BOOST_AUTO_TEST_SUITE_END()


////////////////////////////////////////////////////////////////////////////////
// BendingEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Implements the bending energy from [Grinspun et al. 2003: Discrete Shells].
//  We provide analytical gradients for the energy, but resort to automatic
//  differentiation for Hessians.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/25/2019 15:43:17
////////////////////////////////////////////////////////////////////////////////
#ifndef BENDINGENERGY_HH
#define BENDINGENERGY_HH

#include <cmath>
#include <Eigen/Dense>
#include <array>
#include <MeshFEM/AutomaticDifferentiation.hh>

// Bending energy contributed by a single "hinge" (mesh edge) between two
// triangles.
// The triangle points are indexed as follows:
//            p0-----p1            n1   n2
//              \ 1 / \             ^   ^
//               \ /`.2\             \ /
//               p2   `.\             o
//                      p3
// We call the complement of the dihedral angle "theta". Then the bending energy is given by
//      0.5 (theta - theta_bar)^2 ||e_bar||/h_bar
// (we expect the code using this class to scale everything by the bending stiffness).

// Get the indices corresponding to the four vertices in the hinge stencil for halfedge "he".
//  p0-he->p1
//    \ 1 /2|
//     \ /`.|
//     p2  p3
// Note: "he" lies in triangle 2.
template<class HalfEdge>
std::array<int, 4> bendingHingeStencil(const HalfEdge &he) {
    assert(he.isPrimary() && !he.isBoundary());
    return {{ he.tail().index(),
              he.tip ().index(),
              he.opposite().next().tip().index(),
              he           .next().tip().index() }};
}

// Templated by real number type "_Real" for autodiff.
template<class _Real>
struct HingeEnergy {
    using Real = _Real;
    using Pt = Eigen::Matrix<_Real, 3, 1>;
    using Vec = Pt;

    // Reference configuration quantities
    // (We don't need autodiff types for these...)
    double e_bar_len, h_bar;
    double theta_bar;

    // Deformed configuration quantities
    Eigen::Matrix<Real, 3, 4> deformed_pts;
    Real theta, e_len;
    Real squared_dbl_A1, squared_dbl_A2;
    Vec N1, N2; // un-normalized triangle normals (cross products of edge vectors)
    Real e_01_dot_ehat, e_02_dot_ehat;
    Real e_11_dot_ehat, e_12_dot_ehat;

    // Copy the reference configuration quantities from an existing class of a different type
    template<class _Real2>
    HingeEnergy(HingeEnergy<_Real2> h2) : e_bar_len(h2.e_bar_len), h_bar(h2.h_bar), theta_bar(h2.theta_bar) {
        e_bar_len      = h2.e_bar_len;
        h_bar          = h2.h_bar;
        theta_bar      = h2.theta_bar;

        theta          = h2.theta;
        e_len          = h2.e_len;
        squared_dbl_A1 = h2.squared_dbl_A1;
        squared_dbl_A2 = h2.squared_dbl_A2;
        N1             = h2.N1;
        N2             = h2.N2;
        e_01_dot_ehat  = h2.e_01_dot_ehat;
        e_02_dot_ehat  = h2.e_02_dot_ehat;

        e_11_dot_ehat  = h2.e_11_dot_ehat;
        e_12_dot_ehat  = h2.e_12_dot_ehat;
    }

    HingeEnergy(Eigen::Ref<const Pt> ref_p0,
                Eigen::Ref<const Pt> ref_p1,
                Eigen::Ref<const Pt> ref_p2,
                Eigen::Ref<const Pt> ref_p3) {
        Vec e = ref_p1 - ref_p0;
        e_bar_len = e.norm();
        e /= e_bar_len;

        Vec ref_n1 = (ref_p2 - ref_p0).cross(ref_p1 - ref_p0),
            ref_n2 = (ref_p1 - ref_p0).cross(ref_p3 - ref_p0);
        double dbl_A1 = ref_n1.norm(),
               dbl_A2 = ref_n2.norm();
        h_bar = (dbl_A1 + dbl_A2) / (6.0 * e_bar_len); // 1/6 (h1 + h2) = 1/6 (b * h1 + b * h2) / b = 1/6(2 A1 + 2 A2) / b

        // Note: n1, n2 needn't be normalized since atan2 is invariant to uniform scaling of its arguments.
        theta_bar = atan2(ref_n2.cross(ref_n1).dot(e), ref_n1.dot(ref_n2)); // Note: can't use std::atan2 since this breaks ADL for autodiff types

        setDeformedConfiguration(ref_p0, ref_p1, ref_p2, ref_p3);
    }

    void setDeformedConfiguration(Eigen::Ref<const Pt> p0,
                                  Eigen::Ref<const Pt> p1,
                                  Eigen::Ref<const Pt> p2,
                                  Eigen::Ref<const Pt> p3) {
        deformed_pts.col(0) = p0;
        deformed_pts.col(1) = p1;
        deformed_pts.col(2) = p2;
        deformed_pts.col(3) = p3;

        Vec e = p1 - p0;
        e_len = e.norm();
        e /= e_len;

        N1 = (p2 - p0).cross(p1 - p0),
        N2 = (p1 - p0).cross(p3 - p0);

        squared_dbl_A1 = N1.squaredNorm();
        squared_dbl_A2 = N2.squaredNorm();

        // Note: n1, n2 needn't be normalized since atan2 is invariant to uniform scaling of its arguments.
        theta = atan2(N2.cross(N1).dot(e), N1.dot(N2)); // Note: can't use std::atan2 since this breaks ADL for autodiff types

        e_01_dot_ehat = e.dot(p1 - p2);
        e_02_dot_ehat = e.dot(p1 - p3); // really the negation of e_02 based on the labeling in the derivation figure...

        e_11_dot_ehat = e.dot(p2 - p0);
        e_12_dot_ehat = e.dot(p3 - p0);

        // Effectively disable this hinge's energy in degenerate configurations since
        // these will introduce large and pseudorandom values into the gradient and Hessian,
        // breaking the optimization.
        if ((e_len < 1e-9) || (squared_dbl_A1 < 1e-16) || (squared_dbl_A2 < 1e-16)) {
            e.setZero();
            e[0] = 1.0;
            theta = theta_bar;
            squared_dbl_A1 = 1.0;
            squared_dbl_A2 = 1.0;
            N1.setZero();
            N2.setZero();
            e_01_dot_ehat = e_02_dot_ehat = e_11_dot_ehat = e_12_dot_ehat = 0.0;
        }
    }

    // Gradient of theta with respect to the matrix [p0 | p1 | p2 | p3].
    Eigen::Matrix<Real, 3, 4> gradTheta() const {
        Eigen::Matrix<Real, 3, 4> result;
        result.col(0) = (e_01_dot_ehat / squared_dbl_A1) * N1 + (e_02_dot_ehat / squared_dbl_A2) * N2;
        result.col(1) = (e_11_dot_ehat / squared_dbl_A1) * N1 + (e_12_dot_ehat / squared_dbl_A2) * N2;
        result.col(2) = (-e_len / squared_dbl_A1) * N1;
        result.col(3) = (-e_len / squared_dbl_A2) * N2;
        return result;
    }

    using HessType = Eigen::Matrix<Real, 12, 12>;
    HessType hessTheta() const {
        HessType result;
        using ADType = Eigen::AutoDiffScalar<Eigen::Matrix<Real, 12, 1>>;
        HingeEnergy<ADType> diff_he(*this);

        Eigen::Matrix<ADType, 3, 4> ad_deformed_pts = deformed_pts;

        for (size_t j = 0; j < 12; ++j) {
            ad_deformed_pts.data()[j].derivatives().setZero();
            ad_deformed_pts.data()[j].derivatives()[j] = 1.0;
        }

        diff_he.setDeformedConfiguration(ad_deformed_pts.col(0),
                                         ad_deformed_pts.col(1),
                                         ad_deformed_pts.col(2),
                                         ad_deformed_pts.col(3));
        auto diff_g = diff_he.gradTheta();

        for (size_t i = 0; i < 12; ++i)
            result.row(i) = diff_g.data()[i].derivatives().transpose();

        return result;
    }

    Real energy() const {
        // Note: this is 1/2 the energy in [Grinspun 2003]
        return 0.5 * (theta - theta_bar) * (theta - theta_bar) * e_bar_len / h_bar;
    }

    Eigen::Matrix<Real, 3, 4> gradient() const {
        return ((theta - theta_bar) * e_bar_len / h_bar) * gradTheta();
    }

    HessType hessian() const {
        auto g = gradTheta();
        auto gFlattened = Eigen::Map<Eigen::Matrix<Real, 12, 1>>(g.data());
        return (e_bar_len / h_bar) * (
                gFlattened * gFlattened.transpose() +
                (theta - theta_bar) * hessTheta());
    }
};

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


/*
 * SlidingWindowMathExpressionFilter.hpp
 *
 *  Created on: Aug 18, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include "EigenLab/EigenLab.h"

#include <grid_map_core/grid_map_core.hpp>

#include <filters/filter_base.h>

#include <Eigen/Core>
#include <string>

namespace grid_map {

/*!
 * Parse and evaluate a mathematical matrix expression within a sliding window on a layer of a grid map.
 */
template<typename T>
class SlidingWindowMathExpressionFilter : public filters::FilterBase<T>
{

 public:
  /*!
   * Constructor
   */
  SlidingWindowMathExpressionFilter();

  /*!
   * Destructor.
   */
  virtual ~SlidingWindowMathExpressionFilter();

  /*!
   * Configures the filter from parameters on the parameter server.
   */
  virtual bool configure();

  /*!
   * Takes the minimum out of different layers of a grid map.
   * @param mapIn gridMap with the different layers to take the min.
   * @param mapOut gridMap with an additional layer containing the sum.
   */
  virtual bool update(const T& mapIn, T& mapOut);

 private:
  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayer_;

  //! EigenLab parser.
  EigenLab::Parser<Eigen::MatrixXf> parser_;

  //! Expression to parse.
  std::string expression_;

  //! Window size.
  int windowSize_;

  //! If window length (instead of window size) should be used.
  bool useWindowLength_;

  //! Window length.
  double windowLength_;

  //! If empty cells should be computed as well.
  bool isComputeEmptyCells_;

  //! Edge handling method.
  SlidingWindowIterator::EdgeHandling edgeHandling_;
};

} /* namespace */


#pragma once

#include "calotypes/KernelDensityEstimation.hpp"
#include "calotypes/WeightedSamplers.hpp"
#include "calotypes/DataSelector.hpp"

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace calotypes
{

// TODO Possibly expose ability to seed the engine directly, or give an engine reference?
/*! \brief Performs importance resampling to redraw samples from a set drawn according
 * to a proposal distribution to approximate a target distribution. */
template < class Data,
		   class Engine = boost::random::mt19937,
		   class Resampler = LowVarianceWeightedSampling<Engine> >
void ImportanceResample( const std::vector<Data>& samples,
						 const typename ProbabilityDensityFunction<Data>::Ptr& proposal,
						 const typename ProbabilityDensityFunction<Data>::Ptr& target,
						 unsigned int numSamples, std::vector<Data>& resampled )
{
	// 1. Calculate resampling weights according to target(x)/proposal(x)
	std::vector<double> weights( samples.size() );
// 	std::cout << "weights: " << std::endl;
	for( unsigned int i = 0; i < samples.size(); i++ )
	{
		weights[i] = (*target)( samples[i] ) / (*proposal)( samples[i] );
// 		std::cout << "\t " << samples[i].name << " (" << weights[i] << ")" << std::endl;
	}

	// 2. Resample based on the weights
	Engine engine;
	boost::random::random_device rng;
	engine.seed( rng );
	std::vector<unsigned int> resampleIndices;
	Resampler::Sample( weights, numSamples, resampleIndices, engine );

	// 3. Return the samples
	// TODO Verify that numSamples = resampleIndices.size()?
	resampled.resize( numSamples );
	for( unsigned int i = 0; i < numSamples; i++ )
	{
		resampled[i] = samples[ resampleIndices[i] ];
	}
}

template <class Data>
class ImportanceDataSelector
: public DataSelector<Data>
{
public:

	typedef std::shared_ptr<ImportanceDataSelector> Ptr;
	typedef std::vector<Data> Dataset;

	ImportanceDataSelector( const typename ProbabilityDensityFunction<Data>::Ptr& prop,
							const typename ProbabilityDensityFunction<Data>::Ptr& tar )
	: proposal( prop ), target( tar ) {}

	virtual void SelectData( const Dataset& data, unsigned int subsetSize, Dataset& subset )
	{
		ImportanceResample( data, proposal, target, subsetSize, subset );
	}

private:

	typename ProbabilityDensityFunction<Data>::Ptr proposal;
	typename ProbabilityDensityFunction<Data>::Ptr target;
};

} // end namespace calotypes


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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 <rw/math/Math.hpp>
#include <rw/math/MetricUtil.hpp>
#include <rw/math/Quaternion.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Vector3D.hpp>

#include <boost/math/special_functions/sign.hpp>
#include <gtest/gtest.h>
#include <math.h>

using namespace rw::math;
typedef Quaternion< double > QuatD;    // just abbreviations
typedef Quaternion< float > QuatF;

bool close_enough (Quaternion<> q1, Quaternion<> q2)
{
    return fabs ((q1).getQx () - (q2).getQx ()) < 1e-16 &&
           fabs ((q1).getQy () - (q2).getQy ()) < 1e-16 &&
           fabs ((q1).getQz () - (q2).getQz ()) < 1e-16 &&
           fabs ((q1).getQw () - (q2).getQw ()) < 1e-16;
}

Quaternion<> toQuatN (const Rotation3D<>& rot)
{
    double d = sqrt (std::max (0.0, 1 + rot (0, 0) + rot (1, 1) + rot (2, 2))) / 2;
    double a = sqrt (std::max (0.0, 1 + rot (0, 0) - rot (1, 1) - rot (2, 2))) / 2;
    double b = sqrt (std::max (0.0, 1 - rot (0, 0) + rot (1, 1) - rot (2, 2))) / 2;
    double c = sqrt (std::max (0.0, 1 - rot (0, 0) - rot (1, 1) + rot (2, 2))) / 2;
    a        = boost::math::copysign (a, rot (2, 1) - rot (1, 2));
    b        = boost::math::copysign (b, rot (0, 2) - rot (2, 0));
    c        = boost::math::copysign (c, rot (1, 0) - rot (0, 1));

    return Quaternion<> (a, b, c, d);
}

Quaternion<> toQuatN2 (const Rotation3D<>& rot)
{
    double a, b, c, d;
    double tr = 1.0;

    if (rot (0, 0) > rot (1, 1)) {
        tr -= rot (1, 1);
        if (rot (0, 0) > rot (2, 2)) {
            // 00 was biggest
            tr += rot (0, 0) - rot (2, 2);
            tr = sqrt (tr);
            d  = (rot (1, 2) - rot (2, 1)) / (2 * tr);
            a  = tr / 2;
            b  = (rot (0, 2) + rot (2, 0)) / (2 * tr);
            c  = (rot (1, 0) + rot (0, 1)) / (2 * tr);
        }
        else {
            // 22 was biggest
            tr += rot (2, 2) - rot (0, 0);
            tr = sqrt (tr);
            d  = (rot (1, 0) - rot (0, 1)) / (2 * tr);
            a  = tr / 2;
            b  = (rot (2, 0) + rot (0, 2)) / (2 * tr);
            c  = (rot (1, 2) + rot (2, 1)) / (2 * tr);
        }
    }
    else {
        tr -= rot (0, 0);
        if (rot (1, 1) > rot (2, 2)) {
            // 11 is biggest
            tr += rot (1, 1) - rot (2, 2);
            tr = sqrt (tr);
            d  = (rot (2, 0) - rot (0, 2)) / (2 * tr);
            a  = tr / 2;
            b  = (rot (1, 0) + rot (0, 1)) / (2 * tr);
            c  = (rot (2, 1) + rot (1, 2)) / (2 * tr);
        }
        else {
            // 22 is biggest
            tr += rot (2, 2) - rot (1, 1);
            tr = sqrt (tr);
            d  = (rot (1, 0) - rot (0, 1)) / (2 * tr);
            a  = tr / 2;
            b  = (rot (2, 0) + rot (0, 2)) / (2 * tr);
            c  = (rot (1, 2) + rot (2, 1)) / (2 * tr);
        }
    }

    return Quaternion<> (a, b, c, d);
}

Quaternion<> toQuat (const Rotation3D<>& rot)
{
    double a, b, c, d;
    const double tr = static_cast< double > (rot (0, 0) + rot (1, 1) + rot (2, 2) + 1);

    if (tr > 1e-7) {
        const double s = static_cast< double > (0.5) / static_cast< double > (sqrt (tr));
        d              = static_cast< double > (0.25) / s;
        a              = static_cast< double > (rot (2, 1) - rot (1, 2)) * s;
        b              = static_cast< double > (rot (0, 2) - rot (2, 0)) * s;
        c              = static_cast< double > (rot (1, 0) - rot (0, 1)) * s;
    }
    else {
        if (rot (0, 0) > rot (1, 1) && rot (0, 0) > rot (2, 2)) {
            const double sa =
                static_cast< double > (sqrt (rot (0, 0) - rot (1, 1) - rot (2, 2) + 1.0));
            a = static_cast< double > (0.5) * sa;

            // s == 1 / (2.0  *  sa) == 0.25 / (0.5  *  sa)
            const double s = static_cast< double > (0.25) / a;
            b              = static_cast< double > (rot (0, 1) + rot (1, 0)) * s;
            c              = static_cast< double > (rot (0, 2) + rot (2, 0)) * s;
            d              = static_cast< double > (rot (1, 2) - rot (2, 1)) * s;
        }
        else if (rot (1, 1) > rot (2, 2)) {
            const double sb =
                static_cast< double > (sqrt (rot (1, 1) - rot (2, 2) - rot (0, 0) + 1));
            b = static_cast< double > (0.5) * sb;

            const double s = static_cast< double > (0.25) / b;
            a              = static_cast< double > (rot (0, 1) + rot (1, 0)) * s;
            c              = static_cast< double > (rot (1, 2) + rot (2, 1)) * s;
            d              = static_cast< double > (rot (0, 2) - rot (2, 0)) * s;
        }
        else {
            const double sc =
                static_cast< double > (sqrt (rot (2, 2) - rot (0, 0) - rot (1, 1) + 1));
            c = static_cast< double > (0.5) * sc;

            const double s = static_cast< double > (0.25) / c;
            a              = static_cast< double > (rot (0, 2) + rot (2, 0)) * s;
            b              = static_cast< double > (rot (1, 2) + rot (2, 1)) * s;
            d              = static_cast< double > (rot (0, 1) - rot (1, 0)) * s;
        }
    }
    return Quaternion<> (a, b, c, d);
}

TEST (QuaternionTest, Conversion)
{
    // we generate a large amount of random rotations and and convert to and from rotation3d
    const size_t count = 100000;
    std::vector< Rotation3D<> > rotations (count);
    for (size_t i = 0; i < count; i++) {
        Rotation3D<> rot = Math::ranRotation3D< double > ();
        for (size_t j = 0; j < 30; j++)
            rot = rot * Math::ranRotation3D< double > ();
        if (i == 0)
            rot = RPY<> (0, 0, 0).toRotation3D ();
        rotations[i] = rot;
    }
    const double epsilon = 0.00000001;

    for (size_t i = 0; i < count; i++) {
        Rotation3D<> rot = rotations[i];
        Quaternion<> q (rot);
        Rotation3D<> res = q.toRotation3D ();

        EXPECT_TRUE (MetricUtil::dist2 (res * Vector3D<>::z (), rot * Vector3D<>::z ()) < epsilon);
        EXPECT_TRUE (MetricUtil::dist2 (res * Vector3D<>::y (), rot * Vector3D<>::y ()) < epsilon);
        EXPECT_TRUE (MetricUtil::dist2 (res * Vector3D<>::x (), rot * Vector3D<>::x ()) < epsilon);
    }
}

TEST (QuaternionTest, MiscTest)
{
    // Test Quaternion(T a, T b, T c, T d) constructor
    Quaternion<> q1 (1.0, 2.0, 3.0, 4.0);
    EXPECT_DOUBLE_EQ (q1.getQx (), 1.0);
    EXPECT_DOUBLE_EQ (q1.getQy (), 2.0);
    EXPECT_DOUBLE_EQ (q1.getQz (), 3.0);
    EXPECT_DOUBLE_EQ (q1.getQw (), 4.0);

    // Test toRotation3D and Quaternion(const Rotation3D&) constructor
    Rotation3D<> r1 = q1.toRotation3D ();
    Quaternion<> q2 (r1);

    // Test arithmetic functions
    Quaternion<> a (1, 2, 3, 4), b (4, 3, 2, 1), c (0, 0, 0, 0), h (0, 0, 0, 0);
    EXPECT_TRUE (close_enough (a, Quaternion<> (1.0, 2.0, 3.0, 4.0)));    // quat - quat
    EXPECT_TRUE (close_enough (+a, Quaternion<> (1.0, 2.0, 3.0, 4.0)));
    EXPECT_TRUE (close_enough (-a, Quaternion<> (-1.0, -2.0, -3.0, -4.0)));

    EXPECT_TRUE (close_enough ((a + b), Quaternion<> (5.0, 5.0, 5.0, 5.0)));
    EXPECT_TRUE (close_enough ((a += b), Quaternion<> (5, 5, 5, 5)));
    EXPECT_TRUE (close_enough ((a - b), Quaternion<> (1, 2, 3, 4)));
    EXPECT_TRUE (close_enough ((a -= b), Quaternion<> (1, 2, 3, 4)));

    h = a;
    EXPECT_TRUE (close_enough (h *= b, a * b));
    h = a;
    // BOOST_CHECK(close_enough( h /= b,    a / b ));

    EXPECT_EQ (a , Quaternion<> (1, 2, 3, 4));
    EXPECT_TRUE (!(a != Quaternion<> (1, 2, 3, 4)));
    EXPECT_NE (a , Quaternion<> (0, 2, 3, 4));
    EXPECT_NE (a , Quaternion<> (1, 0, 3, 4));
    EXPECT_NE (a , Quaternion<> (1, 2, 0, 4));
    EXPECT_NE (a , Quaternion<> (1, 2, 3, 0));

    Quaternion< float > af;
    af = cast< float > (a);
    for (size_t i = 0; i < 4; i++)
        EXPECT_EQ ((float) (a (i)) , af (i));
    af = rw::math::cast< float > (a);    // qualified lookup
    for (size_t i = 0; i < 4; i++)
        EXPECT_EQ ((float) (a (i)) , af (i));
}


/*
 * assesment.hpp
 *
 *  Created on: Feb 1, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "typedefs.hpp"

namespace math {

bool almost_equal(float a, float b);
bool almost_equal(double a, double b);
bool almost_equal(float a, float b, float tolerance);
bool almost_equal(float a, float b, double tolerance);
bool almost_equal(double a, double b, double tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(math::Vector2<ElementType> a, math::Vector2<ElementType> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(math::Vector3<ElementType> a, math::Vector3<ElementType> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(math::Matrix2<ElementType> a, math::Matrix2<ElementType> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(math::Matrix3<ElementType> a, math::Matrix3<ElementType> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> a,
		Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal_verbose(Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> a,
		Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal(Eigen::Tensor<ElementType, 3, Eigen::ColMajor> a,
		Eigen::Tensor<ElementType, 3, Eigen::ColMajor> b, ToleranceType tolerance);

template<typename ElementType, typename ToleranceType>
bool almost_equal_verbose(Eigen::Tensor<ElementType, 3, Eigen::ColMajor> a,
		Eigen::Tensor<ElementType, 3, Eigen::ColMajor> b, ToleranceType tolerance);

}//namespace math


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/camera/projection_matrix_utils.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <glog/logging.h>

#include "theia/math/matrix/rq_decomposition.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

void IntrinsicsToCalibrationMatrix(const double focal_length,
                                   const double skew,
                                   const double aspect_ratio,
                                   const double principal_point_x,
                                   const double principal_point_y,
                                   Matrix3d* calibration_matrix) {
  *calibration_matrix <<
      focal_length, skew, principal_point_x,
      0, focal_length * aspect_ratio, principal_point_y,
      0, 0, 1.0;
}

void CalibrationMatrixToIntrinsics(const Matrix3d& calibration_matrix,
                                   double* focal_length,
                                   double* skew,
                                   double* aspect_ratio,
                                   double* principal_point_x,
                                   double* principal_point_y) {
  CHECK_NE(calibration_matrix(2, 2), 0);
  *focal_length = calibration_matrix(0, 0) / calibration_matrix(2, 2);
  *skew = calibration_matrix(0, 1) / calibration_matrix(2, 2);
  *aspect_ratio = calibration_matrix(1, 1) / calibration_matrix(0, 0);
  *principal_point_x = calibration_matrix(0, 2) / calibration_matrix(2, 2);
  *principal_point_y = calibration_matrix(1, 2) / calibration_matrix(2, 2);
}

bool DecomposeProjectionMatrix(const Matrix3x4d pmatrix,
                               Matrix3d* calibration_matrix,
                               Vector3d* rotation,
                               Vector3d* position) {
  RQDecomposition<Matrix3d> rq(pmatrix.block<3, 3>(0, 0));

  Matrix3d rotation_matrix = ProjectToRotationMatrix(rq.matrixQ());

  const double k_det = rq.matrixR().determinant();
  if (k_det == 0) {
    return false;
  }

  Matrix3d& kmatrix = *calibration_matrix;
  if (k_det > 0) {
    kmatrix = rq.matrixR();
  } else {
    kmatrix = -rq.matrixR();
  }

  // Fix the matrix such that all internal parameters are greater than 0.
  for (int i = 0; i < 3; ++i) {
    if (kmatrix(i, i) < 0) {
      kmatrix.col(i) *= -1.0;
      rotation_matrix.row(i) *= -1.0;
    }
  }

  // Solve for t.
  const Vector3d t =
      kmatrix.triangularView<Eigen::Upper>().solve(pmatrix.col(3));

  // c = - R' * t, and flip the sign according to k_det;
  if (k_det > 0) {
    *position = - rotation_matrix.transpose() * t;
  } else {
    *position = rotation_matrix.transpose() * t;
  }

  const Eigen::AngleAxisd rotation_aa(rotation_matrix);
  *rotation = rotation_aa.angle() * rotation_aa.axis();

  return true;
}

bool ComposeProjectionMatrix(const Matrix3d& calibration_matrix,
                             const Vector3d& rotation,
                             const Vector3d& position,
                             Matrix3x4d* pmatrix) {
  const double rotation_angle = rotation.norm();
  if (rotation_angle == 0) {
    pmatrix->block<3, 3>(0, 0) = Matrix3d::Identity();
  } else {
    pmatrix->block<3, 3>(0, 0) = Eigen::AngleAxisd(
        rotation_angle, rotation / rotation_angle).toRotationMatrix();
  }

  pmatrix->col(3) = - (pmatrix->block<3, 3>(0, 0) *  position);
  *pmatrix = calibration_matrix * (*pmatrix);
  return true;
}

}  // namespace theia


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_2 : \f$\log(2)\f$.

    @par Semantic:

    @code
    T r = Log_2<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.6931471805599453094172321214581765680755001343602553);
    @endcode

**/
  template<typename T> T Log_2();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_2. (\f$\log(2)\f$)

      Generate the  constant log_2.

      @return The Log_2 constant for the proper type
    **/
    const boost::dispatch::functor<tag::log_2_> log_2 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log_2.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

//
// *** System
//
#include <iostream>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>

//
// *** ViennaCL
//

// #define VIENNACL_DEBUG_ALL
// #define VIENNACL_DEBUG_BUILD
// #define VIENNACL_HAVE_UBLAS 1
// #define VIENNACL_DEBUG_CUSTOM_OPERATION
#include "viennacl/vector.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_1.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/norm_inf.hpp"
#include "viennacl/generator/custom_operation.hpp"

using namespace boost::numeric;

template <class TYPE>
bool readVectorFromFile ( const std::string & filename, boost::numeric::ublas::vector<TYPE> & vec ) {
    std::ifstream file ( filename.c_str() );

    if ( !file ) return false;

    unsigned int size;
    file >> size;

    if ( size > 20000 )  //keep execution times short
        size = 20000;
    vec.resize ( size );
    for ( unsigned int i = 0; i < size; ++i ) {
        TYPE element;
        file >> element;
        vec[i] = element;
    }

    return true;
}

template <typename ScalarType>
ScalarType diff ( ScalarType & s1, viennacl::scalar<ScalarType> & s2 )
{
    viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
    if ( s1 != s2 )
        return ( s1 - s2 ) / std::max ( fabs ( s1 ), fabs ( s2 ) );
    return 0;
}

template< typename NumericT,unsigned int Alignment, typename Epsilon >
int test ( Epsilon const& epsilon, std::string vecfile ) {
    int retval = EXIT_SUCCESS;

    viennacl::scalar<NumericT>  vcl_res ( 0 );
    ublas::vector<NumericT> vec;
    ublas::vector<NumericT> vec2;

    NumericT res;

    viennacl::generator::gpu_symbolic_scalar<0,NumericT> symres;
    viennacl::generator::symbolic_vector<1,NumericT,Alignment> symv;
    viennacl::generator::symbolic_vector<2,NumericT,Alignment> symv2;
    viennacl::generator::cpu_symbolic_scalar<3,NumericT> symscal;
    viennacl::generator::cpu_symbolic_scalar<2,NumericT> symscal2;

    if ( !readVectorFromFile<NumericT> ( vecfile, vec ) ) {
        std::cout << "Error reading vec file" << std::endl;
        retval = EXIT_FAILURE;
    }
//
    std::cout << "Running tests for vector of size " << vec.size() << std::endl;
	std::cout << "----- Alignment " << Alignment << " -----" << std::endl;
//
    viennacl::vector<NumericT,Alignment> vcl_vec ( vec.size() );
    viennacl::vector<NumericT,Alignment> vcl_vec2 ( vec.size() );
//
    vec2 = vec;
    viennacl::copy ( vec.begin(), vec.end(), vcl_vec.begin() );
    viennacl::copy ( vec2.begin(), vec2.end(), vcl_vec2.begin() );

//     --------------------------------------------------------------------------

    std::cout << "testing inner product..." << std::endl;

    res = ublas::inner_prod ( vec, vec2 );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation(symres = inner_prod ( symv, symv2 ), "inner_prod") ( vcl_res, vcl_vec, vcl_vec2 ) );
    //std::cout << viennacl::generator::custom_operation(symres = inner_prod ( symv, symv2 ), "inner_prod") .kernels_source_code() << std::endl;
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner product" << std::endl;
        std::cout << "  Diff " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing inner product division..." << std::endl;
    res = ublas::inner_prod ( vec, vec2 ) /ublas::inner_prod ( vec, vec );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = inner_prod ( symv, symv2 ) /inner_prod ( symv,symv ), "inner_prod_division" ) ( vcl_res, vcl_vec, vcl_vec2 ) );
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner_prod_division" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing scalar / inner product..." << std::endl;
    res = 4/ublas::inner_prod ( vec, vec );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = symscal2/inner_prod ( symv,symv ),"scalar_division" ) ( vcl_res, vcl_vec, 4.0f ) );
    //std::cout << viennacl::generator::custom_operation ( symres = symscal2/inner_prod ( symv,symv ), "scalar_division" ).kernels_source_code() << std::endl;
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: scalar over inner product" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing inner_prod - ( scal - inner_prod ) " << std::endl;
    res = ublas::inner_prod ( vec, vec2 ) - ( 5.0f - inner_prod ( vec,vec2 ) );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = inner_prod ( symv, symv2 ) - ( symscal - inner_prod ( symv,symv2 ) ), "inner_prod_minus_scal_minus_inprod" ) ( vcl_res, vcl_vec, vcl_vec2, 5.0f ) );
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner_prod minus ( scal minus inner_prod ) " << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    return retval;
}

int main() {
    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Test :: Inner Product" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;

    int retval = EXIT_SUCCESS;

    std::string vecfile ( "../examples/testdata/rhs65025.txt" );

    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;
    {
        typedef float NumericT;
        NumericT epsilon = 1.0E-4;
        std::cout << "# Testing setup:" << std::endl;
        std::cout << "  eps:     " << epsilon << std::endl;
        std::cout << "  numeric: float" << std::endl;
        retval = test<NumericT,1> ( epsilon, vecfile );
//  		retval = test<NumericT,4> ( epsilon, vecfile, resultfile );
//        retval = test<NumericT,16> ( epsilon, vecfile, resultfile );
        if ( retval == EXIT_SUCCESS )
            std::cout << "# Test passed" << std::endl;
        else
            return retval;
    }
}