#include "ceres/rotation.h"
#include "ceres/jet.h"
#include <Eigen/Dense>
#include <TooN/TooN.h>
#include <cmath>
#include <libv/lma/color/console.hpp>
#include <libv/lma/string/string_utiliy.hpp>
#include <libv/lma/time/tictoc.hpp>
#include <libv/lma/numeric/ad/rt/ad.hpp>
#include <libv/lma/numeric/ad/ct/adct.hpp>
#include <typeinfo>

  typedef Eigen::Matrix<double,9,1> Camera;
  typedef Eigen::Matrix<double,3,1> Point3d;
  typedef Eigen::Matrix<double,2,1> Point2d;

  template<class T>
  void analytical_derivative(const Camera& camera_, const Point3d& point_, const Eigen::Vector2d& obs, Eigen::Matrix<double,2,12>& j)
  {
    const T camera[3] = {T(camera_[0],0),T(camera_[1],1),T(camera_[2],2)};
    const T point[3] = {T(point_[0],9),T(point_[1],10),T(point_[2],11)};
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    p[0] += T(camera_[3],3);
    p[1] += T(camera_[4],4);
    p[2] += T(camera_[5],5);
    auto xp = - p[0] / p[2];
    auto yp = - p[1] / p[2];
    const T l1(camera_[7],7);
    const T l2(camera_[8],8);
    auto r2 = xp*xp + yp*yp;
    auto distortion = T(1.0) + r2  * (l1 + l2  * r2);
    T focal(camera_[6],6);
    auto jacobx = focal * distortion * xp - T(obs.x());
    auto jacoby = focal * distortion * yp - T(obs.y());
    for(int i = 0 ; i < 12 ; ++i)
    {
      j(0,i) = jacobx.v[i];
      j(1,i) = jacoby.v[i];
    }
  }

  template<class T>
  void analytical_derivative2(const Camera& camera_, const Point3d& point_, const Point2d& obs, TooN::Matrix<2,12,double>& j)
  {
    typedef T Scalar;
    const std::array<T,9> c = {
      T(camera_[0],0),T(camera_[1],1),T(camera_[2],2),
      T(camera_[3],3),T(camera_[4],4),T(camera_[5],5),
      T(camera_[6],6),T(camera_[7],7),T(camera_[8],8)};

    const std::array<T,3> pt = {T(point_[0],9),T(point_[1],10),T(point_[2],11)};

    std::array<T,2> p;

    const T theta2 = c[0]*c[0] + c[1]*c[1] + c[2]*c[2];
    if (theta2 > Scalar(std::numeric_limits<double>::epsilon()))
    {
      const T
        theta = sqrt(theta2),
        costheta = cos(theta),
        sintheta = sin(theta),
        theta_inverse = 1.0 / theta,
        w[3] = { c[0] * theta_inverse, c[1] * theta_inverse, c[2] * theta_inverse },
        tmp = (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (Scalar(1.0) - costheta),
        p2 =    pt[2] * costheta + (w[0] * pt[1] - w[1] * pt[0]) * sintheta + w[2] * tmp + c[5];
      p[0] = - (pt[0] * costheta + (w[1] * pt[2] - w[2] * pt[1]) * sintheta + w[0] * tmp + c[3]) / p2;
      p[1] = - (pt[1] * costheta + (w[2] * pt[0] - w[0] * pt[2]) * sintheta + w[1] * tmp + c[4]) / p2;
    }
    else
    {
      const T p2 = pt[2] + c[0] * pt[1] - c[1] * pt[0] + c[5];
      p[0] = - (pt[0] + c[1] * pt[2] - c[2] * pt[1] + c[3]) / p2;
      p[1] = - (pt[1] + c[2] * pt[0] - c[0] * pt[2] + c[4]) / p2;
    }

    const T
      r2 = p[0]*p[0] + p[1]*p[1],
      fx = c[6] * (Scalar(1.0) + r2  * (c[7] + c[8]  * r2));

    const typename T::Array
      jacobx = (fx * p[0] - Scalar(obs.x())).infinite(),
      jacoby = (fx * p[1] - Scalar(obs.y())).infinite();

    for(int i = 0 ; i < 12 ; ++i)
    {
      j(0,i) = jacobx[i];
      j(1,i) = jacoby[i];
    }
  }

  template<class T> auto f(const T& a, const T& b, const T& c)
  {
    return - a * 2.5 * b * a * c * a - 2.0 * a * b * a * b / 10.0 + a /2.0 + a * sqrt(b * c * cos( sin( b * b + c) / a * c / 5.0));
  }

  inline void test_adrt(double x, double y, double z, double d[3], double& r)
  {
    AdRt::Ad<double,3> a(x,0),b(y,1),c(z,2);
    AdRt::Ad<double,3> derivatives = f(a,b,c);
    d[0] = derivatives.infinite[0]; d[1] = derivatives.infinite[1]; d[2] = derivatives.infinite[2];
    r = derivatives.value;
  }

  inline void test_adct(double x, double y, double z, double d[3], double& r)
  {
    adct::Ad<double,3> a(x,0),b(y,1),c(z,2);
    auto expr = f(a,b,c);
    auto derivatives = expr.infinite();
    d[0] = derivatives[0];d[1] = derivatives[1];d[2] = derivatives[2];
    r = expr.value();
  }

  inline void test_jet(double x, double y, double z, double d[3], double& r)
  {
    ceres::Jet<double,3> a(x,0),b(y,1),c(z,2);
    auto derivatives = f(a,b,c);
    d[0] = derivatives.v[0];d[1] = derivatives.v[1];d[2] = derivatives.v[2];
    r = derivatives.a;
  }

int main()
{
  std::cout << std::endl << std::endl;
  double d[3] = {0,0,0};
  double r = 0;

  size_t N = 1000000;

  utils::Tic<true> ticAdrt("Adrt");
  for(size_t i = 0 ; i < N; ++i)
    test_adrt(i,i/2,i/3,d,r);
  ticAdrt.disp();

  std::cout << color.red() << " adrt dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r << color.reset() << std::endl;
  d[0] = 0; d[1] = 0, d[2] = 0;
  r = 0;

  utils::Tic<true> ticAd("AdCt");
  for(size_t i = 0 ; i < N; ++i)
    test_adct(i,i/2,i/3,d,r);
  ticAd.disp();

  std::cout << color.red() << " adct dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r << color.reset() << std::endl;

  d[0] = 0; d[1] = 0, d[2] = 0;
  r = 0;
  utils::Tic<true> ticjet("jet");
  for(size_t i = 0 ; i < N; ++i)
    test_jet(i,i/2,i/3,d,r);
  ticjet.disp();

  std::cout << color.red() << " jet  dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r <<  color.reset() << std::endl;

  Camera cam; cam << 0,0,0,10,10,10,10,10,10;
  Point3d pt; pt << 100,200,150;
  Point2d obs; obs << 100,100;

  {
    Eigen::Matrix<double,2,12> jacob;
    utils::Tic<true> jet_proj("Jet Proj");
    for(size_t i = 0 ; i < N; ++i)
      analytical_derivative<ceres::Jet<double,12>>(cam,pt,obs,jacob);
    jet_proj.disp();
    std::cout << jacob << std::endl;
  }

  {
    TooN::Matrix<2,12,double> jacob;
    utils::Tic<true> ad_proj("Ad Proj");
    for(size_t i = 0 ; i < N; ++i)
      analytical_derivative2<adct::Ad<double,12>>(cam,pt,obs,jacob);
    ad_proj.disp();
    std::cout << jacob << std::endl;
  }
//
}


#ifndef STAN_MATH_PRIM_MAT_FUN_TRACE_GEN_QUAD_FORM_HPP
#define STAN_MATH_PRIM_MAT_FUN_TRACE_GEN_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>

namespace stan {
  namespace math {
    /**
     * Compute trace(D B^T A B).
     **/
    template<int RD, int CD, int RA, int CA, int RB, int CB>
    inline double
    trace_gen_quad_form(const Eigen::Matrix<double, RD, CD> &D,
                        const Eigen::Matrix<double, RA, CA> &A,
                        const Eigen::Matrix<double, RB, CB> &B) {
      check_square("trace_gen_quad_form", "A", A);
      check_square("trace_gen_quad_form", "D", D);
      check_multiplicable("trace_gen_quad_form",
                          "A", A,
                          "B", B);
      check_multiplicable("trace_gen_quad_form",
                          "B", B,
                          "D", D);
      return (D*B.transpose()*A*B).trace();
    }

  }
}
#endif


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

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar corner_threshold_degrees,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  Eigen::Matrix<Eigen::Index,Eigen::Dynamic,1> VF,NK;
  vertex_triangle_adjacency(F,V.rows(),VF,NK);
  return per_corner_normals(V,F,corner_threshold_degrees,VF,NK,CN);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedVF,
  typename DerivedNI,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar corner_threshold_degrees,
  const Eigen::MatrixBase<DerivedVF> & VF,
  const Eigen::MatrixBase<DerivedNI> & NI,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Index Index;
  // unit normals
  Eigen::Matrix<Scalar,Eigen::Dynamic,3,Eigen::RowMajor> FN(F.rows(),3);
  // face areas
  Eigen::Matrix<Scalar,Eigen::Dynamic,1> FA(F.rows());
  igl::parallel_for(F.rows(),[&](const Index f)
  {
    const Eigen::Matrix<Scalar,1,3> v10 = V.row(F(f,1))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> v20 = V.row(F(f,2))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> n = v10.cross(v20);
    const Scalar a = n.norm();
    FA(f) = a;
    FN.row(f) = n/a;
  },10000);

  // number of faces
  const Index m = F.rows();
  // valence of faces
  const Index n = F.cols();
  assert(n == 3);

  // initialize output to ***zero***
  CN.setZero(m*n,3);

  const Scalar cos_thresh = cos(corner_threshold_degrees*igl::PI/180);
  // loop over faces
  //for(Index i = 0;i<m;i++)
  igl::parallel_for(F.rows(),[&](const Index i)
  {
    // Normal of this face
    const auto & fnhat = FN.row(i);
    // loop over corners
    for(Index j = 0;j<n;j++)
    {
      const auto & v = F(i,j);
      for(int k = NI[v]; k<NI[v+1]; k++)
      {
        const auto & ifn = FN.row(VF[k]);
        // dot product between face's normal and other face's normal
        const Scalar dp = fnhat.dot(ifn);
        // if difference in normal is slight then add to average
        if(dp > cos_thresh)
        {
          // add to running sum
          CN.row(i*n+j) += ifn*FA(VF[k]);
        }
      }
      // normalize to take average
      CN.row(i*n+j).normalize();
    }
  },10000);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedCI,
  typename DerivedCC,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const Eigen::MatrixBase<DerivedCI> & CI,
  const Eigen::MatrixBase<DerivedCC> & CC,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Index Index;
  assert(CC.rows() == F.rows()*3+1);
  // area weighted normals
  Eigen::Matrix<Scalar,Eigen::Dynamic,3,Eigen::RowMajor> FN(F.rows(),3);
  //for(Index f = 0;f<F.rows();f++)
  igl::parallel_for(F.rows(),[&](const Index f)
  {
    const Eigen::Matrix<Scalar,1,3> v10 = V.row(F(f,1))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> v20 = V.row(F(f,2))-V.row(F(f,0));
    FN.row(f) = v10.cross(v20);
  },10000);

  // number of faces
  const Index m = F.rows();
  // valence of faces
  const Index n = F.cols();
  assert(n == 3);

  // initialize output to ***zero***
  CN.setZero(m*n,3);
  // loop over faces
  igl::parallel_for(m*n,[&](const Index ci)
  {
    for(int k = CC(ci); k<CC(ci+1); k++)
    {
      // add to running sum
      const auto cfk = CI(k);
      CN.row(ci) += FN.row(cfk);
    }
    // normalize to take average
    CN.row(ci).normalize();
  },10000);
}

template <typename DerivedNV, typename DerivedNF, typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedNV> & NV,
  const Eigen::MatrixBase<DerivedNF> & NF,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  const auto m = NF.rows();
  const auto nc = NF.cols();
  CN.resize(m*nc,3);
  for(Eigen::Index i = 0;i<m;i++)
  {
    for(Eigen::Index c = 0;c<nc;c++)
    {
      CN.row(i*nc+c) = NV.row(NF(i,c));
    }
  }
}

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedN,
  typename DerivedVV,
  typename DerivedFF,
  typename DerivedJ,
  typename DerivedNN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  const typename DerivedV::Scalar corner_threshold_degrees,
  Eigen::PlainObjectBase<DerivedN>  & N,
  Eigen::PlainObjectBase<DerivedVV> & VV,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedJ>  & J,
  Eigen::PlainObjectBase<DerivedNN> & NN)
{
  const Eigen::Index m = C.size()-1;
  typedef Eigen::Index Index;
  Eigen::MatrixXd FN;
  per_face_normals(V,I,C,FN,VV,FF,J);
  typedef typename DerivedN::Scalar Scalar;
  Eigen::Matrix<Scalar,Eigen::Dynamic,1> AA;
  doublearea(VV,FF,AA);
  // VF[i](j) = p means p is the jth face incident on vertex i
  // to-do micro-optimization to avoid vector<vector>
  std::vector<std::vector<Eigen::Index>> VF(V.rows());
  for(Eigen::Index p = 0;p<m;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    for(Eigen::Index i = 0;i<np;i++)
    {
      VF[I(C(p)+i)].push_back(p);
    }
  }

  N.resize(I.rows(),3);
  for(Eigen::Index p = 0;p<m;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    Eigen::Matrix<Scalar,3,1> fn = FN.row(p);
    for(Eigen::Index i = 0;i<np;i++)
    {
      N.row(C(p)+i).setZero();
      // Loop over faces sharing this vertex
      for(const auto & n : VF[I(C(p)+i)])
      {
        Eigen::Matrix<Scalar,3,1> ifn = FN.row(n);
        // dot product between face's normal and other face's normal
        Scalar dp = fn.dot(ifn);
        if(dp > cos(corner_threshold_degrees*igl::PI/180))
        {
          // add to running sum
          N.row(C(p)+i) += AA(n) * ifn;
        }
      }
      N.row(C(p)+i).normalize();
    }
  }

  // Relies on order of FF
  NN.resize(FF.rows()*3,3);
  {
    Eigen::Index k = 0;
    for(Eigen::Index p = 0;p<m;p++)
    {
      // number of faces/vertices in this simple polygon
      const Index np = C(p+1)-C(p);
      for(Eigen::Index i = 0;i<np;i++)
      {
        assert(FF(k,0) == I(C(p)+((i+0)%np)));
        assert(FF(k,1) == I(C(p)+((i+1)%np)));
        assert(FF(k,2) == V.rows()+p);
        NN.row(k*3+0) = N.row(C(p)+((i+0)%np));
        NN.row(k*3+1) = N.row(C(p)+((i+1)%np));
        NN.row(k*3+2) = FN.row(p);
        k++;
      }
    }
    assert(k == FF.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
// Clang thinks this is the same as the one below, but Windows doesn't?
// template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, double, Eigen::PlainObjectBase< Eigen::Matrix<double, -1, -1, 0, -1, -1> > &);
#endif


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/**
 * creates synthetic data
 * (1) creates factors according to a distribution
 * (2) creates 2 train matrices
 * (3) creates 1 test file
 */
#include <iostream>

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

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

#include <mf/mf.h>

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

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

int main(int argc, char* argv[]) {
	std::string outDir("/tmp/");

	// parameters for the factorization
	mf_size_type size1 = 100000;
	mf_size_type size2 = 100000;
	mf_size_type nnzSmall = 1000000;
	mf_size_type nnzTest = 100000;
	mf_size_type r = 10;

	double sigma = sqrt(10); // standard deviation for a Normal distribution N(0,10)

	// generate original factors by sampling from a N(0,10) distribution
	std::cout<<"Generating Solution factors..."<<std::endl;
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
	generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));
	//std::cout<<"Storing Solution factors..."<<std::endl;
	//writeMatrix(outDir+"solW.mma",wIn);
	//writeMatrix(outDir+"solH.mma",hIn);

//	std::cout<<"reading solution factors..."<<std::endl;
//	DenseMatrix wIn;
//	DenseMatrixCM hIn;
//	readMatrix(inDir+"solW.mma",wIn);
//	readMatrix(inDir+"solH.mma",hIn);

	// generate a sparse matrices by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix vSmall,vLarge;
	std::cout<<"Generating Small Matrix..."<<std::endl;
	generateRandom(vSmall, nnzSmall, wIn, hIn, random);
	std::cout<<"adding noise..."<<std::endl;
	addRandom(vSmall, random, boost::normal_distribution<>(0,1)); // the noise distribution is N(0,1)
	LOG4CXX_INFO(logger, "Small Data matrix: "
			<< vSmall.size1() << " x " << vSmall.size2() << ", " << vSmall.nnz() << " nonzeros");
	std::cout<<"Storing Small Matrix..."<<std::endl;
	writeMatrix(outDir+"train.mmc",vSmall);

	// create a test matrix (without noise)
	std::cout<<"Generating Test Matrix..."<<std::endl;
	SparseMatrix vTest;
	generateRandom(vTest, nnzTest, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
			<< vTest.size1() << " x " << vTest.size2() << ", " << vTest.nnz() << " nonzeros");
	std::cout<<"Storing Test Matrix..."<<std::endl;
	writeMatrix(outDir+"test.mmc",vTest);

	// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
	std::cout<<"Generating Initial Factors..."<<std::endl;
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
	generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));
	std::cout<<"Storing Initial Factors..."<<std::endl;
	writeMatrix(outDir+"W.mma",w);
	writeMatrix(outDir+"H.mma",h);

	return 0;
}


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

#include <boost/integer.hpp>
#include <iostream>

int main()
{
 std::cout << std::numeric_limits<boost::uint_t<65>::least>::digits;
 return 0;
}


#pragma once

#include <algorithm>
#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
#include <vector>

// ------------------------------------------------------------
#include "aux/hash_specializations.hpp"
#include "laguerren_impl.hpp"

namespace boltzmann {

template <typename NUMERIC>
class LaguerreNKS
{
 public:
  typedef NUMERIC numeric_t;

 public:
  LaguerreNKS(int K)
      : Y_(K + 1)
      , K_(K)
      , is_initialized_(false)
  { }

  void compute(const std::vector<numeric_t>& x);

  /**
   *
   * @param x evaluation points
   * @param n number of evaluation points
   * @param expw evaluation weight: e^(-r^2*expw)
   */
  void compute(const numeric_t* x, unsigned int n, numeric_t expw = numeric_t(0));

  unsigned int get_npoints() const { return Y_[0].shape()[1]; }

 public:
  typedef boost::multi_array<numeric_t, 2> array_t;

 public:
  const NUMERIC* get(unsigned int k, unsigned int alpha) const;
  void info() const;

 private:
  std::vector<array_t> Y_;
  unsigned int K_;
  bool is_initialized_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::compute(const std::vector<numeric_t>& x)
{
  compute(x.data(), x.size());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::compute(const numeric_t* x, unsigned int n, numeric_t expw)
{
  std::vector<numeric_t> x2(n);
  std::transform(x, x + n, x2.begin(), [](const numeric_t& v) { return v * v; });
  // L_n-1
  std::vector<numeric_t> Lnm1(n);
  // L_n-2
  std::vector<numeric_t> Lnm2(n);

  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    Y_[alpha].resize(boost::extents[K_ / 2 + 1][n]);
// init
#pragma omp parallel for
    for (size_t xi = 0; xi < n; ++xi) {
      numeric_t fexp = 1;
      if (::math::abs(expw) > 1e-16) fexp = ::math::exp(-expw * x2[xi]);
      Y_[alpha][0][xi] = boost::math::laguerren(0, alpha, x2[xi]) * fexp;
      Y_[alpha][1][xi] = boost::math::laguerren(1, alpha, x2[xi]) * fexp;
    }

    for (unsigned int kp = 2; kp <= K_ / 2; ++kp) {
#pragma omp parallel for
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][kp][xi] = boost::math::laguerren_next(
            kp - 1, alpha, x2[xi], Y_[alpha][kp - 1][xi], Y_[alpha][kp - 2][xi]);
      }
    }
  }

  boost::multi_array<numeric_t, 2> powers_of_x(boost::extents[K_ + 1][n]);

  for (unsigned int xi = 0; xi < n; ++xi) {
    powers_of_x[0][xi] = 1.0;
  }

  for (unsigned int alpha = 1; alpha <= K_; ++alpha) {
    for (unsigned int xi = 0; xi < n; ++xi) {
      powers_of_x[alpha][xi] = x[xi] * powers_of_x[alpha - 1][xi];
    }
  }

  // add factor x^(2j+k%2)
  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    for (unsigned int kp = 0; kp <= K_ / 2; ++kp) {
      int j = alpha / 2;
      int k = 2 * kp + 2 * j + alpha % 2;
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][kp][xi] *= powers_of_x[2 * j + k % 2][xi];
      }
    }
  }

  is_initialized_ = true;
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
const NUMERIC*
LaguerreNKS<NUMERIC>::get(unsigned int k, unsigned int alpha) const
{
  assert(is_initialized_);
  assert(alpha < Y_.size());
  assert(k < Y_[alpha].shape()[0]);
  return Y_[alpha][k].origin();
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::info() const
{
  unsigned long long int nentries = 0;
  for (unsigned int alpha = 0; alpha < Y_.size(); ++alpha) {
    nentries += Y_[alpha].shape()[0] * Y_[alpha].shape()[1];
  }

  std::cout << " LaguerreN uses " << nentries * sizeof(NUMERIC) / 1e6 << " MB" << std::endl;
}

}  // end namespace boltzmann


/*
 * Driver for ODE integration
 */

#include <iostream>
#include <vector>
#include <cmath>
#include <cassert>
#include <boost/numeric/odeint.hpp>

#include "fast_detector_model.hpp"

/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;

/* the type returned by std::vector.size() */
typedef std::vector< double >::size_type T_vecsize;

/* copy a carray into a vector */
std::vector< double >
carray_to_vector(const double *data, int N)
{
    std::vector< double > vec;
    vec.assign(data, data+N);
    return vec;
}

/* TODO: doc */
struct linear_interpolator {
    double m_timestep;
    std::vector< double > m_interpolation_values;
    int m_mode;
    T_vecsize N;

    linear_interpolator( double timestep, std::vector< double > intvals, int mode) :
                                    m_timestep(timestep),
                                    m_interpolation_values(intvals),
                                    m_mode(mode)
    {
        N = intvals.size();
        /*
        std::cout << "linear interpolator init\n";
        for(int ii=0; ii<10; ii++)
        {
            std::cout << ii << '\t' << intvals[ii] << std::endl;
        }
        */

    }

    double operator() ( const double t )
    {
        double p, i1, i2, w1, w2, r;
        T_vecsize ii1, ii2;
        //linear interpolation
        p = t/m_timestep;
        if (p <= 0)
        {
            r = m_interpolation_values[0];
        }
        else if (p >= N-1)
        {
            r = m_interpolation_values[N-1];
        }
        else
        {
            /* piecewise-constant - return Y0 for t < 0 between t0 and t1
                                           Y1 for t0 <= t < t1 , etc*/
            if(m_mode == 0)
            {
                i1 = floor(p);
                ii1 = (T_vecsize)i1 + 1;
                r = m_interpolation_values[ii1];
            }
            /* linear interpolation */
            else if (m_mode == 1)
            {
                i1 = floor(p);
                i2 = ceil(p);
                w2 = (p-i1);
                w1 = 1.0-w2;
                ii1 = (T_vecsize)i1;
                ii2 = (T_vecsize)i2;
                /*
                std::cout << "p:" << p << " t:" << t << "i1:" << i1 << " i2:" << i2 << std::endl;
                std::cout << w1 << " " << w2 << std::endl;
                std::cout << m_interpolation_values[ii1] << " " << m_interpolation_values[ii2] << std::endl;
                */
                //std::cout << ii1 << " " << ii2 << std::endl;
                r = w1*m_interpolation_values[ii1] + w2*m_interpolation_values[ii2];
            }
            /* invalid mode TODO: how to flag the error? */
            else
            {
                assert(false);
                r = INFINITY;
            }
        }
        return r;
    }
};

// calculate Na, Nb, Nc from generated code (copied from generated.c)
void calc_NaNbNc(double t, double Nrn, double lamp,
                 double &Na, double &Nb, double &Nc)
{
    using namespace std; //for math functions pow, exp
    Na = Nrn*lamrn/(lama + lamp) - Nrn*lamrn*exp(-t*(lama + lamp))/(lama +
            lamp);
    Nb = Nrn*lama*lamrn/(lama*lamb + pow(lamp, 2) + lamp*(lama + lamb)) -
            Nrn*lama*lamrn*exp(-t*(lamb + lamp))/(lama*lamb - pow(lamb, 2) +
            lamp*(lama - lamb)) + Nrn*lama*lamrn*exp(-t*(lama + lamp))/
            (pow(lama, 2) - lama*lamb + lamp*(lama - lamb));
    Nc = Nrn*lama*lamb*lamrn/(lama*lamb*lamc + pow(lamp, 3) + pow(lamp, 2)*
            (lama + lamb + lamc) + lamp*(lama*lamb + lamc*(lama + lamb))) -
            Nrn*lama*lamb*lamrn*exp(-t*(lamc + lamp))/(lama*lamb*lamc +
            pow(lamc, 3) - pow(lamc, 2)*(lama + lamb) + lamp*(lama*lamb +
            pow(lamc, 2) - lamc*(lama + lamb))) + Nrn*lama*lamb*lamrn*
            exp(-t*(lamb + lamp))/(lama*pow(lamb, 2) - pow(lamb, 3) -
            lamc*(lama*lamb - pow(lamb, 2)) + lamp*(lama*lamb - pow(lamb, 2)
            - lamc*(lama - lamb))) - Nrn*lama*lamb*lamrn*exp(-t*(lama + lamp))
            /(pow(lama, 3) - pow(lama, 2)*lamb - lamc*(pow(lama, 2) - lama*lamb)
             + lamp*(pow(lama, 2) - lama*lamb - lamc*(lama - lamb)));
    return;
}

// Compute the steady-state solution for the state variable Y
// returns Yss : state_type( [Nrnd, Nrn, Fa, Fb, Fc] )

state_type calc_steady_state(double Nrn, double Q, double rs, double lamp,
                      double V_tank, double recoil_prob, double eff)
{
    double tt, Na, Nb, Nc, Fa, Fb, Fc;
    state_type Yss;
    double Acc_counts;
    // transit time assuming plug flow in the tank
    tt = V_tank / Q;
    calc_NaNbNc(tt, Nrn, lamp, Na, Nb, Nc);

    // expressions based on these lines from detector_state_rate_of_change
    // dFadt = Q*rs*Na - Fa*lama
    // dFbdt = Q*rs*Nb - Fb*lamb + Fa*lama * (1.0-recoil_prob)
    // dFcdt = Q*rs*Nc - Fc*lamc + Fb*lamb
    Fa = Na*Q*rs/lama;
    Fb = (Q*rs*Nb + Fa*lama * (1.0-recoil_prob)) / lamb;
    Fc = (Q*rs*Nc + Fb*lamb) / lamc;

    // accumulated counts, in this case over one second
    Acc_counts = eff*(Fa*lama + Fc*lamc);
    // pack output into vector
    Yss.push_back(Nrn);
    Yss.push_back(Nrn);
    Yss.push_back(Nrn);
    Yss.push_back(Fa);
    Yss.push_back(Fb);
    Yss.push_back(Fc);
    Yss.push_back(Acc_counts);
    return Yss;
}

//[ rhs_class
/* The rhs of x' = f(x) defined as a class */
struct two_filter_detector {

    linear_interpolator m_bc;
    linear_interpolator m_airt;
    // parameters
    double Q;
    double rs;
    double lamp;
    double eff;
    double Q_external;
    double V_delay;
    double V_delay_2;
    double V_tank;
    double t_delay;
    double recoil_prob;
    double cal_source_strength;
    double cal_begin;
    double cal_duration;
    double inj_source_strength;
    double inj_begin;
    double inj_duration;

    two_filter_detector( linear_interpolator bc,
                         linear_interpolator airt,
                         double * parameters ) : m_bc(bc), m_airt(airt)
    {
        // store parameters
        Q = *parameters++;
        rs = *parameters++;
        lamp = *parameters++;
        eff = *parameters++;
        Q_external = *parameters++;
        V_delay = *parameters++;
        V_delay_2 = *parameters++;
        V_tank = *parameters++;
        t_delay = *parameters++;
        recoil_prob = *parameters++;
        cal_source_strength = *parameters++;
        cal_begin = *parameters++;
        cal_duration = *parameters++;
        inj_source_strength = *parameters++;
        inj_begin = *parameters++;
        inj_duration = *parameters++;

        /*
        std::cout << "Q: " << Q << std::endl
                  << "rs: " << rs << std::endl
                  << "lamp: " << lamp << std::endl
                  << "eff: " << eff << std::endl
                  << "Q_external: " << Q_external << std::endl
                  << "V_delay: " << V_delay << std::endl
                  << "V_delay_2: " << V_delay_2 << std::endl
                  << "V_tank: " << V_tank << std::endl
                  << "t_delay: " << t_delay << std::endl
                  << "recoil_prob: " << recoil_prob << std::endl;
        */
        /*
        std::cout << "t/60\tT\tdTdt\tRn\n";
        for (double t=-60; t<600; t+=60)
        {
            const double delt = 60;
            const double T = m_airt(t);
            double dTdt = (T - m_airt(t-delt)) / delt;
            std::cout << t/60.0 << '\t' << T << '\t' << dTdt << '\t'
                      << m_bc(t)
                      << std::endl;
        }
        */
    }

    void operator() ( const state_type &x , state_type &dxdt , const double t )
    {
        double Nrnd, Nrnd2, Nrn, Fa, Fb, Fc;
        double Nrn_inj; //radon concentration from source injected at inlet
        double Nrn_cal; //radon concentration from calibration source
        double dNrnddt, dNrnd2dt, dNrndt, dFadt, dFbdt, dFcdt, dAcc_countsdt;
        double tt, Na, Nb, Nc;
        // boundary condition
        double Nrn_ext = m_bc(t - t_delay);
        // temperature and temperature change
        double T = m_airt(t - t_delay);
        // for computing temerature rate of change by finite-difference
        const double delt = 60;
        double dTdt = (T - m_airt(t-t_delay-delt)) / delt;
        // copied from theoretical_model.py:detector_state_rate_of_change...
        // unpack state vector
        Nrnd = x[IDX_Nrnd];
        Nrnd2 = x[IDX_Nrnd2];
        Nrn = x[IDX_Nrn];
        Fa = x[IDX_Fa];
        Fb = x[IDX_Fb];
        Fc = x[IDX_Fc];
        // The radon concentration flowing into the inlet needs to be
        // bumped up if the injection source is active
        bool inj_is_active = inj_source_strength > 0.0
                                && (t - t_delay) > inj_begin
                                && (t - t_delay) <= inj_begin+inj_duration;
        if(inj_is_active)
        {
            Nrn_inj = inj_source_strength / Q_external;
        }
        else
        {
            Nrn_inj = 0;
        }

        // The radon concentration flowing into the main tank needs to be
        // bumped up if the calibration source is active
        bool cal_is_active = cal_source_strength > 0.0
                                && (t - t_delay) > cal_begin
                                && (t - t_delay) <= cal_begin+cal_duration;
        if(cal_is_active)
        {
            Nrn_cal = cal_source_strength / Q_external;
        }
        else
        {
            Nrn_cal = 0;
        }

        // make sure that we can't have V_delay_2 > 0 when V_delay == 0
        if (V_delay == 0 && V_delay_2 > 0)
        {
          V_delay = V_delay_2;
          V_delay_2 = 0;
        }

        // effect of delay and tank volumes (allow V_delay to be zero)
        if (V_delay == 0.0) // no delay tanks
        {
            dNrndt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrn)
                     - Nrn*lamrn;
            // Nrnd,Nrnd2 become unimportant, but we need to do something with them
            // so just apply the same equation as for Nrn
            dNrnddt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrnd)
                      - Nrnd*lamrn;
            dNrnd2dt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrnd2)
                                - Nrnd2*lamrn;
        }
        else if (V_delay > 0.0 && V_delay_2 == 0.0) //one delay tank
        {
          dNrnddt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd)
                                                                - Nrnd*lamrn;
          dNrndt = Q_external / V_tank * (Nrnd + Nrn_cal - Nrn) - Nrn*lamrn;
          // unused, but apply same eqn as delay tank 1
          dNrnd2dt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd2)
                                                               - Nrnd2*lamrn;
        }
        else // two delay tanks
        {
            dNrnddt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd)
                                                                  - Nrnd*lamrn;
            dNrnd2dt = Q_external / V_delay_2 * (Nrnd - Nrnd2) - Nrnd2*lamrn;
            dNrndt = Q_external / V_tank * (Nrnd2 + Nrn_cal - Nrn) - Nrn*lamrn;
        }

        // effect of temperature changes causing the tank to 'breathe'
        dNrndt -= Nrnd * dTdt/T;
        // Na, Nb, Nc from steady-state in tank
        // transit time assuming plug flow in the tank
        tt = V_tank / Q;
        calc_NaNbNc(tt, Nrn, lamp, Na, Nb, Nc);
        // compute rate of change of each state variable
        dFadt = Q*rs*Na - Fa*lama;
        dFbdt = Q*rs*Nb - Fb*lamb + Fa*lama * (1.0-recoil_prob);
        dFcdt = Q*rs*Nc - Fc*lamc + Fb*lamb;
        dAcc_countsdt = eff*(Fa*lama + Fc*lamc);
        // pack into dxdt
        dxdt[IDX_Nrnd] = dNrnddt;
        dxdt[IDX_Nrnd2] = dNrnd2dt;
        dxdt[IDX_Nrn] = dNrndt;
        dxdt[IDX_Fa] = dFadt;
        dxdt[IDX_Fb] = dFbdt;
        dxdt[IDX_Fc] = dFcdt;
        dxdt[IDX_Acc_counts] = dAcc_countsdt;
        return;
    }
};
//]

//[ integrate_observer
class push_back_state_and_time
{
    std::vector< state_type >& m_states;
    std::vector< double >& m_times;

    public:
    push_back_state_and_time( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : m_states( states ) , m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_states.push_back( x );
        m_times.push_back( t );
        // std::cout << t << std::endl;
    }

    double getval(T_vecsize idx_time, T_vecsize idx_statevec)
    {
        return m_states[idx_time][idx_statevec];
    }
};
//]

/* Interface function */
int integrate_radon_detector(int N_times,
                             double timestep,
                             int interpolation_mode,
                             double *external_radon_conc,
                             double *airt,
                             double *initial_state,
                             double *state_history,
                             double *parameters)
{
    using namespace boost::numeric::odeint;

    // initialise the boundary conditions object
    std::vector< double > intvals;
    intvals = carray_to_vector(external_radon_conc, N_times);
    linear_interpolator boundary_conditions(timestep, intvals, interpolation_mode);

    // initialise the air temperature object (always use linear intrpolation)
    std::vector< double > airt_intvals;
    airt_intvals = carray_to_vector(airt, N_times);
    linear_interpolator boundary_conditions_airt(timestep, airt_intvals, 1);

    // copy initial state into a state_type object
    state_type x0 = carray_to_vector(initial_state, NUM_STATE_VARIABLES);
    double t0 = 0.0;
    double t1 = timestep*N_times;

    // initialise the RHS of the system of equations
    two_filter_detector system_of_equations(boundary_conditions,
                                            boundary_conditions_airt,
                                            parameters);

    // initialise the stepper (integrator)
    // ref: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html
    // (it would be nicer to use 'auto' for the type, but we'd need extra compiler flags)

    // stepper options: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.stepper_overview

    //// OPTION 1: dense output stepper (step size can be larger than output grid)
    //typedef boost::numeric::odeint::result_of::make_dense_output<
    //    runge_kutta_dopri5< state_type > >::type dense_stepper_type;
    //dense_stepper_type stepper = make_dense_output( 1.0e-6 , 1.0e-5 ,
    //                                    runge_kutta_dopri5< state_type >() );

    // OPTION 2: controlled stepper (error is controlled, but dt must be chosen
    // such that the stepper ends up at exactly the endpoint)
    typedef boost::numeric::odeint::result_of::make_controlled<
        runge_kutta_dopri5< state_type > >::type controlled_stepper_type;
    controlled_stepper_type stepper = make_controlled( 1.0e-6 , 1.0e-5 ,
                                        runge_kutta_dopri5< state_type >() );

    //initialise observer (i.e. container for recording output at each step)
    std::vector< state_type > obs_x_vec;
    std::vector< double > obs_times_vec;
    obs_x_vec.reserve(N_times);
    obs_times_vec.reserve(N_times);
    push_back_state_and_time observer(obs_x_vec, obs_times_vec);

    // integrate the system of odes
    // ref: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/integrate_functions.html
    boost::numeric::odeint::integrate_const(stepper, system_of_equations,
                                            x0, t0, t1, timestep, observer);

    //copy the integration history from the observer into the output array
    double *state_iter = state_history;
    for (int ii=0; ii<N_times; ii++){
        for (int jj=0; jj<NUM_STATE_VARIABLES; jj++){
            *state_iter = observer.getval(ii,jj);
            state_iter++;
        }
    }

    return 0;
}

/* Function for testing the linear interpolation class */
double linear_interpolation(double xi, int N, double timestep, const double *y, const int mode)
{
    assert(mode == 0 || mode == 1);
    std::vector< double > intvals;
    intvals = carray_to_vector(y, N);
    /*
    for(int ii=0; ii<N; ii++){
        std::cout << intvals[ii] << " ";
    }
    std::cout << std::endl;
    */
    linear_interpolator li(timestep, intvals, mode);
    return li(xi);
}


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

#include "theia/sfm/estimators/estimate_uncalibrated_relative_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/eight_point_fundamental_matrix.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/fundamental_matrix_util.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {

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

namespace {

// An estimator for computing the relative pose from 8 feature correspondences
// (via decomposition of the fundamental matrix).
//
// NOTE: Feature correspondences must be in pixel coordinates with the principal
// point removed i.e. principal point at (0, 0). This also assumes negligible
// skew (which is reasonable for most cameras).
class UncalibratedRelativePoseEstimator
    : public Estimator<FeatureCorrespondence, UncalibratedRelativePose> {
 public:
  UncalibratedRelativePoseEstimator() {}

  // 8 correspondences are needed to determine a fundamental matrix and thus a
  // relative pose.
  double SampleSize() const { return 8; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence>& centered_correspondences,
      std::vector<UncalibratedRelativePose>* relative_poses) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    for (int i = 0; i < 8; i++) {
      image1_points.emplace_back(centered_correspondences[i].feature1);
      image2_points.emplace_back(centered_correspondences[i].feature2);
    }

    UncalibratedRelativePose relative_pose;
    if (!NormalizedEightPointFundamentalMatrix(
            image1_points, image2_points, &relative_pose.fundamental_matrix)) {
      return false;
    }

    // Only consider fundamental matrices that we can decompose focal lengths
    // from.
    if (!FocalLengthsFromFundamentalMatrix(
            relative_pose.fundamental_matrix.data(),
            &relative_pose.focal_length1,
            &relative_pose.focal_length2)) {
      return false;
    }

    // TODO(cmsweeney): Should we check if the focal lengths are reasonable?

    // Compose the essential matrix from the fundamental matrix and focal
    // lengths.
    Matrix3d essential_matrix;
    EssentialMatrixFromFundamentalMatrix(
        relative_pose.fundamental_matrix.data(),
        relative_pose.focal_length1,
        relative_pose.focal_length2,
        essential_matrix.data());

    // Normalize the centered_correspondences.
    std::vector<FeatureCorrespondence> normalized_correspondences(
        centered_correspondences.size());
    for (int i = 0; i < centered_correspondences.size(); i++) {
      normalized_correspondences[i].feature1 =
          centered_correspondences[i].feature1 / relative_pose.focal_length1;
      normalized_correspondences[i].feature2 =
          centered_correspondences[i].feature2 / relative_pose.focal_length2;
    }

    GetBestPoseFromEssentialMatrix(essential_matrix,
                                   normalized_correspondences,
                                   &relative_pose.rotation,
                                   &relative_pose.position);
    relative_poses->emplace_back(relative_pose);
    return true;
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& centered_correspondence,
               const UncalibratedRelativePose& relative_pose) const {
    FeatureCorrespondence normalized_correspondence;
    normalized_correspondence.feature1 =
        centered_correspondence.feature1 / relative_pose.focal_length1;
    normalized_correspondence.feature2 =
        centered_correspondence.feature2 / relative_pose.focal_length2;
    if (!IsTriangulatedPointInFrontOfCameras(normalized_correspondence,
                                             relative_pose.rotation,
                                             relative_pose.position)) {
      return std::numeric_limits<double>::max();
    }

    return SquaredSampsonDistance(relative_pose.fundamental_matrix,
                                  centered_correspondence.feature1,
                                  centered_correspondence.feature2);
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(UncalibratedRelativePoseEstimator);
};

}  // namespace

bool EstimateUncalibratedRelativePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& centered_correspondences,
    UncalibratedRelativePose* relative_pose,
    RansacSummary* ransac_summary) {
  UncalibratedRelativePoseEstimator relative_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<UncalibratedRelativePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type,
                                                ransac_params,
                                                relative_pose_estimator);

  // Estimate essential matrix.
  return ransac->Estimate(centered_correspondences,
                          relative_pose,
                          ransac_summary);
}

}  // namespace theia


#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <Eigen/Dense>
#include <Eigen/Core>     // to use Eigen::Map
#include "contourDetection.h"
#include <iostream>
#include <cmath>
#include <fstream>

using namespace cv;
using namespace std;

int canny_thresh = 100;
ImageROI roi = {350, 1100, 400, 1460};
SegmentationType type(Black);

int main(int argc, char const *argv[])
{
  if( argc != 3)  // load 2 images one as target and the another as current
    {
     cout <<" Required two images input " << endl;
     return -1;
    }
  Mat target;
  Mat current;
  target = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  current = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  Mat targetContour = ContourDetect(target, type, roi, canny_thresh);
  ofstream contourdata;
  contourdata.open("contourdata.dat");
  contourdata << targetContour;
  contourdata.close();
  // sampling
  int coldata = targetContour.cols;  // column size
	int rowdata = targetContour.rows;  // row size
  Eigen::Map<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> dataEigen(targetContour.ptr<int>(), rowdata, coldata);
  double radius = 10.0;
  int counterMax = 4;
  int counter = 0;
  Eigen::MatrixXi sampledata = dataEigen.block<1,2>(0,0);
  for (int i = 0; i < dataEigen.rows(); ++i)
  {
    Eigen::RowVector2i v1 = dataEigen.block<1,2>(i,0) - sampledata.block<1,2>(sampledata.rows() - 1,0);
    double disToPoint1 = v1.norm();
    if (disToPoint1 > radius)
    {
      bool flag = 0;
      for (int j = 0; j < sampledata.rows(); ++j)
      {
        Eigen::RowVector2i v2 = dataEigen.block<1,2>(i,0) - sampledata.block<1,2>(j,0);
        double disToPoint2 = v2.norm();
        if (disToPoint2 < radius)
        {
          flag = 1;
          counter++;
          break;
        }
      }
      if (flag == 0)
      {
        Eigen::MatrixXi temp(sampledata.rows() + dataEigen.block<1,2>(i,0).rows(), sampledata.cols());
        temp << sampledata, dataEigen.block<1,2>(i,0);
        sampledata = temp;
      }
    }
    if (counter > counterMax)
    {
      break;
    }
  }
  ofstream sample;
  sample.open("sample.dat");
  sample << sampledata;
  sample.close();
  return 0;
}


#include <tdp/testing/testing.h>
#include <Eigen/Dense>

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>

TEST(setupRaw, KeyframeSLAM) {
  gtsam::SharedNoiseModel model = gtsam::noiseModel::Diagonal::Variances(
        (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished());

  gtsam::Values::shared_ptr initials(new gtsam::Values);
  gtsam::NonlinearFactorGraph::shared_ptr graph(new gtsam::NonlinearFactorGraph);

  float a = 5.*M_PI/180.;
  Eigen::Matrix3d Ra;
  Ra << 1, 0, 0,
       0, cos(a), -sin(a),
       0, sin(a), cos(a);
  a = 6.*M_PI/180.;
  Eigen::Matrix3d Rb;
  Rb << 1, 0, 0,
       0, cos(a), -sin(a),
       0, sin(a), cos(a);

  Eigen::Vector3d ta(0,0,1);
  Eigen::Vector3d tb(0,0,1.1);
  Eigen::Vector3d tc(0.01,0,1.1);

  // setup Pose3s for GTSAM
  gtsam::Pose3 poseA = gtsam::Pose3(gtsam::Rot3(Ra),ta);
  gtsam::Pose3 poseB = gtsam::Pose3(gtsam::Rot3(Rb),tb);
  gtsam::Pose3 poseC = gtsam::Pose3(gtsam::Rot3(Rb),tc);
  gtsam::Pose3 poseAB = poseA.between(poseB);
  gtsam::Pose3 poseAB_obs = poseA.between(poseC);

  initials->insert(0, poseA);
  initials->insert(1, poseB);

  gtsam::PriorFactor<gtsam::Pose3>::shared_ptr factor0(
      new gtsam::PriorFactor<gtsam::Pose3>(0, poseA, model));
  graph->add(factor0);
  gtsam::NonlinearFactor::shared_ptr factor01(
      new gtsam::BetweenFactor<gtsam::Pose3>(0, 1, poseAB, model));
  graph->push_back(factor01);
  gtsam::NonlinearFactor::shared_ptr factor01_obs(
      new gtsam::BetweenFactor<gtsam::Pose3>(0, 1, poseAB_obs, model));
  graph->push_back(factor01_obs);

  graph->print();

  gtsam::GaussNewtonParams params;
  params.setVerbosity("ERROR");
  params.setMaxIterations(1);
  gtsam::GaussNewtonOptimizer optimizer(*graph, *initials, params);
  gtsam::Values results = optimizer.optimize();
}

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


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_POLYNOMIALS_FUNCTIONS_SCALAR_HERMITE_HPP_INCLUDED
#define NT2_POLYNOMIALS_FUNCTIONS_SCALAR_HERMITE_HPP_INCLUDED

#include <nt2/polynomials/functions/hermite.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( hermite_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< arithmetic_<A1> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A1>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::hermite(a0, result_type(a1));
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A1 is floating_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_DISPATCH_IMPLEMENT  (hermite_, tag::cpu_,
                             (A0)(A1),
                             (scalar_< integer_<A0> > )(scalar_< floating_<A1> > )
                            )
  {
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = a1+a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = hermite_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    hermite_next(const uint32_t& n, const T& x, const T1& Hn, const T2& Hnm1)
    {
      return (2 * x * Hn - 2 * n * Hnm1);
    }
  };
} }

#endif


#include <iostream>
#include <Eigen>
#include <fstream>
#include <unistd.h>
#include <cstdlib>
#include "denseBlocksJacobi.h"
#include "denseOverlappingJacobi.h"
#include "denseAsyncBlocksJacobi.h"
#include "denseAsyncOverlappingJacobi.h"
#include "denseAsyncJacobi.h"
#include "denseParallelJacobi.h"
#include "denseSerialJacobi.h"
#include "sparseBlocksJacobi.h"
#include "sparseOverlappingJacobi.h"
#include "sparseAsyncBlocksJacobi.h"
#include "sparseAsyncOverlappingJacobi.h"
#include "sparseAsyncJacobi.h"
#include "sparseParallelJacobi.h"
#include "sparseSerialJacobi.h"
#include "parser.h"
#include "sparseOptimizedBlocksJacobi.h"
#include "sparseOptimizedOverlappingJacobi.h"
#include "denseOptimizedBlocksJacobi.h"
#include "denseOptimizedOverlappingJacobi.h"

using namespace std;
using namespace Eigen;
using namespace Iterative;

auto fromFile = false;
auto debug = false;
auto toCsv = false;
string filename;
string inputMatrix;

ulong matrixSize = 1024;
ulong iterations = 100;
double tolerance = 0.000000001;
int workers = 8;
auto blockSize = 64;

const auto sequential = "sequential";
const auto parallel = "parallel";
const auto parallel_async = "parallel_async";
const auto blocks = "blocks";
const auto blocks_optimized = "blocks_optimized";
const auto blocks_async = "blocks_async";
const auto overlapping = "overlapping";
const auto overlapping_optimized = "overlapping_optimized";
const auto overlapping_async = "overlapping_async";

enum methods {
    SEQUENTIAL,
    PARALLEL,
    PARALLEL_ASYNC,
    BLOCKS,
    BLOCKS_OPTIMIZED,
    BLOCKS_ASYNC,
    OVERLAPPING,
    OVERLAPPING_OPTIMIZED,
    OVERLAPPING_ASYNC
};

auto method = SEQUENTIAL;
string methodString = sequential;

void parse_args(int argc, char *argv[]);
void write_csv(string fileName, std::chrono::duration<double> time, double error, long iteration);

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

    parse_args(argc, argv);

    Eigen::setNbThreads(workers);
    auto error = 0.;

    chrono::time_point<chrono::high_resolution_clock> start_time;
    chrono::time_point<chrono::high_resolution_clock> end_time;
    ColumnVector<double, Dynamic> x;
    srand(42);

#ifdef DENSE

    Matrix<double, Dynamic, Dynamic> A = Matrix<double, Dynamic, Dynamic>::Random(matrixSize, matrixSize);
    ColumnVector<double, Dynamic> b = ColumnVector<double, Dynamic>::Random(matrixSize);

#pragma omp parallel for schedule(static)
    for (int i = 0; i < A.rows(); ++i) {
        auto value = A.row(i).template lpNorm<1>();
        A(i, i) = value;
    }

    denseSerialJacobi<double, Dynamic> serialJacobi(A, b, iterations, tolerance);
    denseParallelJacobi<double, Dynamic> parallelJacobi(A, b, iterations, tolerance, workers);
    denseAsyncJacobi<double, Dynamic> asyncJacobi(A, b, iterations, tolerance, workers);
    denseBlocksJacobi<double, Dynamic> blocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOptimizedBlocksJacobi<double, Dynamic> optimizedBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOverlappingJacobi<double, Dynamic> overlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOptimizedOverlappingJacobi<double, Dynamic> optimizedOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseAsyncBlocksJacobi<double, Dynamic> asyncBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseAsyncOverlappingJacobi<double, Dynamic> asyncOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);

#endif

#ifdef SPARSE

        SparseMatrix<double>A;
        if (fromFile){
            ifstream input(inputMatrix);
            read_matrix<double>(A, input);
            input.close();

        } else {

            A.resize(matrixSize, matrixSize);
            typedef Eigen::Triplet<double> T;

            vector<T> triplets;

            for (int i = 0; i < 100*matrixSize; ++i) {
                triplets.emplace_back(T(abs(rand())%matrixSize,abs(rand())%matrixSize,(double)rand()*1000/RAND_MAX));
                if(i<matrixSize)
                    triplets.emplace_back(T(i,i,(double)rand()*1000/RAND_MAX));
            }

            A.setFromTriplets(triplets.begin(),triplets.end());

            triplets.clear();

            #pragma omp parallel for schedule(static)
            for (int i = 0; i < matrixSize; ++i) {
                auto sum=0.;
                for (int j = 0; j < matrixSize; ++j) {
                    sum+=abs(A.coeff(i,j));
                }
                A.coeffRef(i,i) = sum;
            }
        }

        ColumnVector<double, Dynamic> b = ColumnVector<double, Dynamic>::Zero(A.cols());

        if(!fromFile) b.setRandom();

        sparseSerialJacobi<double> serialJacobi(A, b, iterations, tolerance);
        sparseParallelJacobi<double> parallelJacobi(A, b, iterations, tolerance, workers);
        sparseAsyncJacobi<double> asyncJacobi(A, b, iterations, tolerance, workers);
        sparseBlocksJacobi<double> blocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOptimizedBlocksJacobi<double> optimizedBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOverlappingJacobi<double> overlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOptimizedOverlappingJacobi<double> optimizedOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseAsyncBlocksJacobi<double> asyncBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseAsyncOverlappingJacobi<double> asyncOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);

#endif

    auto iterationsPerformed = 0L;

    switch (method){

        case SEQUENTIAL:
            methodString = sequential;
            cout << "Sequential" << endl;
            start_time = Time::now();
            x = serialJacobi.solve();
            end_time = Time::now();
            error = (b-A*x).template lpNorm<1>()/matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = serialJacobi.getIteration();
            break;
        case PARALLEL:
            methodString = parallel;
            cout << "Parallel" << endl;
            start_time = Time::now();
            x = parallelJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = parallelJacobi.getIteration();
            break;
        case PARALLEL_ASYNC:
            methodString = parallel_async;
            cout << "Parallel async" << endl;
            start_time = Time::now();
            x = asyncJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncJacobi.getIteration();
            break;
        case BLOCKS:
            methodString = blocks;
            cout << "Parallel blocks" << endl;
            start_time = Time::now();
            x = blocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = blocksJacobi.getIteration();

            break;
        case BLOCKS_OPTIMIZED:
            methodString = blocks_optimized;
            cout << "Parallel Optimized blocks" << endl;
            start_time = Time::now();
            x = optimizedBlocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = optimizedBlocksJacobi.getIteration();
            break;
        case BLOCKS_ASYNC:
            methodString = blocks_async;
            cout << "Parallel async blocks" << endl;
            start_time = Time::now();
            x = asyncBlocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncBlocksJacobi.getIteration();

            break;
        case OVERLAPPING:
            methodString = overlapping;
            cout << "Parallel overlapping" << endl;
            start_time = Time::now();
            x = overlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = overlappingJacobi.getIteration();
            break;
        case OVERLAPPING_OPTIMIZED:
            methodString = overlapping_optimized;
            cout << "Parallel optimized overlapping" << endl;
            start_time = Time::now();
            x = optimizedOverlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = optimizedOverlappingJacobi.getIteration();
            break;
        case OVERLAPPING_ASYNC:
            methodString = overlapping_async;
            cout << "Parallel async overlapping blocks" << endl;
            start_time = Time::now();
            x = asyncOverlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncOverlappingJacobi.getIteration();
            break;
    }

    if(toCsv) write_csv(filename, dsec(end_time - start_time), error, iterationsPerformed);

    if(debug) cout << x.transpose() << endl;

//    cerr << "OK" << endl;

    return 0;

}

void parse_args(int argc,  char *argv[]) {

    string arg(argv[1]);
    if (arg == sequential) method = SEQUENTIAL;
    else if (arg == parallel) method = PARALLEL;
    else if (arg == parallel_async) method = PARALLEL_ASYNC;
    else if (arg == blocks) method = BLOCKS;
    else if (arg == blocks_optimized) method = BLOCKS_OPTIMIZED;
    else if (arg == blocks_async) method = BLOCKS_ASYNC;
    else if (arg == overlapping) method = OVERLAPPING;
    else if (arg == overlapping_optimized) method = OVERLAPPING_OPTIMIZED;
    else if (arg == overlapping_async) method = OVERLAPPING_ASYNC;
    else exit(1);

    InputParser input(argc,argv);

    if(input.cmdOptionExists("-w")){

        workers = (int) strtol(input.getCmdOption("-w").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-s")){

        matrixSize = (ulong) strtol(input.getCmdOption("-s").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-i")){

        iterations = (ulong) strtol(input.getCmdOption("-i").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-t")){

        tolerance = stod(input.getCmdOption("-t"));
    }

    if(input.cmdOptionExists("-b")){

        blockSize = (int) strtol(input.getCmdOption("-b").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-p")){
        filename = input.getCmdOption("-p");
        toCsv = true;
        cout << filename << endl;
    }

    if(input.cmdOptionExists("-d")){
        debug = true;
    }

    if(input.cmdOptionExists("-m")){
        inputMatrix = input.getCmdOption("-m");
        fromFile = true;
    }

}

void write_csv(string fileName, std::chrono::duration<double> time, double error, long iteration){

    ifstream test(fileName);
    auto exists = test.good();
    test.close();
    ofstream outFile(fileName, ofstream::out | ofstream::app);

    if(!exists){
        outFile << "algorithm,time,iterations,workers,error,size,blockSize" << endl;
    }

    outFile << methodString << ',' << time.count() << ',' << iteration <<','<< workers << ',' << error << ','
            << matrixSize << ',' << blockSize << endl;

}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing sqr capabilities

    Computes the square of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

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

    is equivalent to:

    @code
    T r = x*x;
    @endcode

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

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

#endif


//
// Copyright © 2019 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(Deserializer)

struct InstanceNormalizationFixture : public ParserFlatbuffersSerializeFixture
{
    explicit InstanceNormalizationFixture(const std::string &inputShape,
                                          const std::string &outputShape,
                                          const std::string &gamma,
                                          const std::string &beta,
                                          const std::string &epsilon,
                                          const std::string &dataType,
                                          const std::string &dataLayout)
    {
        m_JsonString = R"(
    {
        inputIds: [0],
        outputIds: [2],
        layers: [
           {
            layer_type: "InputLayer",
            layer: {
                base: {
                    layerBindingId: 0,
                    base: {
                        index: 0,
                        layerName: "InputLayer",
                        layerType: "Input",
                        inputSlots: [{
                            index: 0,
                            connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                            }],
                        outputSlots: [{
                            index: 0,
                            tensorInfo: {
                                dimensions: )" + inputShape + R"(,
                                dataType: ")" + dataType + R"(",
                                quantizationScale: 0.5,
                                quantizationOffset: 0
                                },
                            }]
                        },
                    }
                },
            },
        {
        layer_type: "InstanceNormalizationLayer",
        layer : {
            base: {
                index:1,
                layerName: "InstanceNormalizationLayer",
                layerType: "InstanceNormalization",
                inputSlots: [{
                        index: 0,
                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                   }],
                outputSlots: [{
                    index: 0,
                    tensorInfo: {
                        dimensions: )" + outputShape + R"(,
                        dataType: ")" + dataType + R"("
                    },
                    }],
                },
            descriptor: {
                dataLayout: ")" + dataLayout + R"(",
                gamma: ")" + gamma + R"(",
                beta: ")" + beta + R"(",
                eps: )" + epsilon + R"(
                },
            },
        },
        {
        layer_type: "OutputLayer",
        layer: {
            base:{
                layerBindingId: 0,
                base: {
                    index: 2,
                    layerName: "OutputLayer",
                    layerType: "Output",
                    inputSlots: [{
                        index: 0,
                        connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                    }],
                    outputSlots: [ {
                        index: 0,
                        tensorInfo: {
                            dimensions: )" + outputShape + R"(,
                            dataType: ")" + dataType + R"("
                        },
                    }],
                }
            }},
        }]
    }
)";
        SetupSingleInputSingleOutput("InputLayer", "OutputLayer");
    }
};

struct InstanceNormalizationFloat32Fixture : InstanceNormalizationFixture
{
    InstanceNormalizationFloat32Fixture():InstanceNormalizationFixture("[ 2, 2, 2, 2 ]",
                                                                       "[ 2, 2, 2, 2 ]",
                                                                       "1.0",
                                                                       "0.0",
                                                                       "0.0001",
                                                                       "Float32",
                                                                       "NHWC") {}
};

BOOST_FIXTURE_TEST_CASE(InstanceNormalizationFloat32, InstanceNormalizationFloat32Fixture)
{
    RunTest<4, armnn::DataType::Float32>(
        0,
         {
             0.f,  1.f,
             0.f,  2.f,

             0.f,  2.f,
             0.f,  4.f,

             1.f, -1.f,
            -1.f,  2.f,

            -1.f, -2.f,
             1.f,  4.f
        },
        {
             0.0000000f, -1.1470304f,
             0.0000000f, -0.2294061f,

             0.0000000f, -0.2294061f,
             0.0000000f,  1.6058424f,

             0.9999501f, -0.7337929f,
            -0.9999501f,  0.5241377f,

            -0.9999501f, -1.1531031f,
             0.9999501f,  1.3627582f
        });
}

BOOST_AUTO_TEST_SUITE_END()


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include "random.hpp"

template<typename T>
struct asum_cl {
  static T asum(const T &x) {
    return std::abs(x);
  }
};

template<typename T>
struct asum_cl<std::complex<T>> {
  static T asum(const std::complex<T> &x) {
    return std::abs(x.real())+std::abs(x.imag());
  }
};

template<typename Vec>
int
iamax(const Vec &v) {
  namespace bindings=boost::numeric::bindings;
  typedef typename bindings::value_type<Vec>::type value_type;
  auto i=bindings::begin(v);
  auto i_end=bindings::end(v);
  auto i_res=i;
  for (; i!=i_end; ++i)
    if (asum_cl<value_type>::asum(*i)>asum_cl<value_type>::asum(*i_res))
      i_res=i;
  return std::distance(bindings::begin(v), i_res);
}

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "ublas using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "ublas using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "eigen using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "eigen using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2O_3_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2O_3_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Sqrt_2o_3 generic tag

     Represents the Sqrt_2o_3 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Sqrt_2o_3, double, 0
                                , 0x3f5105ec, 0x3fea20bd700c2c3ell
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Sqrt_2o_3, Site> dispatching_Sqrt_2o_3(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Sqrt_2o_3, Site>();
   }
   template<class... Args>
   struct impl_Sqrt_2o_3;
  }

  /*!
    Generates value \f$\frac{\sqrt2}3\f$

    @par Semantic:

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

    is similar to:

    @code
    T r = T(sqrt(as_floating<T>(2)))/T(3);
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Sqrt_2o_3, Sqrt_2o_3)
} }

#include <boost/simd/constant/common.hpp>

#endif


#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file

// std
#include <iostream>
#include <stdexcept>
#include <algorithm>
#include <utility>
#include <memory>
#include <chrono>
#include <random>
#include <catch2/catch.hpp>
#include <GridPoint.hpp>
#include <SHRS.hpp>

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// Eigen
#include <Eigen/Dense>

using namespace fsd;

using std::string;
using std::cout;
using std::endl;
using std::vector;
using std::iota;
using std::numeric_limits;
using std::random_device;
using std::mt19937;
using std::uniform_real_distribution;
using std::uniform_int_distribution;
using std::unique_ptr;
using Eigen::Vector2d;

/*
 * @brief get_random_coordinates generates num number of unique and
 * uniform distributed random coordinates in the range of width and height,
 * in O(n) time with knuth-fisher-yates shuffling
 * can be optimized further for num > (width * heigh - 1) / 2
 * @param width
 * @param height
 * @param num num of random coords
 * @return vector with positions
 */
vector<GridPoint> generate_random_coordinates
(GridPoint area_min, GridPoint area_max, size_t num) {
    size_t width = std::abs(area_max.get_column() - area_min.get_column());
    size_t height = std::abs(area_max.get_row() - area_min.get_row());
    if(width == 0 || height == 0) {
        throw std::logic_error("width or height cannot be zero");
    }
    size_t max_val = width * height - 1;
    if(max_val <= num) {
        throw std::logic_error("number if random positions is bigger "
                               " or equal than max possible");
    }
    random_device rd;
    mt19937 eng(rd());
    vector<size_t> vals;
    vector<GridPoint> positions;
    vals.reserve(max_val + 1);
    positions.reserve(num);
    iota (begin(vals), end(vals), 0);
    while(num) {
        uniform_int_distribution<> u_distr(0, max_val);
        auto rnd_number = u_distr(eng);
        auto val_at_rnd = vals[rnd_number];
        vals[rnd_number] = vals[max_val];
        vals[max_val] = val_at_rnd;
        auto x = val_at_rnd % width;
        auto y = val_at_rnd / width;
        positions.emplace_back(GridPoint(x, y));
        max_val--;
        num--;
    }
    return positions;
}

/**
 *@brief adaptor for pointclouds
 */
template <typename point_t>
class PCLSHRSAdaptor {
public:
    using key_t = size_t;
    using data = pcl::PointCloud<point_t>;
    data const &pcl_ref;

    PCLSHRSAdaptor
    (data const &pcl_ref_) :
        pcl_ref(pcl_ref_) {
    }

    inline Vector2d get_point(size_t i_point) const {
        auto const &pnt = pcl_ref.at(i_point);
        return { pnt.x, pnt.y };
    }

    inline void for_all_keys(std::function<void(key_t)> func) const {
        for(size_t i = 0; i < pcl_ref.size(); i++) {
            func(i);
        }
    }

    inline bool find_point(size_t i_point) const {
        return i_point < pcl_ref.size();
    }

    inline size_t size() const {
        return pcl_ref.size();
    }
};

double bench_shrs(const double search_radius_pcl,
                  const Vector2d test_area_min_pcl,
                  const Vector2d test_area_max_pcl,
                  const size_t num_test_points_pcl,
                  const size_t points_to_search_pcl) {
    random_device rd;
    mt19937 eng(rd());

    using point_t = pcl::PointXYZ;
    pcl::PointCloud<point_t> test_pcl;

    SHRS<PCLSHRSAdaptor<point_t>> shrs_map(
                PCLSHRSAdaptor<point_t>(test_pcl),
                search_radius_pcl);

    bool gen_new_cloud = true;
    bool rw_cloud = false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud
            (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>
            ("/home/hsa/shrs_test_pcl.pcd", *read_cloud) == -1
            || gen_new_cloud) {
        // create new
        cout << "could not read cloud, creating new: ";
        uniform_real_distribution<> dis_x(test_area_min_pcl.x(),
                                          test_area_max_pcl.x());
        uniform_real_distribution<> dis_y(test_area_min_pcl.y(),
                                          test_area_max_pcl.y());
        // to save
        pcl::PointCloud<pcl::PointXYZ> save_cloud;
        save_cloud.width = 10;
        save_cloud.height = 100;
        save_cloud.is_dense = false;
        save_cloud.points.resize(num_test_points_pcl);
        for(size_t n = 0; n < num_test_points_pcl; n++) {
            pcl::PointXYZ tmp;
            auto rnd_x = dis_x(eng);
            auto rnd_y = dis_y(eng);
            tmp.x = rnd_x;
            tmp.y = rnd_y;
            tmp.z = 0.;
            save_cloud[n].x = tmp.x;
            save_cloud[n].y = tmp.y;
            save_cloud[n].z = 0;

            test_pcl.push_back(tmp);
        }
        if(rw_cloud) {
            pcl::io::savePCDFileASCII ("/home/hsa/shrs_test_pcl.pcd", save_cloud);
        }
    } else {
        cout << "read cloud from disk " << "\n""";
        test_pcl = *read_cloud;
    }

    auto begin_insert = std::chrono::steady_clock::now();

    shrs_map.insert_all();

    auto end_insert = std::chrono::steady_clock::now();
    auto insert_span = std::chrono::duration <double, std::milli>
            (end_insert - begin_insert).count();
    cout << "inserting " << num_test_points_pcl << " points took " << insert_span
         << " ms " << "\n";

    uniform_int_distribution<> u_distr_pcl(0, num_test_points_pcl - 1);
    double avg_neigbours = 0;
    double max_find = -std::numeric_limits<double>::infinity();
    double min_find =  std::numeric_limits<double>::infinity();
    double avg_find = 0;
    for(size_t iRndPoint = 0; iRndPoint < points_to_search_pcl; iRndPoint++) {
        bool random_idx = false;
        size_t rnd_index = 0;
        if(random_idx) {
            rnd_index = u_distr_pcl(eng);
        } else {
            rnd_index = iRndPoint;
        }
        auto rnd_pnt = test_pcl[rnd_index];
        vector<size_t> neighbours_shrs;
        vector<size_t> neighbours_reference;

        auto begin_find = std::chrono::steady_clock::now();

        shrs_map.find_neighbours(rnd_index,
                                 neighbours_shrs);

        auto end_find = std::chrono::steady_clock::now();
        double find_span = std::chrono::duration <double, std::milli>
                (end_find - begin_find).count();

        max_find = std::max(max_find, find_span);
        min_find = std::min(min_find, find_span);
        avg_find += find_span;
        avg_neigbours += (double)neighbours_shrs.size();

        sort(begin(neighbours_shrs), end(neighbours_shrs));

        for(size_t iOtherPoint = 0; iOtherPoint < num_test_points_pcl; iOtherPoint++) {
            if(iOtherPoint == rnd_index) {
                continue;
            }
            Vector2d  rnd_pnt_vec = { rnd_pnt.x, rnd_pnt.y };
            Vector2d  test_pcl_pnt_vec = { test_pcl[iOtherPoint].x,
                                           test_pcl[iOtherPoint].y };
            if((rnd_pnt_vec - test_pcl_pnt_vec).norm() <
                    (search_radius_pcl)) {
                neighbours_reference.push_back(iOtherPoint);
            }
        }
        sort(begin(neighbours_reference), end(neighbours_reference));
        if(neighbours_reference != neighbours_shrs) {
            cout << "found different neigbours ";
        }
        REQUIRE(neighbours_reference == neighbours_shrs);
    }

    avg_neigbours /= static_cast<double>(points_to_search_pcl);
    avg_find /= static_cast<double>(points_to_search_pcl);
    cout << "found " << avg_neigbours << " neighbours on average,"
                                         " one neighbour search took on average "
         << avg_find << " ms, max " << max_find << " ms and min "
         << min_find << " ms" << endl;

    // benchmark of adjacency matrix
    unordered_map<size_t, vector<size_t>> adj_matrix;
    auto begin_adj = std::chrono::steady_clock::now();

    unique_ptr<SHRS<PCLSHRSAdaptor<point_t>>> bench_map =
            unique_ptr<SHRS<PCLSHRSAdaptor<point_t>>>
                                                    (new SHRS<PCLSHRSAdaptor<point_t>>(PCLSHRSAdaptor<point_t>(test_pcl),
                                                                                search_radius_pcl, &adj_matrix));

    auto end_adj = std::chrono::steady_clock::now();
    double adj_span = std::chrono::duration <double, std::milli>
            (end_adj - begin_adj).count();

    cout << "calculating adjacency list of " << num_test_points_pcl
         << " points with " << avg_neigbours
         << " neighbours on average took " << adj_span << " ms" << endl;
    return adj_span;
}

TEST_CASE("shrs insert and search neigbours with pcl adaptor") {

    vector<size_t> point_sizes_to_test = { 100, 1000, 10000, 100000 };
    vector<size_t> avg_neighbours_to_test = { 3, 7, 10, 20, 30, 50 };

    vector<vector<double>> times;
    auto const search_radius = 25.;
    for(auto size : point_sizes_to_test) {
        for(auto avg_neighbours : avg_neighbours_to_test) {
            auto area_side_len = std::sqrt(M_PI * std::pow(search_radius, 2.)
                                           * size
                                           / static_cast<double>(avg_neighbours));
            // to test also around the midpoint
            auto min_pnt = Vector2d(-(area_side_len / 2.),
                                    -(area_side_len / 2.));
            auto max_pnt = Vector2d(area_side_len / 2., area_side_len / 2.);

            cout << "min_pnt: x: " << min_pnt.x() << ", y: " << min_pnt.y()
                 << "max_pnt: x: " << max_pnt.x() << ", y: " << max_pnt.y();
            auto time_to_calc
                    = bench_shrs(search_radius, min_pnt, max_pnt, size,
                                 static_cast<size_t>(std::sqrt(size)));
            times.push_back({static_cast<double>(size),
                              static_cast<double>(avg_neighbours),
                              static_cast<double>(time_to_calc) });
        }
    }

    cout << "times: \n";

    for(auto const &time_struct : times) {
        cout << "size: " << time_struct[0] << ", "
             << "avg_neighbours: " << time_struct[1] << ", "
             << "time: " << time_struct[2] << " ms\n";
    }
}