///////////////////////////////////////////////////////////////////////////////
// weighted_p_square_quantile.hpp
//
//  Copyright 2005 Daniel Egloff. 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 BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_P_SQUARE_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_P_SQUARE_QUANTILE_HPP_DE_01_01_2006

#include <cmath>
#include <functional>
#include <boost/array.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

namespace boost { namespace accumulators
{

namespace impl {
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_p_square_quantile_impl
    //  single quantile estimation with weighted samples
    /**
        @brief Single quantile estimation with the \f$P^2\f$ algorithm for weighted samples

        This version of the \f$P^2\f$ algorithm extends the \f$P^2\f$ algorithm to support weighted samples.
        The \f$P^2\f$ algorithm estimates a quantile dynamically without storing samples. Instead of
        storing the whole sample cumulative distribution, only five points (markers) are stored. The heights
        of these markers are the minimum and the maximum of the samples and the current estimates of the
        \f$(p/2)\f$-, \f$p\f$ - and \f$(1+p)/2\f$ -quantiles. Their positions are equal to the number
        of samples that are smaller or equal to the markers. Each time a new sample is added, the
        positions of the markers are updated and if necessary their heights are adjusted using a piecewise-
        parabolic formula.

        For further details, see

        R. Jain and I. Chlamtac, The P^2 algorithm for dynamic calculation of quantiles and
        histograms without storing observations, Communications of the ACM,
        Volume 28 (October), Number 10, 1985, p. 1076-1085.

        @param quantile_probability
    */
    template<typename Sample, typename Weight, typename Impl>
    struct weighted_p_square_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        typedef array<float_type, 5> array_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        weighted_p_square_quantile_impl(Args const &args)
          : p(is_same<Impl, for_median>::value ? 0.5 : args[quantile_probability | 0.5])
          , heights()
          , actual_positions()
          , desired_positions()
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            // accumulate 5 first samples
            if (cnt <= 5)
            {
                this->heights[cnt - 1] = args[sample];

                // In this initialization phase, actual_positions stores the weights of the
                // initial samples that are needed at the end of the initialization phase to
                // compute the correct initial positions of the markers.
                this->actual_positions[cnt - 1] = args[weight];

                // complete the initialization of heights and actual_positions by sorting
                if (cnt == 5)
                {
                    // TODO: we need to sort the initial samples (in heights) in ascending order and
                    // sort their weights (in actual_positions) the same way. The following lines do
                    // it, but there must be a better and more efficient way of doing this.
                    typename array_type::iterator it_begin, it_end, it_min;

                    it_begin = this->heights.begin();
                    it_end   = this->heights.end();

                    std::size_t pos = 0;

                    while (it_begin != it_end)
                    {
                        it_min = std::min_element(it_begin, it_end);
                        std::size_t d = std::distance(it_begin, it_min);
                        std::swap(*it_begin, *it_min);
                        std::swap(this->actual_positions[pos], this->actual_positions[pos + d]);
                        ++it_begin;
                        ++pos;
                    }

                    // calculate correct initial actual positions
                    for (std::size_t i = 1; i < 5; ++i)
                    {
                        this->actual_positions[i] += this->actual_positions[i - 1];
                    }
                }
            }
            else
            {
                std::size_t sample_cell = 1; // k

                // find cell k such that heights[k-1] <= args[sample] < heights[k] and adjust extreme values
                if (args[sample] < this->heights[0])
                {
                    this->heights[0] = args[sample];
                    this->actual_positions[0] = args[weight];
                    sample_cell = 1;
                }
                else if (this->heights[4] <= args[sample])
                {
                    this->heights[4] = args[sample];
                    sample_cell = 4;
                }
                else
                {
                    typedef typename array_type::iterator iterator;
                    iterator it = std::upper_bound(
                        this->heights.begin()
                      , this->heights.end()
                      , args[sample]
                    );

                    sample_cell = std::distance(this->heights.begin(), it);
                }

                // increment positions of markers above sample_cell
                for (std::size_t i = sample_cell; i < 5; ++i)
                {
                    this->actual_positions[i] += args[weight];
                }

                // update desired positions for all markers
                this->desired_positions[0] = this->actual_positions[0];
                this->desired_positions[1] = (sum_of_weights(args) - this->actual_positions[0])
                                           * this->p/2. + this->actual_positions[0];
                this->desired_positions[2] = (sum_of_weights(args) - this->actual_positions[0])
                                           * this->p + this->actual_positions[0];
                this->desired_positions[3] = (sum_of_weights(args) - this->actual_positions[0])
                                           * (1. + this->p)/2. + this->actual_positions[0];
                this->desired_positions[4] = sum_of_weights(args);

                // adjust height and actual positions of markers 1 to 3 if necessary
                for (std::size_t i = 1; i <= 3; ++i)
                {
                    // offset to desired positions
                    float_type d = this->desired_positions[i] - this->actual_positions[i];

                    // offset to next position
                    float_type dp = this->actual_positions[i + 1] - this->actual_positions[i];

                    // offset to previous position
                    float_type dm = this->actual_positions[i - 1] - this->actual_positions[i];

                    // height ds
                    float_type hp = (this->heights[i + 1] - this->heights[i]) / dp;
                    float_type hm = (this->heights[i - 1] - this->heights[i]) / dm;

                    if ( ( d >= 1. && dp > 1. ) || ( d <= -1. && dm < -1. ) )
                    {
                        short sign_d = static_cast<short>(d / std::abs(d));

                        // try adjusting heights[i] using p-squared formula
                        float_type h = this->heights[i] + sign_d / (dp - dm) * ( (sign_d - dm) * hp + (dp - sign_d) * hm );

                        if ( this->heights[i - 1] < h && h < this->heights[i + 1] )
                        {
                            this->heights[i] = h;
                        }
                        else
                        {
                            // use linear formula
                            if (d>0)
                            {
                                this->heights[i] += hp;
                            }
                            if (d<0)
                            {
                                this->heights[i] -= hm;
                            }
                        }
                        this->actual_positions[i] += sign_d;
                    }
                }
            }
        }

        result_type result(dont_care) const
        {
            return this->heights[2];
        }

    private:
        float_type p;                    // the quantile probability p
        array_type heights;              // q_i
        array_type actual_positions;     // n_i
        array_type desired_positions;    // n'_i
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_p_square_quantile
//
namespace tag
{
    struct weighted_p_square_quantile
      : depends_on<count, sum_of_weights>
    {
        typedef accumulators::impl::weighted_p_square_quantile_impl<mpl::_1, mpl::_2, regular> impl;
    };
    struct weighted_p_square_quantile_for_median
      : depends_on<count, sum_of_weights>
    {
        typedef accumulators::impl::weighted_p_square_quantile_impl<mpl::_1, mpl::_2, for_median> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_p_square_quantile
// extract::weighted_p_square_quantile_for_median
//
namespace extract
{
    extractor<tag::weighted_p_square_quantile> const weighted_p_square_quantile = {};
    extractor<tag::weighted_p_square_quantile_for_median> const weighted_p_square_quantile_for_median = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_p_square_quantile_for_median)
}

using extract::weighted_p_square_quantile;
using extract::weighted_p_square_quantile_for_median;

}} // namespace boost::accumulators

#endif


#ifndef ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED
#define ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED

// C++ standard library headers
#include <cmath> // for std::sqrt, std::pow, std::abs
#include <cstddef> // for std::size_t
#include <limits>

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

namespace zsvm {

    template <typename T>
    class RealVariationalSolver {

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

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

        std::size_t size;
        MatrixXT overlap_matrix;
        MatrixXT hamiltonian_matrix;
        VectorXT eigenvalues;
        MatrixXT eigenvectors;
        bool clean;

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

        RealVariationalSolver() : size(0), clean(false) {}

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

        void set_basis_size_conservative(std::size_t basis_size) {
            size = basis_size;
            overlap_matrix.conservativeResize(basis_size, basis_size);
            hamiltonian_matrix.conservativeResize(basis_size, basis_size);
            clean = false;
        }

        void set_basis_size_destructive(std::size_t basis_size) {
            size = basis_size;
            overlap_matrix.resize(basis_size, basis_size);
            hamiltonian_matrix.resize(basis_size, basis_size);
            clean = false;
        }

        T &overlap_matrix_element(std::size_t i, std::size_t j) {
            clean = false;
            return overlap_matrix(i, j);
        }

        T &hamiltonian_matrix_element(std::size_t i, std::size_t j) {
            clean = false;
            return hamiltonian_matrix(i, j);
        }

        void compute_eigenstates() {
            if (!clean) {
                Eigen::GeneralizedSelfAdjointEigenSolver<MatrixXT> eigen_solver(
                        hamiltonian_matrix, overlap_matrix,
                        Eigen::ComputeEigenvectors | Eigen::Ax_lBx);
                eigenvalues = eigen_solver.eigenvalues();
                eigenvectors = eigen_solver.eigenvectors();
                clean = true;
            }
        }

    public: // ======================================================== ACCESORS

        bool empty() const { return (size == 0); }

        T get_eigenvalue(std::size_t i) {
            if (!clean) { compute_eigenstates(); }
            return eigenvalues(i);
        }

    private: // ========================== EIGENVALUE COMPUTATION HELPER METHODS

        T secular_objective_function(const T &x, const T &t,
                                     const ArrayXT &beta) const {
            return t - x - (beta / (eigenvalues.array() - x)).sum();
        }

        void find_lower_bracketing_interval(
                T &lower_bound, T &upper_bound,
                const T &strict_upper_bound,
                const T &t, const ArrayXT &beta) const {
            using std::ldexp;
            const T one(1);
            int k = 0;
            lower_bound = strict_upper_bound - ldexp(one, -k);
            upper_bound = strict_upper_bound - ldexp(one, -k - 1);
            T lower_objective = secular_objective_function(
                    lower_bound, t, beta);
            T upper_objective = secular_objective_function(
                    upper_bound, t, beta);
            while (true) {
                if (lower_objective > 0 && upper_objective > 0) {
                    ++k;
                    lower_bound = upper_bound;
                    lower_objective = upper_objective;
                    upper_bound = strict_upper_bound - ldexp(one, -k - 1);
                    upper_objective = secular_objective_function(
                            upper_bound, t, beta);
                } else if (lower_objective < 0 && upper_objective < 0) {
                    --k;
                    upper_bound = lower_bound;
                    upper_objective = lower_objective;
                    lower_bound = strict_upper_bound - ldexp(one, -k);
                    lower_objective = secular_objective_function(
                            lower_bound, t, beta);
                } else {
                    break;
                }
            }
        }

        T solve_secular_equation_bisection(
                T lower_bound, T upper_bound,
                const T &t, const ArrayXT &beta) const {
            using std::abs;
            const T tolerance = 64 * std::numeric_limits<T>::epsilon();
            while (true) {
                const T midpoint = (lower_bound + upper_bound) / 2;
                const T relative_difference = abs(
                        (upper_bound - lower_bound) / midpoint);
                if (relative_difference < tolerance) { return midpoint; }
                const T midpoint_objective = secular_objective_function(
                        midpoint, t, beta);
                if (midpoint_objective == 0) {
                    return midpoint;
                } else if (midpoint_objective > 0) {
                    lower_bound = midpoint;
                } else if (midpoint_objective < 0) {
                    upper_bound = midpoint;
                } else {
                    return std::numeric_limits<T>::quiet_NaN();
                }
            }
        }

    public: // ============================= FAST EIGENVALUE COMPUTATION METHODS

        T minimum_augmented_eigenvalue(
                const T *new_overlap_column,
                const T *new_hamiltonian_column) {
            using std::sqrt;
            if (!clean) { compute_eigenstates(); }
            VectorXT overlap_vector(size);
            for (std::size_t i = 0; i < size; ++i) {
                overlap_vector(i) = new_overlap_column[i];
            }
            VectorXT hamiltonian_vector(size);
            for (std::size_t i = 0; i < size; ++i) {
                hamiltonian_vector(i) = new_hamiltonian_column[i];
            }
            const VectorXT alpha = eigenvectors.transpose() * overlap_vector;
            const T norm_factor = 1 / sqrt(
                    new_overlap_column[size] - alpha.squaredNorm());
            const VectorXT phi = -norm_factor * (eigenvectors * alpha);
            const VectorXT psi = hamiltonian_matrix * phi +
                                 norm_factor * hamiltonian_vector;
            const ArrayXT beta =
                    (eigenvectors.transpose() * psi).array().square();
            const T t = phi.dot(psi) + norm_factor * (
                    hamiltonian_vector.dot(phi) +
                    norm_factor * new_hamiltonian_column[size]);
            T lower_bound, upper_bound;
            find_lower_bracketing_interval(
                    lower_bound, upper_bound, eigenvalues[0], t, beta);
            return solve_secular_equation_bisection(
                    lower_bound, upper_bound, t, beta);
        }

    }; // class RealVariationalSolver

} // namespace zsvm

#endif // ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-euler
    Function object implementing stirling capabilities

    Computes stirling formula for the gamma function

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = stirling(x);
    @endcode

    Computes  \sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.

    The formula implementation is usable for x between 33 and 172,
    according cephes to approximate \f$\Gamma(x).

    @see gamma, gammaln

  **/
  Value stirling(Value const & v0);
} }
#endif

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

#endif


//
//  test_helib.cpp
//  kcalg
//
//  Created by knightc on 2019/7/19.
//  Copyright © 2019 knightc. All rights reserved.
//

#include "opentsb/test.h"

#include <NTL/ZZ.h>

#include <helib/FHEContext.h>
#include <helib/EncryptedArray.h>
#include <helib/FHE.h>
#include <helib/DoubleCRT.h>

#include <stack>
#include <string>
#include <stdlib.h>

using namespace std;
using namespace NTL;

stack<Ctxt> theStack;
FHEcontext* context;
FHESecKey* secretKey;
FHEPubKey* publicKey;
EncryptedArray* ea;
ZZX Gx;

void setupHElib();
bool isOp(string token);
void evaluate(char op);

void greeting(){
    cout <<"Welcome to the homomorphic encryption calculator" <<endl;
    cout <<"Enter expression in reverse polish natation"<<endl;
    cout <<"Enter q to quit"<<endl;
}

void test_helib_all_main(){

    string token;

    greeting();
    setupHElib();
    ea = new EncryptedArray(*context, Gx);

    while(true){

        cin >> token;

        if(token[0] == 'q'){
            break;
        }
        else if(isOp(token)){
            if(theStack.size()<2){
                cout << "not enough numbers on the stack"<<endl;
            }
            else{
                evaluate(token[0]);
            }
        }
        else{
            Ctxt& c0= *(new Ctxt(*publicKey));
            PlaintextArray p0(*ea);
            encode(*ea,p0,atoi(token.data()));
            ea->encrypt(c0, *publicKey, p0);

            theStack.push(c0);
        }
    }

    PlaintextArray p_decrypted(*ea);
    ea->decrypt(theStack.top(), *secretKey, p_decrypted);
    cout << "The answer is: ";
    p_decrypted.print(cout);
    cout << endl;

}

void setupHElib(){
    long p=101;
    long r=1;
    long L=4;
    long c=2;
    long k=80;
    long s=0;
    long d=0;
    long w=64;
    long m=FindM(k,L,c,p,d,s,0);

    context = new FHEcontext(m,p,r);
    buildModChain(*context, L, c);
    Gx = context->alMod.getFactorsOverZZ()[0];

    secretKey = new FHESecKey(*context);
    publicKey = secretKey;

    secretKey->GenSecKey(w);
    addSome1DMatrices(*secretKey); // compute key-switching matrices that we need
}

bool isOp(string token){
    return (token[0] == '+' || token[0] == '-' || token[0] == '*');
}

void evaluate(char op){
    Ctxt *op1,*op2;

    switch(op) {
        case '+':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) += (*op2);
            theStack.push(*op1);
            break;
        case '-':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) -= (*op2);
            theStack.push(*op1);
            break;
        case '*':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) *= (*op2);
            theStack.push(*op1);
            break;
    }
}


// Filename: matrix_free_cg.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 10, N = size * size;

    typedef mtl::mat::poisson2D_dirichlet  matrix_type;
    matrix_type                               A(size, size);
    itl::pc::identity<matrix_type>            P(A);

    mtl::dense_vector<double>                 x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    itl::cyclic_iteration<double>             iter(b, 100, 1.e-11, 0.0, 5);
    cg(A, x, b, P, iter);

    return 0;
}


#include "sigen/toolbox/toolbox.h"
#include "sigen/common/disjoint_set.h"
#include "sigen/common/math.h"
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/scoped_array.hpp>
#include <cmath>
#include <functional>
#include <iostream>
#include <kdtree/kdtree.h>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <utility>
#include <vector>
namespace sigen {
static double norm2(
    const NeuronNodePtr &a,
    const NeuronNodePtr &b) {
  const double dx = std::abs(a->gx_ - b->gx_);
  const double dy = std::abs(a->gy_ - b->gy_);
  const double dz = std::abs(a->gz_ - b->gz_);
  return std::sqrt(dx * dx + dy * dy + dz * dz);
}

// N = left.NumNodes()
// M = right.NumNodes()
// O(N*M)
static std::pair<double, std::pair<int, int> > normNeuronFastPath(const Neuron &left, const Neuron &right) {
  int l, r;
  double minimum = std::numeric_limits<double>::max();
  for (int i = 0; i < (int)left.NumNodes(); ++i) {
    for (int j = 0; j < (int)right.NumNodes(); ++j) {
      double d = norm2(left.storage_[i], right.storage_[j]);
      if (minimum > d) {
        minimum = d;
        l = i;
        r = j;
      }
    }
  }
  return std::make_pair(minimum, std::make_pair(l, r));
}

// N = left.NumNodes()
// M = right.NumNodes()
// O((N + M) log N)
// Use https://github.com/jtsiomb/kdtree
static std::pair<double, std::pair<int, int> > normNeuronSlowPath(const Neuron &left, const Neuron &right) {
  kdtree *tree = kd_create(3);

  boost::scoped_array<int> indexes(new int[left.NumNodes()]);
  for (int i = 0; i < (int)left.NumNodes(); ++i) {
    indexes[i] = i;
    kd_insert3(
        tree,
        left.storage_[i]->gx_,
        left.storage_[i]->gy_,
        left.storage_[i]->gz_,
        &indexes[i]);
  }

  int l, r;
  double minimum = std::numeric_limits<double>::max();
  for (int j = 0; j < (int)right.NumNodes(); ++j) {
    kdres *set = kd_nearest3(
        tree,
        right.storage_[j]->gx_,
        right.storage_[j]->gy_,
        right.storage_[j]->gz_);
    int i = *(int *)kd_res_item_data(set);
    kd_res_free(set);
    double d = norm2(left.storage_[i], right.storage_[j]);
    if (minimum > d) {
      minimum = d;
      l = i;
      r = j;
    }
  }

  kd_free(tree);
  return std::make_pair(minimum, std::make_pair(l, r));
}

static std::pair<double, std::pair<int, int> > normNeuron(const Neuron &left, const Neuron &right) {
  assert(!left.IsEmpty());
  assert(!right.IsEmpty());
  if (std::min(left.NumNodes(), right.NumNodes()) >= 300) {
    return normNeuronSlowPath(left, right);
  } else {
    return normNeuronFastPath(left, right);
  }
}

std::vector<Neuron> Interpolate(const std::vector<Neuron> &input, const double dt, const int vt) {
  const int N = input.size();
  std::vector<Neuron> forest;
  for (int i = 0; i < N; ++i) {
    forest.push_back(input[i].Clone());
  }
  DisjointSet<int> set;
  std::vector<bool> is_not_small(forest.size(), false);
  for (int i = 0; i < N; ++i) {
    if (forest[i].NumNodes() >= vt) {
      set.Add(i);
      is_not_small[i] = true;
    }
  }
  set.SetUp();

  typedef std::pair<double, std::pair<int, int> > priorityQueueNode;
  std::priority_queue<
      priorityQueueNode,
      std::vector<priorityQueueNode>,
      std::greater<priorityQueueNode> >
      pq;

  std::vector<std::map<int, double> > distance(N);

  for (int i = 0; i < N; ++i) {
    if (is_not_small[i]) {
      for (int j = i + 1; j < N; ++j) {
        if (is_not_small[j]) {
          assert(i != j);
          double d = normNeuron(forest[i], forest[j]).first;
          if (d <= dt) {
            pq.push(std::make_pair(d, std::make_pair(i, j)));
            distance[i][j] = d;
            distance[j][i] = d;
          }
        }
      }
    }
  }
  while (!pq.empty()) {
    priorityQueueNode node = pq.top();
    pq.pop();
    int l = node.second.first;
    int r = node.second.second;
    if (forest[l].IsEmpty())
      continue;
    if (forest[r].IsEmpty())
      continue;
    if (set.IsSame(l, r))
      continue;
    std::pair<double, std::pair<int, int> > dist = normNeuron(forest[l], forest[r]);
    set.Merge(l, r);
    forest[l].ConnectToOtherNeuron(dist.second.first, forest[r], dist.second.second);
    forest[r].ConnectToOtherNeuron(dist.second.second, forest[l], dist.second.first);
    forest[l].Extend(forest[r]);
    forest[r].Clear();

    for (std::map<int, double>::iterator it = distance[r].begin(); it != distance[r].end(); ++it) {
      int i = it->first;
      double d = it->second;
      assert(is_not_small[i]);
      if (set.IsSame(l, i) == false && forest[i].IsEmpty() == false) {
        if (distance[l].count(i)) {
          distance[l][i] = std::min(distance[l][i], d);
        } else {
          distance[l][i] = d;
        }

        if (distance[i].count(l)) {
          distance[i][l] = std::min(distance[i][l], d);
        } else {
          distance[i][l] = d;
        }

        pq.push(std::make_pair(d, std::make_pair(l, i)));
      }
    }
  }
  for (int i = 0; i < (int)forest.size(); ++i) {
    if (forest[i].IsEmpty()) {
      forest.erase(forest.begin() + i);
      i--;
    }
  }
  return forest;
}

struct PointAndRadius {
  double gx_, gy_, gz_, radius_;
  void setCoord(const double gx, const double gy, const double gz) {
    gx_ = gx;
    gy_ = gy;
    gz_ = gz;
  }
};

std::vector<Neuron> Smoothing(const std::vector<Neuron> &input, const int n_iter) {
  std::vector<Neuron> forest;
  for (int i = 0; i < (int)input.size(); ++i) {
    forest.push_back(input[i].Clone());
  }
  for (int iter = 0; iter < n_iter; ++iter) {
    std::map<int, PointAndRadius> next_value;
    for (int i = 0; i < (int)forest.size(); ++i) {
      BOOST_FOREACH (NeuronNodePtr node, forest[i].storage_) {
        std::vector<double> gx, gy, gz, radius;
        gx.push_back(node->gx_);
        gy.push_back(node->gy_);
        gz.push_back(node->gz_);
        radius.push_back(node->radius_);
        BOOST_FOREACH (NeuronNode *adj, node->adjacent_) {
          gx.push_back(adj->gx_);
          gy.push_back(adj->gy_);
          gz.push_back(adj->gz_);
          radius.push_back(adj->radius_);
        }
        PointAndRadius next_node;
        next_node.setCoord(Mean(gx), Mean(gy), Mean(gz));
        next_node.radius_ = Mean(radius);
        next_value[node->id_] = next_node;
      }
    }
    for (int i = 0; i < (int)forest.size(); ++i) {
      BOOST_FOREACH (NeuronNodePtr node, forest[i].storage_) {
        PointAndRadius next_node = next_value[node->id_];
        node->setCoord(next_node.gx_, next_node.gy_, next_node.gz_);
        node->radius_ = next_node.radius_;
      }
    }
  }
  return forest;
}

// return max_height
static int clippingDfs(
    NeuronNode *node,
    NeuronNode *parent,
    const int level,
    std::set<int> &will_remove,
    std::map<NeuronNode *, int> &memo) {
  if (memo.count(node))
    return memo[node];
  if (node->CountNumChild(parent) < 2) {
    // If count_num_child == 1
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        return memo[node] = clippingDfs(next, node, level, will_remove, memo) + 1;
      }
    }
    // If count_num_child == 0
    return 1;
  }
  int has_longpath = 0;
  BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
    if (next != parent) {
      int depth = clippingDfs(next, node, level, will_remove, memo);
      if (depth > level)
        has_longpath = true;
    }
  }
  if (has_longpath) {
    int maxdepth = 0;
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        int depth = clippingDfs(next, node, level, will_remove, memo);
        if (depth <= level) {
          will_remove.insert(next->id_);
        }
        maxdepth = std::max(maxdepth, depth);
      }
    }
    return memo[node] = maxdepth + 1;
  } else {
    int maxdepth = 0;
    NeuronNode *longest_child = NULL;
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        int depth = clippingDfs(next, node, level, will_remove, memo);
        if (maxdepth < depth) {
          maxdepth = depth;
          longest_child = next;
        }
      }
    }
    if (maxdepth > 0) {
      BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
        if (next != parent && next != longest_child) {
          will_remove.insert(next->id_);
        }
      }
    }
    return memo[node] = maxdepth + 1;
  }
}

std::vector<Neuron> Clipping(const std::vector<Neuron> &input, const int level) {
  std::set<int> will_remove;
  std::vector<Neuron> forest;
  std::map<NeuronNode *, int> memo;
  for (int i = 0; i < (int)input.size(); ++i) {
    forest.push_back(input[i].Clone());
    clippingDfs(forest[i].get_root(), NULL, level, will_remove, memo);
  }
  for (int i = 0; i < (int)forest.size(); ++i) {
    forest[i].RemoveConnections(will_remove);
  }
  return forest;
}
} // namespace sigen


/*
 *
 * Copyright Jeremy Conlin 2008
 *
 * 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 BOOST_NUMERIC_BINDINGS_LAPACK_HSEQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HSEQR_HPP

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/matrix_traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Compute eigenvalues of an Hessenberg matrix, H.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * hseqr() computes the eigenvalues of a Hessenberg matrix H
     * and, optionally, the matrices T and Z from the Schur decomposition
     * H = Z U Z**T, where U is an upper quasi-triangular matrix (the
     * Schur form), and Z is the orthogonal matrix of Schur vectors.
     *
     * Optionally Z may be postmultiplied into an input orthogonal
     * matrix Q so that this routine can give the Schur factorization
     * of a matrix A which has been reduced to the Hessenberg form H
     * by the orthogonal matrix Q:  A = Q*H*Q**T = (QZ)*U*(QZ)**T.
     *
     * There are two forms of the hseqr function:
     *
     * int hseqr( const char job, A& H, W& w)
     * int hseqr( const char job, const char compz, A& H, W& w, Z& z)
     *
     * The first form does not compute Schur vectors and is equivelant to
     * setting compz = 'N' in the second form.  hseqr returns a '0' if the
     * computation is successful.
     *
     * job.
     *   = 'E': compute eigenvalues only
     *   = 'S': compute eigenvalues and the Schur form U
     *
     * compz. (input)
     *   = 'N':  no Schur vectors are computed;  Equivalent to using the
     *           first form of the hseqr function.
     *   = 'I':  Z is initialized to the unit matrix and the matrix Z
     *           of Schur vectors of H is returned;
     *   = 'V':  Z must contain an orthogonal matrix Q on entry, and
     *           the product Q*Z is returned.
     *
     * H is the Hessenberg matrix whose eigenpairs you're interested
     * in. (input/output) On exit, if computation is successful and
     * job = 'S', then H contains the
     * upper quasi-triangular matrix U from the Schur decomposition
     * (the Schur form); 2-by-2 diagonal blocks (corresponding to
     * complex conjugate pairs of eigenvalues) are returned in
     * standard form, with H(i,i) = H(i+1,i+1) and
     * H(i+1,i)*H(i,i+1) < 0. If computation is successful and
     * job = 'E', the contents of H are unspecified on exit.
     *
     * w (output) contains the computed eigenvalues of H which is the diagonal
     * of U. Must be a complex object.
     *
     * Z. (input/output)
     * If compz = 'N', Z is not referenced.
     * If compz = 'I', on entry Z need not be set and on exit,
     * if computation is successful, Z contains the orthogonal matrix Z of the Schur
     * vectors of H.  If compz = 'V', on entry Z must contain an
     * N-by-N matrix Q, which is assumed to be equal to the unit
     * matrix . On exit, if computation is successful, Z contains Q*Z.
     *
     */

    namespace detail {
        // float
        inline
        int hseqr_backend(const char* job, const char* compz, const int* n,
                const int ilo, const int ihi, float* H, const int ldH,
                float* wr, float* wi, float* Z, const int ldz, float* work,
                const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for floats"
//              << std::endl;
            LAPACK_SHSEQR(job, compz, n, &ilo, &ihi, H, &ldH, wr, wi,
                    Z, &ldz, work, lwork, &info);
            return info;
        }

        // double
        inline
        int hseqr_backend(const char* job, const char* compz, const int* n,
                const int ilo, const int ihi, double* H, const int ldH,
                double* wr, double* wi, double* Z, const int ldz, double* work,
                const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for doubles"
//              << std::endl;
            LAPACK_DHSEQR(job, compz, n, &ilo, &ihi, H, &ldH, wr, wi,
                    Z, &ldz, work, lwork, &info);
            return info;
        }

        // complex<float>
        inline
        int hseqr_backend(const char* job, const char* compz, int* n,
                const int ilo, const int ihi, traits::complex_f* H, const int ldH,
                traits::complex_f* w, traits::complex_f* Z, int ldz,
                traits::complex_f* work, const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for complex<float>"
//              << std::endl;
            LAPACK_CHSEQR(job, compz, n, &ilo, &ihi,
                    traits::complex_ptr(H), &ldH,
                    traits::complex_ptr(w),
                    traits::complex_ptr(Z), &ldz,
                    traits::complex_ptr(work), lwork, &info);
            return info;
        }

        // complex<double>
        inline
        int hseqr_backend(const char* job, const char* compz, int* n,
                const int ilo, const int ihi, traits::complex_d* H, const int ldH,
                traits::complex_d* w, traits::complex_d* Z, int ldz,
                traits::complex_d* work, const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for complex<double>"
//              << std::endl;
            LAPACK_ZHSEQR(job, compz, n, &ilo, &ihi,
                    traits::complex_ptr(H), &ldH,
                    traits::complex_ptr(w),
                    traits::complex_ptr(Z), &ldz,
                    traits::complex_ptr(work), lwork, &info);
            return info;
        }

        template <int N>
        struct Hseqr{};

        template <>
        struct Hseqr< 1 >{
            template < typename A, typename W, typename V>
            int operator() ( const char job, const char compz, A& H, W& w, V& Z ){
//              std::cout << "Inside Hseqr<1>." << std::endl;

                int n = traits::matrix_size1(H);
                typedef typename A::value_type value_type;
                traits::detail::array<value_type> wr(n);
                traits::detail::array<value_type> wi(n);

                // workspace query
                int lwork = -1;
                value_type work_temp;
                int result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                                            traits::matrix_storage(H),
                                            traits::leading_dimension(H),
                                            wr.storage(), wi.storage(),
                                            traits::matrix_storage(Z),
                                            traits::leading_dimension(Z),
                                            &work_temp, &lwork);

                if( result !=0 ) return result;

                lwork = (int) work_temp;
                traits::detail::array<value_type> work(lwork);
                result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                                            traits::matrix_storage(H),
                                            traits::leading_dimension(H),
                                            wr.storage(), wi.storage(),
                                            traits::matrix_storage(Z),
                                            traits::leading_dimension(Z),
                                            work.storage(), &lwork);

                for (int i = 0; i < n; i++)
                    w[i] = std::complex<value_type>(wr[i], wi[i]);

                return result;
            }
        };

        template <>
        struct Hseqr< 2 >{
            template < typename A, typename W, typename V>
            int operator() ( const char job, const char compz, A& H, W& w, V& Z ){
//              std::cout << "Inside Hseqr<2>." << std::endl;

                int n = traits::matrix_size1(H);
                typedef typename A::value_type value_type;

                // workspace query
                int lwork = -1;
                value_type work_temp;
                int result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                        traits::matrix_storage(H),
                        traits::leading_dimension(H),
                        traits::vector_storage(w),
                        traits::matrix_storage(Z), traits::leading_dimension(Z),
                        &work_temp, &lwork);

                if( result !=0 ) return result;

                lwork = (int) std::real(work_temp);
                traits::detail::array<value_type> work(lwork);
                result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                        traits::matrix_storage(H),
                        traits::leading_dimension(H),
                        traits::vector_storage(w),
                        traits::matrix_storage(Z), traits::leading_dimension(Z),
                        work.storage(), &lwork);

                return result;
            }
        };

        template < typename A, typename W, typename V>
        int hseqr( const char job, const char compz, A& H, W& w, V& Z ){
//          std::cout << "I'm inside lapack::detail::hseqr." << std::endl;

            assert ( job == 'E' || job == 'S' );
            assert ( compz == 'N' || compz == 'I' || compz == 'V' );

            typedef typename A::value_type value_type;

            int result = detail::Hseqr< n_workspace_args<value_type>::value >()(
                    job, compz, H, w, Z);

            return result;
        }
    }   // namespace detail

    // Compute eigenvalues without the Schur vectors
    template < typename A, typename W>
    int hseqr( const char job, A& H, W& w){
      // input checking
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
			   typename traits::matrix_traits<A>::matrix_structure,
			   traits::general_t
			   >::value));
#endif

#ifndef NDEBUG
        int const n = traits::matrix_size1(H);
#endif

        typedef typename A::value_type value_type;
        typedef typename W::value_type complex_value_type;

        assert(traits::matrix_size2(H) == n); // Square matrix
        assert(traits::vector_size(w) == n);

        ublas::matrix<value_type, ublas::column_major> Z(1,1);
        return detail::hseqr( job, 'N', H, w, Z );
    }

    // Compute eigenvalues and the Schur vectors
    template < typename A, typename W, typename Z>
    int hseqr( const char job, const char compz, A& H, W& w, Z& z){
      // input checking
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
			   typename traits::matrix_traits<A>::matrix_structure,
			   traits::general_t
			   >::value));
#endif

#ifndef NDEBUG
        int const n = traits::matrix_size1(H);
#endif

        typedef typename A::value_type value_type;
        assert(traits::matrix_size2(H) == n); // Square matrix
        assert(traits::vector_size(w) == n);
        assert(traits::matrix_size2(z) == n);

        return detail::hseqr( job, compz, H, w, z );
    }

  }
}}}

#endif


//
// Copyright Chong Peng 2017
//

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

#include "type_traits.h"
#include "std_vector_interface.h"
#include "symm_davidson_diag.h"
#include "nonsymm_davidson_diag.h"

using namespace code_example;

int main() {

  std::cout.precision(12);

  std::cout << "Nonsymmetric Davidson Diagnolization Using std::vector \n";

  /// typedef the Array type to use
  using Array = std::vector<double>;

  // variables
  const std::size_t n = 200;
  const double sparse = 0.1;
  const std::size_t n_roots = 5;   // number of roots to solve in davidson
  const double converge = 1.0e-10; // convergence threshold in davidson
  const std::size_t max_iter = 100; // max iteration in davidson

  // initialize symmetric matrix
  ColMatrix<double> A = ColMatrix<double>::Zero(n, n);
  for (auto i = 0; i < n; i++) {
    A(i, i) = i + 1;
  }
  A = A + sparse * ColMatrix<double>::Random(n, n);

  EigenVector<double> A_diagonal = A.diagonal();
  // eigen solve
  Eigen::EigenSolver<ColMatrix<double>> es(A);

  EigenVector<double> e = es.eigenvalues().real();
  std::sort(e.data(), e.data()+e.size());
  e = e.segment(0,n_roots);

  std::cout << "Reference Result from EigenSolve: " << std::endl
            << e << std::endl;

  /// construct the SymmDavidsonDiag  object
  NonSymmDavidsonDiag<Array> dvd(n_roots);

  /// make the initial guess use unit vector
  std::vector<Array> guess(n_roots);
  {
    for (std::size_t i = 0; i < n_roots; i++) {
      guess[i] = Array(n, 0.0);
      guess[i][i] = 1;
    }
  }

  /// make the preconditioner

  auto pred = [&A_diagonal](const EigenVector<double> &e,
                            std::vector<Array> &guess) {

    for (std::size_t i = 0; i < guess.size(); i++) {
      const auto ei = e[i];
      auto &guess_i = guess[i];
      const auto n_r = guess_i.size();
      for (std::size_t i = 0; i < n_r; i++) {
        guess_i[i] = guess_i[i] / (ei - A_diagonal[i]);
      }
    }

  };

  /// make the operator
  auto op = [&A,n](const std::vector<Array> &vec) {
    const std::size_t n_vec = vec.size();

    std::vector<Array> HC(n_vec);

    const char trans = 'N';
    const int32_t rows = A.rows();
    const int32_t cols = A.cols();
    const double alpha = 1.0;
    const double beta = 0.0;
    const int32_t inc = 1;
    for (std::size_t i = 0; i < n_vec; i++) {
      HC[i] = Array(vec[i].size(), 0.0);
      dgemv_(&trans, &rows, &cols, &alpha, A.data(), &rows, vec[i].data(), &inc,
             &beta, HC[i].data(), &inc);
    }

    return HC;
  };

  /// solve

  auto eig = dvd.solve(guess, op, pred, converge, max_iter);

  std::cout << "NonSymmDavidsonDiag result: " << std::endl;
  std::cout << eig << std::endl;

  return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 SMOOTH__IMPL__SE3_HPP_
#define SMOOTH__IMPL__SE3_HPP_

#include <Eigen/Core>

#include "common.hpp"
#include "so3.hpp"

namespace smooth {

/**
 * @brief SE(3) Lie Group represented as S^3 ⋉ R3
 *
 * Memory layout
 * -------------
 * Group:    x y z qx qy qz qw
 * Tangent:  vx vy vz Ωx Ωy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ R T ]
 * [ 0 1 ]
 *
 * where R ∈ SO(3) and T = [x y z] ∈ R3
 *
 * Lie algebra Matrix form
 * -----------------------
 * [  0 -Ωz  Ωy vx]
 * [  Ωz  0 -Ωx vy]
 * [ -Ωy Ωx   0 vz]
 * [   0  0   0  0]
 *
 * Constraints
 * -----------
 * Group:   qx * qx + qy * qy + qz * qz + qw * qw = 1
 * Tangent: -pi < Ωx Ωy Ωz <= pi
 */
template<typename _Scalar>
class SE3Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 7;
  static constexpr Eigen::Index Dim     = 4;
  static constexpr Eigen::Index Dof     = 6;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out)
  {
    g_out.template head<6>().setZero();
    g_out(6) = Scalar(1);
  }

  static void setRandom(GRefOut g_out)
  {
    g_out.template head<3>().setRandom();
    SO3Impl<Scalar>::setRandom(g_out.template tail<4>());
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), m_out.template topLeftCorner<3, 3>());
    m_out.template topRightCorner<3, 1>() = g_in.template head<3>();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    SO3Impl<Scalar>::composition(
      g_in1.template tail<4>(), g_in2.template tail<4>(), g_out.template tail<4>());
    Eigen::Matrix<Scalar, 3, 3> R1;
    SO3Impl<Scalar>::matrix(g_in1.template tail<4>(), R1);
    g_out.template head<3>() = R1 * g_in2.template head<3>() + g_in1.template head<3>();
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Matrix<Scalar, 4, 1> so3inv;
    SO3Impl<Scalar>::inverse(g_in.template tail<4>(), so3inv);

    Eigen::Matrix<Scalar, 3, 3> Rinv;
    SO3Impl<Scalar>::matrix(so3inv, Rinv);

    g_out.template head<3>() = -Rinv * g_in.template head<3>();
    g_out.template tail<4>() = so3inv;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::log(g_in.template tail<4>(), a_out.template tail<3>());

    SO3TangentMap M_dr_expinv, M_ad;
    SO3Impl<Scalar>::dr_expinv(a_out.template tail<3>(), M_dr_expinv);
    SO3Impl<Scalar>::ad(a_out.template tail<3>(), M_ad);
    a_out.template head<3>() = (-M_ad + M_dr_expinv) * g_in.template head<3>();
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {

    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(g_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template topRightCorner<3, 3>() *= A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::exp(a_in.template tail<3>(), g_out.template tail<4>());

    SO3TangentMap M_dr_exp, M_Ad;
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), M_dr_exp);
    SO3Impl<Scalar>::Ad(g_out.template tail<4>(), M_Ad);

    g_out.template head<3>() = M_Ad * M_dr_exp * a_in.template head<3>();
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 1>() = a_in.template head<3>();
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    SO3Impl<Scalar>::vee(A_in.template topLeftCorner<3, 3>(), a_out.template tail<3>());
    a_out.template head<3>() = A_in.template topRightCorner<3, 1>();
  }

  static void ad(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(a_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static Eigen::Matrix<Scalar, 3, 3> calculate_q(TRefIn a)
  {
    using std::abs, std::sqrt, std::cos, std::sin;

    const Scalar th2 = a.template tail<3>().squaredNorm();

    Scalar A, B, C;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x%29+%2F+x%5E3+at+x%3D0
      A = Scalar(1) / Scalar(6) - th2 / Scalar(120);
      // https://www.wolframalpha.com/input/?i=series+%28cos+x+-+1+%2B+x%5E2%2F2%29+%2F+x%5E4+at+x%3D0
      B = Scalar(1) / Scalar(24) - th2 / Scalar(720);
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x+-+x%5E3%2F6%29+%2F+x%5E5+at+x%3D0
      C = -Scalar(1) / Scalar(120) + th2 / Scalar(5040);
    } else {
      const Scalar th = sqrt(th2), th_4 = th2 * th2, cTh = cos(th), sTh = sin(th);
      A = (th - sTh) / (th * th2);
      B = (cTh - Scalar(1) + th2 / Scalar(2)) / th_4;
      C = (th - sTh - th * th2 / Scalar(6)) / (th_4 * th);
    }

    Eigen::Matrix<Scalar, 3, 3> V, W;
    SO3Impl<Scalar>::hat(a.template head<3>(), V);
    SO3Impl<Scalar>::hat(a.template tail<3>(), W);

    const Scalar vdw                     = a.template tail<3>().dot(a.template head<3>());
    const Eigen::Matrix<Scalar, 3, 3> WV = W * V, VW = V * W, WW = W * W;

    return Scalar(0.5) * V + A * (WV + VW - vdw * W)
         + B * (W * WV + VW * W + vdw * (Scalar(3) * W - WW)) - C * Scalar(3) * vdw * WW;
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>()    = calculate_q(-a_in);
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_expinv(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>() = -A_out.template topLeftCorner<3, 3>()
                                          * calculate_q(-a_in)
                                          * A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }
};

}  // namespace smooth

#endif  // SMOOTH__IMPL__SE3_HPP_


#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <fstream>
#include "tsai.hpp"

#ifdef USEVISP
#include <visp/vpCalibration.h>
#include <visp/vpMath.h>
#include <visp/vpPose.h>
#include <visp/vpPixelMeterConversion.h>
#endif
//using LTD = SE3dist<float>;
using LTD = float;

#ifdef USEVISP

void assign(Eigen::Matrix4f & a, const 	vpHomogeneousMatrix & b)
{
	vpQuaternionVector q;
	vpTranslationVector t;
	b.extract(q);
	b.extract(t);
	Eigen::Vector3f et(t[0],t[1],t[2]);
	Eigen::Quaternionf eq(q.w(),q.x(),q.y(),q.z());
	a.setIdentity();
	a.block<3,3>(0,0) = Eigen::Matrix3f(eq);
	a.block<3,1>(0,3) = et;
}

void assign(vpHomogeneousMatrix & b,const Eigen::Matrix4f & a)
{
	Eigen::Quaternionf eq(Eigen::Matrix3f(a.block<3,3>(0,0)));
	Eigen::Vector3f et = a.block<3,1>(0,3);

	vpQuaternionVector q(eq.x(),eq.y(),eq.z(),eq.w());
	vpTranslationVector t(et.x(),et.y(),et.z());
	b.buildFrom(t,q);
}

void calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
						std::vector<vpHomogeneousMatrix>& rMe,
						vpHomogeneousMatrix &eMc){

  vpColVector x ;
  unsigned int nbPose = (unsigned int)cMo.size();
  if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
  {
    vpMatrix A ;
    vpColVector B ;
    unsigned int k = 0 ;
    // for all couples ij
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      vpRotationMatrix rRei, ciRo ;
      rMe[i].extract(rRei) ;
      cMo[i].extract(ciRo) ;
      //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;

      for (unsigned int j=i+1; j < nbPose ; j++)
      {
        {
          vpRotationMatrix rRej, cjRo ;
          rMe[j].extract(rRej) ;
          cMo[j].extract(cjRo) ;
	  //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;

          vpRotationMatrix rReij = rRej.t() * rRei;

          vpRotationMatrix cijRo = cjRo * ciRo.t();

          vpThetaUVector rPeij(rReij);

          double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
                              + rPeij[2]*rPeij[2]);

          for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);

          vpThetaUVector cijPo(cijRo) ;
          theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
                       + cijPo[2]*cijPo[2]);
          for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);

          vpMatrix As;
          vpColVector b(3) ;

          As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;

          b =  (vpColVector)cijPo - (vpColVector)rPeij ;           // A.40

          if (k==0)
          {
            A = As ;
            B = b ;
#if 0
            auto w = rReij.t();
            std::cout << "rPeij0 R\n";
            std::cout << w.getCol(0) << "\n" << w.getCol(1) << "\n" << w.getCol(2) << std::endl;
            std::cout << "rPeij0 is " << rPeij[0] << " " << rPeij[1] << " " << rPeij[2] << std::endl;
            std::cout << "cijPo0 is " << cijPo[0] << " " << cijPo[1] << " " << cijPo[2] << std::endl;
            std::cout << "As0 is \n ";
            As.csvPrint (std::cout);
            std::cout<< "b0 " << b[0] << " " << b[1] << " " << b[2] << std::endl;
#endif
          }
          else
          {
            A = vpMatrix::stack(A,As) ;
            B = vpColVector::stack(B,b) ;
          }
          k++ ;
        }
      }
    }

    // the linear system is defined
    // x = AtA^-1AtB is solved
    vpMatrix AtA = A.AtA() ;

    vpMatrix Ap ;
    AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
    x = Ap*A.t()*B ;

//     {
//       // Residual
//       vpColVector residual;
//       residual = A*x-B;
//       std::cout << "Residual: " << std::endl << residual << std::endl;

//       double res = 0;
//       for (int i=0; i < residual.getRows(); i++)
// 	res += residual[i]*residual[i];
//       res = sqrt(res/residual.getRows());
//       printf("Mean residual = %lf\n",res);
//     }

    // extraction of theta and U
    double theta ;
    double   d=x.sumSquare() ;
    for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
    theta = sqrt(x.sumSquare())/2 ;
    theta = 2*asin(theta) ;
    //if (theta !=0)
    if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
    {
      for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
    }
    else
      x = 0 ;
  }

  // Building of the rotation matrix eRc
  vpThetaUVector xP(x[0],x[1],x[2]);
  vpRotationMatrix eRc(xP);

  {
    vpMatrix A ;
    vpColVector B ;
    // Building of the system for the translation estimation
    // for all couples ij
    vpRotationMatrix I3 ;
    I3.eye() ;
    int k = 0 ;
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      vpRotationMatrix rRei, ciRo ;
      vpTranslationVector rTei, ciTo ;
      rMe[i].extract(rRei) ;
      cMo[i].extract(ciRo) ;
      rMe[i].extract(rTei) ;
      cMo[i].extract(ciTo) ;

      for (unsigned int j=i+1 ; j < nbPose ; j++)
      {
        {

          vpRotationMatrix rRej, cjRo ;
          rMe[j].extract(rRej) ;
          cMo[j].extract(cjRo) ;

          vpTranslationVector rTej, cjTo ;
          rMe[j].extract(rTej) ;
          cMo[j].extract(cjTo) ;

          vpRotationMatrix rReij = rRej.t() * rRei ;

          vpTranslationVector rTeij = rTej+ (-rTei);

          rTeij = rRej.t()*rTeij ;

          vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);

          vpTranslationVector b ;
          b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;

          if (k==0)
          {
            A = a ;
            B = b ;
          }
          else
          {
            A = vpMatrix::stack(A,a) ;
            B = vpColVector::stack(B,b) ;
          }
          k++ ;

        }
      }
    }

    // the linear system is solved
    // x = AtA^-1AtB is solved
    vpMatrix AtA = A.AtA() ;
    vpMatrix Ap ;
    vpColVector AeTc ;
    AtA.pseudoInverse(Ap, 1e-6) ;
    AeTc = Ap*A.t()*B ;

//     {
//       // residual
//       vpColVector residual;
//       residual = A*AeTc-B;
//       std::cout << "Residual: " << std::endl << residual << std::endl;
//       double res = 0;
//       for (int i=0; i < residual.getRows(); i++)
// 	res += residual[i]*residual[i];
//       res = sqrt(res/residual.getRows());
//       printf("mean residual = %lf\n",res);
//     }

    vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);

    eMc.insert(eTc) ;
    eMc.insert(eRc) ;
  }
}

#endif
void out(std::ostream & onf, const Eigen::Matrix4f  & q)
{
	for(int i = 0; i < 16; i++)
		onf << q.data()[i] << ' ';
}

Eigen::Matrix4f makerandom(const LTD &ld)
{
  return Eigen::Matrix4f::Identity(); //ld.sample().asMatrix();
}

Eigen::Matrix<float,6,1> distance(Eigen::Matrix4f a, Eigen::Matrix4f b)
{
  return Eigen::Matrix<float,6,1> ::Zero();
/*	SE3group<float> ag(a);
	SE3group<float> bg(b);
	return ag.distance(bg).get();*/
}

// pos and ang
Eigen::Matrix<float,2,1> distance2(Eigen::Matrix4f a, Eigen::Matrix4f b)
{
    //auto q = SE3group<float>(a).distance(SE3group<float>(b)).get();
    return Eigen::Matrix<float,2,1>::Zero(); //{q.segment<3>(0).norm(),q.segment<3>(3).norm()};
}

// pos and ang
template <class T>
T distanceT(Eigen::Matrix<T,4,4> a, Eigen::Matrix<T,4,4> b)
{
    return (a*b.inverse()).template block<3,1>(0,3).norm();
}

template <class T>
Vector3<T> normalize(Vector3<T>  q)
{
	return q / q.norm();
}

int main(int argc, char const *argv[])
{
#if 0
	Eigen::AngleAxisf aa(0.001*M_PI,normalize(Eigen::Vector3f(0.2,0.3,0.4)));
	Eigen::Quaternionf q(aa);
	Eigen::Vector3f p = quat2paratsai(q);
	Eigen::Quaternionf Q = paratsai2quat(p);
	Eigen::Matrix3f R = paratsai2rot(p);
	Eigen::Quaternionf QR(R);
	Eigen::Vector3f pp = paratsai2paratsaiprime(p);
	Eigen::Vector3f ppp = paratsaiprime2paratsai(pp);

	float pangle = paratsai2theta(p);
	std::cout << "angle original  " << aa.angle() << std::endl;
	std::cout << "angle from para " << pangle << std::endl;
	std::cout << "angle from quat " << Eigen::AngleAxisf(Q).angle() << std::endl;
	std::cout << "angle from rot  " << Eigen::AngleAxisf(R).angle() << std::endl;
	std::cout << q << std::endl;
	std::cout << Q << std::endl;
	std::cout << QR << std::endl;
	std::cout << ppp.transpose() << std::endl;
	std::cout << p.transpose() << std::endl;
#endif

    if(argc < 2)
        return -1;
    using FT = double;
    std::ifstream inf(argv[1],std::ios::binary);
    std::vector<Eigen::Matrix<FT,4,4> > cMm,rMe;
    Eigen::Matrix<FT,4,4> m1fp,m2fp,m1f,m2f;
    std::cout << "assuming file of pairs of matrices: cMm and cMe" << std::endl;
    while(inf)
    {
        Eigen::Matrix<double,4,4,Eigen::RowMajor> m1,m2;
        inf.read((char*)m1.data(),16*sizeof(double));
        if(!inf)
            break;
        inf.read((char*)m2.data(),16*sizeof(double));
        if(!inf)
            break;
        // data is stored rowmajor
        m1f = m1.cast<FT>();
        m2f = m2.cast<FT>();
        cMm.push_back(m1f);
        rMe.push_back(m2f);
        if(cMm.size() == 1)
        {
            std::cout << "m1[0] is\n" << m1f << std::endl;
            std::cout << "m2[0] is\n" << m2f << std::endl;
        }
        else
        {
            // previous is valid

            // use 2D (pos,rot) or 1D (pos) distance
            //std::cout << "Diff m1: " << distance2(m1f,m1fp).transpose() << " Diff m2: " << distance2(m2f,m2fp).transpose() << std::endl;
            std::cout << "Diff m1: " << distanceT(m1f,m1fp) << " Diff m2: " << distanceT(m2f,m2fp) << std::endl;
        }
        m1fp = m1f;
        m2fp = m2f;

    }
    Eigen::Matrix<FT,4,4> r;

        FT res = calibrationTsai(cMm,rMe,r);

        std::cout << "Output:   \n" << r << std::endl;
        std::cout << "Residual:   \n" << res << std::endl;
        /*std::cout << "Expected: \n" << ec << std::endl;
	std::cout << "THIS diff:  " << distance(r,ec).transpose() << std::endl;
	std::cout << "!THIS error: " << distance(r,ec).transpose().norm() << std::endl;
    */
#ifdef USEVISP
	vpCalibration c;
	std::vector<vpHomogeneousMatrix> vcMo(cMm.size());
	std::vector<vpHomogeneousMatrix> vrMe(cMm.size());
	vpHomogeneousMatrix veMc;
    Eigen::Matrix4f eMc;
	for(int i = 0; i < cMm.size(); i++)
	{
		assign(vcMo[i],cMm[i].cast<float>());
		assign(vrMe[i],rMe[i].cast<float>());
	}
         calibrationTsai(vcMo,vrMe,veMc);
	assign(eMc,veMc);

        std::cout << "MY output res:\n" << res << std::endl;
        std::cout << "VISP output:\n" << eMc << std::endl;
  std::ofstream onf("tsaiout.bin",std::ios::binary);
  Matrix4<double> out(eMc.cast<FT>());
  onf.write((char*)out.data(),sizeof(double)*16);
/*	std::cout << "Expected:   \n" << ec << std::endl;
	std::cout << "VISP diff:  " << distance(eMc,ec).transpose() << std::endl;
	std::cout << "!VISP error:" << distance(eMc,ec).transpose().norm() << std::endl;
*/
#endif
#if 0
	if(argc == 2)
	{
		std::ofstream onf(argv[1]);
		out(onf,ec);		onf << std::endl;
		out(onf,r);		onf << std::endl;
		onf << cMm.size() << std::endl;
		for(int i = 0; i < cMm.size(); i++)
		{
			out(onf,cMm[i]);
			onf << std::endl;
		}
		for(int i = 0; i < rMe.size(); i++)
		{
			out(onf,rMe[i]);
			onf << std::endl;
		}
	}
#endif
	return 0;
}

/*
 * 0.000  1.000  0.000  0.038
-1.000 -0.000  0.000  0.012
0.000  0.000  1.000  0.015
0.000  0.000  0.000  1.000
*/


#include "Util.h"
#include "Matrix.h"

#include <random>
#include <time.h>
#include <cmath>
#include <fstream>
#include <float.h>

#include <opencv2/opencv.hpp>
#include <boost/tokenizer.hpp>

using namespace std;

int seed = time(0);
std::default_random_engine random_engine(seed);

namespace fns{
	double relu(double x){
		if(x > 0) return x;
		else return (double) 0;
	}
	double sigmoid(double x){
		return (1.0/(1.0 + exp(-x)));
	}
	double tan(double x){
		return tanh(x);
	}
	double relu_gradient(double x){
		if(x > 0) return (double) 1;
		else return (double) 0.2;
	}
	double sigmoid_gradient(double x){
		return (x*(1-x));
	}
	double tan_gradient(double x){
		return (1-(x*x));
	}
	double softmax(double x){
		if(isnan(x)) return 0;
		return exp(x);
	}
}

namespace pre_process{
	int process_mnist_images(const char* path, std::vector<std::unique_ptr<Matrix> > &Xtrain,
		std::vector<std::unique_ptr<std::vector<double> > > &Ytrain, unsigned int nr_images){
		std::string str(path);	// convert char* to string
		const int width = 28;
		const int height = 28;
		const int LABELS = 10;

		for(unsigned int i=0; i < LABELS; i++){
			std::vector<cv::String> files;	// vector of strings to store file names
			cv::glob(path + std::to_string(i), files, true);
				// true means recursively read from path
			for(unsigned int k=0; k < (nr_images/LABELS); k++){
				cv::Mat img = cv::imread(files[k]);
				if(img.empty()) continue;	//only proceed further if the file is not empty
				std::unique_ptr<Matrix> image = std::make_unique<Matrix>(width, height, true);
				for(unsigned int h=0; h<height; h++){
					for(unsigned int w=0; w<width; w++){
						image->set(h,w,(double)(img.at<uchar>(h,w)/255.0));
					}
				}
				Xtrain.emplace_back(std::move(image));
				std::unique_ptr<std::vector<double> > vr = std::make_unique<std::vector<double> >(LABELS, 0);
				(*vr)[i] = 1.0;
				Ytrain.emplace_back(std::move(vr));
			}
		}
		return 0;
	}

	int process_mnist_csv(const char* filename, std::vector<std::vector<double> > &Xtrain,
		std::vector<std::vector<double> > &Ytrain){
		std::string data(filename);
		ifstream in(data.c_str());

		if(!in.is_open()) return 1;

		typedef boost::tokenizer< boost::escaped_list_separator<char> > Tokenizer;
		std::vector<std::string> svec;
		std::string line;

		while(getline(in, line)){
			Tokenizer tok(line);
			auto it = tok.begin();
			int label = std::stoi(*it);
			std::vector<double> labels(10, 0.0);
			labels[label] = 1.0;

			svec.assign(std::next(it, 1), tok.end());

			std::vector<double> dvec(svec.size());
			std::transform(svec.begin(), svec.end(), dvec.begin(), [](const std::string& val)
			{
				return (std::stod(val)/255); // divide by 255 for normalization, since each pixel is 8 bit
			});

			Xtrain.push_back(dvec);
			Ytrain.push_back(labels);
		}
		cout << "processed the input file" << endl;
		return 0;
	}

	void process_image(const char* filename){
		std::vector<double> image;
		cv::Mat img = cv::imread(filename);
		if(img.empty()){
			std::cout << "No Image" << std::endl;
		}
		else{
			if(img.isContinuous()){
				image.assign(img.datastart, img.dataend);
				for(unsigned int j=0; j < image.size(); j++){
					cout << image[j] << " " ;
				}
				cout << endl << image.size();
			}
			else{
				std::cout << "Not Continous !" << std::endl;
			}
		}
	}
}


//    Copyright 2017 Rainer Gemulla
//
//    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.
/** \file
 *
 * Illustrates matrix factorization with Lee's GKL method. We first creates factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 5000000;
	mf_size_type rank = 10;

	// parameters for Lee01
	unsigned epochs = 20;

	// generate original factors by sampling from a uniform[0,1] distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, rank);
	DenseMatrixCM hIn(rank, size2);
	generateRandom(wIn, random,  boost::uniform_real<>(0, 1));
	generateRandom(hIn, random, boost::uniform_real<>(0, 1));
	// div2(wIn, sums2(wIn));
	// div1(hIn, sums1(hIn));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and sample from a Poisson with mean equal to the entry
	// TODO: this generation process does not match the factorization model since we sample
	//       from the Poisson only at some entries of wh
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	applyPoisson(v, random);
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	LOG4CXX_INFO(logger, "Loss with original factors: " << gkl(v, wIn, hIn));

	// generate initial factors by sampling from a uniform[0,1] distribution
	DenseMatrix w(size1, rank);
	DenseMatrixCM h(rank, size2);
	generateRandom(w, random, boost::uniform_real<>(0, 1));
	generateRandom(h, random, boost::uniform_real<>(0, 1));
	div2(w, sums2(w));
	div1(h, sums1(h));
	double scaleFactor = sqrt(sum(v));
	mult(w, scaleFactor);
	mult(h, scaleFactor);

	// perform the factorization
	FactorizationData<> data(v, w, h);
	Trace trace;
	lee01Gkl(data, epochs, trace);

	// write the trace
	LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/lee01-gkl-trace.R");
	trace.toRfile("/tmp/lee01-gkl-trace.R", "lee01.gkl");

	return 0;
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing cospi capabilities

    cosine of angle in \f$\pi\f$ multiples: \f$\cos(\pi x)\f$.

    @par Semantic:

   The semantics of the function are similar to @ref cos ones.
    see @ref cos for further details

    @par Note

    However as it conveys a peculiar meaning,  unlike the orher cosine, cospi is defined
    for integral types and the result of cospi(n) coincides with \f$(-1)^n\f$.

    Take care that large floating entries are always integral and even !

    @see sincospi, cos, cosd

  **/
  Value cospi(Value const & v0);
} }
#endif

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

#endif


#pragma once
#include <Eigen/Geometry>
#include "mtao/types.hpp"

namespace mtao::geometry {
    template <bool OutputEdges=false, typename T=double, int D=3>
        auto  bounding_box_mesh(const Eigen::AlignedBox<T,D>& bb) {
            mtao::ColVectors<T,D> V(D,1<<D);
            if constexpr(D == 2) {
                mtao::ColVectors<int,2> E(2,4);
                V << bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeft),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRight),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeft),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRight);
                E << 0,0,3,2,
                     1,2,1,3;
                return std::make_tuple(V,E);
            } else {//D == 3
                mtao::ColVectors<int,3> F(3,12);
                V <<
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeftFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRightFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeftFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRightFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeftCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRightCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeftCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRightCeil);
                //0:000
                //1:010
                //2:100
                //3:110
                //4:001
                //5:011
                //6:101
                //7:111
                F <<
                    6,2,3,1,7,3,6,7,4,5,5,7,
                    0,0,0,0,2,2,4,4,0,0,1,1,
                    4,6,2,3,6,7,7,5,5,1,7,3;

                if constexpr(OutputEdges) {
                    mtao::ColVectors<int,2> E(2,12);
                    E << 0,0,0,1,1,2,2,3,4,4,5,6,
                      1,2,4,3,5,3,6,7,5,6,7,7;
                    return std::make_tuple(V,F,E);
                } else {
                    return std::make_tuple(V,F);
                }
            }
        }
}


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

/*  下面两个函数的作用是把数字或者数组转为可用于运算操作的vector
 *  因为当加密和解密过后，最后连续的n个0会被隐藏
 *  例如[2 2 0 0]→[2 2], [0 0 0 0]→[]
 *  []转为long输出时会出错，所以在末尾加上1使得0可以正确地被提取出来
 */

// 数字转为可用于运算操作的vector
Vec<ZZ> num2validVec(long num)
{
	Vec<ZZ> v;
	v.SetLength(2);
	v[0] = num;
	v[1] = 1;
	return v;
}

// 数组转为可用于运算操作的vector
Vec<ZZ> arr2validVec(long* num, int arrLen)
{
	Vec<ZZ> v;
	v.SetLength(arrLen+1);
	for (int i=0; i<arrLen; i++)
		v[i] = num[i];
	v[arrLen] = 1;
	return v;
}

// 四则运算全同态解密（只实现了数字作为运算符的解密，数组作为运算符的有待修缮）
long FHE_ptDec(ZZX ptxt, long p)
{
	long ptDec;
	conv(ptDec, ptxt[0]);
	if (ptDec > p/2)
		ptDec -= p;
	return ptDec;
}

// 全同态加法
Ctxt FHE_Add(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctSum = Ea;
	ctSum += Eb;
	return ctSum;
}

// 全同态乘法
Ctxt FHE_Mul(Ctxt Ea, Ctxt Eb, long p, const FHESecKey& secretKey)
{
	ZZX ptEa, ptEb;
	secretKey.Decrypt(ptEa, Ea);
	secretKey.Decrypt(ptEb, Eb);
	// 解密判断乘数中是否有0，如果是，则返回0的密文
	if (FHE_ptDec(ptEa, p) == 0)
		return Ea;
	else if (FHE_ptDec(ptEb, p) == 0)
		return Eb;
	else
	{
		Ctxt ctMul = Ea;
		ctMul *= Eb;
		return ctMul;
	}
}

// 全同态减法
Ctxt FHE_Sub(Ctxt Ea, Ctxt Eb, const FHEPubKey& publicKey)
{
	// sub = op2*(-1)+op1
	Ctxt minus1(publicKey);
	Vec<ZZ> m1 = num2validVec(-1);
	publicKey.Encrypt(minus1, to_ZZX(m1));
	Ctxt ctSub = Eb;
	ctSub *= minus1;
	ctSub += Ea;
	return ctSub;
}

// 全同态除法
Ctxt FHE_Div(Ctxt Ea, Ctxt Eb, long p,
			 const FHEPubKey& publicKey, const FHESecKey& secretKey)
{
	int quotient = 0;  // 初始化商quotient为0
	ZZX ptMul, ptMul2, ptSub, ptSum, ptEa, ptEb;

	/* 判断思路（设被除数和除数为op1和op2）：
	 * 1. op2=0时，输出"Error: Invalid Denominator."，否则继续
	 * 2. op1=0时，返回0的密文，否则继续
	 * 3. 判断op1和op2是否同号,可用op1·op2是否为正来判断
	 *    3-1. 同号: 当op1=0时跳出循环，否则
	 *               sub=op1-op2，如果sub和op1同号(op1和op2同为负数的时候sub也是负数)则quotient递增且op1=sub，否则跳出循环
	 *    3-2. 异号: 当op1=0时跳出循环，否则
	 *               sum=op1+op2(因为是异号)，如果sum和op1同号则quotient递减(异号相除结果为负数)且op1=sum，否则跳出循环
	 */
	bool positive = true;
	secretKey.Decrypt(ptEa, Ea);
	long EaDec = FHE_ptDec(ptEa, p);
	if(EaDec < 0) positive = false;
	secretKey.Decrypt(ptEb, Eb);
	long EbDec = FHE_ptDec(ptEb, p);
	if (EbDec == 0)
	{
		cout << "Error: Invalid Denominator." << endl;
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
	else if (EaDec == 0)
	{
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
	else {
		secretKey.Decrypt(ptMul, FHE_Mul(Ea, Eb, p, secretKey));
		// 两操作数同号
		if(FHE_ptDec(ptMul, p) >= 0)
		{
			while (1)
			{
				secretKey.Decrypt(ptEa, Ea);
				long EaDec = FHE_ptDec(ptEa, p);
				if (EaDec == 0) break;
				else
				{
					Ctxt ctSub = FHE_Sub(Ea, Eb, publicKey);
					secretKey.Decrypt(ptSub, ctSub);
					long sub = FHE_ptDec(ptSub, p);
					if (sub >= 0 && positive || sub <= 0 && !positive)
					{
						Ea = ctSub;
						quotient ++;
					}
					else break;
				}
			}
		}
		// 两操作数异号
		else
		{
			while (1)
			{
				secretKey.Decrypt(ptEa, Ea);
				long EaDec = FHE_ptDec(ptEa, p);
				if (EaDec == 0) break;
				else
				{
					Ctxt ctSum = FHE_Add(Ea, Eb);
					secretKey.Decrypt(ptMul2, FHE_Mul(ctSum, Ea, p, secretKey));
					long temp = FHE_ptDec(ptMul2, p);
					if (temp >= 0)
					{
						Ea = ctSum;
						quotient --;
					}
					else break;
				}
			}
		}
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
}

int main()
{
	long m = 0;    // 确定系数
	long p = 1021; // 模量，定义超过p/2的数为负数，负数的真值x=D[E[x]]-p
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);

	ZZX G = context.alMod.getFactorsOverZZ()[0];

	// 生成公钥
	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	// 初始化密文
	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);

	/* Test: 输入两个不全为0的数，输出四则运算结果，输入两个0可退出	*/
	long op1, op2;
	while (!(op1==0 && op2==0))
	{
		cin >> op1 >> op2;
		Vec<ZZ> h1 = num2validVec(op1);
		Vec<ZZ> h2 = num2validVec(op2);

		publicKey.Encrypt(Ea, to_ZZX(h1));
		publicKey.Encrypt(Eb, to_ZZX(h2));

		cout << "Operator 1 : " << op1 << " , Operator 2 : " << op2 << endl;

		ZZX ptSum;
		secretKey.Decrypt(ptSum, FHE_Add(Ea, Eb));
		cout << "ptSum : " << FHE_ptDec(ptSum, p) << endl;

		ZZX ptMul;
		secretKey.Decrypt(ptMul, FHE_Mul(Ea, Eb, p, secretKey));
		cout << "ptMul : " << FHE_ptDec(ptMul, p) << endl;

		ZZX ptSub;
		secretKey.Decrypt(ptSub, FHE_Sub(Ea, Eb, publicKey));
		cout << "ptSub : " << FHE_ptDec(ptSub, p) << endl;

		ZZX ptDiv;
		secretKey.Decrypt(ptDiv, FHE_Div(Ea, Eb, p, publicKey, secretKey));
		cout << "ptDiv : " << FHE_ptDec(ptDiv, p) << endl;
	}

	return 0;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_MEAN_HPP_INCLUDED
#define NDHIST_STATS_MEAN_HPP_INCLUDED 1

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/expectation.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
AxisValueType
calc_axis_mean_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    return calc_axis_expectation_impl<AxisValueType, WeightValueType>(h, 1, axis);
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the mean value along the given axis of the given ndhist
 *     object.
 *     Since the mean is equal to the first order expectation, this function
 *     just calls the expectation function to calculate the first order
 *     expectation value.
 *     If None is given as axis, the mean value for all axes of the ndhist
 *     object will be calculated and returned as a tuple. But if the
 *     dimensionality of the ndhist object is 1, a scalar value is returned.
 *
 * @note This function is only defined for ndhist objects with POD type axis
 *     values AND POD type weight values.
 */
boost::python::object
mean(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py
}// namespace stats
}// namespace ndhist

#endif // !NDHIST_STATS_MEAN_HPP_INCLUDED