// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_face_normals.h"
#include <Eigen/Geometry>

#define SQRT_ONE_OVER_THREE 0.57735026918962573
template <typename DerivedV, typename DerivedF, typename DerivedZ, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  Eigen::Matrix<typename DerivedN::Scalar,3,1> Z(0,0,0);
  return per_face_normals(V,F,Z,N);
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals_stable(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  typedef Eigen::Matrix<typename DerivedV::Scalar,1,3> RowVectorV3;
  typedef typename DerivedV::Scalar Scalar;

  const size_t m = F.rows();

  N.resize(F.rows(),3);
  // Grad all points
  for(size_t f = 0;f<m;f++)
  {
    const RowVectorV3 p0 = V.row(F(f,0));
    const RowVectorV3 p1 = V.row(F(f,1));
    const RowVectorV3 p2 = V.row(F(f,2));
    const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0);
    const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1);
    const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2);

    // careful sum
    for(int d = 0;d<3;d++)
    {
      // This is a little _silly_ in terms of complexity, but its recursive
      // implementation is clean looking...
      const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 =
        [&sum3](Scalar a, Scalar b, Scalar c)->Scalar
      {
        if(fabs(c)>fabs(a))
        {
          return sum3(c,b,a);
        }
        // c < a
        if(fabs(c)>fabs(b))
        {
          return sum3(a,c,b);
        }
        // c < a, c < b
        if(fabs(b)>fabs(a))
        {
          return sum3(b,a,c);
        }
        return (a+b)+c;
      };

      N(f,d) = sum3(n0(d),n1(d),n2(d));
    }
    // sum better not be sure, or else NaN
    N.row(f) /= N.row(f).norm();
  }

}

#include "cotmatrix.h"

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedN,
  typename DerivedVV,
  typename DerivedFF,
  typename DerivedJ>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::PlainObjectBase<DerivedN> & N,
  Eigen::PlainObjectBase<DerivedVV> & VV,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  assert(V.cols() == 3);
  typedef Eigen::Index Index;
  typedef typename DerivedN::Scalar Scalar;
  // Use Bunge et al. algorithm in igl::cotmatrix to insert a point for each
  // polygon which minimizes squared area.
  {
    Eigen::SparseMatrix<Scalar> _1,_2,P;
    igl::cotmatrix(V,I,C,_1,_2,P);
    VV = P*V;
  }
  // number of polygons
  const Eigen::Index m = C.size()-1;
  N.resize(m,3);
  FF.resize(C(m),3);
  J.resize(C(m));
  {
    Eigen::Index k = 0;
    for(Eigen::Index p = 0;p<m;p++)
    {
      N.row(p).setZero();
      // number of faces/vertices in this simple polygon
      const Index np = C(p+1)-C(p);
      for(Eigen::Index i = 0;i<np;i++)
      {
        FF.row(k) <<
          I(C(p)+((i+0)%np)),
          I(C(p)+((i+1)%np)),
          V.rows()+p;
        J(k) = p;
        k++;
        typedef Eigen::Matrix<Scalar,1,3> V3;
        N.row(p) +=
          V3(VV.row(I(C(p)+((i+0)%np)))-VV.row(V.rows()+p)).cross(
          V3(VV.row(I(C(p)+((i+1)%np)))-VV.row(V.rows()+p)));
      }
      // normalize to take average
      N.row(p) /= N.row(p).stableNorm();
    }
    assert(k == FF.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
// Nonsense template. Where'd this come from? AABB nonsense?
namespace igl{template<> void per_face_normals<Eigen::Matrix<double, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&){} }
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 1, 3, 1, 1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 1, -1, -1>, Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1>, Eigen::Matrix<float, -1, -1, 1, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<class Eigen::Matrix<double,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<double,-1,-1,0,-1,-1> >(class Eigen::MatrixBase<class Eigen::Matrix<double,-1,3,0,-1,3> > const &,class Eigen::MatrixBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,-1,0,-1,-1> > &);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 3, 0, 2, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 2, 3, 0, 2, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
#endif


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

/*
Copyright (C) 2012 Klaus Spanderen

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

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

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

/*! \file sparsematrix.hpp
    \brief typedef for boost sparse matrix class
*/

#ifndef quantlib_sparse_matrix_hpp
#define quantlib_sparse_matrix_hpp

#include <ql/qldefines.hpp>

#if !defined(QL_NO_UBLAS_SUPPORT)

#include <ql/math/array.hpp>

#if defined(QL_PATCH_MSVC)
#pragma warning(push)
#pragma warning(disable:4180)
#pragma warning(disable:4127)
#endif

#if defined(__clang__) && BOOST_VERSION > 105300
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#if BOOST_VERSION == 106400
#include <boost/serialization/array_wrapper.hpp>
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>

#if defined(QL_PATCH_MSVC)
#pragma warning(pop)
#endif

#if defined(__clang__) && BOOST_VERSION > 105300
#pragma clang diagnostic pop
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {
    typedef boost::numeric::ublas::compressed_matrix<Real> SparseMatrix;
    typedef boost::numeric::ublas::matrix_reference<SparseMatrix>
        SparseMatrixReference;

    inline Disposable<Array> prod(const SparseMatrix& A, const Array& x) {
        Array b(x.size(), 0.0);

        for (Size i=0; i < A.filled1()-1; ++i) {
            const Size begin = A.index1_data()[i];
            const Size end   = A.index1_data()[i+1];
            Real t=0;
            for (Size j=begin; j < end; ++j) {
                t += A.value_data()[j]*x[A.index2_data()[j]];
            }

            b[i]=t;
        }
        return b;
    }
}

#endif
#endif


#define PY_ARRAY_UNIQUE_SYMBOL superimg_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/extended_p_square_quantile.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/cgp2d/cgp2d.hxx"
#include "seglib/cgp2d/cgp2d_python.hxx"
#include "seglib/distances/distance.hxx"

namespace python = boost::python;

namespace cgp2d {

    // tgrid and input image type
    typedef Cgp<CoordinateType,LabelType> CgpType;
    typedef CgpType::TopologicalGridType TopologicalGridType;

    void mergeFeatures(
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<1,float>              weights,
        vigra::NumpyArray<2,float>              features,
        vigra::NumpyArray<2,float>              mergedFeatures,
        vigra::NumpyArray<1,float>              weightBuffer
    ){
        CGP_ASSERT_OP(labeling.shape(0),==,features.shape(0));
        CGP_ASSERT_OP(labeling.shape(0),==,mergedFeatures.shape(0));
        CGP_ASSERT_OP(features.shape(1),==,mergedFeatures.shape(1));
        CGP_ASSERT_OP(weightBuffer.shape(0),==,features.shape(0));

        // initalize with zeros
        std::fill(mergedFeatures.begin(),mergedFeatures.begin()+numberOfLabels,0.0);
        std::fill(weightBuffer.begin(),weightBuffer.begin()+numberOfLabels,0.0);

        const size_t nItems = labeling.shape(0);
        const size_t nFeatures = features.shape(1);

        // accumulate
        for(size_t i=0;i<nItems;++i){

            // get the label
            const size_t label = labeling(i);
            // get the weight
            const float weight = weights(i);
            // accumulate features
            for(size_t f=0;f<nFeatures;++f){
                mergedFeatures(label,f)+=weight*features(i,f);
            }
            // accumulate weights
            weightBuffer(i)+=weight;
        }
        // normalize

        for(size_t label=0;label<numberOfLabels;++label){
            const float weight=weights(label);
            for(size_t f=0;f<nFeatures;++f){
                mergedFeatures(label,f)/=weight;
            }
        }

    }

    float withinClusterDist(
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<1,float>              weights,
        vigra::NumpyArray<2,float>              features,
        vigra::NumpyArray<2,float>              mergedFeatures
    ){
        CGP_ASSERT_OP(labeling.shape(0),==,features.shape(0));
        CGP_ASSERT_OP(labeling.shape(0),==,mergedFeatures.shape(0));
        CGP_ASSERT_OP(features.shape(1),==,mergedFeatures.shape(1));

        const size_t nItems = labeling.shape(0);
        const size_t nFeatures = features.shape(1);

        float totalD = 0.0;

        for(size_t i=0;i<nItems;++i){

            // get the label
            const size_t label = labeling(i);

            // compute the distance between the feature of the item (superpixel)
            // and the feature of the cluster (cluster of superpixel / "HyperRegion")
            vigra::MultiArrayView<1,float> a=features.bindInner(i);
            vigra::MultiArrayView<1,float> b=mergedFeatures.bindInner(label);
            const  float d = distances::Distance<float>::klDivergenz(a.begin(),a.end(),b.begin(),b.end());
            // get the weight of the item (superpixel size)
            const float weight=weights(i);

            totalD += weight*d;

            //weightBuffer(l)+=weight
        }
        return totalD;
    }

    float betweenClusterDist(
        const Cgp<CoordinateType,LabelType> &   cgp,
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<2,float>              mergedFeatures
    ){

        float totalD=0.0f;

        std::set<size_t> used;
        const size_t nBoundaries = cgp.numCells(1);
        for(size_t b=0;b<nBoundaries;++b){
            const size_t r1 = cgp.bound<1>(b,0)-1;
            const size_t r2 = cgp.bound<1>(b,1)-1;

            size_t l1 =labeling(r1);
            size_t l2 =labeling(r2);

            // active boundarie ?
            if(l1!=l2){

                if(l2<l1){
                    std::swap(l1,l2);
                }
                const size_t key = l1+ l2*numberOfLabels;
                if(used.find(key)==used.end()){
                    used.insert(key);
                    vigra::MultiArrayView<1,float> a=mergedFeatures.bindInner(l1);
                    vigra::MultiArrayView<1,float> b=mergedFeatures.bindInner(l2);
                    const float d = distances::Distance<float>::klDivergenz(a.begin(),a.end(),b.begin(),b.end());
                    totalD+=d;
                }
            }
        }
        return totalD;
    }

    void export_merge(){

        python::def("_mergeFeatures",vigra::registerConverters(&mergeFeatures),
            (
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("weights"),
                python::arg("features"),
                python::arg("mergedFeatures"),
                python::arg("weightBuffer")
            )
        );

        python::def("_withinClusterDist",vigra::registerConverters(&withinClusterDist),
            (
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("weights"),
                python::arg("features"),
                python::arg("mergedFeatures")
            )
        );

        python::def("_betweenClusterDist",vigra::registerConverters(&betweenClusterDist),
            (
                python::arg("cgp"),
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("mergedFeatures")
            )
        );

    }

}


#include "scan_calculator.h"
#include <graycode.h>
#include <multiview.h>
#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <boost/thread.hpp>

void ScanCalculator::ScanWorker(int threadId, int startRow, int endRow)
{
	MultiView stereoView;
	stereoView.ReadRawCal("rawcal.ini");

	std::vector<cv::Point2f> p;
	std::vector<cv::Point3f> lines;
	for(int r = startRow; r < endRow; r++)//for(int r = 1; r < size.height-5; r++)
	{
		for(int c = 1; c < 1290; c++)//for(int c = 1; c < size.width-5; c++)
		{
			p.push_back(cv::Point2f(c, r));
		}
	}
	cv::computeCorrespondEpilines(p, 1, F, lines);
	//std::cout << "size of lines: "<< lines.size() << std::endl;

	cv::Mat cam0pnts(1,1,CV_64FC2);
	cv::Mat cam1pnts(1,1,CV_64FC2);

	//find correspondence and triangulate
	T2DLINED l;
	int counter = 0;
	for(int r = startRow; r < endRow; r++)
	{
		//std::cout << "thread " << threadId << " : " << r << std::endl;
		for(int c = 1; c < 1290; c++)//for(int c = 1; c < size.width-5; c++)
		{
			cv::Mat pnts3D = cv::Mat(1,1,CV_64FC4);

			l.p0.x = 0;
			l.p0.y = -(lines[counter].z/lines[counter].y);
			double m = -(lines[counter].x / lines[counter].y);
			double angle = atan(m);
			l.vr.x = 1250*cos(angle);
			l.vr.y = 1250*sin(angle);

			double _U=0, _V=0;
			double srcW = BLInterpolate(p[counter].x, p[counter].y, absL);
			counter++;

			if(!stereoView.DetectPtOnEplipolarLine(l, srcW, absR, p[counter-1].x, p[counter-1].y, _U, _V, 2, PP2))
			{
				continue;
			}

			cam0pnts.at<cv::Point2d>(0) = p[counter-1];
			cam1pnts.at<cv::Point2d>(0) = cv::Point2d(_U, _V);

			cv::triangulatePoints(PP1,PP2,cam0pnts,cam1pnts,pnts3D);
			pnts3D = pnts3D / pnts3D.at<double>(3);

			clouds[threadId].push_back(pnts3D);
		}
	}
}

void LoadAbsolutePhase(const char* filename, cv::Mat& absPhase)
{
    cv::Mat dummy = cv::imread(filename, CV_16U);
    short result = 0;

    for(int r = 0; r < absPhase.rows; r++)
    {
        for(int c = 0; c < absPhase.cols; c++)
        {
            unsigned short val = dummy.at<unsigned short>(cv::Point2i(c,r));

            result = val;
            if(val > 32767)
                result = val - 65536;

            absPhase.at<short>(cv::Point2i(c,r)) = result;
        }
    }
}

ScanCalculator::ScanCalculator()
{
}

ScanCalculator::~ScanCalculator()
{

}

bool ScanCalculator::StartCalculation(std::vector<std::vector<cv::Mat> >& sequence, CalibrationResult& c_result)
{
	int height = sequence[0][0].rows;
	int width = sequence[0][0].cols;
    cv::Size size = cv::Size(sequence[0][0].cols, sequence[0][0].rows);
    CalculateAbsPhase(sequence);

	absL = cv::Mat(height, width, CV_16S);
    absR = cv::Mat(height, width, CV_16S);
    LoadAbsolutePhase("absL.tiff", absL);
    LoadAbsolutePhase("absR.tiff", absR);

    cv::stereoRectify(c_result.K[0], c_result.D[0], c_result.K[1], c_result.D[1], size, c_result.R, c_result.T, R1, R2, P1, P2, Q);
	F = c_result.F;

    //P = [K*R -(K*R)*C];
    cv::Mat exterior = cv::Mat::zeros(4,4,CV_64F);
    cv::Mat middle = cv::Mat::eye(3,4,CV_64F);
    //std::cout << middle << std::endl;

    PP1 = cv::Mat::zeros(3,4,CV_64F);
    cv::Mat rotmat;
    cv::Rodrigues(c_result.rvecs[0][0], rotmat);
    //rotmat = rotmat.inv();
    exterior.at<double>(0,0) = rotmat.at<double>(0,0);
    exterior.at<double>(0,1) = rotmat.at<double>(0,1);
    exterior.at<double>(0,2) = rotmat.at<double>(0,2);
    exterior.at<double>(1,0) = rotmat.at<double>(1,0);
    exterior.at<double>(1,1) = rotmat.at<double>(1,1);
    exterior.at<double>(1,2) = rotmat.at<double>(1,2);
    exterior.at<double>(2,0) = rotmat.at<double>(2,0);
    exterior.at<double>(2,1) = rotmat.at<double>(2,1);
    exterior.at<double>(2,2) = rotmat.at<double>(2,2);
    exterior.at<double>(0,3) = c_result.tvecs[0][0].at<double>(0);
    exterior.at<double>(1,3) = c_result.tvecs[0][0].at<double>(1);
    exterior.at<double>(2,3) = c_result.tvecs[0][0].at<double>(2);
    exterior.at<double>(3,0) = 0;
    exterior.at<double>(3,1) = 0;
    exterior.at<double>(3,2) = 0;
    exterior.at<double>(3,3) = 1;
    PP1 = c_result.K[0] * middle * exterior;
    PP1 = PP1 / PP1.at<double>(2,3);
    std::cout << "P hesaplanan 1" << PP1 << std::endl;

    PP2 = cv::Mat::zeros(3,4,CV_64F);
    cv::Mat rotmat2;
    cv::Rodrigues(c_result.rvecs[1][0], rotmat2);
    exterior.at<double>(0,0) = rotmat2.at<double>(0,0);
    exterior.at<double>(0,1) = rotmat2.at<double>(0,1);
    exterior.at<double>(0,2) = rotmat2.at<double>(0,2);
    exterior.at<double>(1,0) = rotmat2.at<double>(1,0);
    exterior.at<double>(1,1) = rotmat2.at<double>(1,1);
    exterior.at<double>(1,2) = rotmat2.at<double>(1,2);
    exterior.at<double>(2,0) = rotmat2.at<double>(2,0);
    exterior.at<double>(2,1) = rotmat2.at<double>(2,1);
    exterior.at<double>(2,2) = rotmat2.at<double>(2,2);
    exterior.at<double>(0,3) = c_result.tvecs[1][0].at<double>(0);
    exterior.at<double>(1,3) = c_result.tvecs[1][0].at<double>(1);
    exterior.at<double>(2,3) = c_result.tvecs[1][0].at<double>(2);
    exterior.at<double>(3,0) = 0;
    exterior.at<double>(3,1) = 0;
    exterior.at<double>(3,2) = 0;
    exterior.at<double>(3,3) = 1;
    PP2 = c_result.K[1] * middle * exterior;
    PP2 = PP2 / PP2.at<double>(2,3);
    std::cout << "P hesaplanan 2" << PP2 << std::endl;

	boost::thread_group tg;
	int thread_amount = 64;
	clouds.resize(thread_amount);
	int step = 960 / thread_amount;
	for(int i = 0; i<thread_amount; i++)
	{
		tg.create_thread(boost::bind(&ScanCalculator::ScanWorker, this, i, (i*step)+1, (i+1)*step ));
	}

	int t0 = time(NULL);
	tg.join_all();
	int t1 = time(NULL);
	printf ("time = %d secs\n", t1 - t0);

	std::ofstream file("out.txt");
	for(int i = 0; i < clouds.size(); i++)
	{
		m_ptCloud.insert(m_ptCloud.end(), clouds[i].begin(), clouds[i].end());
	}

    return true;
}

std::vector<cv::Mat>* ScanCalculator::GetCloud()
{
    return& m_ptCloud;
}

void ScanCalculator::CalculateAbsPhase(std::vector<std::vector<cv::Mat> >& sequence)
{
    int height = sequence[0][0].rows;
    int width = sequence[0][0].cols;
    cv::Mat absPhaseL = cv::Mat::zeros( height, width, CV_16U );
    cv::Mat absPhaseR = cv::Mat::zeros( height, width, CV_16U );
    CalculateGP(absPhaseL, sequence[0], 1, 16, 17);
    cv::imwrite("absL.tiff", absPhaseL);
    CalculateGP(absPhaseR, sequence[1], 1, 16, 17);
    cv::imwrite("absR.tiff", absPhaseR);
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
//#include "../../point_to_plane_tait_bryan_wc_jacobian.h"
//#include "../../point_to_plane_rodrigues_wc_jacobian.h"
//#include "../../point_to_plane_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"
#include "../include/cauchy.h"
#include "../../distance_point_to_plane_tait_bryan_wc_jacobian.h"
#include "../../distance_point_to_plane_rodrigues_wc_jacobian.h"
#include "../../distance_point_to_plane_quaternion_wc_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<Eigen::Affine3d> trajectory;
std::vector<Eigen::Affine3d> planes_global;
std::vector<std::vector<Eigen::Affine3d>> planes_local;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	TaitBryanPose pose;
	pose.px = -40;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = -20;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = -0;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = 20;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = 40;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	for(size_t i = 0 ; i < 100; i++){
		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		planes_global.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}

	planes_local.resize(trajectory.size());
	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d m_inv = trajectory[i].inverse();

		for(size_t j = 0 ; j < planes_global.size(); j++){
			Eigen::Affine3d m_line_local = m_inv * planes_global[j];
			planes_local[i].push_back(m_line_local);
		}
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("distance_point_to_plane");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();

	for(size_t i = 0 ; i < planes_local.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		for(size_t j = 0 ; j < planes_local[i].size(); j++){
			Eigen::Affine3d m_line_global = m * planes_local[i][j];

			glBegin(GL_LINES);
				glColor3f(1.0f, 0.0f, 0.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,0), m_line_global(1,3) + m_line_global(1,0), m_line_global(2,3) + m_line_global(2,0));

				glColor3f(0.0f, 1.0f, 0.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,1), m_line_global(1,3) + m_line_global(1,1), m_line_global(2,3) + m_line_global(2,1));

				glColor3f(0.0f, 0.0f, 1.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,2), m_line_global(1,3) + m_line_global(1,2), m_line_global(2,3) + m_line_global(2,2));
			glEnd();
		}
	}

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
			for(size_t i = 0 ; i  < trajectory.size(); i++){
				TaitBryanPose pose;
				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				trajectory[i] = trajectory[i] * affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_tait_bryan_wc(delta,
									pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 6> delta_jacobian;
							delta_distance_point_to_plane_tait_bryan_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 6; jj++){
									int ic = i * 6;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 6, trajectory.size() * 6);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					trajectory[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			for(size_t i = 0; i < trajectory.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(trajectory[i]);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				trajectory[i] = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						RodriguesPose pose = pose_rodrigues_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_rodrigues_wc(delta,
									pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 6> delta_jacobian;
							delta_distance_point_to_plane_rodrigues_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 6; jj++){
									int ic = i * 6;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 6, trajectory.size() * 6);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					trajectory[i] = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_quaternion_wc(delta,
									pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 7> delta_jacobian;
							delta_distance_point_to_plane_quaternion_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 7; jj++){
									int ic = i * 7;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);
			tripletListA.emplace_back(ir + 6 , 6, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);
			tripletListB.emplace_back(ir + 6 , 0, 0);

			for(size_t i = 0 ; i < trajectory.size(); i++){
				int ic = i * 7;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 7, trajectory.size() * 7);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];
					trajectory[i] = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to trajectory" << std::endl;
	std::cout << "t: optimize Tait-Bryan" << std::endl;
	std::cout << "r: optimize Rodrigues" << std::endl;
	std::cout << "q: optimize Quaternion" << std::endl;
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/**
 * @file tableLookup.cpp
 * @brief Code for homomorphic table lookup and fixed-point functions
 */
#include <limits>
#include <cmath>
#include <cstdlib>
#include <stdexcept>
#include <NTL/BasicThreadPool.h>
#include <helib/intraSlot.h>
#include <helib/tableLookup.h>

#ifdef HELIB_DEBUG
#include <helib/debugging.h>
#endif

namespace helib {

static void recursiveProducts(const CtPtrs& products,
                              const CtPtrs_slice& array);
static double pow2_double(long n); // compute 2^n as double

// For an n-size array, compute the 2^n products
//     products[j] = \prod_{i s.t. j_i=1} array[i]
//                   \times \prod_{i s.t. j_i=0}(a-array[i])
void computeAllProducts(/*Output*/ CtPtrs& products,
                        /*Index*/ const CtPtrs& array,
                        std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  long nBits = array.size();
  if (lsize(products) > 0) {
    long nBits2 = NTL::NumBits(lsize(products) - 1); // ceil(log_2(size))
    if (nBits > nBits2)
      nBits = nBits2; // ignore extra bits in 'array'
  }
  if (nBits < 1)
    return; // do nothing
  // Output cannot be bigger than 2^16
  assertTrue(nBits <= 16, "Output cannot be bigger than 2^16");

  if (lsize(products) == 0) // try to set the output size
    products.resize(1L << nBits, &array);
  for (long i = 0; i < lsize(products); i++)
    products[i]->clear();

  // Check that we have enough levels, try to bootstrap otherwise
  assertNotNull(array.ptr2nonNull(),
                "Invalid array (could not find non-null Ctxt)");
  long bpl = array.ptr2nonNull()->getContext().BPL();
  if (findMinBitCapacity(array) < (NTL::NumBits(nBits) + 1) * bpl) {
    const Ctxt* ct = array.ptr2nonNull(); // find some non-null Ctxt
    assertNotNull(unpackSlotEncoding,
                  "unpackSlotEncoding must not be null when bootstrapping");
    assertTrue(ct->getPubKey().isBootstrappable(),
               "Cannot bootstrap with non-bootstrappable public key");
    packedRecrypt(array,
                  *unpackSlotEncoding,
                  *(ct->getContext().ea),
                  /*belowLevel=*/nBits + 3);
  }
  if (findMinBitCapacity(array) < (NTL::NumBits(nBits) + 1) * bpl)
    throw LogicError("not enough levels for table lookup");

  // Call the recursive function that computes the products
  recursiveProducts(products, CtPtrs_slice(array, 0, nBits));
}

// The input is a plaintext table T[] and an array of encrypted bits
// I[], holding the binary representation of an index i into T.
// The output is the encrypted value T[i].
void tableLookup(Ctxt& out,
                 const std::vector<zzX>& table,
                 const CtPtrs& idx,
                 std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  out.clear();
  std::vector<Ctxt> products(lsize(table),
                             out); // to hold subset products of idx
  CtPtrs_vectorCt pWrap(products); // A wrapper

  // Compute all products of encrypted bits =: b_i
  computeAllProducts(pWrap, idx, unpackSlotEncoding);

  // Compute the sum b_i * T[i]
  NTL_EXEC_RANGE(lsize(table), first, last)
  for (long i = first; i < last; i++)
    products[i].multByConstant(table[i]); // p[i] = p[i]*T[i]
  NTL_EXEC_RANGE_END
  for (long i = 0; i < lsize(table); i++)
    out += products[i];
}

// A counterpart of tableLookup. The input is an encrypted table T[]
// and an array of encrypted bits I[], holding the binary representation
// of an index i into T.  This function increments by one the entry T[i].
void tableWriteIn(const CtPtrs& table,
                  const CtPtrs& idx,
                  std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  const Ctxt* ct = table.ptr2nonNull(); // find some non-null Ctxt
  long size = lsize(table);
  if (size == 0)
    return;
  std::vector<Ctxt> products(size, Ctxt(ZeroCtxtLike, *ct));
  CtPtrs_vectorCt pWrap(products); // A wrapper

  // Compute all products of encrypted bits =: b_i
  computeAllProducts(pWrap, idx, unpackSlotEncoding);

  // increment each entry of T[i] by products[i]
  NTL_EXEC_RANGE(lsize(table), first, last)
  for (long i = first; i < last; i++)
    *table[i] += products[i];
  NTL_EXEC_RANGE_END
}

// The function buildLookupTable is documented in tableLookup.h.
// The output is returned in T, size of T will be 2^{nbits_in}.
// For every signed integer x with bit-size 'nbits_in', we will have
//     T[x] = f(x * 2^{scale_in}) * 2^{-scale_out}),
// rounded to the nearest integer and truncated to 'nbits_out' bits.
// The bits are packed inside the slots, so it is assumed that each
// slot has enough room to fit these many bits. (Otherwise we only
// keep as many low-order bits as fit in a slot.)
void buildLookupTable(std::vector<zzX>& T, // encoded result is returned in T
                      std::function<double(double)> f,
                      long nbits_in, // number of precision bits
                      long scale_in, // scaling factor
                      long sign_in,  // 1: 2's complement signed, 0: unsigned
                      long nbits_out,
                      long scale_out,
                      long sign_out,
                      const EncryptedArray& ea)
{
  HELIB_TIMER_START;
  // tables of size > 2^{16} are not supported
  assertTrue(nbits_in <= 16, "tables of size > 2^{16} are not supported");
  long sz = 1L << nbits_in;
  T.resize(sz);

  double pow2_scale_in = pow2_double(scale_in);        // 2^{nbits_in}
  double pow2_neg_scale_out = pow2_double(-scale_out); // 2^{-nbits_out}

  // Compute the largest and smallest values that can be in T
  long largest_value, smallest_value;
  if (sign_out) { // values in T are encoded in 2's complement
    largest_value = (1L << (nbits_out - 1)) - 1;
    smallest_value = -(1L << (nbits_out - 1));
  } else { // values in T are all non-negative
    largest_value = (1L << nbits_out) - 1;
    smallest_value = 0;
  }

  for (long i = 0; i < sz; i++) { // Compute the entries of T
    long x;
    if (sign_in) { // indexes into T are in 2's complement
      long sign_bit = (1L << (nbits_in - 1)) & i;
      x = i - 2 * sign_bit;
    } else
      x = i; // indexes into T are all non-negative

    // Compute the value that should go in the table as rounded double
    double scaled_x = double(x) * pow2_scale_in;
    double y = round(f(scaled_x) * pow2_neg_scale_out);

    // saturated arithmetic (set to smallest or largest values)
    // NOTE: this should work fine even if y is an infinity
    long value;
    if (std::isnan(y))
      value = 0;
    else if (y > largest_value)
      value = largest_value;
    else if (y < smallest_value)
      value = smallest_value;
    else
      value = y;

    // convert to unsigned and mask to nbits_out bits
    unsigned long uvalue = value;
    uvalue &= ((1UL << nbits_out) - 1UL); // keep only bottom nbits_out bits

    packConstant(T[i], uvalue, nbits_out, ea);
  }
}

// A recursive function to compute, for an n-size array, the 2^n products
//     products[j] = \prod_{i s.t. j_i=1} array[i]
//                   \times \prod_{i s.t. j_i=0}(a-array[i])
// It is assume that 'products' size <= 2^n, else only 1st 2^n entries are set
static void recursiveProducts(const CtPtrs& products, const CtPtrs_slice& array)
{
  long nBits = lsize(array);
  long N = lsize(products);
  if (nBits == 0 || N == 0)
    return; // nothing to do

  if (N > (1L << nBits))
    N = (1L << nBits);
  else if (N < (1L << (nBits - 1)))
    nBits = NTL::NumBits(N - 1); // Ensure nBits <= ceil(log2(N))

  if (N <= 2) { // edge condition
    *products[0] = *array[0];
    products[0]->negate();
    products[0]->addConstant(NTL::ZZ(1)); // out[0] = 1-in
    if (N > 1)
      *products[1] = *array[0]; // out[1] = in
  }
  // optimization for n=2: a single multiplication instead of 4
  else if (N <= 4) {
    *products[0] = *array[1];           // x1
    products[0]->multiplyBy(*array[0]); // x1 x0

    *products[1] = *array[0];     // x0
    *products[1] -= *products[0]; // x0 - x1 x0 = (1-x1)x0

    *products[2] = *array[1];     // x1
    *products[2] -= *products[0]; // x1 - x1 x0 = x1(1-x0)

    if (N > 3)
      *products[3] = *products[0]; // x1 x0

    products[0]->addConstant(NTL::ZZ(1)); // 1 +x1 x0
    *products[0] -= *array[1];            // 1 +x1 x0 -x1
    *products[0] -= *array[0];            // 1 +x1 x0 -x1 -x0 = (1-x1)(1-x0)
  } else {                                // split the array into two parts;
    // first part is highest pow(2) < n, second part is what is left

    long n1 = 1L << (NTL::NumBits(nBits) - 1); // largest power of two <= n
    if (nBits <= n1)
      n1 = n1 / 2;               // largest power of two < n
    long k = 1L << n1;           // size of first part
    long l = 1L << (nBits - n1); // size of second part

    const Ctxt* ct = array.ptr2nonNull(); // find some non-null Ctxt
    std::vector<Ctxt> products1(k, Ctxt(ZeroCtxtLike, *ct));
    std::vector<Ctxt> products2(l, Ctxt(ZeroCtxtLike, *ct));

    // compute first part of the array
    recursiveProducts(CtPtrs_vectorCt(products1), CtPtrs_slice(array, 0, n1));

    // recursive call on second part of array
    recursiveProducts(CtPtrs_vectorCt(products2),
                      CtPtrs_slice(array, n1, nBits - n1));

    // multiplication to get all subset products
    NTL_EXEC_RANGE(lsize(products), first, last)
    for (long ii = first; ii < last; ii++) {
      long j = ii / k;
      long i = ii - j * k;
      *products[ii] = products1[i];
      products[ii]->multiplyBy(products2[j]);
    }
    NTL_EXEC_RANGE_END
  }
}

static double pow2_double(long n) // compute 2^n as double
{
  double res = 1;
  long abs_n = std::labs(n);

  for (long i = 0; i < abs_n; i++)
    res *= 2;
  if (n < 0)
    res = 1 / res;
  return res;
}

} // namespace helib


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_ARCH_COMMON_SIMD_FUNCTION_REVERSEBITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REVERSEBITS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/shift_left.hpp>
#include <boost/simd/function/simd/shr.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints8_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x55); //binary: 0101...
        const utype m2  = utype(0x33); //binary: 00110011..
        const utype m4  = utype(0x0f); //binary:  4 zeros,  4 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints64_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x5555555555555555ull); //binary: 0101...
        const utype m2  = utype(0x3333333333333333ull); //binary: 00110011..
        const utype m4  = utype(0x0f0f0f0f0f0f0f0full); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff00ff00ff00ffull); //binary:  8 zeros,  8 ones ...
        const utype m16 = utype(0x0000ffff0000ffffull); //binary:  16 zeros,  16 ones ...
        const utype m32 = utype(0x00000000ffffffffull); //binary:  32 zeros,  32 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        // swap shorts ...
        v = bitwise_or(bitwise_and(shr(v, 16), m16), shift_left(bitwise_and(v, m16), 16));
        // swap ints ...
        v = bitwise_or(bitwise_and(shr(v, 32), m32), shift_left(bitwise_and(v, m32), 32));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints16_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x5555); //binary: 0101...
        const utype m2  = utype(0x3333); //binary: 00110011..
        const utype m4  = utype(0x0f0f); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff); //binary:  8 zeros,  8 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints32_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x55555555); //binary: 0101...
        const utype m2  = utype(0x33333333); //binary: 00110011..
        const utype m4  = utype(0x0f0f0f0f); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff00ff); //binary:  8 zeros,  8 ones ...
        const utype m16 = utype(0x0000ffff); //binary:  16 zeros,  16 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        // swap shorts ...
        v = bitwise_or(bitwise_and(shr(v, 16), m16), shift_left(bitwise_and(v, m16), 16));
        return bitwise_cast<A0>(v);
        }
   };
} } }

#endif


/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file BasicGasPricer.cpp
 * @author Gav Wood <i@gavwood.com>
 * @date 2015
 */

#pragma warning(push)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include <boost/math/distributions/normal.hpp>
#pragma warning(pop)
#pragma GCC diagnostic pop
#include "BasicGasPricer.h"
#include "BlockChain.h"
using namespace std;
using namespace dev;
using namespace dev::eth;

void BasicGasPricer::update(BlockChain const& _bc)
{
	unsigned c = 0;
	h256 p = _bc.currentHash();
	m_gasPerBlock = _bc.info(p).gasLimit();

	map<u256, u256> dist;
	u256 total = 0;

	// make gasPrice versus gasUsed distribution for the last 1000 blocks
	while (c < 1000 && p)
	{
		BlockHeader bi = _bc.info(p);
		if (bi.transactionsRoot() != EmptyTrie)
		{
			auto bb = _bc.block(p);
			RLP r(bb);
			BlockReceipts brs(_bc.receipts(bi.hash()));
			size_t i = 0;
			for (auto const& tr: r[1])
			{
				Transaction tx(tr.data(), CheckTransaction::None);
				u256 gu = brs.receipts[i].cumulativeGasUsed();
				dist[tx.gasPrice()] += gu;
				total += gu;
				i++;
			}
		}
		p = bi.parentHash();
		++c;
	}

	// fill m_octiles with weighted gasPrices
	if (total > 0)
	{
		m_octiles[0] = dist.begin()->first;

		// calc mean
		u256 mean = 0;
		for (auto const& i: dist)
			mean += i.first * i.second;
		mean /= total;

		// calc standard deviation
		u256 sdSquared = 0;
		for (auto const& i: dist)
			sdSquared += i.second * (i.first - mean) * (i.first - mean);
		sdSquared /= total;

		if (sdSquared)
		{
			long double sd = sqrt(sdSquared.convert_to<long double>());
			long double normalizedSd = sd / mean.convert_to<long double>();

			// calc octiles normalized to gaussian distribution
			boost::math::normal gauss(1.0, (normalizedSd > 0.01) ? normalizedSd : 0.01);
			for (size_t i = 1; i < 8; i++)
				m_octiles[i] = u256(mean.convert_to<long double>() * boost::math::quantile(gauss, i / 8.0));
			m_octiles[8] = dist.rbegin()->first;
		}
		else
		{
			for (size_t i = 0; i < 9; i++)
				m_octiles[i] = (i + 1) * mean / 5;
		}
	}
}


// C++ standard library headers
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <limits>
#include <numeric>
#include <vector>

// Boost headers
#include <boost/math/interpolators/pchip.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

// triumf++ headers
#include <triumf/bnmr/nuclei.hpp>
#include <triumf/nmr/dipole_dipole.hpp>
#include <triumf/nmr/nuclei.hpp>
#include <triumf/numpy.hpp>
#include <triumf/srim/pdf.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/pippard.hpp>

// ROOT headers
#include <ROOT/RCsvDS.hxx>
#include <ROOT/RDF/RInterface.hxx>
#include <ROOT/RDataFrame.hxx>
#include <TCanvas.h>
#include <TGraph.h>

//
double slr_fcn(double d, double R_0, double d_0) {
  // return d < d_0 ? 1.5 * R_0 : R_0 * std::exp((d - d_0) / (10.0 * d_0));
  return d < d_0 ? 1.0 * R_0 : 0.5 * R_0;
}

/// model slr rate
template <typename T = double>
T slr_rate_z(T z, T temperature, T critical_temperature, T gap_meV, T xi_0,
             T mean_free_path, T lambda_0, T exponent, T applied_field,
             T dipole_field, T correlation_rate, T slr_constant, T slr_exponent,
             T surface_thickness, T surface_rate) {
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  if (_z_ < 0.0) {
    return surface_rate;
  } else {
    // calculate the local field from the screening profile
    T screened_field =
        temperature > critical_temperature
            ? applied_field
            : triumf::superconductivity::pippard::field_penetration<T>(
                  _z_, temperature, critical_temperature, gap_meV, xi_0,
                  mean_free_path, lambda_0, exponent, applied_field);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<double>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

const double TEMPERATURE = 2.5;
const double T_C = 9.25;
const double D_0 = triumf::superconductivity::bcs::gap_meV<double>(TEMPERATURE);
const double XI_0 = 38.0;
const double ELL = 1e4;
const double LAMBDA_0 = 40.0;
const double EXPONENT = 4.0;
const double B_0 = 0.02;
const double B_D = 5e-5;
const double NU_C = 0.1 / 23.8e-6;
const double SLR_C = 0.75;
const double SLR_N = 1.0;
const double SURFACE_THICKNESS = 5.0;
const double SURFACE_RATE = 10.0;

constexpr double GLOBAL_R_0 = 1.0;
constexpr double GLOBAL_D_0 = 15.0;

//
template <typename T = double> class DepthAverage {
public:
  // constructor
  DepthAverage(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha = df.Take<T>("Alpha").GetValue();
    _alpha_error = df.Take<T>("Alpha Error").GetValue();
    _beta = df.Take<T>("Beta").GetValue();
    _beta_error = df.Take<T>("Beta Error").GetValue();
    _z_max = df.Take<T>("Max (nm)").GetValue();
    _z_max_error = df.Take<T>("Max Error (nm)").GetValue();
  };

  //
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  //
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  //
  T alpha(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> alpha_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_alpha)));
    return alpha_interpolator(energy_keV);
  };

  //
  T beta(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> beta_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_beta)));
    return beta_interpolator(energy_keV);
  };

  //
  T z_max(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> z_max_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_z_max)));
    return z_max_interpolator(energy_keV);
  };

  //
  T z_average(T energy_keV) {
    T a = alpha(energy_keV);
    T b = beta(energy_keV);
    T zm = z_max(energy_keV);
    return zm * a / (a + b);
  };

  /*
  // depth-averaging using numeric integration
  T operator()(T energy_keV) {
    // T a = alpha(energy_keV);
    // T b = beta(energy_keV);
    // T zm = z_max(energy_keV);
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      // return slr_fcn(z, GLOBAL_R_0, GLOBAL_D_0) *
      return slr_rate_z<T>(z, TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0,
                           EXPONENT, B_0, B_D, NU_C, SLR_C, SLR_N,
                           SURFACE_THICKNESS, SURFACE_RATE) *
             triumf::srim::pdf::modified_beta<T>(
                 z, alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
    };
    T Q =
        integrator.integrate(integrand, 0.0, z_max(energy_keV),
                             std::cbrt(std::numeric_limits<double>::epsilon()));
    return Q;
  };
  */

  // depth-averaging using "histogram" summation
  T operator()(T energy_keV) {
    constexpr std::size_t n = 201;
    // bin edges
    std::vector<T> z_edge =
        triumf::numpy::linspace<T>(0.0, z_max(energy_keV), n);
    // bin widths - adjust ranges by one for "correct" size
    std::vector<T> dz(n - 1);
    std::adjacent_difference(std::begin(z_edge) + 1, std::end(z_edge),
                             std::begin(dz));
    // bin centres
    std::vector<T> z(n - 1);
    // probabilities
    std::vector<T> p_z(n - 1);
    std::vector<T> weights(n - 1);
    std::vector<T> slr_rates(n - 1);

    for (std::size_t i = 0; i < z.size(); ++i) {
      z.at(i) = z_edge.at(i) + 0.5 * dz.at(i);
      p_z.at(i) = triumf::srim::pdf::modified_beta<T>(
          z.at(i), alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
      // std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";
      weights.at(i) = dz.at(i) * p_z.at(i);
      // slr_rates.at(i) = slr_fcn(z.at(i), GLOBAL_R_0, GLOBAL_D_0);
      slr_rates.at(i) = slr_rate_z<T>(z.at(i), TEMPERATURE, T_C, D_0, XI_0, ELL,
                                      LAMBDA_0, EXPONENT, B_0, B_D, NU_C, SLR_C,
                                      SLR_N, SURFACE_THICKNESS, SURFACE_RATE);
    }

    T sum_weights = std::reduce(std::begin(weights), std::end(weights));
    // std::cout << "sum_weights = " << sum_weights << "\n";
    T sum_weights_slr = std::transform_reduce(
        std::begin(weights), std::end(weights), std::begin(slr_rates), 0.0);
    // std::cout << "sum_weights_slr = " << sum_weights_slr << "\n";
    T weighted_average = sum_weights_slr / sum_weights;
    return weighted_average;
  };

private:
  // vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha;
  std::vector<T> _alpha_error;
  std::vector<T> _beta;
  std::vector<T> _beta_error;
  std::vector<T> _z_max;
  std::vector<T> _z_max_error;
};

void test_depth_averaging() {
  const double alpha = 2.5;
  const double beta = 4.5;
  const double z_max = 250.0;

  //
  // const double R_0 = 10.0;
  // const double d_0 = 25.0; // (1.0 / 6.0) * z_max;

  // bin edges
  std::vector<double> z_edge = triumf::numpy::linspace<double>(0.0, z_max, 201);
  // bin widths - adjust ranges by one for "correct" size
  std::vector<double> dz(z_edge.size() - 1);
  std::adjacent_difference(std::begin(z_edge) + 1, std::end(z_edge),
                           std::begin(dz));
  // bin centres
  std::vector<double> z(z_edge.size() - 1);
  // probabilities
  std::vector<double> p_z(z_edge.size() - 1);

  std::vector<double> weights(z_edge.size() - 1);

  std::vector<double> slr_rates(z_edge.size() - 1);

  std::cout << std::setprecision(16);
  std::cout << std::fixed;

  for (std::size_t i = 0; i < z.size(); ++i) {
    z.at(i) = z_edge.at(i) + 0.5 * dz.at(i);
    p_z.at(i) = triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
    // std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";

    weights.at(i) = dz.at(i) * p_z.at(i);
    // slr_rates.at(i) = slr_fcn(z.at(i), GLOBAL_R_0, GLOBAL_D_0);
    slr_rates.at(i) = slr_rate_z<double>(
        z.at(i), TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0, EXPONENT, B_0, B_D,
        NU_C, SLR_C, SLR_N, SURFACE_THICKNESS, SURFACE_RATE);
  }

  double sum_weights = std::reduce(std::begin(weights), std::end(weights));
  std::cout << "sum_weights = " << sum_weights << "\n";

  double sum_weights_slr = std::transform_reduce(
      std::begin(weights), std::end(weights), std::begin(slr_rates), 0.0);
  std::cout << "sum_weights_slr = " << sum_weights_slr << "\n";

  double weighted_average = sum_weights_slr / sum_weights;
  // std::cout << "weighted_average = " << weighted_average << "\n";

  /*
  for (std::size_t i = 0; i < z.size(); ++i) {
    p_z.at(i) = triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
    std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";
  }
  */

  // double sum = std::reduce(std::begin(p_z), std::end(p_z));
  // std::cout << "sum = " << sum << "\n";

  //

  /*
  std::cout << dz.size() << "\n";
  for (auto &bw : dz) {
    std::cout << bw << "\n";
  }
  */

  /*
  std::transform(std::begin(z), std::end(z), std::begin(p_z), [&](double d) {
    return triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
  });
  */

  TCanvas *c_p = new TCanvas();

  TGraph *g_p = new TGraph(z.size(), z.data(), p_z.data());
  g_p->SetTitle(";Depth (nm);Stopping probability (nm^{-1})");
  g_p->SetMarkerStyle(kFullCircle);
  g_p->SetMarkerColor(kBlack);
  g_p->Draw("AP");

  TCanvas *c_slr = new TCanvas();

  TGraph *g_slr = new TGraph(z.size(), z.data(), slr_rates.data());
  g_slr->SetTitle(";Depth (nm);Model SLR Rate (s^{-1})");
  g_slr->SetMarkerStyle(kFullCircle);
  g_slr->SetMarkerColor(kBlack);
  g_slr->Draw("AP");
  // g_slr->GetYaxis()->SetRangeUser(0, 10);

  // logarithmic scale
  // gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  c_slr->Print("toy_depth_model.pdf", "EmbedFonts");

  // use numeric integration!
  // the "summation" approach (which is error-prone) converges to this answer
  // when the number of "bins" becomes large (e.g., ~1000).
  static boost::math::quadrature::tanh_sinh<double> slr_integrator;
  auto slr_integrand = [&](double d) {
    return triumf::srim::pdf::modified_beta(d, alpha, beta, z_max) *
           slr_fcn(d, GLOBAL_R_0, GLOBAL_D_0);
    /*
    slr_rate_z<double>(d, TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0,
                       EXPONENT, B_0, B_D, NU_C, SLR_C, SLR_N,
                       SURFACE_THICKNESS, SURFACE_RATE);
    */
  };
  double Q = slr_integrator.integrate(slr_integrand, 0.0, z_max);
  std::cout << "weighted_average    = " << weighted_average << "\n";
  std::cout << "numeric_integration = " << Q << "\n";
  double tolerance = std::sqrt(std::numeric_limits<double>::epsilon());
  std::cout << "tolerance           = " << tolerance << "\n";

  DepthAverage<double> da("srim_profile_8li_nb_fitpar.csv");
  // double delta = 0.001;
  std::vector<double> energies =
      triumf::numpy::linspace<double>(da.energy_min(), da.energy_max(), 100);
  std::vector<double> rates;
  std::vector<double> mean_depths;

  auto time_start = std::chrono::steady_clock::now();
  for (auto &e : energies) {
    rates.push_back(da(e));
    mean_depths.push_back(da.z_average(e));
  }
  auto time_stop = std::chrono::steady_clock::now();

  TCanvas *c_r = new TCanvas();

  TGraph *g_r = new TGraph(energies.size(), energies.data(), rates.data());
  g_r->SetMarkerStyle(kFullCircle);
  g_r->SetMarkerColor(kRed);
  g_r->SetTitle(";Energy (keV);Depth averaged SLR rate 1/T_{1} (s^{-1})");

  g_r->Draw("AP");
  // g_r->GetYaxis()->SetRangeUser(0, 10);

  // logarithmic scale
  // gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  c_r->Print("toy_depth_average.pdf", "EmbedFonts");

  TCanvas *c_rz = new TCanvas();

  TGraph *g_rz =
      new TGraph(mean_depths.size(), mean_depths.data(), rates.data());
  g_rz->SetMarkerStyle(kFullCircle);
  g_rz->SetMarkerColor(kRed);
  g_rz->SetTitle(";z_{average} (nm);Depth averaged SLR rate 1/T_{1} (s^{-1})");

  g_rz->Draw("AP");
  // g_rz->GetYaxis()->SetRangeUser(0, 10);

  c_rz->Print("toy_depth_average2.pdf", "EmbedFonts");

  std::chrono::duration<double> elapsed_seconds = time_stop - time_start;
  std::cout << "Elapsed time: " << elapsed_seconds.count() << " (s)\n";
}

#ifndef __CLING__
int main() {
  test_depth_averaging();
  return EXIT_SUCCESS;
}
#endif


#include <iostream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <math.h>

#include "NRGclasses.hpp"

void OneChQS_UpdateQm1fQ(CNRGmatrix &Qm1fNQ,CNRGarray Aeig,
			 CNRGbasisarray Abasis){

  boost::numeric::ublas::matrix<double> Mtemp;
  boost::numeric::ublas::matrix<double> fnw;

  int icount=0;

  Qm1fNQ.ClearAll();

  Qm1fNQ.SyncNRGarray(Aeig);

  // Find blocks such that Q'=Q+1, S'=S+-1/2

  for (int ii=0;ii<Aeig.NumBlocks();ii++)
    {
      double Qi=Aeig.GetQNumber(ii,0);
      double Si=Aeig.GetQNumber(ii,1);

      int Nstibl=Aeig.GetBlockSize(ii);

      for (int jj=ii+1;jj<Aeig.NumBlocks();jj++)
	{

	  double Qj=Aeig.GetQNumber(jj,0);
	  double Sj=Aeig.GetQNumber(jj,1);

	  int Nstjbl=Aeig.GetBlockSize(jj);

	  int kkp=0;
	  //if (Sj==Si+0.5) kkp=23;
	  //if (Sj==Si-0.5) kkp=32;
	  if (fabs(Sj-(Si+0.5))<1.0E-10) kkp=23;
	  if (fabs(Sj-(Si-0.5))<1.0E-10) kkp=32;

	  //	  if ( (Qj==Qi+1.0)&&( kkp!=0 ) )
	  if ( ( fabs(Qj-(Qi+1.0))<1.0E-10 )&&( kkp!=0 ) )
	    {

	      cout << "  Setting up Block i : " << ii
		   << " (size " <<  Nstibl
		   << ") x Block j " << jj
		   << " (size " << Nstjbl << ")" << endl;

 	      Qm1fNQ.MatBlockMap.push_back(ii);
 	      Qm1fNQ.MatBlockMap.push_back(jj);
	      Qm1fNQ.MatBlockBegEnd.push_back(icount);
	      icount+=Nstibl*Nstjbl;
	      Qm1fNQ.MatBlockBegEnd.push_back(icount-1);

	      // Sets the uBLAS matrix for Z_QS(iblock)
	      // Sets the uBLAS matrix for Z_QS(jblock)

	      cout << "    Setting up BLAS matrices..." << endl;
// 	      boost::numeric::ublas::matrix<double> Zibl=Aeig.EigVec2BLAS(ii);
// 	      boost::numeric::ublas::matrix<double> Zjbl=Aeig.EigVec2BLAS(jj);

	      boost::numeric::ublas::matrix<double> Zibl(Nstibl,Nstibl);
	      boost::numeric::ublas::matrix<double> Zjbl(Nstjbl,Nstjbl);
	      Zibl=Aeig.EigVec2BLAS(ii);
	      Zjbl=Aeig.EigVec2BLAS(jj);

	      cout << "    ...Zs done ..." << endl;
	      // Sets the uBLAS matrix <type|fn|type^'> with a bunch of zeroes

	      boost::numeric::ublas::matrix<double> fnbasis (Nstibl,Nstjbl);

	      // Check time
	      boost::timer t;
	      double time_elapsed;

	      // Loop on each block
	      int istbl=0;
	      for (int ist=Abasis.GetBlockLimit(ii,0);
		   ist<=Abasis.GetBlockLimit(ii,1);ist++)
		{
		  int typei=Abasis.iType[ist];
		  int stcfi=Abasis.StCameFrom[ist];
		  //cout << "Type i : " << typei << endl;
		  int jstbl=0;
		  for (int jst=Abasis.GetBlockLimit(jj,0);
		       jst<=Abasis.GetBlockLimit(jj,1);jst++)
		    {
		      int typej=Abasis.iType[jst];
		      int stcfj=Abasis.StCameFrom[jst];
		      //cout << "Type j : " << typej << endl;
		      // setting MatBlockMap
//  		      Qm1fNQ.MatBlockMap.push_back(ii);
//  		      Qm1fNQ.MatBlockMap.push_back(jj);

		      fnbasis(istbl,jstbl)=0.0;

		      // Both basis sates have to originate from the same
		      // previous state! Otherwise, zero.

		      if (stcfi==stcfj)
			{
			  if (
			      ( (typei==1) && (typej==2) &&(kkp==23) )||
			      ( (typei==1) && (typej==3) &&(kkp==32) )
			      )
			    fnbasis(istbl,jstbl)=1.0;
			  if ( (typej==4)&&(typei==2)&&(kkp==32) )
			    fnbasis(istbl,jstbl)=-sqrt((2.0*Si+1.0)/(2.0*Si));
			  if ( (typej==4)&&(typei==3)&&(kkp==23) )
			    fnbasis(istbl,jstbl)=sqrt((2.0*Si+1.0)/(2.0*Si+2.0));
			}
		      //cout << "fn(" << istbl << "," << jstbl << ") = ";
		      //cout << fnbasis(istbl,jstbl) << endl;

		      jstbl++;
		    }
		  istbl++;

		}

	      cout << "    ...fnbasis done." << endl;

	      // Multiply the 3 of them. Zi has eigvecs in ROWS!

	      cout << "    Multiplying BLAS matrices... " << endl;

//  	      boost::numeric::ublas::matrix<double> Mtemp=prod(fnbasis,trans(Zjbl));
//  	      boost::numeric::ublas::matrix<double> fnw=prod (Zibl,Mtemp);
// 	      fnw=prod (Zibl,Mtemp);

	      //Mtemp.resize(Nstibl,Nstjbl);
	      fnw.resize(Nstibl,Nstjbl);
 	      //noalias(Mtemp)=prod(fnbasis,trans(Zjbl));
	      //cout << "    ...one done..." << endl;
 	      //noalias(fnw)=prod (Zibl, Mtemp );

	      t.restart();
 	      noalias(fnw)=prod (Zibl,
			boost::numeric::ublas::matrix<double>(prod(fnbasis,trans(Zjbl))) );
	      time_elapsed=t.elapsed();
	      cout << "    ...done. Elapsed time:" << time_elapsed << endl;

//   	      cout << "Z(i="<<ii<<")  : " <<  Zibl << endl;
//   	      cout << "Z(j="<<jj<<")  : " <<  Zjbl << endl;
//    	      cout << "fbasis   :" <<  fnbasis << endl;
//    	      cout << "Zi.fbasis.ZjT : " <<  fnw << endl;

	      // Add to Qm1fNQ
	      for (int ii=0;ii<fnw.size1();ii++)
		{
		  for (int jj=0;jj<fnw.size2();jj++)
		    {
		      Qm1fNQ.MatEl.push_back(fnw(ii,jj));
		    }
		}

	    }
	}
      //Loop in jblock
    }
  // Loop in iblock

//    cout << "Qm1fNQ MatBlockMap : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockMap.size();ii++)
//      cout << Qm1fNQ.MatBlockMap[ii] << " ";
//    cout << endl;

// Too slow.
//     Qm1fNQ.FilterMap_SetBegEnd();

//    cout << "Filtered Qm1fNQ MatBlockMap : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockMap.size();ii++)
//      cout << Qm1fNQ.MatBlockMap[ii] << " ";
//    cout << endl;
//    cout << "Qm1fNQ MatBlockBegEnd : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockBegEnd.size();ii++)
//      cout << Qm1fNQ.MatBlockBegEnd[ii] << " ";
//    cout << endl;

//   for (int ibl=0;ibl<Qm1fNQ.NumMatBlocks();ibl++)
//     {
//       Qm1fNQ.PrintMatBlock(ibl);
//     }

}


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G.Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "closed_form_calibration.h"

#include "g2o/types/sclam2d/odometry_measurement.h"

#include <iostream>
#include <limits>

#include <Eigen/SVD>

#define SQR(X)		( std::pow(X,2) )
#define CUBE(X)		( std::pow(X,3) )

namespace g2o {

bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
{
  std::vector<VelocityMeasurement, Eigen::aligned_allocator<VelocityMeasurement> > velMeasurements;
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& odomMotion = measurements[i].odomMotion;
    const double& timeInterval = measurements[i].timeInterval;
    MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
    VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
    velMeasurements.push_back(velMeas);
  }

  double J_21, J_22;
  {
    Eigen::MatrixXd A(measurements.size(), 2);
    Eigen::VectorXd x(measurements.size());
    for (size_t i = 0; i < measurements.size(); ++i) {
      const SE2& laserMotion = measurements[i].laserMotion;
      const double& timeInterval = measurements[i].timeInterval;
      const VelocityMeasurement& velMeas = velMeasurements[i];
      A(i, 0) = velMeas.vl() * timeInterval;
      A(i, 1) = velMeas.vr() * timeInterval;
      x(i) = laserMotion.rotation().angle();
    }
    // (J_21, J_22) = (-r_l / b, r_r / b)
    Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
    //std::cout << linearSolution.transpose() << std::endl;
    J_21 = linearSolution(0);
    J_22 = linearSolution(1);
  }

  // construct M
  Eigen::Matrix<double, 5, 5> M;
  M.setZero();
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& laserMotion = measurements[i].laserMotion;
    const double& timeInterval = measurements[i].timeInterval;
    const VelocityMeasurement& velMeas = velMeasurements[i];
    Eigen::Matrix<double, 2, 5> L;
    double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
    double o_theta_k = omega_o_k * timeInterval;
    double sx = 1.;
    double sy = 0.;
    if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
      sx = sin(o_theta_k)       / o_theta_k;
      sy = (1 - cos(o_theta_k)) / o_theta_k;
    }
    double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
    double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
    L(0, 0) = -c_x;
    L(0, 1) = 1 - cos(o_theta_k);
    L(0, 2) = sin(o_theta_k);
    L(0, 3) = laserMotion.translation().x();
    L(0, 4) = -laserMotion.translation().y();
    L(1, 0) = -c_y;
    L(1, 1) = - sin(o_theta_k);
    L(1, 2) = 1 - cos(o_theta_k);
    L(1, 3) = laserMotion.translation().y();
    L(1, 4) = laserMotion.translation().x();
    M.noalias() += L.transpose() * L;
  }
  //std::cout << M << std::endl;

  // compute lagrange multiplier
  // and solve the constrained least squares problem
  double m11 = M(0,0);
  double m13 = M(0,2);
  double m14 = M(0,3);
  double m15 = M(0,4);
  double m22 = M(1,1);
  double m34 = M(2,3);
  double m35 = M(2,4);
  double m44 = M(3,3);

  double a = m11 * SQR(m22) - m22 * SQR(m13);
  double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34)
    - 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34)
    + 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
  double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
    + 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
    - 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
    - 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
    + m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
    - SQR(m22) * SQR(m15) * m44;

  // solve the quadratic equation
  double lambda1, lambda2;
  if(a < std::numeric_limits<double>::epsilon()) {
    if(b <= std::numeric_limits<double>::epsilon())
      return false;
    lambda1 = lambda2 = -c/b;
  } else {
    double delta = b*b - 4*a*c;
    if (delta < 0)
      return false;
    lambda1 = 0.5 * (-b-sqrt(delta)) / a;
    lambda2 = 0.5 * (-b+sqrt(delta)) / a;
  }

  Eigen::VectorXd x1 = solveLagrange(M, lambda1);
  Eigen::VectorXd x2 = solveLagrange(M, lambda2);
  double err1 = x1.dot(M * x1);
  double err2 = x2.dot(M * x2);

  const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
  odomParams(0) = - calibrationResult(0) * J_21;
  odomParams(1) = calibrationResult(0) * J_22;
  odomParams(2) = calibrationResult(0);

  laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));

  return true;
}

Eigen::VectorXd ClosedFormCalibration::solveLagrange(const Eigen::Matrix<double,5,5>& M, double lambda)
{
  // A = M * lambda*W (see paper)
  Eigen::Matrix<double,5,5> A;
  A.setZero();
  A(3,3) = A(4,4) = lambda;
  A.noalias() += M;

  // compute the kernel of A by SVD
  Eigen::JacobiSVD< Eigen::Matrix<double,5,5> > svd(A, ComputeFullV);
  Eigen::VectorXd result = svd.matrixV().col(4);
  //for (int i = 0; i < 5; ++i)
  //std::cout << "singular value " << i << " "  << svd.singularValues()(i) << std::endl;
  //std::cout << "kernel base " << result << std::endl;

  // enforce the conditions
  // x_1 > 0
  if (result(0) < 0.)
    result *= -1;
  // x_4^2 + x_5^2 = 1
  double scale = sqrt(pow(result(3), 2) + pow(result(4), 2));
  result /= scale;

  return result;
}

} // end namespace