// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE
#define MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE

#include <concepts>
#include <boost/numeric/linear_algebra/intrinsic_concept_maps.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>

namespace math {

concept Commutative<typename Operation, typename Element>
  : std::Callable2<Operation, Element, Element>
{
    axiom Commutativity(Operation op, Element x, Element y)
    {
	op(x, y) == op(y, x);
    }
};

concept SemiGroup<typename Operation, typename Element>
  : std::Callable2<Operation, Element, Element>
{
    axiom Associativity(Operation op, Element x, Element y, Element z)
    {
	op(x, op(y, z)) == op(op(x, y), z);
    }
};

concept Monoid<typename Operation, typename Element>
  : SemiGroup<Operation, Element>
{
    typename identity_result_type;
    identity_result_type identity(Operation, Element);

    axiom Neutrality(Operation op, Element x)
    {
	op( x, identity(op, x) ) == x;
	op( identity(op, x), x ) == x;
    }
};

auto concept Inversion<typename Operation, typename Element>
{
    typename result_type;
    result_type inverse(Operation, Element);

};

concept PIMonoid<typename Operation, typename Element>
  : Monoid<Operation, Element>,
    Inversion<Operation, Element>
{
    bool is_invertible(Operation, Element);

    requires std::Convertible<Inversion<Operation, Element>::result_type, Element>;

    axiom Invertibility(Operation op, Element x)
    {
	// Only for invertible elements:
	if (is_invertible(op, x))
	    op( x, inverse(op, x) ) == identity(op, x);
	if ( is_invertible(op, x) )
	    op( inverse(op, x), x ) == identity(op, x);
    }
}

#if 0
    // Alternative approach to convert the result of inversion to Element
    // Unfortunately, this doesn't compile
    template <typename Operation, typename Element>
        requires PIMonoid<Operation, Element>
    concept_map PIMonoid<Operation, Inversion<Operation, Element>::result_type> {}
#endif

concept Group<typename Operation, typename Element>
  : PIMonoid<Operation, Element>
{
    bool is_invertible(Operation, Element) { return true; }

    // Just in case somebody redefines is_invertible
    axiom AlwaysInvertible(Operation op, Element x)
    {
	is_invertible(op, x);
    }

    // In fact this is implied by AlwaysInvertible and inherited Invertibility axiom
    // Maybe remove
    axiom GlobalInvertibility(Operation op, Element x)
    {
	op( x, inverse(op, x) ) == identity(op, x);
	op( inverse(op, x), x ) == identity(op, x);
    }
};

auto concept AbelianGroup<typename Operation, typename Element>
  : Group<Operation, Element>, Commutative<Operation, Element>
{};

// =======================
// Operator-based concepts
// =======================

concept Additive<typename Element>
  : std::HasPlus<Element>
{
    typename plus_assign_result_type;
    plus_assign_result_type operator+=(Element& x, Element y)
    {
	x= x + y; return x;
    }

    requires std::Convertible<plus_assign_result_type, Element&>;

    // Do we need the opposite conversion too?
    // This line produces a compiler error
    // requires std::Convertible<add<Element>::result_type,
    //                           std::HasPlus<Element>::result_type>;

    axiom Consistency(add<Element> op, Element x, Element y)
    {
	op(x, y) == x + y;
	op(x, y) == (x += y, x);
    }
}

auto concept AdditiveCommutative<typename Element>
  : Additive<Element>,
    Commutative< add<Element>, Element >
{}

auto concept AdditiveSemiGroup<typename Element>
  : Additive<Element>,
    SemiGroup< add<Element>, Element >
{}

#ifdef COMPILER_WITHOUT_OVERLOAD_ERROR  // Uncompilable due to error in compiler
concept AdditiveMonoid<typename Element>
  : AdditiveSemiGroup<Element>,
    Monoid< add<Element>, Element >
{
    Element zero(Element x)
    {
	return identity(add<Element>(), x);
    }

    // If we don't use the default definition
    axiom IdentityConsistency (add<Element> op, Element x)
    {
	zero(x) == identity(op, x);
    }
};

concept AdditivePIMonoid<typename Element>
  : std::HasMinus<Element>, AdditiveMonoid<Element>,
    PIMonoid< add<Element>, Element >
{
    typename minus_assign_result_type;
    minus_assign_result_type operator-=(Element& x, Element y)
    {
	x= x - y; return x;
    }

    requires std::Convertible<minus_assign_result_type, Element&>;

    typename unary_result_type;
    unary_result_type operator-(Element x)
    {
	return zero(x) - x;
    }

    axiom InverseConsistency(add<Element> op, Element x, Element y)
    {
	// consistency between additive and functor concept
	if ( is_invertible(op, y) )
	    op(x, inverse(op, y)) == x - y;
	if ( is_invertible(op, y) )
	    op(x, y) == (x -= y, x);

	// consistency of unary inversion
	if ( is_invertible(op, y) )
	    inverse(op, y) == -y;

	// consistency between unary and binary -
	if ( is_invertible(op, x) )
	    identity(op, x) - x == -x;
    }
}

auto concept AdditiveGroup<typename Element>
  : AdditivePIMonoid<Element>,
    Group< add<Element>, Element >
{};

auto concept AdditiveAbelianGroup<typename Element>
  : AdditiveGroup<Element>,
    Commutative< add<Element>, Element >
{}

#endif

concept Multiplicative<typename Element>
  : std::HasMultiply<Element>
{
    typename times_assign_result_type;
    times_assign_result_type operator*=(Element& x, Element y)
    {
	x= x * y; return x;
    }

    requires std::Convertible<times_assign_result_type, Element&>;

    // Do we need the opposite conversion too?
    // This line produces a compiler error
    // requires std::Convertible<mult<Element>::result_type,
    //                           std::HasMultiply<Element>::result_type>;

    axiom Consistency(mult<Element> op, Element x, Element y)
    {
	op(x, y) == x * y;
	op(x, y) == (x *= y, x);
    }
}

auto concept MultiplicativeCommutative<typename Element>
  : Multiplicative<Element>,
    Commutative< mult<Element>, Element >
{}

auto concept MultiplicativeSemiGroup<typename Element>
  : Multiplicative<Element>,
    SemiGroup< mult<Element>, Element >
{}

#ifdef COMPILER_WITHOUT_OVERLOAD_ERROR  // Uncompilable due to error in compiler
concept MultiplicativeMonoid<typename Element>
  : MultiplicativeSemiGroup<Element>,
    Monoid< mult<Element>, Element >
{
    Element one(Element x)
    {
	return identity(mult<Element>(), x);
    }

    // If we don't use the default definition
    axiom IdentityConsistency (math::mult<Element> op, Element x)
    {
	one(x) == identity(op, x);
    }
};

concept MultiplicativePIMonoid<typename Element>
  : std::HasDivide<Element>, MultiplicativeMonoid<Element>,
    PIMonoid< mult<Element>, Element >
{
    typename divide_assign_result_type;
    divide_assign_result_type operator/=(Element& x, Element y)
    {
	x= x / y; return x;
    }

    requires std::Convertible<divide_assign_result_type, Element&>;

    axiom InverseConsistency(mult<Element> op, Element x, Element y)
    {
	// consistency between multiplicative and functor concept
	if ( is_invertible(op, y) )
	    op(x, inverse(op, y)) == x / y;
	if ( is_invertible(op, y) )
	    op(x, y) == (x /= y, x);
    }
}

auto concept MultiplicativeGroup<typename Element>
  : MultiplicativePIMonoid<Element>,
    Group< mult<Element>, Element >
{};

auto concept MultiplicativeAbelianGroup<typename Element>
  : MultiplicativeGroup<Element>,
    Commutative< mult<Element>, Element >
{}

// ==========================
// Concepts with 2 operations
// ==========================

concept Distributive<typename AddOp, typename MultOp, typename Element>
{
    axiom Distributivity(AddOp add, MultOp mult, Element x, Element y, Element z)
    {
	// From left
	mult(x, add(y, z)) == add(mult(x, y), mult(x, z));
	// from right
	mult(add(x, y), z) == add(mult(x, z), mult(y, z));
    }
}

auto concept Ring<typename AddOp, typename MultOp, typename Element>
  : AbelianGroup<AddOp, Element>,
    SemiGroup<MultOp, Element>,
    Distributive<AddOp, MultOp, Element>
{}

auto concept RingWithIdentity<typename AddOp, typename MultOp, typename Element>
  : Ring<AddOp, MultOp, Element>,
    Monoid<MultOp, Element>
{}

concept DivisionRing<typename AddOp, typename MultOp, typename Element>
  : RingWithIdentity<AddOp, MultOp, Element>,
    Inversion<MultOp, Element>
{
    // 0 != 1, otherwise trivial
    axiom ZeroIsDifferentFromOne(AddOp add, MultOp mult, Element x)
    {
	identity(add, x) != identity(mult, x);
    }

    // Non-zero divisibility from left and from right
    axiom NonZeroDivisibility(AddOp add, MultOp mult, Element x)
    {
	if (x != identity(add, x))
	    mult(inverse(mult, x), x) == identity(mult, x);
	if (x != identity(add, x))
	    mult(x, inverse(mult, x)) == identity(mult, x);
    }
}

auto concept Field<typename AddOp, typename MultOp, typename Element>
  : DivisionRing<AddOp, MultOp, Element>,
    Commutative<MultOp, Element>
{}

auto concept OperatorRing<typename Element>
  : AdditiveAbelianGroup<Element>,
    MultiplicativeSemiGroup<Element>,
    Ring<add<Element>, mult<Element>, Element>
{}

auto concept OperatorRingWithIdentity<typename Element>
  : OperatorRing<Element>,
    MultiplicativeMonoid<Element>,
    RingWithIdentity<add<Element>, mult<Element>, Element>
{}

auto concept OperatorDivisionRing<typename Element>
  : OperatorRingWithIdentity<Element>,
    MultiplicativePIMonoid<Element>,
    DivisionRing<add<Element>, mult<Element>, Element>
{}

auto concept OperatorField<typename Element>
  : OperatorDivisionRing<Element>,
    Field<add<Element>, mult<Element>, Element>
{}

#endif

concept IntrinsicType<typename T> {}

concept IntrinsicArithmetic<typename T> : IntrinsicType<T> {}

concept IntrinsicIntegral<typename T> : IntrinsicArithmetic<T> {}

concept IntrinsicSignedIntegral<typename T>
  : std::SignedIntegralLike<T>,
    IntrinsicIntegral<T>
{}

concept IntrinsicUnsignedIntegral<typename T>
  : std::UnsignedIntegralLike<T>,
    IntrinsicIntegral<T>
{}

concept IntrinsicFloatingPoint<typename T>
  : std::FloatingPointLike<T>,
    IntrinsicArithmetic<T>
{}

#if 0

// ====================
// Default Concept Maps
// ====================

// ==============
// Arithmetic
// ==============

// ----------------
// Signed integrals
// ----------------

template <typename T>
  requires IntrinsicSignedIntegral<T>
concept_map OperatorRingWithIdentity<T> {}

template <typename T>
  requires IntrinsicSignedIntegral<T>
concept_map MultiplicativeCommutative<T> {}

// ------------------
// Unsigned integrals
// ------------------

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map AdditiveCommutative<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map AdditiveMonoid<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map MultiplicativeCommutative<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map MultiplicativeMonoid<T> {}

// ---------------
// Floationg Point
// ---------------

template <typename T>
  requires IntrinsicFloatingPoint<T>
concept_map Field<T> {}

template <typename T>
  requires IntrinsicFloatingPoint<T>
concept_map Field< std::complex<T> > {}

// ===========
// Min and Max
// ===========

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Commutative< max<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Monoid< max<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Commutative< min<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Monoid< min<T>, T > {}

// ==========
// And and Or
// ==========

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T>
concept_map Commutative< std::logical_and<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T>
concept_map Monoid< std::logical_and<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalOr<T>
concept_map Commutative< std::logical_or<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalOr<T>
concept_map Monoid< std::logical_or<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T> && std::HasLogicalOr<T>
concept_map Distributive<std::logical_and<T>, std::logical_or<T>, T> {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T> && std::HasLogicalOr<T>
concept_map Distributive<std::logical_or<T>, std::logical_and<T>, T> {}

// ==================
// Bitwise operations
// ==================

// not yet defined

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_and<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Monoid< bit_and<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_or<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Monoid< bit_or<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Distributive<bit_and<T>, bit_or<T>, T> {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Distributive<bit_or<T>, bit_and<T>, T> {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_xor<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map SemiGroup< bit_xor<T>, T > {}

// ====================
// String concatenation
// ====================

concept_map AdditiveMonoid<std::string> {}

#endif

} // namespace math

#endif // MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE


//
// Created by huangkun on 2020/8/26.
//

#include <Eigen/SparseCholesky>
#include <cmath>
#include <algorithm>
#include <numeric>

#include <opengv2/spline/BsplineSO3.hpp>

opengv2::BsplineSO3::BsplineSO3(int p,
                                const std::vector<Sophus::SO3d, Eigen::aligned_allocator<Sophus::SO3d>> &samples,
                                int controlPointsNum, const std::vector<double> &correspondingUs,
                                const std::vector<std::vector<Eigen::Vector3d>> &derivativeSamples,
                                double derivativeWeight) : degree_(p), derivativeWeight_(derivativeWeight) {
    // if dataPoints given, then do approximation
    if (!samples.empty()) {
        samples_ = samples;

        // Data registration
        if (!correspondingUs.empty()) {
            correspondingUs_ = correspondingUs;
        } else {
            // Data registration using chord length, Range: [0,1]
            correspondingUs_.resize(samples_.size());

            double d = 0;
            for (int i = 1; i < samples_.size(); ++i) {
                d += 2 * std::acos((samples_[i] * samples_[i - 1].inverse()).unit_quaternion().w());
            }

            correspondingUs_.front() = 0;
            correspondingUs_.back() = 1;
            for (int i = 1; i < samples_.size() - 1; ++i) {
                correspondingUs_[i] = correspondingUs_[i - 1] +
                                      2 * std::acos((samples_[i] * samples_[i - 1].inverse()).unit_quaternion().w()) /
                                      d;
            }
        }

        // derivative
        if (!derivativeSamples.empty()) {
            derivativeSamples_ = derivativeSamples;
        }

        // initialization
        if (controlPointsNum < 0)
            controlPointsNum = std::max(int(samples_.size() / 3), degree_ + 1);
        if (controlPointsNum <= degree_) {
            std::cerr << "BsplineSO3 Constructor: control points number should greater than degree!" << std::endl;
            return;
        }
        knotSpacing(controlPointsNum);
        initialGuess();
        optimizeCP();
    }
}

void opengv2::BsplineSO3::knotSpacing(int controlPointsNum) {
    /*** knot initialization (9.68) in NURBS book ***/
    knotVector_.resize(controlPointsNum + degree_ + 1);
    std::fill_n(knotVector_.begin(), degree_ + 1, correspondingUs_.front());
    std::fill_n(knotVector_.rbegin(), degree_ + 1, correspondingUs_.back());
    double d = samples_.size() / double(controlPointsNum - degree_);
    for (int j = 1; j <= controlPointsNum - 1 - degree_; j++) {
        int i = floor(j * d);
        double alpha = j * d - i;
        knotVector_[degree_ + j] = (1 - alpha) * correspondingUs_[i - 1] + alpha * correspondingUs_[i];
    }
}

void opengv2::BsplineSO3::derBasisFuns(double u, size_t spanIdx, int derivativeLimit,
                                       std::vector<std::vector<double>> &ders) const {
    // check
    if (knotVector_.empty()) {
        std::cerr << "Function derBasisFuns: knotVector is empty!" << std::endl;
        return;
    }

    ders.resize(derivativeLimit + 1);
    for (auto itr = ders.begin(); itr != ders.end(); itr++) {
        itr->assign(degree_, 0);
    }

    std::vector<std::vector<double>> basisN;

    // 0-th derivative: $\beta_{k,i}(u) = \sum_{j=i}^{k} N_{j,p}(u), i \in [k-p+1, k]$.
    // since \sum_{j=k-p}^{k} N_{j,p}(u) = 1
    basis(u, spanIdx, 0, degree_, basisN);
    ders[0][degree_ - 1] = basisN[0][degree_];
    for (int i = degree_ - 2; i >= 0; i--) {
        ders[0][i] = ders[0][i + 1] + basisN[0][i + 1];
    }

    // \alpha-th derivative, \alpha >= 1
    if (derivativeLimit > 0) {
        basisN.clear();
        basis(u, spanIdx, derivativeLimit - 1, degree_ - 1, basisN);

        for (int alpha = 1; alpha <= derivativeLimit; alpha++) {
            for (int i = spanIdx - degree_ + 1; i <= spanIdx; i++) {
                ders[alpha][i - (spanIdx - degree_ + 1)] =
                        degree_ / (knotVector_[i + degree_] - knotVector_[i]) *
                        basisN[alpha][i - (spanIdx - degree_ + 1)];
            }
        }
    }
}

void opengv2::BsplineSO3::basis(double u, size_t spanIdx, int derivativeLimit, int degree,
                                std::vector<std::vector<double>> &ders) const {
    ders.resize(derivativeLimit + 1);
    for (auto &it:ders) {
        it.assign(degree + 1, 0);
    }

    double ndu[degree + 1][degree + 1]; // store the basis functions and knot differences

    // store (in an alternating fashion) the two most recently computed rows a_{k,j} and a_{k-1,j}
    double a[2][degree + 1];

    std::vector<double> left, right;
    left.resize(degree + 1);
    right.resize(degree + 1);

    ndu[0][0] = 1;
    for (int j = 1; j <= degree; j++) {
        left[j] = u - knotVector_[spanIdx + 1 - j];
        right[j] = knotVector_[spanIdx + j] - u;
        double saved = 0.0;
        for (int r = 0; r < j; ++r) {
            ndu[j][r] = right[r + 1] + left[j - r];
            double temp = ndu[r][j - 1] / ndu[j][r];

            ndu[r][j] = saved + right[r + 1] * temp;
            saved = left[j - r] * temp;
        }
        ndu[j][j] = saved;
    }

    /* Load the basis functions */
    for (int j = 0; j <= degree; j++)
        ders[0][j] = ndu[j][degree];

    if (derivativeLimit > 0) {
        /*** This section computes the derivatives (Eq.[2.9]) ***/
        /* Load over function index */
        for (int r = 0; r <= degree; r++) {
            int s1 = 0, s2 = 1; // Alternate rows in array a
            a[0][0] = 1;

            // loop to compute kth derivative
            for (int k = 1; k <= derivativeLimit; k++) {
                double d = 0;
                int rk = r - k, pk = degree - k;
                if (r >= k) {
                    a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
                    d = a[s2][0] * ndu[rk][pk];
                }

                int j1, j2;
                if (rk >= -1)
                    j1 = 1;
                else
                    j1 = -rk;

                if (r - 1 <= pk)
                    j2 = k - 1;
                else
                    j2 = degree - r;

                for (int j = j1; j <= j2; j++) {
                    a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][rk + j];
                    d += a[s2][j] * ndu[rk + j][pk];
                }

                if (r <= pk) {
                    a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
                    d += a[s2][k] * ndu[r][pk];
                }

                ders[k][r] = d;

                // switch rows
                std::swap(s1, s2);
            }
        }

        /* Multiply through by correct factors (Eq. [2.9]) */
        int r = degree;
        for (int k = 1; k <= derivativeLimit; ++k) {
            for (int j = 0; j <= degree; j++)
                ders[k][j] *= r;

            r *= (degree - k);
        }
    }
}

// TODO: an acceleration penalty factor
void opengv2::BsplineSO3::initialGuess() {
    /*** check ***/
    if (samples_.empty()) {
        std::cerr << "Function initialGuess: dataPoints is empty!" << std::endl;
        return;
    }
    if (knotVector_.empty()) {
        std::cerr << "Function initialGuess: knotVector is empty!" << std::endl;
        return;
    }

    int controlPointsNum = knotVector_.size() - degree_ - 1;

    // initialize controlPoints
    controlPoints_.resize(controlPointsNum);
    controlPoints_.front() = samples_.front();
    controlPoints_.back() = samples_.back();

    /*** Problem ***/
    Eigen::SparseMatrix<double> N(samples_.size(), controlPointsNum);
    Eigen::SparseMatrix<double> M(samples_.size(), controlPointsNum);
    std::vector<std::vector<double>> ders;
    for (int k = 0; k < samples_.size(); ++k) {
        size_t spanIdx = findSpan(correspondingUs_[k]);
        basis(correspondingUs_[k], spanIdx, 0, degree_, ders);

        for (int j = 0; j <= degree_; ++j) {
            if (ders[0][j] != 0)
                N.insert(k, spanIdx - degree_ + j) = ders[0][j]; // N_{i-p+j,p}^{0}(\bar{u}_k)
        }
    }

    // formulating B
    std::vector<Eigen::Matrix<double, 4, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 1>>> B;
    B.assign(controlPointsNum - 2, Eigen::Matrix<double, 4, 1>::Zero());
    for (int l = 1; l <= controlPointsNum - 2; ++l) {
        // for l-th col of N
        for (Eigen::SparseMatrix<double>::InnerIterator it(N, l); it; ++it) {
            // N_{l,p}(\bar{u}_k), valid for column-major
            int k = it.row();
            if (k != 0 && k != samples_.size() - 1) {
                B[l - 1] += it.value() * (samples_[k].unit_quaternion().coeffs() -
                                          N.coeff(k, 0) * samples_.front().unit_quaternion().coeffs() -
                                          N.coeff(k, controlPointsNum - 1) *
                                          samples_.back().unit_quaternion().coeffs());
            }
        }
    }

    // formulating M,N
    Eigen::SparseMatrix<double> Nc = N.block(1, 1, samples_.size() - 2, controlPointsNum - 2);
    Eigen::SparseMatrix<double> A = Nc.transpose() * Nc;

    // solving
    A.makeCompressed();
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(A);
    if (solver.info() != Eigen::Success) {
        std::cerr << "Function initialGuess: decomposition failed!" << std::endl;
        return;
    }
    Eigen::VectorXd b(controlPointsNum - 2);

    std::vector<Eigen::Matrix<double, 4, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 1>>> cp;
    cp.resize(controlPointsNum - 2);
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < B.size(); ++j) {
            b[j] = B[j][i];
        }
        Eigen::VectorXd x = solver.solve(b);
        if (solver.info() != Eigen::Success) {
            std::cerr << "Function initialGuess: solving failed!" << std::endl;
            return;
        }
        for (int j = 1; j <= controlPointsNum - 2; ++j) {
            cp[j - 1][i] = x[j - 1];
        }
    }
    for (int j = 1; j <= controlPointsNum - 2; ++j) {
        controlPoints_[j].setQuaternion(Eigen::Quaterniond(cp[j - 1]));
    }
}

void opengv2::BsplineSO3::optimizeCP() {
    ceres::Problem problem;
    ceres::LocalParameterization *SO3_parameterization = new LocalParameterizationSO3();

    // Specify local update rule for our parameter
    for (Sophus::SO3d &cp:controlPoints_) {
        problem.AddParameterBlock(cp.data(), Sophus::SO3d::num_parameters, SO3_parameterization);
    }
    problem.SetParameterBlockConstant(controlPoints_.front().data());
    problem.SetParameterBlockConstant(controlPoints_.back().data());

    // pre-calculate spline basis
    std::unordered_map<int, std::shared_ptr<std::vector<std::vector<double>>>> basisFuns;
    std::unordered_map<int, size_t> spanIdxs;
    for (int i = 0; i < samples_.size(); ++i) {
        double u = correspondingUs_[i];
        auto basisFun = std::make_shared<std::vector<std::vector<double>>>();
        size_t spanIdx = findSpan(u);
        derBasisFuns(u, spanIdx, 0, *basisFun);

        basisFuns[i] = basisFun;
        spanIdxs[i] = spanIdx;
    }

    // Create and add cost functions. Derivatives will be evaluated via automatic differentiation
    for (int i = 0; i < samples_.size(); ++i) {
        auto spanIdx = spanIdxs[i];
        ceres::CostFunction *cost_function;
        if (degree_ == 4) {
            cost_function = P4ApproximationError::Create(samples_[i].inverse(), basisFuns[i]);
            problem.AddResidualBlock(cost_function, nullptr,
                                     controlPoints_[spanIdx - 4 + 0].data(),
                                     controlPoints_[spanIdx - 4 + 1].data(),
                                     controlPoints_[spanIdx - 4 + 2].data(),
                                     controlPoints_[spanIdx - 4 + 3].data(),
                                     controlPoints_[spanIdx - 4 + 4].data());
        } else if (degree_ == 3) {
            cost_function = P3ApproximationError::Create(samples_[i].inverse(), basisFuns[i]);
            problem.AddResidualBlock(cost_function, nullptr,
                                     controlPoints_[spanIdx - 3 + 0].data(),
                                     controlPoints_[spanIdx - 3 + 1].data(),
                                     controlPoints_[spanIdx - 3 + 2].data(),
                                     controlPoints_[spanIdx - 3 + 3].data());
        }
    }

    // Set solver options (precision / method)
    ceres::Solver::Options options;
    options.gradient_tolerance = Sophus::Constants<double>::epsilon();
    options.function_tolerance = Sophus::Constants<double>::epsilon();
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;
}

void opengv2::BsplineSO3::evaluate(double u, int derivativeLimit, Sophus::SO3d &val,
                                   std::vector<Eigen::Vector3d> &Ders) const {
    if (controlPoints_.empty()) {
        std::cerr << "Function evaluate: controlPoints is empty!" << std::endl;
        return;
    }
    if (derivativeLimit > 2) {
        std::cerr << "Function evaluate: derivativeLimit should <= 2!" << std::endl;
        return;
    }
    Ders.clear();

    // compute basis, [k-p+1,k]
    std::vector<std::vector<double>> basis;
    size_t spanIdx = findSpan(u);
    derBasisFuns(u, spanIdx, derivativeLimit, basis);

    // d_{k-p+j}, j \in [1,p]
    std::vector<Eigen::Vector3d> d; // since Sophus::SO3d::DoF = 3
    for (int j = 1; j <= degree_; ++j) {
        d.push_back(
                (controlPoints_[spanIdx - degree_ + j - 1].inverse() * controlPoints_[spanIdx - degree_ + j]).log());
    }

    // A_j(u), j \in [1,p]
    std::vector<Sophus::SO3d, Eigen::aligned_allocator<Sophus::SO3d>> A;
    for (int j = 1; j <= degree_; ++j) {
        int j_idx = j - 1;
        A.push_back(Sophus::SO3d::exp(basis[0][j_idx] * d[j_idx]));
    }

    // derivative = 0
    val = controlPoints_[spanIdx - degree_ + 0];
    for (int j = 1; j <= degree_; ++j) {
        int j_idx = j - 1;
        val *= A[j_idx];
    }

    if (derivativeLimit >= 1) {
        std::vector<std::vector<Eigen::Vector3d>> angVDer; // since Sophus::SO3d::DoF = 3
        angVDer.resize(derivativeLimit);
        for (auto &ad: angVDer) {
            ad.assign(degree_ + 1, Eigen::Vector3d::Zero());
        }

        for (int j = 2; j <= degree_ + 1; j++) {
            int j_idx = j - 1;
            angVDer[0][j_idx] =
                    A[j_idx - 1].inverse().Adj() * angVDer[0][j_idx - 1] + basis[1][j_idx - 1] * d[j_idx - 1];
        }

        Ders.push_back(angVDer[0].back());

        if (derivativeLimit >= 2) {
            for (int j = 2; j <= degree_ + 1; j++) {
                int j_idx = j - 1;
                angVDer[1][j_idx] = basis[1][j_idx - 1] * Sophus::SO3d::lieBracket(angVDer[0][j_idx], d[j_idx - 1]) +
                                    A[j_idx - 1].inverse().Adj() * angVDer[1][j_idx - 1] +
                                    basis[2][j_idx - 1] * d[j_idx - 1];
            }

            Ders.push_back(angVDer[1].back());
        }
    }
}


/*=============================================================================
Copyright (c) 2016 Paul W. Bible

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 ENRICHMENT_TOOLS
#define ENRICHMENT_TOOLS

#include <stdlib.h>

#include <ggtk/AnnotationData.hpp>
#include <ggtk/GoGraph.hpp>
#include <ggtk/SetUtilities.hpp>

#include <boost/math/distributions.hpp>
#include <boost/unordered_map.hpp>

//! The EnrichmentTools namespace provides simple functions for calulating GO term enrichment.
/*!
	This namespace defines free functions that allow enrichment p-values to be calculated.
	These funcitons can serve as the foundation for more sophisticated enrichment anlayis.
*/
namespace EnrichmentTools{

	//! A method for determining which genes are annotated with the given term or a child of that term.
	/*!
		This method calculates the set of the genes annotated with a given term or transatively with a child of that term.
	*/
	inline boost::unordered_set<std::string> getDescendantGenes(GoGraph *go, AnnotationData *data, const std::string &term){
		boost::unordered_set<std::string> descendants = go->getDescendantTerms(term);
		descendants.insert(term);

		boost::unordered_set<std::string>::iterator si;
		boost::unordered_set<std::string> genes;

		for(si = descendants.begin(); si != descendants.end(); ++si){
			std::string currentTerm = *si;
			//std::cout << *si << " " << go->getTermName(*si) << std::endl;
			data->addGenesForGoTerm(currentTerm,genes);
		}
		//std::cout << genes.size() << std::endl;

		return genes;
	};

	//! A method for calculating the result of a hypergeometic test.
	/*!
		This method calculates p-value of a hypergeometice test give 4 values.
		The sample size,         n
		The population success   K
		The the population size  N
		The test value           k

		Answers the question:
		"What is probability of seeing value of k or more successes
		  in a sample of size n, given that the population of size N
		  contains K total successes."
	*/
	inline double oneSidedRawPvalue_hyper(size_t sample, size_t success,size_t population,size_t test_value){
		double sum = 0.0;
		boost::math::hypergeometric dist(sample,success,population);
		for(size_t i = test_value; i <= sample && i <= success; ++i){
			double prob = boost::math::pdf(dist,i);
			sum += prob;
		}
		return sum;
	};

	//! A method to calculate the enrichment of a specific term in a sample of genes.
	/*!
		This method performs a hypergeometic test of enrichment for a term given
		a set of genes that serves as the sample. The population is taken as all genes
		in the annotation database.
	*/
	inline double enrichmentSignificance(GoGraph *go, AnnotationData *data,
											boost::unordered_set<std::string> &genes,
											const std::string &term)
	{
		boost::unordered_set<std::string> termGenes = getDescendantGenes(go, data, term);
		boost::unordered_set<std::string> sharedGenes = SetUtilities::set_intersection(genes,termGenes);

		if(sharedGenes.size() == 0){
			return 1.0;
		}

		size_t sampleSize = genes.size();
		size_t sampleWithTerm = sharedGenes.size();
		size_t populationWithTerm = termGenes.size();
		size_t populationSize = data->getNumGenes();

		return oneSidedRawPvalue_hyper(sampleSize,populationWithTerm,populationSize,sampleWithTerm);
	};

};
#endif


#pragma once

#include <crab/common/os.hpp>
#include <boost/functional/hash.hpp>
#include <gmp.h>

// TODO: replace ikos with crab namespace. This class has nothing to
// do with the ikos one. Kept for now for compatibility issues with
// some clients.
namespace ikos {

// GMP can convert directly from/to signed/unsigned long and
// signed/unsigned int. However, the C++11 standard only guarantees:
//
//  - unsigned/signed int  >= 16 bits.
//  - unsigned/signed long >= 32 bits.
//

// TODO/FIXME:
//
// We don't have a conversion from GMP numbers to 64-bit integers,
// because GMP cannot convert directly from/to int64_t or
// uint64_t. For that, we need to use mpz_export and mpz_import but
// they are significantly more expensive.
//
// Note that the actual size of **long** integer varies depending on
// the architecture and OS (see e.g.,
// https://en.cppreference.com/w/cpp/language/types). For instance,
// both Linux and mac OS on an Intel 64, the size of long integers is
// 8 bytes. But for Windows on Intel 64, the size is 4 bytes.

class z_number {
  friend class q_number;

private:
  mpz_t _n;

public:

  // overloaded typecast operators
  explicit operator long() const;
  explicit operator int() const;

  z_number();
  z_number(signed long long int n);
  z_number(const std::string& s, unsigned base = 10);

  static z_number from_ulong(unsigned long n);
  static z_number from_slong(signed long n);
  static z_number from_mpz_t(mpz_t n);
  static z_number from_mpz_srcptr(mpz_srcptr n);

  z_number(const z_number& o);
  z_number(z_number&& o);
  z_number& operator=(const z_number& o);
  z_number& operator=(z_number&& o);

  ~z_number();

  mpz_srcptr get_mpz_t() const { return _n; }

  mpz_ptr get_mpz_t() { return _n; }

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  bool fits_sint() const;

  bool fits_slong() const;

  z_number operator+(z_number x) const;

  z_number operator*(z_number x) const;

  z_number operator-(z_number x) const;

  z_number operator-() const;

  z_number operator/(z_number x) const;

  z_number operator%(z_number x) const;

  z_number& operator+=(z_number x);

  z_number& operator*=(z_number x);

  z_number& operator-=(z_number x);

  z_number& operator/=(z_number x);

  z_number& operator%=(z_number x);

  z_number& operator--();

  z_number& operator++();

  z_number operator++(int);

  z_number operator--(int);

  bool operator==(z_number x) const;

  bool operator!=(z_number x) const;

  bool operator<(z_number x) const;

  bool operator<=(z_number x) const;

  bool operator>(z_number x) const;

  bool operator>=(z_number x) const;

  z_number operator&(z_number x) const;

  z_number operator|(z_number x) const;

  z_number operator^(z_number x) const;

  z_number operator<<(z_number x) const;

  z_number operator>>(z_number x) const;

  z_number fill_ones() const;

  void write(crab::crab_os& o) const;

}; // class z_number

class q_number {

private:
  mpq_t _n;

public:

  q_number();
  q_number(double n);

  q_number(const std::string& s, unsigned base = 10);
  q_number(const z_number& n);
  q_number(const z_number& n, const z_number& d);

  static q_number from_mpq_t(mpq_t n);
  static q_number from_mpz_t(mpz_t n);
  static q_number from_mpq_srcptr(mpq_srcptr q);

  q_number(const q_number& o);
  q_number(q_number&& o);
  q_number& operator=(const q_number& o);
  q_number& operator=(q_number&& o);

  ~q_number();

  mpq_srcptr get_mpq_t() const { return _n; }

  mpq_ptr get_mpq_t() { return _n; }

  double get_double() const;

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  q_number operator+(q_number x) const;

  q_number operator*(q_number x) const;

  q_number operator-(q_number x) const;

  q_number operator-() const;

  q_number operator/(q_number x) const;

  q_number& operator+=(q_number x);

  q_number& operator*=(q_number x);

  q_number& operator-=(q_number x);

  q_number& operator/=(q_number x);

  q_number& operator--();

  q_number& operator++();

  q_number operator--(int);

  q_number operator++(int);

  bool operator==(q_number x) const;

  bool operator!=(q_number x) const;

  bool operator<(q_number x) const;

  bool operator<=(q_number x) const;

  bool operator>(q_number x) const;

  bool operator>=(q_number x) const;

  z_number numerator() const;

  z_number denominator() const;

  z_number round_to_upper() const;

  z_number round_to_lower() const;

  void write(crab::crab_os& o) const;

}; // class q_number

inline crab::crab_os& operator<<(crab::crab_os& o, const z_number& z) {
  z.write(o);
  return o;
}

inline crab::crab_os& operator<<(crab::crab_os& o, const q_number& q) {
  q.write(o);
  return o;
}

/** for boost::hash_combine **/
inline std::size_t hash_value(const z_number& z) {
  return z.hash();
}

inline std::size_t hash_value(const q_number& q) {
  return q.hash();
}
} //end namespace

/** for specializations of std::hash **/
namespace std {
template<>
struct hash<ikos::z_number> {
  size_t operator()(const ikos::z_number& z) const {
    return z.hash();
  }
};

template<>
struct hash<ikos::q_number> {
  size_t operator()(const ikos::q_number& q) const {
    return q.hash();
  }
};
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_GRADIENT_DESCENT_H_
#define TURI_GRADIENT_DESCENT_H_

#include <flexible_type/flexible_type.hpp>
#include <Eigen/Core>

#include <optimization/utils.hpp>
#include <optimization/optimization_interface.hpp>
#include <optimization/regularizer_interface.hpp>
#include <optimization/line_search-inl.hpp>
#include <table_printer/table_printer.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
// 1. Constant line seach tuning?

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup gradient_descent Gradient Descent
 * \{
 */

/**
 *
 * Solve a first_order_optimization_iterface model with a gradient descent
 * method.
 *
 * \param[in,out] model  Model with first order optimization interface.
 * \param[in] init_point Starting point for the solver.
 * \param[in,out] opts   Solver options.
 * \returns stats        Solver return stats.
 * \param[in] reg        Shared ptr to an interface to a regularizer.
 * \tparam Vector        Sparse or dense gradient representation.
 *
 *
*/
template <typename Vector = DenseVector>
inline solver_return gradient_descent(first_order_opt_interface& model,
    const DenseVector& init_point,
    std::map<std::string, flexible_type>& opts,
    const std::shared_ptr<regularizer_interface> reg=NULL){

    // Benchmarking utils.
    timer t;
    double start_time = t.current_time();

    logprogress_stream << "Starting Gradient Descent " << std::endl;
    logprogress_stream << "--------------------------------------------------------" << std::endl;
    std::stringstream ss;
    ss.str("");

    // Step 1: Algorithm option init
    // ------------------------------------------------------------------------
    // Check that all solver options are present.
    // Load options
    size_t iter_limit = opts["max_iterations"];
    double convergence_threshold = opts["convergence_threshold"];
    double step_size = opts["step_size"];
    size_t iters = 1;
    solver_return stats;

    // Print progress
    table_printer printer(
        model.get_status_header({"Iteration", "Passes", "Step size", "Elapsed Time"}));
    printer.print_header();

    // First compute the residual. Sometimes, you already have the solution
    // during the starting point. In these settings, you don't want to waste
    // time performing a step of the algorithm.
    DenseVector point = init_point;
    Vector gradient(point.size());
    double func_value;
    model.compute_first_order_statistics(point, gradient, func_value);
    double residual = compute_residual(gradient);

    stats.func_evals++;
    stats.gradient_evals++;

    // Needs to store previous point and gradient information
    DenseVector delta_point = point;
    delta_point.setZero();

    // First iteration will take longer. Warn the user.
    logprogress_stream <<"Tuning step size. First iteration could take longer"
                       <<" than subsequent iterations." << std::endl;

    // Nan Checking!
    if (!std::isfinite(residual)) {
      stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_OVERFLOW;
    }

    // Step 2: Algorithm starts here
    // ------------------------------------------------------------------------
    // While not converged
    while((residual >= convergence_threshold) && (iters <= iter_limit)){

      // Line search for step size.
      ls_return ls_stats;

      // Pick line search based on regularizers.
      if (reg != NULL){
        step_size  *= 2;
        ls_stats =  backtracking(model,
                                 step_size,
                                 func_value,
                                 point,
                                 gradient,
                                 -gradient,
                                 reg);
      } else {
          ls_stats =  more_thuente(model,
                                   step_size,
                                   func_value,
                                   point,
                                   gradient,
                                   -gradient);
      }

      // Add info from line search
      stats.func_evals += ls_stats.func_evals;
      stats.gradient_evals += ls_stats.gradient_evals;
      step_size = ls_stats.step_size;

      // Line search failed
      if (ls_stats.status == false){
        stats.status = OPTIMIZATION_STATUS::OPT_LS_FAILURE;
        break;
      }

      // \delta x_k = x_{k} - x_{k-1}
      delta_point =  point;
      point = point -step_size * gradient;
      if (reg != NULL)
        reg->apply_proximal_operator(point, step_size);
      delta_point = point - delta_point;

      // Numerical error: Insufficient progress.
      if (delta_point.norm() <= OPTIMIZATION_ZERO){
        stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_ERROR;
        break;
      }
      // Numerical error: Numerical overflow. (Step size was too large)
      if (!delta_point.array().array().isFinite().all()) {
        stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_OVERFLOW;
        break;
      }

      // Compute residual norm (to check for convergence)
      model.compute_first_order_statistics(point, gradient, func_value);
      stats.num_passes++;
      residual = compute_residual(gradient);
      iters++;

      // Print progress
      auto stat_info = {std::to_string(iters),
                        std::to_string(stats.num_passes),
                        std::to_string(step_size),
                        std::to_string(t.current_time())};

      auto row = model.get_status(point, stat_info);
      printer.print_progress_row_strs(iters, row);
    }

    printer.print_footer();

    // Step 3: Return optimization model status.
    // ------------------------------------------------------------------------
    if (stats.status == OPTIMIZATION_STATUS::OPT_UNSET) {
      if (iters < iter_limit){
        stats.status = OPTIMIZATION_STATUS::OPT_OPTIMAL;
      } else {
        stats.status = OPTIMIZATION_STATUS::OPT_ITERATION_LIMIT;
      }
    }
    stats.iters = iters;
    stats.residual = residual;
    stats.gradient = gradient;
    stats.func_value = func_value;
    stats.solve_time = t.current_time() - start_time;
    stats.solution = point;
    stats.progress_table = printer.get_tracked_table();

    // Display solver stats
    log_solver_summary_stats(stats);
    return stats;
}

} // optimizaiton

/// \}
} // turicreate

#endif


/** hoNFFT.cpp */

#include "hoNFFT.h"

#include "hoNDFFT.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_reductions.h"
#include "hoNDArray_utils.h"

#include "vector_td_utilities.h"
#include "vector_td_operators.h"
#include "vector_td_io.h"

#include <algorithm>
#include <vector>
#include <cmath>
#include <stdexcept>

#include <boost/range/algorithm/transform.hpp>

#include "GadgetronTimer.h"

#include "NFFT.hpp"
#include "NDArray_utils.h"

#include "hoGriddingConvolution.h"

using namespace std;

namespace Gadgetron {

    namespace {

        template<typename T, unsigned int D>
        struct FFTD {
        };

        template<typename T>
        struct FFTD<T, 1> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft1c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft1c(array);
                }
                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)));
            }
        };

        template<typename T>
        struct FFTD<T, 2> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale ) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft2c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft2c(array);
                }

                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)*array.get_size(1)));

            }
        };

        template<typename T>
        struct FFTD<T, 3> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft3c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft3c(array);
                }

                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)*array.get_size(1)*array.get_size(2)));
            }
        };

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 1>& image_dims,
            const ConvolutionKernel<REAL, 1, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,1> image_dims_real(image_dims);
            for (int x = 0; x < image_dims[0]; x++){
                auto offset = x - image_dims_real[0]/2;
                deapodization(x) = kernel.get(offset, 0);
            }
            return deapodization;
        }

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 2>& image_dims,
            const ConvolutionKernel<REAL, 2, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,2> image_dims_real(image_dims);
            for (int y = 0; y < image_dims[1]; y++) {
                auto offset_y = y - image_dims_real[1]/2;
                auto weight_y = kernel.get(offset_y, 1);

                for (int x = 0; x < image_dims[0]; x++) {
                    auto offset_x = x - image_dims_real[0]/2;
                    auto weight_x = kernel.get(offset_x, 0);

                    deapodization(x,y) = weight_x*weight_y;
                }
            }
            return deapodization;
        }

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 3>& image_dims,
            const ConvolutionKernel<REAL, 3, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,3> image_dims_real(image_dims);
            for (int z = 0; z < image_dims[2]; z++) {
                auto offset_z = z - image_dims_real[2]/2;
                auto weight_z = kernel.get(offset_z, 2);

                for (int y = 0; y < image_dims[1]; y++) {
                    auto offset_y = y - image_dims_real[1]/2;
                    auto weight_y = kernel.get(offset_y, 1);

                    for (int x = 0; x < image_dims[0]; x++) {
                        auto offset_x = x - image_dims_real[0]/2;
                        auto weight_x = kernel.get(offset_x, 0);

                        deapodization(x,y,z) = weight_x*weight_y*weight_z;
                    }
                }
            }
            return deapodization;
        }
    }

    template<class REAL, unsigned int D>
    hoNFFT_plan<REAL, D>::hoNFFT_plan(
        const vector_td<size_t, D> &matrix_size,
        const vector_td<size_t, D> &matrix_size_os,
        REAL W)
      : NFFT_plan<hoNDArray,REAL,D>(matrix_size,matrix_size_os,W)
    {
        this->deapodization_filter_IFFT = compute_deapodization_filter(
            this->matrix_size_os_, this->conv_->get_kernel());
        this->deapodization_filter_FFT = deapodization_filter_IFFT;

        FFTD<std::complex<REAL>,D>::fft(
            deapodization_filter_IFFT, NFFT_fft_mode::BACKWARDS, true);
        FFTD<std::complex<REAL>,D>::fft(
            deapodization_filter_FFT, NFFT_fft_mode::FORWARDS, true);

        boost::transform(deapodization_filter_IFFT,
                         deapodization_filter_IFFT.begin(),
                         [](auto val) { return REAL(1)/val; });
        boost::transform(deapodization_filter_FFT,
                         deapodization_filter_FFT.begin(),
                         [](auto val) { return REAL(1)/val; });
    }

    template<class REAL, unsigned int D>
    hoNFFT_plan<REAL, D>::hoNFFT_plan(
        const vector_td<size_t, D> &matrix_size,
        REAL oversampling_factor,
        REAL W)
      : NFFT_plan<hoNDArray, REAL, D>(matrix_size, oversampling_factor, W)
    {
        this->deapodization_filter_IFFT = compute_deapodization_filter(
            this->matrix_size_os_, this->conv_->get_kernel());
        this->deapodization_filter_FFT = deapodization_filter_IFFT;

        FFTD<std::complex<REAL>, D>::fft(
            deapodization_filter_IFFT, NFFT_fft_mode::BACKWARDS, false);
        FFTD<std::complex<REAL>, D>::fft(
            deapodization_filter_FFT, NFFT_fft_mode::FORWARDS, false);

        boost::transform(deapodization_filter_IFFT,
                         deapodization_filter_IFFT.begin(),
                         [](auto val) { return REAL(1) / val; });
        boost::transform(deapodization_filter_FFT,
                         deapodization_filter_FFT.begin(),
                         [](auto val) { return REAL(1) / val; });
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::compute(
            const hoNDArray<ComplexType> &d,
            hoNDArray<ComplexType> &m,
            const hoNDArray<REAL> *dcw,
            NFFT_comp_mode mode
    ) {
        const auto *pd = reinterpret_cast<const hoNDArray<complext<REAL>> *>(&d);
        auto *pm = reinterpret_cast<hoNDArray<complext<REAL>> *>(&m);

        this->compute(*pd, *pm, dcw, mode);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::compute(
            const hoNDArray<complext<REAL>> &d,
            hoNDArray<complext<REAL>> &m,
            const hoNDArray<REAL> *dcw,
            NFFT_comp_mode mode
    ) {
       NFFT_plan<hoNDArray,REAL,D>::compute(d,m,dcw,mode);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::mult_MH_M(
            const hoNDArray<complext<REAL>> &in,
            hoNDArray<complext<REAL>> &out,
            const hoNDArray<REAL>* dcw
    ) {
        const hoNDArray<ComplexType> *pin = reinterpret_cast<const hoNDArray<ComplexType> *>(&in);
        hoNDArray<ComplexType> *pout = reinterpret_cast<hoNDArray<ComplexType> *>(&out);

        this->mult_MH_M(*pin, *pout, dcw);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::mult_MH_M(
            const hoNDArray<ComplexType> &in,
            hoNDArray<ComplexType> &out,
            const hoNDArray<REAL>* dcw
    ) {
        std::vector<size_t> dims = {this->number_of_samples,this->number_of_frames};
        auto batches = in.get_number_of_elements()/(prod(this->matrix_size_)*this->number_of_frames);
        dims.push_back(batches);

        hoNDArray<ComplexType> tmp(dims);
        compute(in, tmp, dcw, NFFT_comp_mode::FORWARDS_C2NC);
        compute(tmp, out,dcw, NFFT_comp_mode::BACKWARDS_NC2C);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::fft(
            hoNDArray<ComplexType> &d,
            NFFT_fft_mode mode,
            bool do_scale
    ) {
        FFTD<std::complex<REAL>, D>::fft(d, mode,do_scale);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::fft(
            hoNDArray<complext<REAL>> &d,
            NFFT_fft_mode mode,
            bool do_scale
    ) {
        hoNDArray<ComplexType> *pd = reinterpret_cast<hoNDArray<ComplexType> *>(&d);
        this->fft(*pd,mode,do_scale);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::deapodize(
            hoNDArray<ComplexType> &d,
            bool fourierDomain
    ) {
        if (fourierDomain){
            d *= deapodization_filter_FFT;
        } else {
            d *= deapodization_filter_IFFT;
        }
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::deapodize(
            hoNDArray<complext<REAL>> &d,
            bool fourierDomain
    ) {
        hoNDArray<ComplexType> *pd = reinterpret_cast<hoNDArray<ComplexType> *>(&d);
        this->deapodize(*pd,fourierDomain);
    }

    template<class REAL, unsigned int D>
    boost::shared_ptr<hoNFFT_plan<REAL,D>> NFFT<hoNDArray,REAL,D>::make_plan(const Gadgetron::vector_td<size_t, D> &matrix_size,
                                        const Gadgetron::vector_td<size_t, D> &matrix_size_os, REAL W) {
        return boost::make_shared<hoNFFT_plan<REAL,D>>(matrix_size,matrix_size_os,W);
    }

}

template
class Gadgetron::hoNFFT_plan<float, 1>;

template
class Gadgetron::hoNFFT_plan<float, 2>;

template
class Gadgetron::hoNFFT_plan<float, 3>;

template
class Gadgetron::hoNFFT_plan<double, 1>;

template
class Gadgetron::hoNFFT_plan<double, 2>;

template
class Gadgetron::hoNFFT_plan<double, 3>;

template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,1>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,2>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,3>;

template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,1>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,2>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,3>;


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

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

// include necessary system headers
#include <iostream>

// ViennaFEM includes:
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

// ViennaMath includes:
#include "viennamath/expression.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

int main()
{
  typedef viennagrid::tetrahedral_3d_mesh                                                 DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;
  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainer;
  typedef viennagrid::result_of::iterator<VertexContainer>::type                          VertexIterator;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_reader;
    my_reader(my_domain, segments, "../examples/data/sshape3d-pimped.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    exit(EXIT_FAILURE);
  }

  //
  // Specify PDE:
  //
  FunctionSymbol u(0, viennamath::unknown_tag<>());   //an unknown function used for PDE specification
  Equation poisson_equ_1 = viennamath::make_equation( viennamath::laplace(u), -1);

  MatrixType system_matrix;
  VectorType load_vector;

  //
  // Setting boundary information on domain (this should come from device specification)
  //
  //setting some boundary flags:
  VertexContainer vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIterator vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    //boundary for first equation: Homogeneous Dirichlet everywhere
    if (viennagrid::point(my_domain, *vit)[2] == 3.0 || viennagrid::point(my_domain, *vit)[1] == 3.0 )
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0);
  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  fem_assembler(viennafem::make_linear_pde_system(poisson_equ_1, u),
                my_domain,
                system_matrix,
                load_vector
               );

  VectorType pde_result = viennacl::linalg::solve(system_matrix, load_vector, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix, pde_result) - load_vector) << std::endl;

  //
  // Writing solution back to domain (discussion about proper way of returning a solution required...)
  //
  viennafem::io::write_solution_to_VTK_file(pde_result, "sshape_3d", my_domain, segments, storage, 0);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Poisson solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


/********************************************************************************
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_INTERIORPOINTOPTIMIZER_HPP
#define RW_MATH_INTERIORPOINTOPTIMIZER_HPP

#include <rw/math/Math.hpp>

#include <iostream>
#include <boost/function.hpp>
#include <boost/bind.hpp>

namespace rw {
namespace math {

class InteriorPointOptimizer
{
public:

    typedef boost::function<void(const Eigen::VectorXd& x,
                                 double& f,
                                 Eigen::VectorXd& df,
                                 Eigen::MatrixXd& ddf) > ObjectFunction;

    typedef void*(Q& q) ObjectFunction;

    typedef boost::function<void(const Eigen::VectorXd& x,
                                 size_t no,
                                 Eigen::VectorXd& g,
                                 Eigen::MatrixXd& dg,
                                 Eigen::MatrixXd& ddq) > ConstraintFunction;

    InteriorPointOptimizer(size_t n,
                           size_t m,
                           ObjectFunction objectFunction,
                           ConstraintFunction constraintFunction);

    virtual ~InteriorPointOptimizer();

    int solve(const Eigen::VectorXd& x_init);

    void setAccuracy(double accuracy);
    double getAccuracy();

    void verify_user_defined_objective_and_constraints();

protected:
    InteriorPointOptimizer(size_t n, size_t m);

    void initialize();

    virtual void objectFunction(const Eigen::VectorXd& x,
                                double &f,
                                Eigen::VectorXd &df,
                                Eigen::MatrixXd &ddf);

    virtual void constraintFunction(const Eigen::VectorXd& x,
                                    int i,
                                    Eigen::VectorXd &a,
                                    Eigen::MatrixXd &da,
                                    Eigen::MatrixXd &dda);

private:
    ObjectFunction compute_f_info_EXT;
    ConstraintFunction compute_con_info_i_EXT;

    void choleskySolve(int n_e,
                       int bw,
                       Eigen::MatrixXd &A,
                       Eigen::VectorXd &b,
                       Eigen::VectorXd &x);

    void compute_f_info(Eigen::MatrixXd &A,
                        Eigen::VectorXd &RHS);

    void compute_con_info(Eigen::MatrixXd &A,
                          Eigen::VectorXd &RHS);

    void merit_info(Eigen::VectorXd &x,
                    Eigen::VectorXd &s,
                    double &phi,
                    double &eta);

    void Dmerit_info(Eigen::VectorXd &x,
                     Eigen::VectorXd &s,
                     Eigen::VectorXd &dx,
                     Eigen::VectorXd &ds,
                     double &Dphi,
                     double &eta);

    void update(Eigen::VectorXd &x,
                Eigen::VectorXd &dx,
                Eigen::VectorXd &s,
                Eigen::VectorXd &z);

    const size_t N;
    const size_t M;
    double _accuracy;
    Eigen::VectorXd _x;
    Eigen::VectorXd _s;
    Eigen::VectorXd _z;
    double _mu, _eta;

    // objective and derivatives
    double _f;
    Eigen::VectorXd _df;
    Eigen::MatrixXd _ddf;

    // constraints and derivatives (second derivative only stored for one constraint at a time)
    Eigen::VectorXd _a;
    Eigen::MatrixXd _da;
    Eigen::MatrixXd _dda;

};

} //end namespace math
} //end namespace rw

#endif //end include guard


/*
 Copyright (c) 2015-2017 Paul Lagrée, Siyu Lei, Silviu Maniu, Luyi Mo

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 */

#ifndef __oim__BetaInfluence__
#define __oim__BetaInfluence__

#include "common.hpp"
#include "InfluenceDistribution.hpp"

#include <boost/math/distributions.hpp>
#include <random>
#include <sys/time.h>
#include <math.h>

class BetaInfluence: public InfluenceDistribution {
 private:
  double alpha_prior_, beta_prior_;
  double alpha_, beta_;
  double quartile_med_;
  double quartile_upper_;
  double quartile_stdev_;
  double original_mean_;
  std::default_random_engine gen_;

 public:
  BetaInfluence(double alpha, double beta, double orig)
      : alpha_prior_(alpha), beta_prior_(beta), alpha_(alpha), beta_(beta),
        original_mean_(orig), gen_(seed_ns()) {
    update_quartiles();
  }

  void update(unode_int hit, unode_int miss) {
    alpha_ += (double)hit;
    beta_ += (double)miss;
    hits_ += hit;
    misses_ += miss;
    update_quartiles();
  };

  void update_prior(double new_alpha, double new_beta) {
    alpha_prior_ = (new_alpha) > 0 ? new_alpha : 1.0;
    beta_prior_ = (new_beta) > 0 ? new_beta : 1.0;
    alpha_ = alpha_prior_ + (double)hits_;
    beta_ = beta_prior_ + (double)misses_;
    update_quartiles();
  }

  double mean() { return (double)alpha_ / (double)(alpha_ + beta_); }

  double sample(unsigned int interval) {
    if (interval == INFLUENCE_MED) {
      return quartile_med_;
    } else if (interval == INFLUENCE_UPPER) {
      return quartile_upper_;
    } else if(interval == INFLUENCE_UCB) {
      double val = quartile_med_ + sqrt(3.0 * log(round_) /
          (2.0 * (alpha_ + beta_)));
      return (val < 1) ? val : 1.0;
    } else if (interval == INFLUENCE_THOMPSON) {
      std::gamma_distribution<double> a(alpha_, 1.0);
      std::gamma_distribution<double> b(beta_, 1.0);
      double x = a(gen_);
      double y = b(gen_);
      return x / (x + y);
    } else { // Case where we shift the distributions by theta stdev (EG)
      double val = quartile_med_ + (interval - (double)THETA_OFFSET - 1.0)
          * quartile_stdev_;
      val = val < 1 ? val : 1.0;
      return val > 0 ? val : 0.0;
    }
    return quartile_med_;
  }

  double sq_error() {
    return (quartile_med_ - original_mean_) * (quartile_med_ - original_mean_);
  }

 private:
  void update_quartiles() {
    boost::math::beta_distribution<> dist(alpha_, beta_);
    quartile_med_ = alpha_ / (alpha_ + beta_);
    quartile_stdev_ = sqrt(alpha_ * beta_ / (alpha_ + beta_ + 1.0))
        / (alpha_ + beta_);
    quartile_upper_ = quantile(dist, 0.75);
  }
};

#endif /* defined(__oim__BetaInfluence__) */


#pragma once

#include <Eigen/Dense>
#include <vector>

namespace polyfem
{

///
/// Clip a polygon by a half-plane.
/// https://github.com/alicevision/geogram/blob/cfbc0a5827d71d59f8bcf0369cc1731ef12f82ef/src/examples/graphics/demo_Delaunay2d/main.cpp#L677
///
/// @param[in]  P       { Input polygon }
/// @param[in]  q1      { First endpoint of the clipping line }
/// @param[in]  q2      { Second endpoint of the clipping line }
/// @param[out] result  { Clipped polygon }
///
void clip_polygon_by_half_plane(const Eigen::MatrixXd &P, const Eigen::RowVector2d &q1,
	const Eigen::RowVector2d &q2, Eigen::MatrixXd &result);

///
/// Determine the kernel of the given polygon.
///
/// @param[in]  IV    { #IV x (2|3) vertex positions around the input polygon }
/// @param[out] OV    { #OV x (2|3) vertex positions around the output polygon }
///
void compute_visibility_kernel(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV);

///
/// Determine whether a polygon is star-shaped or not.
///
/// @param[in]  IV    { #IV x (2|3) of vertex positions around the polygon }
/// @param[out] bary  { The barycenter of the kernel }
///
/// @return     True if star shaped, False otherwise.
///
bool is_star_shaped(const Eigen::MatrixXd &IV, Eigen::RowVector3d &bary);

///
/// Compute offset polygon
///
/// @param[in]  IV    { #IV x 2 of vertex positions for the input polygon }
/// @param[out] OV    { #OV x 2 of vertex positions for the offset polygon }
/// @param[in]  eps   { Offset distance }
///
void offset_polygon(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, double eps);

///
/// Compute whether points are inside a polygon
///
/// @param[in]  IV      { #IV x 2 of vertex positions for the input polygon }
/// @param[in]  Q       { #Q x 2 of query point positions }
/// @param[out] inside  { Whether the i-th query point is inside or not }
///
/// @return     Number of points inside
///
int is_inside(const Eigen::MatrixXd &IV, const Eigen::MatrixXd &Q, std::vector<bool> &inside);

///
/// Sample points on a polygon, evenly spaced from each other
///
/// @param[in]  IV           { #IV x 2 vertex positions for the input polygon }
/// @param[in]  num_samples  { Desired number of samples }
/// @param[out] S            { #S x 2 output sample positions }
///
void sample_polygon(const Eigen::MatrixXd &IV, int num_samples, Eigen::MatrixXd &S);

} // namespace polyfem


/********************************************************************************
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_INERTIAMATRIX_HPP
#define RW_MATH_INERTIAMATRIX_HPP

/**
 * @file InertiaMatrix.hpp
 */

#if !defined(SWIG)
#include "Rotation3D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /* @{*/

    /**
     * @brief A 3x3 inertia matrix
     */
    template< class T = double > class InertiaMatrix
    {
      public:
        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< T, 3, 3 > Base;
#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r13 \f$ r_{13} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         * @param r23 \f$ r_{23} \f$
         * @param r31 \f$ r_{31} \f$
         * @param r32 \f$ r_{32} \f$
         * @param r33 \f$ r_{33} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{ccc}
         *  r_{11} & r_{12} & r_{13} \\
         *  r_{21} & r_{22} & r_{23} \\
         *  r_{31} & r_{32} & r_{33}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        InertiaMatrix (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32, T r33)
        {
            _matrix (0, 0) = r11;
            _matrix (0, 1) = r12;
            _matrix (0, 2) = r13;
            _matrix (1, 0) = r21;
            _matrix (1, 1) = r22;
            _matrix (1, 2) = r23;
            _matrix (2, 0) = r31;
            _matrix (2, 1) = r32;
            _matrix (2, 2) = r33;
        }
#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{ccc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}} & \robabx{a}{b}{\mathbf{k}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         * @param k @f$ \robabx{a}{b}{\mathbf{k}} @f$
         */

#endif
        InertiaMatrix (const rw::math::Vector3D< T >& i, const rw::math::Vector3D< T >& j,
                       const rw::math::Vector3D< T >& k)
        {
            _matrix (0, 0) = i[0];
            _matrix (0, 1) = j[0];
            _matrix (0, 2) = k[0];
            _matrix (1, 0) = i[1];
            _matrix (1, 1) = j[1];
            _matrix (1, 2) = k[1];
            _matrix (2, 0) = i[2];
            _matrix (2, 1) = j[2];
            _matrix (2, 2) = k[2];
        }

        /**
         * @brief constructor - where only the diagonal is set
         * @param i [in] m(0,0)
         * @param j [in] m(1,1)
         * @param k [in] m(2,2)
         */
        InertiaMatrix (T i = 0.0, T j = 0.0, T k = 0.0)
        {
            _matrix (0, 0) = i;
            _matrix (0, 1) = 0;
            _matrix (0, 2) = 0;
            _matrix (1, 0) = 0;
            _matrix (1, 1) = j;
            _matrix (1, 2) = 0;
            _matrix (2, 0) = 0;
            _matrix (2, 1) = 0;
            _matrix (2, 2) = k;
        }

        /**
         * @brief Construct an internal matrix from a Eigen::MatrixBase
         * It is the responsibility of the user that 3x3 matrix is indeed an
         * inertia matrix.
         */
        explicit InertiaMatrix (const Base& r) : _matrix (r) {}

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        T& operator() (size_t row, size_t column) { return _matrix (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const T& operator() (size_t row, size_t column) const { return _matrix (row, column); }
#if defined(SWIG)
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Returns reference to the internal 3x3 matrix
         */
        const Base& e () const { return _matrix; }

        /**
         * @brief Returns reference to the internal 3x3 matrix
         */
        Base& e () { return _matrix; }

#if !defined(SWIG)
        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        friend InertiaMatrix operator* (const rw::math::Rotation3D< T >& aRb,
                                        const InertiaMatrix& bRc)
        {
            return InertiaMatrix (aRb.e () * bRc.e ());
        }
#endif
#if !defined(SWIGJAVA)

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
#endif
        InertiaMatrix operator* (const rw::math::Rotation3D< T >& bRc) const
        {
            return InertiaMatrix (this->e () * bRc.e ());
        }

        /**
         * @brief Calculates the addition between the two InertiaMatrices
         */
        InertiaMatrix operator+ (const InertiaMatrix& I2) const
        {
            return InertiaMatrix (this->e () + I2.e ());
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        rw::math::Vector3D< T > operator* (const rw::math::Vector3D< T >& bVc) const
        {
            return rw::math::Vector3D< T > (this->e () * bVc.e ());
        }
#if !defined(SWIG)
        /**
         * @brief Writes rotation matrix to stream
         * @param os [in/out] output stream to use
         * @param r [in] rotation matrix to print
         * @return the updated output stream
         */
        friend std::ostream& operator<< (std::ostream& os, const InertiaMatrix& r)
        {
            return os << r.e ();
        }
#else
        TOSTRING (rw::math::InertiaMatrix< T >);
#endif

        /**
         * @brief Make inertia matrix for a solid sphere.
         * @param mass [in] mass of solid sphere.
         * @param radi [in] radius of sphere.
         * @return the inertia matrix.
         */
        static InertiaMatrix< T > makeSolidSphereInertia (T mass, T radi)
        {
            T tmpV = (T) (2.0 / 5.0) * mass * radi * radi;
            return InertiaMatrix< T > (tmpV, 0, 0, 0, tmpV, 0, 0, 0, tmpV);
        }

        /**
         * @brief Make inertia matrix for a hollow sphere.
         * @param mass [in] mass of hollow sphere.
         * @param radi [in] radius of sphere.
         * @return the inertia matrix.
         */
        static InertiaMatrix< T > makeHollowSphereInertia (T mass, T radi)
        {
            T tmpV = (T) (2.0 / 3.0) * mass * radi * radi;
            return InertiaMatrix< T > (tmpV, 0, 0, 0, tmpV, 0, 0, 0, tmpV);
        }

        /**
         * @brief calculates the inertia of a cuboid where the reference frame is in the
         * center of the cuboid with
         * @param mass
         * @param x
         * @param y
         * @param z
         * @return
         */
        static InertiaMatrix< T > makeCuboidInertia (T mass, T x, T y, T z)
        {
            return InertiaMatrix< T > ((T) (1 / 12.0 * mass * (y * y + z * z)),
                                       0,
                                       0,
                                       0,
                                       (T) (1 / 12.0 * mass * (x * x + z * z)),
                                       0,
                                       0,
                                       0,
                                       (T) (1 / 12.0 * mass * (x * x + y * y)));
        }

      private:
        Base _matrix;
    };
#if !defined(SWIGJAVA)

    /**
     * @brief Calculates the inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */

    #endif
    template< class Q > InertiaMatrix< Q > inverse (const InertiaMatrix< Q >& aRb)
    {
        return InertiaMatrix< Q > (aRb.e ().inverse ());
    }

    /**
     * @brief Casts InertiaMatrix<T> to InertiaMatrix<Q>
     * @param rot [in] InertiaMatrix with type T
     * @return InertiaMatrix with type Q
     */
    template< class Q, class T > InertiaMatrix< Q > cast (const InertiaMatrix< T >& rot)
    {
        InertiaMatrix< Q > res;
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
                res (i, j) = static_cast< Q > (rot (i, j));
        return res;
    }
#if !defined(SWIG)
    extern template class rw::math::InertiaMatrix< double >;
    extern template class rw::math::InertiaMatrix< float >;
#else
    SWIG_DECLARE_TEMPLATE (InertiaMatrixd, rw::math::InertiaMatrix< double >);
    SWIG_DECLARE_TEMPLATE (InertiaMatrixf, rw::math::InertiaMatrix< float >);
#endif

    using InertiaMatrixd = InertiaMatrix< double >;
    using InertiaMatrixf = InertiaMatrix< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void write (const rw::math::InertiaMatrix< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void write (const rw::math::InertiaMatrix< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void read (rw::math::InertiaMatrix< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::InertiaMatrix
         */
        template<>
        void read (rw::math::InertiaMatrix< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <cstdlib>
#include <cassert>
#include <random>
#include <cmath>
#include "Edec.hpp"
static int  n =2;
static int  N =12;
static int q =11158;
//四舍五入取整数
inline int mod(double num)
{
    int dev=(int)std::round(num/q);
    return num-q*dev;
}
boost::numeric::ublas::matrix<int > FinalS(n+1,1);
inline int RandomNum()
{
    std::random_device rd;

    std::mt19937 rng(rd());

    //在这里取到一个范围在0～2q的随机 数字
    //在取一个矩阵的时候mod q 取四舍五入
    std::uniform_int_distribution<int> uni(0,2*q);

    auto random_integer = uni(rng);

    return random_integer;
}
boost::numeric::ublas::matrix<int > GetRandomMatrixS()
{
    using matrix=boost::numeric::ublas::matrix<int>;

    matrix m1(n+1,1);

    m1(0,0)=1;

    for(unsigned int i=1;i<n+1;++i)
    {
      m1(i,0)=mod(RandomNum());
    }

    return m1;
}
boost::numeric::ublas::matrix<int > GetRandomMatrixA()
{
    using matrix=boost::numeric::ublas::matrix<int>;

    matrix m1(N,n);

    for(unsigned int x=0;x<N;++x)
    {
        for(unsigned int y=0;y<n;++y)
        {
            m1(x,y)=mod(RandomNum());
        }
    }

    return m1;
}

boost::numeric::ublas::matrix<int >  GetMatrixB()
{
    using matrix=boost::numeric::ublas::matrix<int>;
    //先取 e == N χ 。
    matrix e(N,1);

    for(int i=0;i<N;++i)
    {
        e(i,0)=RandomNum()%2;

    }
//     std::cout<<e<<"\n";
    matrix b(N,1);

    auto MatrixA=GetRandomMatrixA();

    auto MatrixS=GetRandomMatrixS();

    FinalS=MatrixS;

    matrix MatrixSp(n,1);

    for(unsigned int i=1;i<=n;++i)
    {
        MatrixSp(i-1,0)=MatrixS(i,0);
    }

    auto MatrixB=boost::numeric::ublas::prod(MatrixA, MatrixSp);

    b=( MatrixB+e);

    matrix MatrixAfinal(N,n+1);

    auto col1=boost::numeric::ublas::column(b, 0);

    MatrixA=-MatrixA;

    for(unsigned int i=0;i<N;++i)
    {
        for(unsigned int j=0;j<n+1;++j)
        {
            if(j==0){
                MatrixAfinal(i,0)=b(i,0);

            }

            else
            {
                MatrixAfinal(i,j)=MatrixA(i,j-1);

            }
        }

    }

    return MatrixAfinal;

}

//对消息的加密
//secret为要加密的数字
boost::numeric::ublas::matrix<int > Enc(int secret)
{
    using matrix=boost::numeric::ublas::matrix<int>;
    //密文 c
    matrix c(n+1,1);

    matrix r (N,1);

    auto MatrixA=GetMatrixB();

    for(unsigned int i=0;i<N;++i)
    {
        r(i,0)=RandomNum()%2;
    }

    //倒置矩阵
    MatrixA= boost::numeric::ublas::trans(MatrixA);

    //A倒置T * r
    matrix tmp(n+1,1);

   tmp= boost::numeric::ublas::prod(MatrixA, r);

    //构建加密消息
    matrix m(n+1,1);

    m(0,0)=secret;
    m(1,0)=0;
    m(2,0)=0;
    for(unsigned i=1;i<n+1;++i)
    {
        m(i,0)=m(i,0)*(q/2);
    }
    m(0,0)=m(0,0)*(q/2);

    c=m+tmp;

    return c;
}
inline int InnerProduct(int (&arr1)[3],int (&arr2)[3])
{
    int res=0;
    for(unsigned int i=0;i<n+1;++i)
    {
        res+=arr1[i]*arr2[i];
    }
    return res;
}

int  Dec(int val)
{
    //c为密文
    auto c=Enc(val);

    int arr1[3];
    int arr2[3];

    auto res1=boost::numeric::ublas::column(c, 0);

    auto res2=boost::numeric::ublas::column(FinalS, 0);

    for(int i=0;i<n+1;++i)
    {
        arr1[i]=res1(i);
        arr2[i]=res2(i);
    }
  int   res=InnerProduct(arr1, arr2);

    double qtwo=(double)2/q;

    res=res*qtwo;

    return res%2;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <iostream>

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

//#include "sphere.hpp"
//#include "clusterer.hpp"
//#include "sphericalKMeans.hpp"
#include "ddpmeans.hpp"
//#include "dpvMFmeans.hpp"
//#include "dir.hpp"
//#include "cat.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class DDPvMFMeans : public DDPMeans<T>
{
public:
  DDPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
      T lambda, T beta, T Q, boost::mt19937* pRndGen);
  virtual ~DDPvMFMeans();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual void updateLabelsParallel();
  virtual void updateLabels();
  virtual void updateCenters();
  virtual void nextTimeStep(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx);
  virtual void updateState(); // after converging for a single time instant
//  virtual MatrixXu mostLikelyInds(uint32_t n,
//     Matrix<T,Dynamic,Dynamic>& deviates);
//  virtual T avgIntraClusterDeviation();
  virtual uint32_t indOfClosestCluster(int32_t i);

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);

protected:
//  T lambda_;
//  Sphere<T> S_;

  T beta_;
  T Q_; //TODO!

  Matrix<T,Dynamic,Dynamic> xSums_;

  virtual Matrix<T,Dynamic,1> computeSum(uint32_t k);
  virtual void computeSums(void); // updates internal xSums_

  virtual void solveProblem1(T gamma, T age, T& phi, T& theta);
  virtual void solveProblem2(const Matrix<T,Dynamic,1>& xSum, T zeta, T age, T w,
      T& phi, T& theta, T& eta);

  virtual T distToUninstantiated(const Matrix<T,Dynamic,1>& x_i, uint32_t k);
  virtual void reInstantiatedOldCluster(const Matrix<T,Dynamic,1>& xSum, uint32_t k);

};
// --------------------------- impl -------------------------------------------

template<class T>
DDPvMFMeans<T>::DDPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
    T lambda, T beta, T Q, boost::mt19937* pRndGen)
  : DDPMeans<T>(spx,lambda,0.,0.,pRndGen), beta_(beta), Q_(Q)
{
  this->Kprev_ = 0; // so that centers are initialized directly from sample mean
  this->psPrev_ = this->ps_;
  xSums_ = Matrix<T,Dynamic,Dynamic>::Zero(this->D_,1);

  assert(-2.0 < this->lambda_ && this->lambda_ < 0.0);
}

template<class T>
DDPvMFMeans<T>::~DDPvMFMeans()
{}

template<class T>
uint32_t DDPvMFMeans<T>::indOfClosestCluster(int32_t i)
{
  int z_i = this->K_;
  T sim_closest = this->lambda_+1.;
  T sim_k = 0.;
  cout<<"cluster dists "<<i<<": "<< sim_closest;
  for (uint32_t k=0; k<this->K_; ++k)
  {
    if(this->Ns_(k) == 0)
    {// cluster not instantiated yet in this timestep
      sim_k = distToUninstantiated(this->spx_->col(i), k);
    }else{ // cluster instantiated
      sim_k = dist(this->ps_.col(k), this->spx_->col(i));
    }
    cout<<" "<<sim_k;
    if(closer(sim_k, sim_closest))
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  cout<<" => z_i="<<z_i<<endl;
  return z_i;
}

template<class T>
T DDPvMFMeans<T>::distToUninstantiated(const Matrix<T,Dynamic,1>& x_i, uint32_t k)
{

  T phi, theta, eta;
  T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.0),
          (x_i.transpose()*this->psPrev_.col(k))(0))));
  solveProblem2(x_i, zeta, this->ts_[k], this->ws_[k], phi,theta,eta);

//  cout<<" phi = "<<phi<<" age="<<this->ts_[k]<<endl;

  return this->ws_[k]*(cos(theta)-1.)
    +this->ts_[k]*beta_*(cos(phi)-1.)
    +cos(eta) // no minus 1 here cancels with Z(tau) from the two other assignments
    +Q_*this->ts_[k];
//  return cos(theta) + this->ts_[k]*cos(phi) + cos(eta);

  // TODO: this was the old wrong way
//  T phi, theta;
//  solveProblem1( acos( max(static_cast<T>(-1.),min(static_cast<T>(1.),
//            (this->ps_.col(k).transpose()*x_i)(0)))), this->ts_[k], phi, theta);
//  return cos(theta) + beta_*this->ts_[k]*cos(phi);
}

template<class T>
void DDPvMFMeans<T>::updateLabels()
{
// TODO not sure how to parallelize
//#pragma omp parallel for
  for(uint32_t i=0; i<this->N_; ++i)
  {
    uint32_t z_i = indOfClosestCluster(i);
    if(z_i == this->K_)
    { // start a new cluster
      this->ps_.conservativeResize(this->D_,this->K_+1);
      this->Ns_.conservativeResize(this->K_+1);
      this->ps_.col(this->K_) = this->spx_->col(i);
      this->Ns_(z_i) = 1.;
      this->K_ ++;
//      cout<<" added new cluster center at "<<this->spx_->col(i).transpose()<<endl;
    } else {
      if(this->Ns_[z_i] == 0)
      { // instantiated an old cluster
        reInstantiatedOldCluster(this->spx_->col(i), z_i);
      }
      this->Ns_(z_i) ++;
    }
    if(this->z_(i) != this->UNASSIGNED) this->Ns_(this->z_(i)) --;
    this->z_(i) = z_i;
  }

  for(uint32_t k=0; k<this->K_; ++k)
        this->Ns_(k) =0;

#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        this->Ns_(k) ++;
      }
  cout<<" Ns = "<<this->Ns_.transpose()<<endl;
};

template<class T>
void DDPvMFMeans<T>::updateLabelsParallel()
{
// TODO not sure how to parallelize
  Matrix<T,Dynamic,1> psNew(this->D_,1);
  psNew.fill(0);
  int32_t iNew = -1;
  uint32_t reInstantiateOld = 0;
  Matrix<T,Dynamic,1> reInstantiateFrom(this->D_,1);
  reInstantiateFrom.fill(0);
  int32_t iReinstantiate = -1;
  cout<<"::updateLabelsParallel"<<endl;
  uint32_t i0=0;
  do{
    iNew = -1;
    iReinstantiate = -1;
#pragma omp parallel for
    for(uint32_t i=i0; i<this->N_; ++i)
    {
      uint32_t z_i = indOfClosestCluster(i);
#pragma omp critical
      {
        if(z_i == this->K_)
        { // start a new cluster
          if(iNew < 0)
          {
            psNew = this->spx_->col(i);
            iNew = i;
          }
        } else {
          if(this->Ns_[z_i] == 0 && iReinstantiate < 0)
          { // instantiated an old cluster
            reInstantiateOld = z_i;
            reInstantiateFrom =  this->spx_->col(i);
            iReinstantiate = i;
          }
          //        this->Ns_(z_i) ++;
        }
      }
      //    if(this->z_(i) != this->UNASSIGNED) this->Ns_(this->z_(i)) --;
      this->z_(i) = z_i;
    }
    cout<<iReinstantiate<<" "<<iNew<<endl;

    if(iReinstantiate >= 0 && iNew >= 0)
    {
      if(iNew < iReinstantiate)
      {
        this->ps_.conservativeResize(this->D_,this->K_+1);
        this->Ns_.conservativeResize(this->K_+1);
        this->ps_.col(this->K_) = psNew;
        this->Ns_(this->K_) = 1.;
        this->K_ ++;
        i0 = iNew;
      }else{
        reInstantiatedOldCluster(reInstantiateFrom, reInstantiateOld);
        i0 = iReinstantiate;
      }
    }else if(iReinstantiate < 0 && iNew >= 0)
    {
      this->ps_.conservativeResize(this->D_,this->K_+1);
      this->Ns_.conservativeResize(this->K_+1);
      this->ps_.col(this->K_) = psNew;
      this->Ns_(this->K_) = 1.;
      this->K_ ++;
      i0 = iNew;
    }else if(iReinstantiate >= 0 && iNew < 0)
    {
      reInstantiatedOldCluster(reInstantiateFrom, reInstantiateOld);
        i0 = iReinstantiate;
    }
  }while(iReinstantiate >= 0 || iNew >= 0);

  //TODO get counts from GPU
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        this->Ns_(k) ++;
      }

};

template<class T>
void DDPvMFMeans<T>::reInstantiatedOldCluster(const Matrix<T,Dynamic,1>& xSum, uint32_t k)
{
//  cout<<"xSum: "<<xSum.transpose()<<endl;
  T phi, theta, eta;
  T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.0),
          (xSum.transpose()*this->psPrev_.col(k))(0)/xSum.norm())));
  solveProblem2(xSum , zeta, this->ts_[k], this->ws_[k], phi,theta,eta);

  // rotate point from mean_k towards previous mean by angle eta?
  this->ps_.col(k) = rotationFromAtoB<T>(xSum/xSum.norm(),
      this->psPrev_.col(k), eta/(phi*this->ts_[k]+theta+eta))
};

template<class T>
void DDPvMFMeans<T>::computeSums(void)
{
  xSums_ = Matrix<T,Dynamic,Dynamic>::Zero(this->D_, this->K_);
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
    for(uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        xSums_.col(k) += this->spx_->col(i);
      }
}

template<class T>
Matrix<T,Dynamic,1> DDPvMFMeans<T>::computeSum(uint32_t k)
{
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
    }
  return mean_k;
}

template<class T>
void DDPvMFMeans<T>::updateCenters()
{
//  xSums_ = computeSums();
  computeSums();
//#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
  {
//    Matrix<T,Dynamic,1> mean_k = this->computeCenter(k);
    if (this->Ns_(k) > 0)
    { // have data to update kth cluster
      if(k < this->Kprev_)
      { //TODO
        reInstantiatedOldCluster(xSums_.col(k), k);
      }else{
        this->ps_.col(k)= xSums_.col(k)/xSums_.col(k).norm(); //mean_k;
      }
    }
  cout<<this->ps_<<endl;
    assert(this->ps_(0,k) == this->ps_(0,k));
  }
};

template<class T>
void DDPvMFMeans<T>::nextTimeStep(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx)
{
  assert(this->D_ == spx->rows());
  if(this->spx_.get() != spx.get()) this->spx_ = spx; // update the data
  this->N_ = spx->cols();
  this->z_.resize(this->N_);
  this->z_.fill(this->UNASSIGNED);
};

template<class T>
void DDPvMFMeans<T>::updateState()
{
//  xSums_ = computeSums(); // already computed from last updateCenters (and no
//  label changes since)

  for(uint32_t k=0; k<this->K_; ++k)
  {
    if (k<this->ws_.size() && this->Ns_(k) > 0)
    { // instantiated cluster from previous time;
      T phi, theta, eta;
      T zeta = acos(max(static_cast<T>(-1.),min(static_cast<T>(1.),
              (xSums_.col(k).transpose()*this->ps_.col(k))(0)
              / xSums_.col(k).norm())));
      solveProblem2(xSums_.col(k), zeta, this->ts_[k], this->ws_[k], phi,theta,eta);
      this->ws_[k] = this->ws_[k]*cos(theta)
        + beta_*this->ts_[k]*cos(phi)
        + xSums_.col(k).norm()*cos(eta);

      cout<<this->ws_[k]<<" : "<<theta<<" "<<phi<<" "<<eta
        <<" "<< xSums_.col(k).norm()<<endl;

//      this->ws_[k] = this->ws_[k]*cos(theta) + beta_*this->ts_[k]*cos(phi)
//        + xSums_.col(k).norm()*cos(eta);
//      this->ws_[k] = 1./(1./this->ws_[k] + this->ts_[k]*tau_) + this->Ns_(k);
      this->ts_[k] = 0; // re-instantiated -> age is 0
    }else if(k >= this->ws_.size()){
      // new cluster
      this->ts_.push_back(0);
      //TODO
      this->ws_.push_back(xSums_.col(k).norm());//this->Ns_(k));
    }

      assert(this->ws_[k] == this->ws_[k]);

    this->ts_[k] ++; // increment all ages
    cout<<"cluster "<<k
      <<"\tN="<<this->Ns_(k)
      <<"\tage="<<this->ts_[k]
      <<"\tweight="<<this->ws_[k]
      <<"\tcenter: "<<this->ps_.col(k).transpose()<<endl;
  }
  this->psPrev_ = this->ps_;
  this->Kprev_ = this->K_;
};

template<class T>
T DDPvMFMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
//  return acos(min(1.0,max(-1.0,(a.transpose()*b)(0)))); // angular similarity
  return a.transpose()*b; // cosine similarity
};

template<class T>
bool DDPvMFMeans<T>::closer(T a, T b)
{
//  return a<b; // if dist a is greater than dist b a is closer than b (angular dist)
  return a>b; // if dist a is greater than dist b a is closer than b (cosine dist)
};

template<class T>
void DDPvMFMeans<T>::solveProblem1(T gamma, T age, T& phi, T& theta)
{
  // solves
  // (1)  sin(phi) beta = sin(theta)
  // (2)  gamma = T phi + theta
  // for phi and theta
  phi = 0.0;

  for (uint32_t i=0; i< 10; ++i)
  {
    T sinPhi = sin(phi);
    T f = - gamma + age*phi + asin(beta_*sinPhi);
    // mathematica
    T df = age + (beta_*cos(phi))/sqrt(1.-beta_*beta_*sinPhi*sinPhi);
    T dPhi = f/df;
    phi = phi - dPhi; // Newton iteration
//    cout<<"@i="<<i<<": "<<phi<<"\t"<<dPhi<<endl;
    if(fabs(dPhi) < 1e-6) break;
  }

  theta = asin(beta_*sin(phi));
};

template<class T>
void DDPvMFMeans<T>::solveProblem2(const Matrix<T,Dynamic,1>& xSum, T zeta,
    T age, T w, T& phi, T& theta, T& eta)
{
  // solves
  // w sin(theta) = beta sin(phi) = ||xSum||_2 sin(eta)
  // eta + T phi + theta = zeta = acos(\mu0^T xSum/||xSum||_2)
  phi = 0.0;

//  cout<<"w="<<w<<" age="<<age<<" zeta="<<zeta<<endl;

  T L2xSum = xSum.norm();
  for (uint32_t i=0; i< 10; ++i)
  {
    T sinPhi = sin(phi);
    T cosPhi = cos(phi);
    T f = - zeta + asin(beta_/L2xSum *sinPhi) + age * phi + asin(beta_/w *sinPhi);
    T df = age + (beta_*cosPhi)/sqrt(L2xSum*L2xSum -
        beta_*beta_*sinPhi*sinPhi) + (beta_*cosPhi)/sqrt(w*w -
        beta_*beta_*sinPhi*sinPhi);

    T dPhi = f/df;

    phi = phi - dPhi; // Newton iteration
//    cout<<"@i="<<i<<": "<<"f="<<f<<" df="<<df<<" phi="<<phi<<"\t"<<dPhi<<endl;
    if(fabs(dPhi) < 1e-6) break;
  }

  theta = asin(beta_/w *sin(phi));
  eta = asin(beta_/L2xSum *sin(phi));
};