//  (C) Copyright John Maddock 2005-2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_FRACTION_INCLUDED
#define BOOST_MATH_TOOLS_FRACTION_INCLUDED

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/cstdint.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/mpl/if.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{ namespace math{ namespace tools{

namespace detail
{

   template <class T>
   struct is_pair : public boost::false_type{};

   template <class T, class U>
   struct is_pair<std::pair<T,U> > : public boost::true_type{};

   template <class Gen>
   struct fraction_traits_simple
   {
       typedef typename Gen::result_type result_type;
       typedef typename Gen::result_type value_type;

       static result_type a(const value_type& v)
       {
          return 1;
       }
       static result_type b(const value_type& v)
       {
          return v;
       }
   };

   template <class Gen>
   struct fraction_traits_pair
   {
       typedef typename Gen::result_type value_type;
       typedef typename value_type::first_type result_type;

       static result_type a(const value_type& v)
       {
          return v.first;
       }
       static result_type b(const value_type& v)
       {
          return v.second;
       }
   };

   template <class Gen>
   struct fraction_traits
       : public boost::mpl::if_c<
         is_pair<typename Gen::result_type>::value,
         fraction_traits_pair<Gen>,
         fraction_traits_simple<Gen> >::type
   {
   };

} // namespace detail

//
// continued_fraction_b
// Evaluates:
//
// b0 +       a1
//      ---------------
//      b1 +     a2
//           ----------
//           b2 +   a3
//                -----
//                b3 + ...
//
// Note that the first a0 returned by generator Gen is disarded.
//
template <class Gen>
typename detail::fraction_traits<Gen>::result_type continued_fraction_b(Gen& g, int bits)
{
   BOOST_MATH_STD_USING // ADL of std names

   typedef detail::fraction_traits<Gen> traits;
   typedef typename traits::result_type result_type;
   typedef typename traits::value_type value_type;

   result_type factor = ldexp(1.0f, 1 - bits); // 1 / pow(result_type(2), bits);
   result_type tiny = tools::min_value<result_type>();

   value_type v = g();

   result_type f, C, D, delta;
   f = traits::b(v);
   if(f == 0)
      f = tiny;
   C = f;
   D = 0;

   do{
      v = g();
      D = traits::b(v) + traits::a(v) * D;
      if(D == 0)
         D = tiny;
      C = traits::b(v) + traits::a(v) / C;
      if(C == 0)
         C = tiny;
      D = 1/D;
      delta = C*D;
      f = f * delta;
   }while(fabs(delta - 1) > factor);

   return f;
}

template <class Gen>
typename detail::fraction_traits<Gen>::result_type continued_fraction_b(Gen& g, int bits, boost::uintmax_t& max_terms)
{
   BOOST_MATH_STD_USING // ADL of std names

   typedef detail::fraction_traits<Gen> traits;
   typedef typename traits::result_type result_type;
   typedef typename traits::value_type value_type;

   result_type factor = ldexp(1.0f, 1 - bits); // 1 / pow(result_type(2), bits);
   result_type tiny = tools::min_value<result_type>();

   value_type v = g();

   result_type f, C, D, delta;
   f = traits::b(v);
   if(f == 0)
      f = tiny;
   C = f;
   D = 0;

   boost::uintmax_t counter(max_terms);

   do{
      v = g();
      D = traits::b(v) + traits::a(v) * D;
      if(D == 0)
         D = tiny;
      C = traits::b(v) + traits::a(v) / C;
      if(C == 0)
         C = tiny;
      D = 1/D;
      delta = C*D;
      f = f * delta;
   }while((fabs(delta - 1) > factor) && --counter);

   max_terms = max_terms - counter;

   return f;
}

//
// continued_fraction_a
// Evaluates:
//
//            a1
//      ---------------
//      b1 +     a2
//           ----------
//           b2 +   a3
//                -----
//                b3 + ...
//
// Note that the first a1 and b1 returned by generator Gen are both used.
//
template <class Gen>
typename detail::fraction_traits<Gen>::result_type continued_fraction_a(Gen& g, int bits)
{
   BOOST_MATH_STD_USING // ADL of std names

   typedef detail::fraction_traits<Gen> traits;
   typedef typename traits::result_type result_type;
   typedef typename traits::value_type value_type;

   result_type factor = ldexp(1.0f, 1-bits); // 1 / pow(result_type(2), bits);
   result_type tiny = tools::min_value<result_type>();

   value_type v = g();

   result_type f, C, D, delta, a0;
   f = traits::b(v);
   a0 = traits::a(v);
   if(f == 0)
      f = tiny;
   C = f;
   D = 0;

   do{
      v = g();
      D = traits::b(v) + traits::a(v) * D;
      if(D == 0)
         D = tiny;
      C = traits::b(v) + traits::a(v) / C;
      if(C == 0)
         C = tiny;
      D = 1/D;
      delta = C*D;
      f = f * delta;
   }while(fabs(delta - 1) > factor);

   return a0/f;
}

template <class Gen>
typename detail::fraction_traits<Gen>::result_type continued_fraction_a(Gen& g, int bits, boost::uintmax_t& max_terms)
{
   BOOST_MATH_STD_USING // ADL of std names

   typedef detail::fraction_traits<Gen> traits;
   typedef typename traits::result_type result_type;
   typedef typename traits::value_type value_type;

   result_type factor = ldexp(1.0f, 1-bits); // 1 / pow(result_type(2), bits);
   result_type tiny = tools::min_value<result_type>();

   value_type v = g();

   result_type f, C, D, delta, a0;
   f = traits::b(v);
   a0 = traits::a(v);
   if(f == 0)
      f = tiny;
   C = f;
   D = 0;

   boost::uintmax_t counter(max_terms);

   do{
      v = g();
      D = traits::b(v) + traits::a(v) * D;
      if(D == 0)
         D = tiny;
      C = traits::b(v) + traits::a(v) / C;
      if(C == 0)
         C = tiny;
      D = 1/D;
      delta = C*D;
      f = f * delta;
   }while((fabs(delta - 1) > factor) && --counter);

   max_terms = max_terms - counter;

   return a0/f;
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_FRACTION_INCLUDED


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   GaussianBayesNet.cpp
 * @brief  Chordal Bayes Net, the result of eliminating a factor graph
 * @author Frank Dellaert, Varun Agrawal
 */

#include <gtsam/base/timing.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>

#include <boost/range/adaptor/reversed.hpp>
#include <fstream>

using namespace std;
using namespace gtsam;

// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);

namespace gtsam {

  // Instantiate base class
  template class FactorGraph<GaussianConditional>;

  /* ************************************************************************* */
  bool GaussianBayesNet::equals(const This& bn, double tol) const
  {
    return Base::equals(bn, tol);
  }

  /* ************************************************************************ */
  VectorValues GaussianBayesNet::optimize() const {
    VectorValues solution;  // no missing variables -> create an empty vector
    return optimize(solution);
  }

  VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
    // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
    // solve each node in reverse topological sort order (parents first)
    for (auto cg : boost::adaptors::reverse(*this)) {
      // i^th part of R*x=y, x=inv(R)*y
      // (Rii*xi + R_i*x(i+1:))./si = yi =>
      // xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
      solution.insert(cg->solve(solution));
    }
    return solution;
  }

  /* ************************************************************************ */
  VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const {
    VectorValues result;  // no missing variables -> create an empty vector
    return sample(result, rng);
  }

  VectorValues GaussianBayesNet::sample(VectorValues result,
                                        std::mt19937_64* rng) const {
    // sample each node in reverse topological sort order (parents first)
    for (auto cg : boost::adaptors::reverse(*this)) {
      const VectorValues sampled = cg->sample(result, rng);
      result.insert(sampled);
    }
    return result;
  }

  /* ************************************************************************ */
  VectorValues GaussianBayesNet::sample() const {
    return sample(&kRandomNumberGenerator);
  }

  VectorValues GaussianBayesNet::sample(VectorValues given) const {
    return sample(given, &kRandomNumberGenerator);
  }

  /* ************************************************************************ */
  VectorValues GaussianBayesNet::optimizeGradientSearch() const
  {
    gttic(GaussianBayesTree_optimizeGradientSearch);
    return GaussianFactorGraph(*this).optimizeGradientSearch();
  }

  /* ************************************************************************* */
  VectorValues GaussianBayesNet::gradient(const VectorValues& x0) const {
    return GaussianFactorGraph(*this).gradient(x0);
  }

  /* ************************************************************************* */
  VectorValues GaussianBayesNet::gradientAtZero() const {
    return GaussianFactorGraph(*this).gradientAtZero();
  }

  /* ************************************************************************* */
  double GaussianBayesNet::error(const VectorValues& x) const {
    return GaussianFactorGraph(*this).error(x);
  }

  /* ************************************************************************* */
  VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const
  {
    VectorValues result;
    // TODO this looks pretty sketchy. result is passed as the parents argument
    //  as it's filled up by solving the gaussian conditionals.
    for (auto cg: boost::adaptors::reverse(*this)) {
      result.insert(cg->solveOtherRHS(result, rhs));
    }
    return result;
  }

  /* ************************************************************************* */
  // gy=inv(L)*gx by solving L*gy=gx.
  // gy=inv(R'*inv(Sigma))*gx
  // gz'*R'=gx', gy = gz.*sigmas
  VectorValues GaussianBayesNet::backSubstituteTranspose(const VectorValues& gx) const
  {
    // Initialize gy from gx
    // TODO: used to insert zeros if gx did not have an entry for a variable in bn
    VectorValues gy = gx;

    // we loop from first-eliminated to last-eliminated
    // i^th part of L*gy=gx is done block-column by block-column of L
    for(const sharedConditional& cg: *this)
      cg->solveTransposeInPlace(gy);

    return gy;
  }

  ///* ************************************************************************* */
  //VectorValues GaussianBayesNet::optimizeGradientSearch() const
  //{
  //  gttic(Compute_Gradient);
  //  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
  //  VectorValues grad = gradientAtZero();
  //  double gradientSqNorm = grad.dot(grad);
  //  gttoc(Compute_Gradient);

  //  gttic(Compute_Rg);
  //  // Compute R * g
  //  Errors Rg = GaussianFactorGraph(*this) * grad;
  //  gttoc(Compute_Rg);

  //  gttic(Compute_minimizing_step_size);
  //  // Compute minimizing step size
  //  double step = -gradientSqNorm / dot(Rg, Rg);
  //  gttoc(Compute_minimizing_step_size);

  //  gttic(Compute_point);
  //  // Compute steepest descent point
  //  scal(step, grad);
  //  gttoc(Compute_point);

  //  return grad;
  //}

  /* ************************************************************************* */
  Ordering GaussianBayesNet::ordering() const {
    GaussianFactorGraph factorGraph(*this);
    auto keys = factorGraph.keys();
    // add frontal keys in order
    Ordering ordering;
    for (const sharedConditional& cg : *this)
      if (cg) {
        for (Key key : cg->frontals()) {
          ordering.push_back(key);
          keys.erase(key);
        }
      }
    // add remaining keys in case Bayes net is incomplete
    for (Key key : keys) ordering.push_back(key);
    return ordering;
  }

  /* ************************************************************************* */
  pair<Matrix, Vector> GaussianBayesNet::matrix(const Ordering& ordering) const {
    // Convert to a GaussianFactorGraph and use its machinery
    GaussianFactorGraph factorGraph(*this);
    return factorGraph.jacobian(ordering);
  }

  /* ************************************************************************* */
  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
    // recursively call with default ordering
    const auto defaultOrdering = this->ordering();
    return matrix(defaultOrdering);
  }

  ///* ************************************************************************* */
  //VectorValues GaussianBayesNet::gradient(const VectorValues& x0) const
  //{
  //  return GaussianFactorGraph(*this).gradient(x0);
  //}

  ///* ************************************************************************* */
  //VectorValues GaussianBayesNet::gradientAtZero() const
  //{
  //  return GaussianFactorGraph(*this).gradientAtZero();
  //}

  /* ************************************************************************* */
  double GaussianBayesNet::determinant() const
  {
    return exp(logDeterminant());
  }

  /* ************************************************************************* */
  double GaussianBayesNet::logDeterminant() const {
    double logDet = 0.0;
    for (const sharedConditional& cg : *this) {
      if (cg->get_model()) {
        Vector diag = cg->R().diagonal();
        cg->get_model()->whitenInPlace(diag);
        logDet += diag.unaryExpr([](double x) { return log(x); }).sum();
      } else {
        logDet +=
            cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
      }
    }
    return logDet;
  }

  /* ************************************************************************* */

} // namespace gtsam


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestInteropEigen
#include <boost/test/unit_test.hpp>

#include <boost/compute/closure.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/interop/eigen.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bcl = boost::compute;

BOOST_AUTO_TEST_CASE(eigen)
{
    Eigen::MatrixXf mat(3, 3);
    mat << 1, 2, 3,
           6, 5, 4,
           7, 8, 9;

    // copy matrix to gpu buffer
    bcl::vector<float> vec(9, context);
    bcl::eigen_copy_matrix_to_buffer(mat, vec.begin(), queue);
    CHECK_RANGE_EQUAL(float, 9, vec, (1, 6, 7, 2, 5, 8, 3, 4, 9));

    // transpose matrix and then copy to gpu buffer
    mat = mat.transpose().eval();
    bcl::eigen_copy_matrix_to_buffer(mat, vec.begin(), queue);
    CHECK_RANGE_EQUAL(float, 9, vec, (1, 2, 3, 6, 5, 4, 7, 8, 9));

    // set matrix to zero and copy data back from gpu buffer
    mat.setZero();
    bcl::eigen_copy_buffer_to_matrix(vec.begin(), mat, queue);
    BOOST_CHECK(mat.isZero() == false);
    BOOST_CHECK_EQUAL(mat.sum(), 45);
}

BOOST_AUTO_TEST_CASE(eigen_types)
{
    BOOST_CHECK(std::strcmp(bcl::type_name<Eigen::Vector2i>(), "int2") == 0);
    BOOST_CHECK(std::strcmp(bcl::type_name<Eigen::Vector2f>(), "float2") == 0);
    BOOST_CHECK(std::strcmp(bcl::type_name<Eigen::Vector4f>(), "float4") == 0);
    BOOST_CHECK(std::strcmp(bcl::type_name<Eigen::Vector4d>(), "double4") == 0);
}

BOOST_AUTO_TEST_CASE(multiply_matrix4)
{
    std::vector<Eigen::Vector4f> host_vectors;
    std::vector<Eigen::Matrix4f> host_matrices;

    Eigen::Matrix4f matrix;
    matrix << 1, 2, 0, 3,
              2, 1, 2, 0,
              0, 3, 1, 2,
              2, 0, 2, 1;

    host_vectors.push_back(Eigen::Vector4f(1, 2, 3, 4));
    host_vectors.push_back(Eigen::Vector4f(4, 3, 2, 1));
    host_vectors.push_back(Eigen::Vector4f(1, 2, 3, 4));
    host_vectors.push_back(Eigen::Vector4f(4, 3, 2, 1));

    // store the eigen 4x4 matrix as a float16
    bcl::float16_ M =
        bcl::eigen_matrix4f_to_float16(matrix);

    // returns the result of M*x
    BOOST_COMPUTE_CLOSURE(Eigen::Vector4f, transform4x4, (const Eigen::Vector4f x), (M),
    {
        float4 r;
        r.x = dot(M.s048c, x);
        r.y = dot(M.s159d, x);
        r.z = dot(M.s26ae, x);
        r.w = dot(M.s37bf, x);
        return r;
    });

    bcl::vector<Eigen::Vector4f> vectors(4, context);
    bcl::vector<Eigen::Vector4f> results(4, context);

    bcl::copy(host_vectors.begin(), host_vectors.end(), vectors.begin(), queue);

    bcl::transform(
        vectors.begin(), vectors.end(), results.begin(), transform4x4, queue
    );

    std::vector<Eigen::Vector4f> host_results(4);
    bcl::copy(results.begin(), results.end(), host_results.begin(), queue);

    BOOST_CHECK((matrix * host_vectors[0]) == host_results[0]);
    BOOST_CHECK((matrix * host_vectors[1]) == host_results[1]);
    BOOST_CHECK((matrix * host_vectors[2]) == host_results[2]);
    BOOST_CHECK((matrix * host_vectors[3]) == host_results[3]);
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 5, Eigen::Dynamic> test_long_5_D(const Eigen::Matrix<boost::int64_t, 5, Eigen::Dynamic> & M)
{
	return M;
}
void export_long_5_D()
{
	boost::python::def("test_long_5_D",test_long_5_D);
}


// Copyright 2004 The Trustees of Indiana University.

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef _ALG_CTX_CLSC_H
#define _ALG_CTX_CLSC_H

#include <boost/graph/exterior_property.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <iostream>

#include "ctx_bfs_visitor.hpp"
using namespace boost;

namespace nctx {

  template <typename G, typename Param, typename Tag, typename Rest,
    typename DistanceMap,
    typename WeightMap,
    typename PredecessorMap,
    typename DecisionFunction>
  inline void closeness_centrality_ctx_dispatch(const G& g,
                                typename graph_traits<G>::vertex_descriptor start,
                                const bgl_named_params<Param,Tag,Rest>& params,
                                DistanceMap &distancemap,
                                WeightMap &weightmap,
                                PredecessorMap &pred,
                                typename property_traits<PredecessorMap>::value_type pred_undef,
                                DecisionFunction &clsn_decision_fct){
    //~ auto distance_map = b::get_param(params, b::vertex_distance);
    //~ auto weight_map = b::get_param(params, b::edge_weight);
    //~ auto distance_inf = b::get_param(params, b::distance_inf_t());

    typedef context_dijkstra_visitor<G, DistanceMap, WeightMap, PredecessorMap> visitor_type;

    // can't use size_t inf here. this produces uncorrect centrality values.
    // using the number of vertices is the logical inf here
    visitor_type visitor(start, distancemap, weightmap, clsn_decision_fct, pred, pred_undef, typename property_traits<DistanceMap>::value_type(num_vertices(g)));

    boost::dijkstra_shortest_paths_no_color_map(g, start,
        params
        .visitor(visitor));
  }

  // DocString: closenness_ctx
/**
 * @brief Closeness centrality with dynamic contextual constraints.
 *
 * Using this function allows obtaining closeness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.
 *
 * The function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the centrality calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.
 *
 * The three nodes are passed as indices allowing for access of (external) attribute and other associated information.
 *
 * Note that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.
 *
 * If the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.
 *
 * @param g The graph object
 * @param clsn_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.
 *
 */
  template <typename Graph, typename Param, typename Tag, typename Rest, typename DecisionFunction>
  void closeness_centrality_ctx(const Graph& g,
                                const bgl_named_params<Param,Tag,Rest>& params,
                                DecisionFunction &clsn_decision_fct){
    typedef exterior_vertex_property<Graph, int> DistanceProperty;
    typedef typename DistanceProperty::matrix_type DistanceMatrix;
    typedef typename DistanceProperty::matrix_map_type DistanceMatrixMap;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    DistanceMatrix distances_matr(b::num_vertices(g));
    DistanceMatrixMap dm(distances_matr, g);

    auto edge_weight_map = choose_const_pmap(get_param(params, edge_weight), g, edge_weight);

    const int n = b::num_vertices(g);

    for(auto start : make_iterator_range(vertices(g))){
      std::vector<size_t> distances(n, 0);

      auto distance_map = make_iterator_property_map(distances.begin(), choose_param(get_param(params, vertex_index), dummy_property_map()));
      auto dummy_pred = dummy_property_map();

      closeness_centrality_ctx_dispatch(g, start,
        params
        .distance_map(distance_map),
        distance_map,
        edge_weight_map,
        dummy_pred,
        0,
        clsn_decision_fct);

      for(auto u : make_iterator_range(vertices(g))){
        //~ std::cout << start << " -> " << u << ": " << distances[u] << std::endl;
        dm[start][u] = distances[u];
      }
    }

    typedef exterior_vertex_property<Graph, float> ClosenessProperty;
    typedef typename ClosenessProperty::container_type ClosenessContainer;
    //~ typedef typename ClosenessProperty::map_type ClosenessMap;
    ClosenessContainer cents(n);
    //~ ClosenessMap cm(cents, g);
    all_closeness_centralities(g, dm, choose_param(get_param(params, vertex_centrality), dummy_property_map()));
  }

} // end namespace boost

#endif // BOOST_GRAPH_BRANDES_BETWEENNESS_CENTRALITY_HPP


//
// boost-geometry-render
//
// Copyright (c) 2011
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <cstdlib>
#include <gl/glut.h>
#include <gl/graphics.hpp>
#include <gl/geometry_render.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/thread.hpp>
#include <boost/phoenix.hpp>

namespace bg = boost::geometry;

template<typename Geometry>
struct geometry_renderer{
	geometry_renderer(Geometry& geometry) : geometry_(geometry){};
	void operator ()(){
		glColor3f(1.0f, 0.0f, 0.0f);
		glPointSize(5.0f);
		gl::render(geometry_);
	}
private:
	Geometry& geometry_;
};

template<typename Geometry>
geometry_renderer<Geometry>
make_geometry_renderer(Geometry& geometry){
	return geometry_renderer<Geometry>(geometry);
}

template<typename Geometry>
struct point_adder{
	point_adder(Geometry& geometry) : geometry_(geometry){};
	void operator ()(int button, int state, int x, int y){
		bg::point_type<Geometry>::type point(x / 250.0f - 1.0f, y / -250.0f + 1.0f);
		geometry_.push_back(point);
	}
private:
	Geometry& geometry_;
};

template<typename Geometry>
point_adder<Geometry>
make_point_adder(Geometry& geometry){
	return point_adder<Geometry>(geometry);
}

void
idle(){
	boost::this_thread::sleep(boost::posix_time::milliseconds(16));
	glutPostRedisplay();
}

int
main(int argc, char* argv[]){

	typedef bg::model::d2::point_xy<float> point_type;
	typedef bg::model::multi_point<point_type> multi_point_type;
	multi_point_type multi_point;

	gl::graphics g(argc, argv, 500, 500);
	gl::displayfunc(g, make_geometry_renderer(multi_point));
	gl::idlefunc(g, idle);
	gl::mousefunc(g, make_point_adder(multi_point));
	g.run();

	return 0;
}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP

#include <boost/assert.hpp>
#include <boost/core/nvp.hpp>
#include <boost/histogram/fwd.hpp> // for mean<>
#include <boost/throw_exception.hpp>
#include <stdexcept>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/** Calculates mean and variance of sample.

  Uses Welfords's incremental algorithm to improve the numerical
  stability of mean and variance computation.
*/
template <class RealType>
class mean {
public:
  mean() = default;
  mean(const RealType& n, const RealType& mean, const RealType& variance) noexcept
      : sum_(n), mean_(mean), sum_of_deltas_squared_(variance * (n - 1)) {}

  void operator()(const RealType& x) noexcept {
    sum_ += static_cast<RealType>(1);
    const auto delta = x - mean_;
    mean_ += delta / sum_;
    sum_of_deltas_squared_ += delta * (x - mean_);
  }

  void operator()(const weight_type<RealType>& w, const RealType& x) noexcept {
    sum_ += w.value;
    const auto delta = x - mean_;
    mean_ += w.value * delta / sum_;
    sum_of_deltas_squared_ += w.value * delta * (x - mean_);
  }

  template <class T>
  mean& operator+=(const mean<T>& rhs) noexcept {
    if (sum_ != 0 || rhs.sum_ != 0) {
      const auto tmp = mean_ * sum_ + static_cast<RealType>(rhs.mean_ * rhs.sum_);
      sum_ += rhs.sum_;
      mean_ = tmp / sum_;
    }
    sum_of_deltas_squared_ += static_cast<RealType>(rhs.sum_of_deltas_squared_);
    return *this;
  }

  mean& operator*=(const RealType& s) noexcept {
    mean_ *= s;
    sum_of_deltas_squared_ *= s * s;
    return *this;
  }

  template <class T>
  bool operator==(const mean<T>& rhs) const noexcept {
    return sum_ == rhs.sum_ && mean_ == rhs.mean_ &&
           sum_of_deltas_squared_ == rhs.sum_of_deltas_squared_;
  }

  template <class T>
  bool operator!=(const mean<T>& rhs) const noexcept {
    return !operator==(rhs);
  }

  const RealType& count() const noexcept { return sum_; }
  const RealType& value() const noexcept { return mean_; }
  RealType variance() const noexcept { return sum_of_deltas_squared_ / (sum_ - 1); }

  template <class Archive>
  void serialize(Archive& ar, unsigned version) {
    if (version == 0) {
      // read only
      std::size_t sum;
      ar& make_nvp("sum", sum);
      sum_ = static_cast<RealType>(sum);
    } else {
      ar& make_nvp("sum", sum_);
    }
    ar& make_nvp("mean", mean_);
    ar& make_nvp("sum_of_deltas_squared", sum_of_deltas_squared_);
  }

private:
  RealType sum_ = 0, mean_ = 0, sum_of_deltas_squared_ = 0;
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED

namespace boost {
namespace serialization {

template <class T>
struct version;

// version 1 for boost::histogram::accumulators::mean<RealType>
template <class RealType>
struct version<boost::histogram::accumulators::mean<RealType>>
    : std::integral_constant<int, 1> {};

} // namespace serialization
} // namespace boost

namespace std {
template <class T, class U>
/// Specialization for boost::histogram::accumulators::mean.
struct common_type<boost::histogram::accumulators::mean<T>,
                   boost::histogram::accumulators::mean<U>> {
  using type = boost::histogram::accumulators::mean<common_type_t<T, U>>;
};
} // namespace std

#endif

#endif


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <ITMLib/Utils/ITMMath.h>

#include <tvgutil/misc/AttitudeUtil.h>
using namespace tvgutil;

//#################### HELPER FUNCTIONS ####################

void check_close(float a, float b, float TOL)
{
  BOOST_CHECK_CLOSE(a, b, TOL);
}

void check_close(const float *v1, const float *v2, size_t size, float TOL)
{
  for(size_t i = 0; i < size; ++i) check_close(v1[i], v2[i], TOL);
}

void check_close(const Vector3f& v1, const Vector3f& v2, float TOL)
{
  check_close(v1.v, v2.v, v1.size(), TOL);
}

void check_close(const Vector4f& v1, const Vector4f& v2, float TOL)
{
  check_close(v1.v, v2.v, v1.size(), TOL);
}

void check_close(const Matrix3f& m1, const Matrix3f& m2, float TOL)
{
  check_close(m1.m, m2.m, 9, TOL);
}

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_AttitudeUtil)

BOOST_AUTO_TEST_CASE(test_corner_cases)
{
  const float TOL = 1e-4f;

  float angle = 0.0f;
  Vector3f axis(1.0f, 0.0f, 0.0f);

  Vector3f rotationVector;
  AttitudeUtil::axis_angle_to_rotation_vector(axis.v, &angle, rotationVector.v);
  check_close(rotationVector, Vector3f(0.0f, 0.0f, 0.0f), TOL);

  {
    float angle;
    Vector3f axis;
    AttitudeUtil::rotation_vector_to_axis_angle(rotationVector.v, axis.v, &angle);

    // By convention the zero rotation vector corresponds to a zero rotation around the x-axis.
    check_close(angle, 0.0f, TOL);
    check_close(axis, Vector3f(1.0f, 0.0f, 0.0f), TOL);
  }

  Vector4f q;
  AttitudeUtil::rotation_vector_to_quaternion(rotationVector.v, q.v);
  check_close(q, Vector4f(1.0f, 0.0f, 0.0f, 0.0f), TOL);

  {
    float angle;
    Vector3f axis;
    AttitudeUtil::quaternion_to_axis_angle(q.v, axis.v, &angle);
    check_close(angle, 0.0f, TOL);
    check_close(axis, Vector3f(1.0f, 0.0f, 0.0f), TOL);
  }
}

BOOST_AUTO_TEST_CASE(test_conversions)
{
  const float TOL = 1e-4f;

  // Start from an axis-angle representation.
  float angle = static_cast<float>(M_PI) / 4.0f;
  Vector3f axis(1.0f, 0.0f, 0.0f);

  // Start conversions.
  Vector3f rotationVector;
  AttitudeUtil::axis_angle_to_rotation_vector(axis.v, &angle, rotationVector.v);
  check_close(rotationVector, Vector3f(static_cast<float>(M_PI) / 4.0f, 0.0f, 0.0f), TOL);

  Vector4f quaternion;
  AttitudeUtil::rotation_vector_to_quaternion(rotationVector.v, quaternion.v);
  check_close(quaternion, Vector4f(0.9238795f, 0.3826834f, 0.0f, 0.0f), TOL);

  {
    Vector3f rotationVector_;
    AttitudeUtil::quaternion_to_rotation_vector(quaternion.v, rotationVector_.v);
    check_close(rotationVector, rotationVector_, TOL);
  }

  Matrix3f rotationMatrix;
  AttitudeUtil::quaternion_to_rotation_matrix(quaternion.v, rotationMatrix.m, AttitudeUtil::COL_MAJOR);
  const float root2inv = 1.0f / sqrt(2.0f);
  check_close(
    rotationMatrix,
    Matrix3f(
      1.0f, 0.0f, 0.0f,
      0.0f, root2inv, -root2inv,
      0.0f, root2inv,  root2inv
    ),
    TOL
  );

  {
    Matrix3f rotationMatrix_;
    AttitudeUtil::axis_angle_to_rotation_matrix(axis.v, &angle, rotationMatrix_.m, AttitudeUtil::COL_MAJOR);
    check_close(rotationMatrix, rotationMatrix_, TOL);
  }

  {
    Vector4f quaternion_;
    AttitudeUtil::rotation_matrix_to_quaternion(rotationMatrix.m, quaternion_.v, AttitudeUtil::COL_MAJOR);
    check_close(quaternion, quaternion_, TOL);
  }

  {
    Vector3f axis_;
    float angle_;
    AttitudeUtil::quaternion_to_axis_angle(quaternion.v, axis_.v, &angle_);
    check_close(angle, angle_, TOL);
    check_close(axis, axis_, TOL);
  }

  {
    Vector3f axis_;
    float angle_;
    AttitudeUtil::rotation_matrix_to_axis_angle(rotationMatrix.t().getValues(), axis_.v, &angle_, AttitudeUtil::ROW_MAJOR);
    check_close(angle, angle_, TOL);
    check_close(axis, axis_, TOL);
  }

  {
    Vector3f axis_;
    float angle_;
    AttitudeUtil::rotation_vector_to_axis_angle(rotationVector.v, axis_.v, &angle_);
    check_close(angle, angle_, TOL);
    check_close(axis, axis_, TOL);
  }
}

BOOST_AUTO_TEST_SUITE_END()


/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#ifndef ADOBE_ALGORITHM_SORTED_HPP
#define ADOBE_ALGORITHM_SORTED_HPP

#include <algorithm>
#include <functional>
#include <iterator>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/bind.hpp>

#include <adobe/functional/operator.hpp>

/*************************************************************************************************/

namespace adobe {

/*************************************************************************************************/
/*!
\defgroup sorted sorted
\ingroup sorting
*/

/*!
\ingroup sorted
*/
template <  typename I, // I models InputIterator
            typename O> // O models StrictWeakOrdering on value_type(I)
I sorted(I f, I l, O o)
{

    f = std::adjacent_find(f, l, !boost::bind(o, _1, _2));
    if (f != l) ++f;
}

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <typename I> // I models InputIterator, value_type(I) models LessThanComparable
I sorted(I f, I l)
{
    return sorted(f, l, less());
}

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <  typename I, // I models InputIterator
            typename O> // O models StrictWeakOrdering on value_type(I)
inline bool is_sorted(I f, I l, O o)
{
    return std::adjacent_find(f, l, !boost::bind(o, _1, _2)) == l;
}

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <typename I> // I models InputIterator, value_type(I) models LessThanComparable
inline bool is_sorted(I f, I l)
{ return is_sorted(f, l, less()); }

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <  typename I, // I models ForwardIterator
            typename C, // C models StrictWeakOrdering(T, T)
            typename P> // P models UnaryFunction(value_type(I)) -> T
inline bool is_sorted(I f, I l, C c, P p)
{
    return std::adjacent_find(f, l,
        !boost::bind(c, boost::bind(p, _1), boost::bind(p, _2))) == l;
}

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <  typename I, // I models ForwardRange
            typename C, // C models StrictWeakOrdering(T, T)
            typename P> // P models UnaryFunction(value_type(I)) -> T
inline bool is_sorted(const I& r, C c, P p)
{ return is_sorted(boost::begin(r), boost::end(r), c, p); }

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <  typename I, // I models ForwardRange
            typename C> // C models StrictWeakOrdering(T, T)
inline bool is_sorted(const I& r, C c)
{ return is_sorted(boost::begin(r), boost::end(r), c, identity<typename std::iterator_traits<I>::value_type>()); }

/*************************************************************************************************/

/*!
\ingroup sorted
*/
template <  typename I> // I models ForwardRange
inline bool is_sorted(const I& r)
{ return is_sorted(boost::begin(r), boost::end(r), less()); }

/*************************************************************************************************/

} // namespace adobe

/*************************************************************************************************/

#endif

/*************************************************************************************************/


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <comma/application/command_line_options.h>
#include <comma/application/signal_flag.h>
#include <comma/csv/stream.h>
#include <comma/csv/binary.h>
#include <boost/tokenizer.hpp>

struct point
{
    point(): x(0), y(0), z(0), block(0), id(0){}
    boost::posix_time::ptime timestamp;
    double x;
    double y;
    double z;
    comma::uint32 block;
    comma::uint32 id;
    double size;
};

static bool outputsize=false;

namespace comma
{
    namespace visiting
    {
        template <> struct traits< point >
        {
            template < typename K, typename V > static void visit( const K&, point& p, V& v )
            {
                v.apply( "t", p.timestamp );
                v.apply( "x", p.x );
                v.apply( "y", p.y );
                v.apply( "z", p.z );
                v.apply( "block", p.block );
                v.apply( "id", p.id );
                v.apply( "size", p.size );
            }

            template < typename K, typename V > static void visit( const K&, const point& p, V& v )
            {
                v.apply( "t", p.timestamp );
                v.apply( "x", p.x );
                v.apply( "y", p.y );
                v.apply( "z", p.z );
                v.apply( "block", p.block );
                v.apply( "id", p.id );
                v.apply( "size", p.size );
            }
        };
    }
}

typedef std::vector< std::pair<point,std::string> > block_t;

void partitions_to_centroids(block_t& block, block_t& centroids, std::vector<double>& sizes)
{
    block_t::iterator itr;

    while(!block.empty())
    {
        itr=block.begin();
        centroids.push_back(*itr); // store other information

        comma::uint32 id=itr->first.id;

        Eigen::MatrixXd points(3,1);
        points(0,0)=itr->first.x;
        points(1,0)=itr->first.y;
        points(2,0)=itr->first.z;

        block.erase(itr);

        while(itr!=block.end())
        {
            if(itr->first.id==id)
            {
                points.conservativeResize(Eigen::NoChange,points.cols()+1);
                points(0,points.cols()-1)=itr->first.x;
                points(1,points.cols()-1)=itr->first.y;
                points(2,points.cols()-1)=itr->first.z;

                block.erase(itr);
            }
            else
            {
                itr++;
            }
        }

        //get centroid
        Eigen::MatrixXd Mean=points.rowwise().mean();
        centroids.back().first.x=Mean(0,0);
        centroids.back().first.y=Mean(1,0);
        centroids.back().first.z=Mean(2,0);

        //get bounding box
        Eigen::MatrixXd Mins=points.rowwise().minCoeff();
        Eigen::MatrixXd Maxs=points.rowwise().maxCoeff();
        Eigen::MatrixXd Extents=Maxs-Mins;
        sizes.push_back(Extents.maxCoeff());
    }
}

void publish_centroids(const block_t& centroids, const std::vector<double>& sizes, comma::csv::output_stream<point>& ostream)
{
    //publish centroids
    for(unsigned int cntr=0; cntr<centroids.size(); cntr++)
    {
        if(ostream.is_binary())
        {
            ostream.write(centroids.at(cntr).first,centroids.at(cntr).second);
            if(outputsize)
            {
                std::cout.write( reinterpret_cast< const char* >( &sizes.at(cntr) ), sizeof( double ) );
            }
        }
        else
        {
            std::string line=centroids.at(cntr).second;
            if(outputsize)
            {
                line+=","+boost::lexical_cast<std::string>(sizes.at(cntr));
            }
            ostream.write(centroids.at(cntr).first,line);
        }
    }
    ostream.flush();
}

int main( int argc, char** argv )
{
    try
    {
        comma::command_line_options options( argc, argv );
        if( options.exists( "--help,-h" ) )
        {
            std::cerr << "gets the centroid of a partitioned point cloud based on id" << std::endl;
            std::cerr << "parititions do not have to be in sequence"<<std::endl;
            std::cerr << std::endl;
            std::cerr << "usage: cat points.csv | points-to-centroids [<options>]" << std::endl;
            std::cerr << std::endl;
            std::cerr << "input: partitioned point cloud where field id corresponds to the partition number" << std::endl;
            std::cerr << std::endl;
            std::cerr << "output: the centroid of each partition with a size field optionally appended" << std::endl;
            std::cerr << std::endl;
            std::cerr << "<options>" << std::endl;
            std::cerr << "    --output-size: if present output partition size" << std::endl;
            std::cerr << comma::csv::options::usage() << std::endl;
            std::cerr << std::endl;
            exit(-1);
        }
        outputsize=options.exists("--output-size");
        comma::csv::options csv( options );
        csv.full_xpath = false;
        comma::csv::input_stream<point> istream(std::cin,csv);
        comma::csv::output_stream<point> ostream(std::cout,csv);
        comma::signal_flag is_shutdown;

        //initialise so that first point is interpreted as a new block
        comma::uint32 last_block = std::numeric_limits< comma::uint32 >::max();

        std::vector< std::pair<point,std::string> > block;

        while(!is_shutdown && std::cin.good() && !std::cin.eof())
        {
            const point* input=istream.read();
            block_t centroids;
            std::vector<double> sizes;
            if(!input)
            {
                //parition last block
                if(!block.empty())
                {
                    partitions_to_centroids(block,centroids,sizes);
                    publish_centroids(centroids,sizes,ostream);
                }
                return(0);
            }
            if(input->block != last_block && !block.empty())
            {
                partitions_to_centroids(block,centroids,sizes);
                publish_centroids(centroids,sizes,ostream);
            }
            std::string line;
            if( csv.binary() )
            {
                line.resize( csv.format().size() );
                ::memcpy( &line[0], istream.binary().last(), csv.format().size() );
            }
            else
            {
                line = comma::join( istream.ascii().last(), csv.delimiter );
            }
            std::pair<point,std::string> input_pair = std::make_pair( *input, line );

            block.push_back(input_pair);
            last_block=input->block;
        }

        return 0;
    }
    catch( std::exception& ex ) { std::cerr << "points-to-centroids: " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << "points-to-centroids: unknown exception" << std::endl; }
    return 1;
}


//libs for reading csv
#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#include "examples.h"
#include "seal/seal.h"
#include <iostream>

#include <typeinfo>
#include <unistd.h>

#include <chrono>
#include <thread>

using namespace std;
using namespace seal;

/*
 *  * A class to read data from a csv file.
 *   */
class CSVReader{
	std::string fileName;
	std::string delimeter;

public:
	CSVReader(std::string filename, std::string delm = ",") :								fileName(filename), delimeter(delm)
	{ }

	// Function to fetch data from a CSV File
	std::vector<std::vector<std::string> > getData();
};

/*
 * Parses through csv file line by line and returns the data
 * in vector of vector of strings.
 */

std::vector<std::vector<std::string> > CSVReader::getData(){
	std::ifstream file(fileName);

	std::vector<std::vector<std::string> > dataList;

	std::string line = "";
	// Iterate through each line and split the content using delimeter
	getline(file, line);
	while (getline(file, line)){
		std::vector<std::string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
		dataList.push_back(vec);
	}

	// Close the File
	file.close();

	return dataList;
}

EncryptionParameters createParameters(){

	/*EncryptionParameters parms(scheme_type::BFV);

	size_t poly_modulus_degree = 4096;
	parms.set_poly_modulus_degree(poly_modulus_degree);
	parms.set_coeff_modulus(CoeffModulus::BFVDefault(poly_modulus_degree));
	parms.set_plain_modulus(256);*/

    	EncryptionParameters parms(scheme_type::CKKS);

	size_t poly_modulus_degree = 32768;
    	parms.set_poly_modulus_degree(poly_modulus_degree);

	//amount of bits of prime: 60, 40, 40, 60
    	parms.set_coeff_modulus(CoeffModulus::Create(poly_modulus_degree, { 60, 40, 40, 40, 40, 40, 40, 40, 40, 60 }));

	return parms;
}

void decryptAndPrint(Decryptor* decryptor,CKKSEncoder* encoder,Ciphertext enc, string id){

	Plaintext plain_res;
	print_line(__LINE__);
	cout << "Decrypt and decode: " << id << "." << endl;
	decryptor->decrypt(enc, plain_res);
	vector<double> output;
	encoder->decode(plain_res, output);
	cout << "    + Result vector ...... Correct." << endl;
	print_vector(output);

}

Ciphertext g(Ciphertext x, Evaluator* evaluator, CKKSEncoder* encoder, Decryptor* decryptor, RelinKeys relin_keys, double scale, std::shared_ptr<SEALContext>* context){

	cout << "relin_keys: " << relin_keys.size() << endl;

	Ciphertext x1 = x;
	Ciphertext x2;
	evaluator->square(x1, x2);
        evaluator->relinearize_inplace(x2, relin_keys);
	evaluator->rescale_to_next_inplace(x2);

	Ciphertext x3 = x2;

	cout << "    + Modulus chain index for x1: "
		<< (*context)->get_context_data(x1.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x3: "
		<< (*context)->get_context_data(x3.parms_id())->chain_index() << endl;

	parms_id_type last_parms_id = x2.parms_id();
	evaluator->mod_switch_to_inplace(x1, last_parms_id);

	cout << "    + Modulus chain index for x1 after mod switch: "
		<< (*context)->get_context_data(x1.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x3 after mod switch: "
		<< (*context)->get_context_data(x3.parms_id())->chain_index() << endl;

	evaluator->multiply_inplace(x3,x1);
	evaluator->relinearize_inplace(x3, relin_keys);
	evaluator->rescale_to_next_inplace(x3);

	cout << "    ++++ Encrypted size of x3 = " << util::sub_safe(x3.size(),size_t(2)) << endl;

	Ciphertext x5 = x3;

	cout << "    + Modulus chain index for x2: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x5: "
		<< (*context)->get_context_data(x5.parms_id())->chain_index() << endl;

	last_parms_id = x5.parms_id();
	evaluator->mod_switch_to_inplace(x2, last_parms_id);

	cout << "    + Modulus chain index for x2 after mod switch: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x5 after mod switch: "
		<< (*context)->get_context_data(x5.parms_id())->chain_index() << endl;

	evaluator->multiply_inplace(x5,x2);
	evaluator->relinearize_inplace(x5, relin_keys);
	evaluator->rescale_to_next_inplace(x5);

	cout << "    ++++ Encrypted size of x5 = " << util::sub_safe(x5.size(),size_t(2)) << endl;

	cout << "relin_keys: " << relin_keys.size() << endl;

	Ciphertext x7 = x5;

	cout << "    + Modulus chain index for x2: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x7: "
		<< (*context)->get_context_data(x7.parms_id())->chain_index() << endl;

	last_parms_id = x5.parms_id();
	evaluator->mod_switch_to_inplace(x2, last_parms_id);

	cout << "    + Modulus chain index for x2 after mod switch: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x7 after mod switch: "
		<< (*context)->get_context_data(x7.parms_id())->chain_index() << endl;

	evaluator->multiply_inplace(x7,x2);
	evaluator->relinearize_inplace(x7, relin_keys);
	evaluator->rescale_to_next_inplace(x7);

	cout << "    ++++ Encrypted size of x7 = " << util::sub_safe(x7.size(),size_t(2)) << endl;

	Ciphertext x9 = x7;

	cout << "    + Modulus chain index for x2: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x9: "
		<< (*context)->get_context_data(x9.parms_id())->chain_index() << endl;

	last_parms_id = x9.parms_id();
	evaluator->mod_switch_to_inplace(x2, last_parms_id);

	cout << "    + Modulus chain index for x2 after mod switch: "
		<< (*context)->get_context_data(x2.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for x9 after mod switch: "
		<< (*context)->get_context_data(x9.parms_id())->chain_index() << endl;

	cout << "    ++++ Encrypted size of x9 = " << util::sub_safe(x9.size(),size_t(2)) << endl;

	evaluator->multiply_inplace(x9,x2);

	cout << "    ++++ Encrypted size of x9 = " << util::sub_safe(x9.size(),size_t(2)) << endl;

	evaluator->relinearize_inplace(x2, relin_keys);
	evaluator->relinearize_inplace(x9, relin_keys);
	evaluator->rescale_to_next_inplace(x9);

	/*double w0 = 0.5;
	double w1 = 0.2159198015;
	double w3 = -0.0082176259;
	double w5 = 0.0001825597;
	double w7 = -0.0000018848;
	double w9 = 0.0000000072;*/

	Plaintext w0_plain, w1_plain, w3_plain, w5_plain, w7_plain, w9_plain;
	encoder->encode(0.5, scale, w0_plain);
	encoder->encode(0.2159198015, scale, w1_plain);
	encoder->encode(-0.0082176259, scale, w3_plain);
	encoder->encode(0.0001825597, scale, w5_plain);
	encoder->encode(-0.0000018848, scale, w7_plain);
	encoder->encode(0.0000000072, scale, w9_plain);

	Ciphertext y1 = x1;

	cout << "    + Modulus chain index for x1: "
		<< (*context)->get_context_data(x1.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for w1_plain: "
		<< (*context)->get_context_data(w1_plain.parms_id())->chain_index() << endl;

	last_parms_id = x1.parms_id();
	evaluator->mod_switch_to_inplace(w1_plain, last_parms_id);

	evaluator->multiply_plain_inplace(y1,w1_plain);
	evaluator->relinearize_inplace(y1, relin_keys);
	evaluator->rescale_to_next_inplace(y1);
	decryptAndPrint(decryptor,encoder,y1,"y1");

	Ciphertext y3 = x3;

	cout << "    + Modulus chain index for x3: "
		<< (*context)->get_context_data(x3.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for w3_plain: "
		<< (*context)->get_context_data(w3_plain.parms_id())->chain_index() << endl;

	last_parms_id = x3.parms_id();
	evaluator->mod_switch_to_inplace(w3_plain, last_parms_id);

	evaluator->multiply_plain_inplace(y3,w3_plain);
	evaluator->relinearize_inplace(y3, relin_keys);
	evaluator->rescale_to_next_inplace(y3);
	decryptAndPrint(decryptor,encoder,y3,"y3");

	Ciphertext y5 = x5;

	cout << "    + Modulus chain index for x5: "
		<< (*context)->get_context_data(x5.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for w5_plain: "
		<< (*context)->get_context_data(w5_plain.parms_id())->chain_index() << endl;

	last_parms_id = x5.parms_id();
	evaluator->mod_switch_to_inplace(w5_plain, last_parms_id);

	evaluator->multiply_plain_inplace(y5,w5_plain);
	evaluator->relinearize_inplace(y5, relin_keys);
	evaluator->rescale_to_next_inplace(y5);
	decryptAndPrint(decryptor,encoder,y5,"y5");

	Ciphertext y7 = x7;

	cout << "    + Modulus chain index for x7: "
		<< (*context)->get_context_data(x7.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for w7_plain: "
		<< (*context)->get_context_data(w7_plain.parms_id())->chain_index() << endl;

	last_parms_id = x7.parms_id();
	evaluator->mod_switch_to_inplace(w7_plain, last_parms_id);

	evaluator->multiply_plain_inplace(y7,w7_plain);
	evaluator->relinearize_inplace(y7, relin_keys);
	decryptAndPrint(decryptor,encoder,y7,"y7");
	evaluator->rescale_to_next_inplace(y7);

	Ciphertext y9 = x9;

	cout << "    + Modulus chain index for x9: "
		<< (*context)->get_context_data(x9.parms_id())->chain_index() << endl;
	cout << "    + Modulus chain index for w9_plain: "
		<< (*context)->get_context_data(w9_plain.parms_id())->chain_index() << endl;

	last_parms_id = x9.parms_id();
	evaluator->mod_switch_to_inplace(w9_plain, last_parms_id);

	evaluator->multiply_plain_inplace(y9,w9_plain);
	evaluator->relinearize_inplace(y9, relin_keys);
	decryptAndPrint(decryptor,encoder,y9,"y9");
	evaluator->rescale_to_next_inplace(y9);

	cout << "relin_keys: " << relin_keys.size() << endl;

	cout << "    +++++ Modulus chain index for y9: "
		<< (*context)->get_context_data(y9.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y7: "
		<< (*context)->get_context_data(y7.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y5: "
		<< (*context)->get_context_data(y5.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y3: "
		<< (*context)->get_context_data(y3.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y1: "
		<< (*context)->get_context_data(y1.parms_id())->chain_index() << endl;

	last_parms_id = y9.parms_id();
	evaluator->mod_switch_to_inplace(y7, last_parms_id);
	evaluator->mod_switch_to_inplace(y5, last_parms_id);
	evaluator->mod_switch_to_inplace(y3, last_parms_id);
	evaluator->mod_switch_to_inplace(y1, last_parms_id);

	cout << "    +++++ Modulus chain index for y9 after mod switch: "
		<< (*context)->get_context_data(y9.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y7 after mod switch: "
		<< (*context)->get_context_data(y7.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y5 after mod switch: "
		<< (*context)->get_context_data(y5.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y3 after mod switch: "
		<< (*context)->get_context_data(y3.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for y1 after mod switch: "
		<< (*context)->get_context_data(y1.parms_id())->chain_index() << endl;

	//cout << fixed << setprecision(10);

	cout <<	"y9 and y7 are in the same scale? " << util::are_close<double>(y9.scale(), y7.scale()) << endl;
	cout <<	"y9 scale: " << y9.scale() << endl;
	cout <<	"y7 scale: " << y7.scale() << endl;
	cout <<	"y5 scale: " << y5.scale() << endl;
	cout <<	"y3 scale: " << y3.scale() << endl;
	cout <<	"y1 scale: " << y1.scale() << endl;

	cout << "pow(2.0, 40) = " << pow(2.0, 40) << endl;

	y9.scale() = pow(2.0, 40);
	y7.scale() = pow(2.0, 40);
	y5.scale() = pow(2.0, 40);
	y3.scale() = pow(2.0, 40);
	y1.scale() = pow(2.0, 40);

	cout <<	"y9 scale after change scaling: " << y9.scale() << endl;
	cout <<	"y7 scale after change scaling: " << y7.scale() << endl;
	cout <<	"y5 scale after change scaling: " << y5.scale() << endl;
	cout <<	"y3 scale after change scaling: " << y3.scale() << endl;
	cout <<	"y1 scale after change scaling: " << y1.scale() << endl;

	Ciphertext z;
	std::vector<Ciphertext> factors;
	factors.push_back(y9);
	factors.push_back(y7);
	factors.push_back(y5);
	factors.push_back(y3);
	factors.push_back(y1);

	evaluator->add_many(factors,z);
	evaluator->relinearize_inplace(z, relin_keys);
	//evaluator->rescale_to_next_inplace(z);

	decryptAndPrint(decryptor,encoder,z,"z");

	cout << "    +++++ Modulus chain index for z: "
		<< (*context)->get_context_data(z.parms_id())->chain_index() << endl;
	cout << "    +++++ Modulus chain index for w0_plain: "
		<< (*context)->get_context_data(w0_plain.parms_id())->chain_index() << endl;
	last_parms_id = z.parms_id();
	evaluator->mod_switch_to_inplace(w0_plain, last_parms_id);
	cout << "    +++++ Modulus chain index for w0_plain after mod switch: "
		<< (*context)->get_context_data(w0_plain.parms_id())->chain_index() << endl;

	cout <<	"z and w0_plain are in the same scale? " << util::are_close<double>(z.scale(), w0_plain.scale()) << endl;
	cout <<	"z scale: " << z.scale() << endl;
	cout <<	"w0_plain scale: " << w0_plain.scale() << endl;

	z.scale() = pow(2.0, 40);

	cout <<	"z scale: " << z.scale() << endl;
	cout <<	"w0_plain scale: " << w0_plain.scale() << endl;

	evaluator->add_plain_inplace(z,w0_plain);

	return z;
}

Ciphertext h(vector<Ciphertext> x, Ciphertext theta, Evaluator* evaluator, CKKSEncoder* encoder, Decryptor* decryptor, RelinKeys relin_keys, double scale, std::shared_ptr<SEALContext>* context){

	for(Ciphertext xRow : x){
		decryptAndPrint(decryptor,encoder,xRow,"xRow");

		Ciphertext theta_temp = theta;

		evaluator->multiply_inplace(theta_temp,xRow);
		evaluator->relinearize_inplace(theta_temp, relin_keys);
		evaluator->rescale_to_next_inplace(theta_temp);

		Ciphertext sum;

	}

}

int main(){

	EncryptionParameters parms = createParameters();

	auto context = SEALContext::Create(parms);
	print_line(__LINE__);
	cout << "Set encryption parameters and print" << endl;
	print_parameters(context);
	cout << endl;

    	KeyGenerator keygen(context);
    	auto public_key = keygen.public_key();
    	auto secret_key = keygen.secret_key();
    	auto relin_keys = keygen.relin_keys();

	Encryptor encryptor(context, public_key);
	Evaluator evaluator(context);
    	Decryptor decryptor(context, secret_key);

    	CKKSEncoder encoder(context);

    	size_t slot_count = encoder.slot_count();
    	cout << "Number of slots: " << slot_count << endl;

	double scale = pow(2.0, 40);

	//sigmoid
	/*print_line(__LINE__);
	vector<double> input{ -0.5 };
	cout << "Input vector: " << endl;
	print_vector(input);

	Plaintext plain;
	print_line(__LINE__);
	cout << "Encode input vector." << endl;
	encoder.encode(input, scale, plain);

	Ciphertext encrypted;
	print_line(__LINE__);
	cout << "Encrypt input vector and square." << endl;
	encryptor.encrypt(plain, encrypted);

	Ciphertext enc_sq = g(encrypted, &evaluator, &encoder,&decryptor, relin_keys, scale, &context);

	Plaintext plain_res;
	print_line(__LINE__);
	cout << "Decrypt and decode." << endl;
	decryptor.decrypt(enc_sq, plain_res);
	vector<double> output;
	encoder.decode(plain_res, output);
	cout << "    + Result vector ...... Correct." << endl;
	print_vector(output, 4, 10);*/

	//hypothesis
	print_line(__LINE__);
	vector<vector<double>> inputMatrix{
				{0.5, 0.5, 0.5},
				{0.5, 0.5, 0.5},
				{0.5, 0.5, 0.5}	};
	vector<Plaintext> plain_x;
	for(vector<double> inputVector : inputMatrix){
		Plaintext plain_v;
		encoder.encode(inputVector, scale, plain_v);
		plain_x.push_back(plain_v);
	}
	vector<Ciphertext> encrypted_x_vec;
	for(Plaintext plain : plain_x){
		Ciphertext encrypted_x;
		encryptor.encrypt(plain, encrypted_x);
		encrypted_x_vec.push_back(encrypted_x);
	}

	vector<double> inputVector{ 0.5, 0.5, 0.5 };
	Plaintext plain_theta;
	encoder.encode(inputVector, scale, plain_theta);
	Ciphertext encrypted_theta;
	encryptor.encrypt(plain_theta, encrypted_theta);

	Ciphertext hx = h(encrypted_x_vec, encrypted_theta, &evaluator, &encoder, &decryptor, relin_keys, scale, &context);

	/*print_line(__LINE__);
	cout << "Encode input vector." << endl;
	encoder.encode(input, scale, plain);

	Ciphertext encrypted;
	print_line(__LINE__);
	cout << "Encrypt input vector and square." << endl;
	encryptor.encrypt(plain, encrypted);

	Ciphertext enc_sq = g(encrypted, &evaluator, &encoder,&decryptor, relin_keys, scale, &context);

	Plaintext plain_res;
	print_line(__LINE__);
	cout << "Decrypt and decode." << endl;
	decryptor.decrypt(enc_sq, plain_res);
	vector<double> output;
	encoder.decode(plain_res, output);
	cout << "    + Result vector ...... Correct." << endl;
	print_vector(output, 4, 10);*/

	/*
	// Creating an object of CSVWriter
	CSVReader reader("mimic10lines.csv");

	// Get the data from CSV File
	std::vector<std::vector<std::string> > dataList = reader.getData();
	// each Ciphertext contains a vector of values
	std::vector<Ciphertext> encryptedMatrix;
	// Print the content of row by row on screen
	for(std::vector<std::string> vec : dataList){
		vector<double> line;
		for(std::string data : vec){
			double v = atof(data.c_str());
			line.push_back(v);
			std::cout<< v << ", ";
		}
		Plaintext plain;
		encoder.encode(line, scale, plain);
		Ciphertext encrypted;
		encryptor.encrypt(plain, encrypted);
		encryptedMatrix.push_back(encrypted);
		std::cout<<std::endl;
	}

	std::cout << " Decrypting! " << std::endl << std::endl;
	for(Ciphertext encrypted : encryptedMatrix){
		//for(Ciphertext encrypted : encryptedVector){
		Plaintext plain_result;
		decryptor.decrypt(encrypted, plain_result);
		vector<double> output;
		encoder.decode(plain_result, output);
		cout << output.size() << std::endl;
		print_vector(output,8,3);
	}*/

	return 0;

}


// File: array_initialization.cpp

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

int main(int, char**)
{
    using namespace mtl;

    double array[][4]= {{3, 7.2,   0, 6},
			{2, 4.444, 5, 3},
			{1, 7,     9, 2}};

    dense2D<double> A(array);

    std::cout << "A = \n" << A << "\n";

    return 0;
}


// Copyright 2012 John P. Maloney
//
// Distributed under the Apache License, Version 2.0.
// (See accompanying file LICENSE_2_0 or copy at
// http://www.apache.org/licenses/LICENSE-2.0)
//
// To build:
//   $ cd ~/basic/tests
//   $ g++ -I ~/thirdparty/boost boost_example.cpp -o boost_example
//
// To run:
//   $ echo 1 2 3 | ./boost_example
//
// Reference:
// http://www.boost.org/doc/libs/1_49_0/more/getting_started/unix-variants.html
//
#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

int main()
{
    using namespace boost::lambda;
    typedef std::istream_iterator<int> in;

    std::for_each(
        in(std::cin), in(), std::cout << (_1 * 3) << " " );
}


#include <string>
#include <iostream>
#include <cassert>

#include <omp.h>

#include <boost/lexical_cast.hpp>

#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>
#include <frovedis/ml/kernel/kernel_svm.hpp>

using kernel_t = float;

int main(int argc, char** argv) {
  frovedis::use_frovedis use(argc, argv);

  // log
  //frovedis::set_loglevel(DEBUG);

  bool multiple_mpi_procs = frovedis::get_nodesize() > 1;
  if (multiple_mpi_procs) {
    std::cerr << "---------- CAUTION ----------" << std::endl
	      << "Kernel SVM uses openmp parallelization." << std::endl
	      << "This program is launched with multiple MPI processes, but other than one (master) is not actually working." << std::endl
	      << "You should assign each process in different nodes to prevent interference" << std::endl;
  }

  std::cerr << "number of openmp threads = " << omp_get_max_threads() << std::endl;

  auto train_x = frovedis::make_crs_matrix_local_load<kernel_t>("./train.mat").to_rowmajor();
  auto train_y = frovedis::make_dvector_loadline<kernel_t>("./train.label").gather();

  double tol = 0.001;
  double C = 1.0;
  int cache_size = 4;  // in unit of MB
  int max_iter = 100;
  std::string kernel_ty_str = "rbf";  // or "poly", "sigmoid"
  double gamma = 0.01;

  frovedis::kernel_csvc_model<kernel_t> model(tol, C, cache_size, max_iter, kernel_ty_str, gamma);
  model.train(train_x, train_y);

  model.save("./model");
  frovedis::kernel_csvc_model<kernel_t> model2;
  model2.load("./model");

  auto test_x = frovedis::make_crs_matrix_local_load<kernel_t>("./test.mat").to_rowmajor();
  auto test_y = frovedis::make_dvector_loadline<kernel_t>("./test.label").gather();

  auto pred_y = model2.predict(test_x);

  size_t num_correct = 0;
  for (size_t i = 0; i < pred_y.size(); i++) {
    num_correct += int(test_y[i] == pred_y[i]);
  }
  double acc = 1.0 * num_correct / pred_y.size();
  std::cout << "Accuracy: " << acc << std::endl;

  return 0;
}


#include <gtest/gtest.h>

#include "manif/Rn.h"

#include "../common_tester.h"

#include <Eigen/StdVector>

using namespace manif;

// specialize std::vector for 'fixed-size vectorizable' Eigen object
// that are multiple of 32 bytes
// @todo: investigate why only this alignment is troublesome
// especially, SO3 wasn't an issue despite being Eigen::Vector4d too...
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R4d)

#ifdef MANIF_COVERAGE_ENABLED

MANIF_TEST(R4d);
MANIF_TEST_JACOBIANS(R4d);

#else

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R8d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R8f)

TEST(TEST_RN, TEST_RN_VEC_ASSIGN_OP)
{
  {
    R1d::DataType data = R1d::DataType::Random();

    R1d r1;
    r1 = data;

    EXPECT_EIGEN_NEAR(data, r1.coeffs());
  }

  {
    R2d::DataType data = R2d::DataType::Random();

    R2d r2;
    r2 = data;

    EXPECT_EIGEN_NEAR(data, r2.coeffs());
  }

  {
    R3d::DataType data = R3d::DataType::Random();

    R3d r3;
    r3 = data;

    EXPECT_EIGEN_NEAR(data, r3.coeffs());
  }

  {
    R4d::DataType data = R4d::DataType::Random();

    R4d r4;
    r4 = data;

    EXPECT_EIGEN_NEAR(data, r4.coeffs());
  }

  {
    R5d::DataType data = R5d::DataType::Random();

    R5d r5;
    r5 = data;

    EXPECT_EIGEN_NEAR(data, r5.coeffs());
  }

  {
    R6d::DataType data = R6d::DataType::Random();

    R6d r6;
    r6 = data;

    EXPECT_EIGEN_NEAR(data, r6.coeffs());
  }

  {
    R7d::DataType data = R7d::DataType::Random();

    R7d r7;
    r7 = data;

    EXPECT_EIGEN_NEAR(data, r7.coeffs());
  }

  {
    R8d::DataType data = R8d::DataType::Random();

    R8d r8;
    r8 = data;

    EXPECT_EIGEN_NEAR(data, r8.coeffs());
  }

  {
    R9d::DataType data = R9d::DataType::Random();

    R9d r9;
    r9 = data;

    EXPECT_EIGEN_NEAR(data, r9.coeffs());
  }
}

// This is a little too heavy for coverage and not relevant...

MANIF_TEST(R1f);
MANIF_TEST(R2f);
MANIF_TEST(R3f);
MANIF_TEST(R4f);
MANIF_TEST(R5f);
MANIF_TEST(R6f);
MANIF_TEST(R7f);
MANIF_TEST(R8f);
MANIF_TEST(R9f);

MANIF_TEST_MAP(R1f);
MANIF_TEST_MAP(R2f);
MANIF_TEST_MAP(R3f);
MANIF_TEST_MAP(R4f);
MANIF_TEST_MAP(R5f);
MANIF_TEST_MAP(R6f);
MANIF_TEST_MAP(R7f);
MANIF_TEST_MAP(R8f);
MANIF_TEST_MAP(R9f);

MANIF_TEST_JACOBIANS(R1f);
MANIF_TEST_JACOBIANS(R2f);
MANIF_TEST_JACOBIANS(R3f);
MANIF_TEST_JACOBIANS(R4f);
MANIF_TEST_JACOBIANS(R5f);
MANIF_TEST_JACOBIANS(R6f);
MANIF_TEST_JACOBIANS(R7f);
MANIF_TEST_JACOBIANS(R8f);
MANIF_TEST_JACOBIANS(R9f);

MANIF_TEST(R1d);
MANIF_TEST(R2d);
MANIF_TEST(R3d);
MANIF_TEST(R4d);
MANIF_TEST(R5d);
MANIF_TEST(R6d);
MANIF_TEST(R7d);
MANIF_TEST(R8d);
MANIF_TEST(R9d);

MANIF_TEST_MAP(R1d);
MANIF_TEST_MAP(R2d);
MANIF_TEST_MAP(R3d);
MANIF_TEST_MAP(R4d);
MANIF_TEST_MAP(R5d);
MANIF_TEST_MAP(R6d);
MANIF_TEST_MAP(R7d);
MANIF_TEST_MAP(R8d);
MANIF_TEST_MAP(R9d);

MANIF_TEST_JACOBIANS(R1d);
MANIF_TEST_JACOBIANS(R2d);
MANIF_TEST_JACOBIANS(R3d);
MANIF_TEST_JACOBIANS(R4d);
MANIF_TEST_JACOBIANS(R5d);
MANIF_TEST_JACOBIANS(R6d);
MANIF_TEST_JACOBIANS(R7d);
MANIF_TEST_JACOBIANS(R8d);
MANIF_TEST_JACOBIANS(R9d);

#endif

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#pragma once

#include <boost/assert.hpp>
#include <iostream>
#include <map>
#include <ostream>
#include <tuple>
#include <vector>

#include "enum/enum.hpp"
#include "aux/filtered_range.hpp"

namespace boltzmann {
namespace ct_dense {

/**
 * @brief Variable sized block compressed row storage format. E.g. CSR made up of variable sized
 * blocks. This class stores only locations of non-zero blocks and no actual values/entries.
 *
 * @tparam ALIGN Align to ALIGN units (e.g. doubles, floats, ...), this is not alignment in bytes.
 *
 * This is a compressed version of \ref MultiSlice.
 *
 * Notation: y = A*x
 * Thus x_offset, x_extent refer to columns and y_offset, y_extent to rows.
 *
 */
template <int ALIGN = 4>
class VBCRSSparsity
{
 public:
  typedef unsigned int size_t;
  typedef long unsigned int mem_size_t;
  struct block_t
  {
    block_t() {}
    block_t(size_t offset_, size_t extent_)
        : offset(offset_)
        , extent(extent_)
    {
    }
    size_t offset;
    size_t extent;
  };
  typedef int block_idx;

 public:
  /// align subblocks to `align` doubles
  static size_t align;

 public:
  VBCRSSparsity() { /* empty */}

  template <typename INDEX_TYPE, typename SUBBLOCK>
  void init(const std::multimap<INDEX_TYPE, SUBBLOCK>& multi_slices, const int K);

  template <typename INDEX_TYPE, typename SUBBLOCK, typename SUPER_BLOCK>
  void init(const std::multimap<INDEX_TYPE, SUBBLOCK>& multi_slice,
            const std::vector<SUPER_BLOCK>& super_blocks);

  /**
   * @param idx block row index
   *
   * for use in \ref get_block
   *
   * @return begin of row
   */
  size_t row_begin(size_t idx) const;

  /**
   * @param idx block row index
   *
   * for use in \ref get_block
   *
   * @return end of row
   */
  size_t row_end(size_t idx) const;

  /**
   * @param block_idx
   *
   * to iterate over a block row use \ref row_begin, \ref row_end as input
   *
   * @return column offset, column width
   */
  const block_t& get_block(block_idx block_idx) const;

  /**
   * @param idx block row index
   *
   * @return row offset, row height (as struct{offset, extent}).
   */
  const block_t& get_row_info(size_t idx) const;

  /*
   * number of storage units (assuming each block is aligned to 4 units)
   *
   * 4 units because we store 64bit floating point numbers and align to 256 bits (AVX)
   */
  mem_size_t memsize() const;
  size_t dimz() const;
  /// number of rows / columns
  size_t nblock_rows() const;
  /// total number of blocks
  size_t nblocks() const;
  /// number of nonzero entries
  size_t nnz() const;

  void save(std::ostream& out) const;

 private:
  /// length = number of block rows + 1
  std::vector<block_idx> row_ptr_;
  /// length = number of block rows
  std::vector<block_t> block_rows_;  // row offset, row height (for each block row)
  /// length = nnz
  std::vector<block_t> block_columns_;  // column offset, column width (for each block)
  /// length = nnz+1
  // std::vector<mem_size_t> mem_offsets_;  // memory offset
  mem_size_t memsize_;
  size_t dimz_;
  size_t nnz_;
  bool is_initialized_ = false;
};

template <int ALIGN>
unsigned int VBCRSSparsity<ALIGN>::align = ALIGN;

template <int ALIGN>
template <typename INDEX_TYPE, typename SUBBLOCK>
void
VBCRSSparsity<ALIGN>::init(const std::multimap<INDEX_TYPE, SUBBLOCK>& multi_slice, const int K)
{
  BOOST_ASSERT(is_initialized_ == false);
  block_idx block_count = 0;
  int row_id = 0;
  // size_t row_offset = 0;
  mem_size_t mem_offset = 0;
  nnz_ = 0;
  block_rows_.resize(2 * K - 1);
  row_ptr_.resize(2 * K);
  for (int k = 0; k < K; ++k) {
    for (auto t : {TRIG::COS, TRIG::SIN}) {
      // sin(0*phi) == 0 is of course not contained in the basis
      if (k == 0 && t == TRIG::SIN) continue;
      // row_ptr points to the first block in this row
      row_ptr_[row_id] = block_count;
      auto key = std::make_tuple(k, t);
      if (multi_slice.find(key) == multi_slice.end()) {
        block_rows_[row_id] = block_t(0, 0);
        // this row is empty
      } else {
        // key = (angular index, sin or cos)
        auto range = multi_slice.equal_range(key);
        block_rows_[row_id] = block_t(range.first->second.offset_x, range.first->second.size_x);
        dimz_ = range.first->second.size_z;
        for (auto row_it = range.first; row_it != range.second; ++row_it) {
          auto& sblock = row_it->second;
          block_columns_.push_back(block_t(sblock.offset_y, sblock.size_y));
          // leading dimension (lda) (e.g. row in col-major matrix storage)
          // of a sub-block must be 128bit aligned!
          mem_size_t lsize =
              (sblock.size_x + (align - (sblock.size_x % align))) * sblock.size_y * sblock.size_z;
          nnz_ += sblock.size_x * sblock.size_y * sblock.size_z;
          mem_offset += lsize;
          block_count++;
        }
      }
      row_id++;
      // std::cout << std::get<0>(key) << " " << std::get<1>(key) << "\n";
    }
  }
  row_ptr_[row_id] = block_count;
  // one past last byte
  // mem_offsets_.push_back(mem_offset);
  memsize_ = mem_offset;
  // one past last row
  BOOST_ASSERT(row_id == 2 * K - 1);
  // BOOST_ASSERT(mem_offsets_.size() == block_count + 1);
  BOOST_ASSERT((*row_ptr_.rbegin()) == block_count);
  BOOST_ASSERT(multi_slice.size() == this->nblocks());
  is_initialized_ = true;
}

template <int ALIGN>
template <typename INDEX_TYPE, typename SUBBLOCK, typename SUPER_BLOCK>
void
VBCRSSparsity<ALIGN>::init(const std::multimap<INDEX_TYPE, SUBBLOCK>& multi_slice,
                           const std::vector<SUPER_BLOCK>& super_blocks)
{
  // INDEX_TYPE := tuple<k, t>
  // k: polynomial degree
  // t: enum TRIG (eg. SIN or COS)

  nnz_ = 0;
  block_rows_.resize(super_blocks.size());
  row_ptr_.resize(super_blocks.size() + 1);
  block_idx block_count = 0;
  std::map<size_t, size_t> sblock_offset_to_idx;
  size_t offset = 0;
  for (unsigned int i = 0; i < super_blocks.size(); ++i) {
    const auto& sblock = super_blocks[i];
    size_t loffset = sblock.index_first;
    size_t extent = sblock.extent;
    block_rows_[i] = block_t(loffset, extent);
    offset += extent;
    sblock_offset_to_idx[offset] = i;
  }

  // for each entry in super_blocks find super_block_row_id (row id)
  // and super_block_col_id (column id)
  typedef int row_idx_t;
  typedef int col_idx_t;
  typedef std::pair<row_idx_t, col_idx_t> block_key_t;
  std::set<block_key_t> block_nnz_ids;
  for (const auto& elem : multi_slice) {
    int row_offset = elem.second.offset_x;
    int col_offset = elem.second.offset_y;
    auto it_row = sblock_offset_to_idx.upper_bound(row_offset);
    BOOST_ASSERT(it_row != sblock_offset_to_idx.end());
    auto it_col = sblock_offset_to_idx.upper_bound(col_offset);
    BOOST_ASSERT(it_col != sblock_offset_to_idx.end());
    size_t scol_idx = it_row->second;
    size_t srow_idx = it_col->second;
    block_nnz_ids.insert(std::make_pair(srow_idx, scol_idx));
    dimz_ = elem.second.size_z;
  }

  // for (auto elem : block_nnz_ids) {
  //   std::cout << "block_nnz: " << elem.first << ", " << elem.second << "\n";
  // }

  // todo: fill row_ptr and block_columns_
  mem_size_t mem_offset = 0;
  size_t block_counter = 0;
  for (unsigned int srow_idx = 0; srow_idx < super_blocks.size(); ++srow_idx) {
    row_ptr_[srow_idx] = block_counter;
    size_t row_extent = super_blocks[srow_idx].extent;
    auto range = filtered_range(
        block_nnz_ids.begin(), block_nnz_ids.end(), [srow_idx](const block_key_t& key) {
          return key.first == srow_idx;
        });
    std::vector<block_key_t> elems(std::get<0>(range), std::get<1>(range));
    // debug/test output
    //std::cout << "on block " << srow_idx << " found " << elems.size() << " elements\n";
    for (const block_key_t& elem : elems) {
      //std::cout << elem.first << ", " << elem.second << "\n";
      size_t scol_idx = elem.second;
      const auto& sblock = super_blocks[scol_idx];
      int col_begin = sblock.index_first;
      size_t col_extent = sblock.extent;
      block_columns_.push_back(block_t(col_begin, col_extent));
      block_counter++;
      mem_size_t lsize = (row_extent + (align - (row_extent % align))) * col_extent * dimz_;
      nnz_ += row_extent*col_extent*dimz_;
      mem_offset += lsize;
    }
  }
  // last row_ptr_ points one past the last block
  row_ptr_[super_blocks.size()] = block_counter;
  memsize_ = mem_offset;

  BOOST_ASSERT(block_nnz_ids.size() == this->nblocks());
  is_initialized_ = true;
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::row_begin(size_t idx) const
{
  BOOST_ASSERT(idx < row_ptr_.size());
  return row_ptr_[idx];
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::row_end(size_t idx) const
{
  BOOST_ASSERT(idx + 1 < row_ptr_.size());
  return row_ptr_[idx + 1];
}

template <int ALIGN>
inline const typename VBCRSSparsity<ALIGN>::block_t&
VBCRSSparsity<ALIGN>::get_block(block_idx block_idx) const
{
  BOOST_ASSERT(block_idx < block_columns_.size());
  return block_columns_[block_idx];
}

template <int ALIGN>
inline const typename VBCRSSparsity<ALIGN>::block_t&
VBCRSSparsity<ALIGN>::get_row_info(size_t idx) const
{
  BOOST_ASSERT(idx < block_rows_.size());
  return block_rows_[idx];
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::mem_size_t
VBCRSSparsity<ALIGN>::memsize() const
{
  // return *(mem_offsets_.rbegin());
  return memsize_;
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::dimz() const
{
  return dimz_;
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::nblock_rows() const
{
  BOOST_ASSERT(block_rows_.size());
  // there is one pointer past the end, thus -1
  return block_rows_.size();
}

template <int ALIGN>
inline void
VBCRSSparsity<ALIGN>::save(std::ostream& out) const
{
  out << "row_begin col_begin row_extent col_extent\n";
  for(size_t row_idx = 0; row_idx < this->nblock_rows(); ++row_idx) {
    size_t row_offset = get_row_info(row_idx).offset;
    size_t row_extent = get_row_info(row_idx).extent;
    for(size_t ptr = row_begin(row_idx); ptr < row_end(row_idx); ++ptr) {
      size_t col_offset = get_block(ptr).offset;
      size_t col_extent = get_block(ptr).extent;
      out << row_offset << " "
          << col_offset << " "
          << row_extent << " "
          << col_extent << "\n";
    }
  }
}

template <int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::nblocks() const
{
  return (*row_ptr_.rbegin());
}

template<int ALIGN>
inline typename VBCRSSparsity<ALIGN>::size_t
VBCRSSparsity<ALIGN>::nnz() const
{
  return nnz_;
}

}  // ct_dense
}  // end namespace boltzmann


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <boost/geometry.hpp>

namespace pyinterp {
namespace detail {
namespace geometry {

/// Coordinate type
template <typename Point>
using coordinate_t = boost::geometry::coordinate_type<Point>;

namespace point {

/// Class allowing dynamic access to the axis of a point.
/// A recursive call is made as long as the requested axis is not reached.
///
/// @tparam Point Point handled
/// @tparam Axis The current axis handled by the class.
/// @tparam Count The number of dimension handled by the point
template <typename Point, size_t Axis, size_t Count>
struct GetDimension {
  /// Get accessor
  ///
  /// @param point Point handled.
  /// @param axis The axis of the point to be read.
  static typename coordinate_t<Point>::type get(const Point &point,
                                                size_t axis) {
    if (Axis == axis) {
      return boost::geometry::get<Axis>(point);
    }
    return GetDimension<Point, Axis + 1, Count>::get(point, axis);
  }

  /// Set accessor
  ///
  /// @param point Point handled.
  /// @param value Value to store in the point
  /// @param axis The axis of the point to be read.
  static void set(Point &point, const typename coordinate_t<Point>::type &value,
                  size_t axis) {
    if (Axis == axis) {
      return boost::geometry::set<Axis>(point, value);
    }
    return GetDimension<Point, Axis + 1, Count>::set(point, value, axis);
  }
};

/// Class allowing dynamic access to the axis of a point.
/// This class is used when recursive calls are completed.
///
/// @tparam Point Point handled
/// @tparam Count The number of dimension handled by the point
template <typename Point, size_t Count>
struct GetDimension<Point, Count, Count> {
  /// Get accessor
  static typename coordinate_t<Point>::type get(const Point & /*point*/,
                                                size_t /*axis*/) {
    return std::numeric_limits<typename coordinate_t<Point>::type>::max();
  }

  /// Set accessor
  static void set(Point & /*point*/,
                  const typename coordinate_t<Point>::type & /*value*/,
                  size_t /*axis*/) {}
};

/// Get a point value for a given axis
template <typename Point>
typename coordinate_t<Point>::type get(const Point &point, size_t axis) {
  return GetDimension<
      Point, 0, boost::geometry::dimension<Point>::type::value>::get(point,
                                                                     axis);
}

/// Set a point value for a given axis
template <typename Point>
void set(Point &point, const typename coordinate_t<Point>::type &value,
         size_t axis) {
  return GetDimension<
      Point, 0, boost::geometry::dimension<Point>::type::value>::set(point,
                                                                     value,
                                                                     axis);
}

}  // namespace point

/// Points of a 2-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using Point2D =
    boost::geometry::model::point<T, 2, boost::geometry::cs::cartesian>;

/// Points of a 3-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using Point3D =
    boost::geometry::model::point<T, 3, boost::geometry::cs::cartesian>;

/// Points of a N-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
/// @tparam N Number of dimensions of the Cartesian space.
template <typename T, size_t N>
using PointND =
    boost::geometry::model::point<T, N, boost::geometry::cs::cartesian>;

/// Points of a 2-dimensional spherical equatorial space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using EquatorialPoint2D = boost::geometry::model::point<
    T, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;

/// Points of a 3-dimensional spherical equatorial space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using EquatorialPoint3D = boost::geometry::model::point<
    T, 3, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;

}  // namespace geometry
}  // namespace detail
}  // namespace pyinterp


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

// This file was created by Steffen Urban (urbste@googlemail.com) or
// company address (steffen.urban@zeiss.com)
// December 2018

#include <math.h>
#include <Eigen/Core>
#include <random>
#include "gtest/gtest.h"

#include "theia/sfm/pose/four_point_focal_length_radial_distortion.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"

namespace theia {

namespace {

const int min_nr_points = 4;
const double image_width = 3000;

using Eigen::Array;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector2d;
using Eigen::Vector3d;

RandomNumberGenerator rng(64);

void DistortPoint(const Eigen::Vector2d& point2d, const double& distortion,
                  Eigen::Vector2d* distorted_point) {
  const double r_u_sq = point2d[0] * point2d[0] + point2d[1] * point2d[1];

  const double denom = 2.0 * distortion * r_u_sq;
  const double inner_sqrt = 1.0 - 4.0 * distortion * r_u_sq;

  // If the denominator is nearly zero then we can evaluate the distorted
  // coordinates as k or r_u^2 goes to zero. Both evaluate to the identity.
  if (std::abs(denom) < 1e-15 || inner_sqrt < 0.0) {
    (*distorted_point)[0] = point2d[0];
    (*distorted_point)[1] = point2d[1];
  } else {
    const double scale = (1.0 - std::sqrt(inner_sqrt)) / denom;
    (*distorted_point)[0] = point2d[0] * scale;
    (*distorted_point)[1] = point2d[1] * scale;
  }
}

void UndistortPoint(const Eigen::Vector2d& distorted_point,
                    const double& radial_distortion,
                    Eigen::Vector2d* undistorted_point) {
  const double r_d_sq = distorted_point[0] * distorted_point[0] +
                        distorted_point[1] * distorted_point[1];

  const double undistortion = 1.0 / (1.0 + radial_distortion * r_d_sq);
  (*undistorted_point)[0] = distorted_point[0] * undistortion;
  (*undistorted_point)[1] = distorted_point[1] * undistortion;
}

void P4pfrTestWithNoise(const Matrix3d& gt_rotation,
                        const Vector3d& gt_translation,
                        const double focal_length,
                        const double radial_distortion,
                        const std::vector<Vector3d>& world_points_vector,
                        const double noise) {
  // Camera intrinsics matrix.
  const Matrix3d camera_matrix =
      Eigen::DiagonalMatrix<double, 3>(focal_length, focal_length, 1.0);
  Matrix<double, 3, 4> gt_transformation, gt_projection;
  gt_transformation << gt_rotation, gt_translation;
  gt_projection = camera_matrix * gt_transformation;

  // Reproject 3D points to get undistorted image points.
  std::vector<Eigen::Vector2d> distorted_image_points_vector(min_nr_points);
  for (int i = 0; i < min_nr_points; ++i) {
    Eigen::Vector3d point_in_cam =
        camera_matrix * (gt_rotation * world_points_vector[i] + gt_translation);
    Eigen::Vector2d undistorted_image_point = point_in_cam.hnormalized();
    DistortPoint(undistorted_image_point, radial_distortion,
                 &distorted_image_points_vector[i]);
    if (noise) {
      AddNoiseToProjection(noise, &rng, &distorted_image_points_vector[i]);
    }
  }

  // Run the P4Pfr algorithm.
  std::vector<Matrix3d> soln_rotations;
  std::vector<Vector3d> soln_translations;
  std::vector<double> soln_focal_lenghts;
  std::vector<double> soln_radial_distortions;

//  CHECK(FourPointsPoseFocalLengthRadialDistortion(
//      distorted_image_points_vector, world_points_vector, 2000.0, 0.0, -1e-5,
//      -1e-10, &soln_rotations, &soln_translations, &soln_radial_distortions,
//      &soln_focal_lenghts));

  double smallest_error = std::numeric_limits<double>::max();
  int solution_idx = -1;
  for (int i = 0; i < soln_radial_distortions.size(); ++i) {
    double error_sum = 0.0;
    // Check the reprojection error.
    for (int n = 0; n < min_nr_points; n++) {
      Matrix3d K = Vector3d(soln_focal_lenghts[i], soln_focal_lenghts[i], 1.0)
                       .asDiagonal();
      Eigen::Vector3d point_in_cam =
          K *
          (soln_rotations[i] * world_points_vector[n] + soln_translations[i]);
      Eigen::Vector2d undistorted_image_point = point_in_cam.hnormalized();
      Eigen::Vector2d undistorted_point;
      Eigen::Vector2d distorted_point;
      //UndistortPoint(distorted_image_points_vector[n],
      //               soln_radial_distortions[i],&undistorted_point);
      DistortPoint(undistorted_image_point, soln_radial_distortions[i],
                   &distorted_point);

      //double reproj_error =
      //    (undistorted_point - undistorted_image_point).squaredNorm();
      double reproj_error =
          (distorted_point - distorted_image_points_vector[n]).squaredNorm();
      error_sum += reproj_error;
    }
    if (error_sum < smallest_error) {
      smallest_error = error_sum;
      solution_idx = i;
    }
  }

  if (solution_idx >= 0) {
    // Expect poses are near.
    EXPECT_TRUE(test::ArraysEqualUpToScale(
        9, gt_rotation.data(), soln_rotations[solution_idx].data(), 1e-1));
    // The position is more noisy than the rotation usually.
    EXPECT_TRUE(test::ArraysEqualUpToScale(
        3, gt_translation.data(), soln_translations[solution_idx].data(),
        1e-1));

    // Expect focal length is near.
    static const double kFocalLengthTolerance = 0.3;
    EXPECT_NEAR(soln_focal_lenghts[solution_idx], focal_length,
                kFocalLengthTolerance * focal_length);
    // Expect radial distortion is near.
    EXPECT_NEAR(soln_radial_distortions[solution_idx], radial_distortion,
                std::abs(kFocalLengthTolerance * radial_distortion));
  } else {
    EXPECT_NEAR(1, 2, 0);
    VLOG(1) << "P4Pfr did not find a solution.";
  }
}

void BasicTest(const double noise) {
  // focal length
  const double focal_length = 1000;
  // radial distortion
  const double radial_distortion = -1e-7;

  const double x = -0.10;  // rotation of the view around x axis
  const double y = -0.20;  // rotation of the view around y axis
  const double z = 0.30;   // rotation of the view around z axis

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0, -sin(z), cos(z), 0, 0, 0, 1;
  Ry << cos(y), 0, -sin(y), 0, 1, 0, sin(y), 0, cos(y);
  Rx << 1, 0, 0, 0, cos(x), sin(x), 0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation = Vector3d(-0.00950692, 0.0171496, 0.0508743);

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector(min_nr_points);
  Map<Matrix<double, 3, min_nr_points>> world_points(
      world_points_vector[0].data());
  world_points << -0.42941, 0.000621211, -0.350949, -1.45205, 0.415794,
      -0.556605, -1.92898, -1.89976, 1.4949, 0.838307, 1.41972, 1.25756;
  P4pfrTestWithNoise(gt_rotation, gt_translation, focal_length,
                     radial_distortion, world_points_vector, noise);
}

void PlanarTestWithNoise(const double noise) {
  // focal length
  const double focal_length = 1000;
  // radial distortion
  const double radial_distortion = -1e-7;

  const double size = 2;
  const double depth = 5;

  const double x = -0.10;  // rotation of the view around x axis
  const double y = -0.20;  // rotation of the view around y axis
  const double z = 0.10;   // rotation of the view around z axis

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0, -sin(z), cos(z), 0, 0, 0, 1;
  Ry << cos(y), 0, -sin(y), 0, 1, 0, sin(y), 0, cos(y);
  Rx << 1, 0, 0, 0, cos(x), sin(x), 0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation = Vector3d(1.1, 0.2, 0.0);

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector(min_nr_points);
  world_points_vector[0] = Vector3d(-size / 2, -size / 2, depth + 1e-10);
  world_points_vector[1] = Vector3d(size / 2, -size / 2, depth - 1e-10);
  world_points_vector[2] = Vector3d(size / 2, size / 2, depth + 1e-8);
  world_points_vector[3] = Vector3d(-size / 2, size / 2, depth - 1e-7);

  P4pfrTestWithNoise(gt_rotation, gt_translation, focal_length,
                     radial_distortion, world_points_vector, noise);
}

TEST(P4Pfr, BasicTest) { BasicTest(0.0); }

TEST(P4Pfr, BasicNoiseTest) { BasicTest(0.5); }

TEST(P4Pfr, PlanarTestNoNoise) { PlanarTestWithNoise(0.0); }

TEST(P4Pfr, PlanarTestWithNoise) { PlanarTestWithNoise(0.5); }
}  // namespace
}  // namespace theia


#include "lipton-tarjan.h"
#include <iostream>
#include <fstream>
#include <csignal>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/graph/graphviz.hpp>
using namespace std;
using namespace boost;

Graph load_graph()
{
        ifstream in("graph_in");
        if( !in ){
                cerr << "file \"in\" not found!\n";
                exit(0);
        }

        string str;
	Graph g;
        while( getline(in, str) ){
                uint   colon = str.find(",");
                string stra  = str.substr(0, colon); trim(stra);
                string strb  = str.substr(colon+1 ); trim(strb);
                uint   a     = lexical_cast<uint>(stra);
                uint   b     = lexical_cast<uint>(strb);
                add_edge(a, b, g);
        }
        return g;
}

struct Coord
{
        size_t x, y;
};

typedef vector<Coord> StraightLineDrawingStorage;

Graph* gg;

struct pos_writer
{
        template <class VertexOrEdge>
        void operator() (ostream& out, const VertexOrEdge& v) const
        {
                Graph& g = *gg;
                int x = g[v].x;
                int y = g[v].y;
                out << "[pos=\"" << lexical_cast<int>(x) << ',' << lexical_cast<int>(y) << "!\"]";
        }
};

void save_graph(Graph g, Embedding* embedding, vector<VertexDescriptor> ordering)
{
        typedef iterator_property_map<StraightLineDrawingStorage::iterator, property_map<Graph, vertex_index_t>::type> StraightLineDrawing;

        StraightLineDrawingStorage straight_line_drawing_storage(num_vertices(g));
        StraightLineDrawing        straight_line_drawing        (straight_line_drawing_storage.begin(), get(vertex_index,g));

        chrobak_payne_straight_line_drawing(g, *embedding, ordering.begin(), ordering.end(), straight_line_drawing);

        ofstream f2("out_graph.txt");
        graph_traits<Graph>::vertex_iterator vi, vi_end;
        int i = 0;
        for( tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi ){
                Coord coord(get(straight_line_drawing,*vi));
                f2 << coord.x << ", " << coord.y << '\n';
                g[i].x = coord.x;
                g[i].y = coord.y;
                i++;
        }

        gg = &g;

        ofstream f("out_graph.dot");
        write_graphviz(f, g, pos_writer());

}

int main()
{
        auto g = load_graph();
	auto p = lipton_tarjan(g);
	save_graph(g, p.embedding, p.ordering);
}