// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_DIM_HPP
#define BOOST_UNITS_DIM_HPP

#include <boost/static_assert.hpp>

#include <boost/type_traits/is_same.hpp>

#include <boost/mpl/arithmetic.hpp>

#include <boost/units/config.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/detail/dim_impl.hpp>

/// \file dim.hpp
/// \brief Handling of fundamental dimension/exponent pairs.

namespace boost {

namespace units {

namespace detail {

struct dim_tag { };

}

/// \brief Dimension tag/exponent pair for a single fundamental dimension.
///
/// \details
/// The dim class represents a single dimension tag/dimension exponent pair.
/// That is, @c dim<tag_type,value_type> is a pair where @c tag_type represents the
/// fundamental dimension being represented and @c value_type represents the
/// exponent of that fundamental dimension as a @c static_rational. @c tag_type must
/// be a derived from a specialization of @c base_dimension.
/// Specialization of the following Boost.MPL metafunctions are provided
///
///     - @c mpl::plus for two @c dims
///     - @c mpl::minus for two @c dims
///     - @c mpl::negate for a @c dim
///
/// These metafunctions all operate on the exponent, and require
/// that the @c dim operands have the same base dimension tag.
/// In addition, multiplication and division by @c static_rational
/// is supported.
///
///     - @c mpl::times for a @c static_rational and a @c dim in either order
///     - @c mpl::divides for a @c static_rational and a @c dim in either order
///
/// These metafunctions likewise operate on the exponent only.
template<typename T,typename V>
struct dim
{
    typedef dim             type;
    typedef detail::dim_tag tag;
    typedef T               tag_type;
    typedef V               value_type;
};

} // namespace units

} // namespace boost

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::dim, 2)

#endif

#ifndef BOOST_UNITS_DOXYGEN

namespace boost {

namespace mpl {

// define MPL operators acting on dim<T,V>

template<>
struct plus_impl<boost::units::detail::dim_tag,boost::units::detail::dim_tag>
{
    template<class T0, class T1>
    struct apply
    {
        BOOST_STATIC_ASSERT((boost::is_same<typename T0::tag_type,typename T1::tag_type>::value == true));
        typedef boost::units::dim<typename T0::tag_type, typename mpl::plus<typename T0::value_type, typename T1::value_type>::type> type;
    };
};

template<>
struct minus_impl<boost::units::detail::dim_tag,boost::units::detail::dim_tag>
{
    template<class T0, class T1>
    struct apply
    {
        BOOST_STATIC_ASSERT((boost::is_same<typename T0::tag_type,typename T1::tag_type>::value == true));
        typedef boost::units::dim<typename T0::tag_type, typename mpl::minus<typename T0::value_type, typename T1::value_type>::type> type;
    };
};

template<>
struct times_impl<boost::units::detail::dim_tag,boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef boost::units::dim<typename T0::tag_type, typename mpl::times<typename T0::value_type, T1>::type> type;
    };
};

template<>
struct times_impl<boost::units::detail::static_rational_tag,boost::units::detail::dim_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef boost::units::dim<typename T1::tag_type, typename mpl::times<T0, typename T1::value_type>::type> type;
    };
};

template<>
struct divides_impl<boost::units::detail::dim_tag,boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef boost::units::dim<typename T0::tag_type, typename mpl::divides<typename T0::value_type, T1>::type> type;
    };
};

template<>
struct divides_impl<boost::units::detail::static_rational_tag,boost::units::detail::dim_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef boost::units::dim<typename T1::tag_type, typename mpl::divides<T0, typename T1::value_type>::type> type;
    };
};

template<>
struct negate_impl<boost::units::detail::dim_tag>
{
    template<class T0>
    struct apply
    {
        typedef boost::units::dim<typename T0::tag_type,typename mpl::negate<typename T0::value_type>::type> type;
    };
};

} // namespace mpl

} // namespace boost

#endif

#endif // BOOST_UNITS_DIM_HPP


#include "calibrator.hpp"

#include <NaturalNeighbourInterpolator.hpp>
#include <ChessboardSampling.hpp>
#include <PlaneFit.hpp>
#include <resampler.hpp>

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <chrono>
#include <cmath>

#include <map>
#include <algorithm>
#include <random>
#include <unistd.h>
#include <limits>
#include <fstream>

#include <string.h> // memset

namespace{

  template <class T>
  inline std::string
  toString(T value)
  {
    std::ostringstream stream;
    stream << value;
    return stream.str();
  }

  float
  gauss(float x, float sigma, float mean){
    return (1.0f/(sigma*sqrt(2.0f * M_PI))) * exp( -0.5f * ((x-mean)/sigma) * ((x-mean)/sigma) );
  }

}

/*static*/ bool Calibrator::using_nni = false;

Calibrator::Calibrator()
  : m_nni_possible(0)
{}

Calibrator::~Calibrator(){
  if(m_nni_possible){
    delete [] m_nni_possible;
  }
}

void
Calibrator::postFilterSamples(CalibVolume* cv, std::vector<samplePoint>& sps, const RGBDConfig& cfg, const float post_filter_error_sd, const float post_filter_percentile){

  std::vector<float> errors_3D;
  std::vector<float> errors_2D;

  std::vector<std::pair<float, unsigned>> errors_3D_VK;
  std::vector<std::pair<float, unsigned>> errors_2D_VK;

  const unsigned cv_width = cv->width;
  const unsigned cv_height = cv->height;
  const unsigned cv_depth = cv->depth;

  for(unsigned i = 0; i < sps.size(); ++i){

    const float x = cv_width  *  ( sps[i].tex_depth.u) / cfg.size_d.x;
    const float y = cv_height *  ( sps[i].tex_depth.v)/ cfg.size_d.y;
    const float z = cv_depth  *  ( sps[i].depth - cv->min_d)/(cv->max_d - cv->min_d);

    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
    uv  tex = getTrilinear(cv->cv_uv,  cv_width, cv_height, cv_depth, x , y , z );

    const float d_x = sps[i].pos_real[0] - pos.x;
    const float d_y = sps[i].pos_real[1] - pos.y;
    const float d_z = sps[i].pos_real[2] - pos.z;

    const float d_u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
    const float d_v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;

    const float err_3D(glm::length(glm::vec3(d_x, d_y, d_z)));
    const float err_2D(glm::length(glm::vec2(d_u * cfg.size_rgb.x, d_v * cfg.size_rgb.y)));
    errors_3D.push_back(err_3D);
    errors_2D.push_back(err_2D);

    errors_3D_VK.push_back(std::pair<float, unsigned>(err_3D, i));
    errors_2D_VK.push_back(std::pair<float, unsigned>(err_2D, i));
  }

  if(post_filter_error_sd > 0.0){
    double mean3D, mean2D, sd3D, sd2D;
    calcMeanSD(errors_3D, mean3D, sd3D);
    calcMeanSD(errors_2D, mean2D, sd2D);

    unsigned out_3D = 0;
    const float out_3D_thresh = mean3D + post_filter_error_sd * sd3D;
    for(const auto& s_id_VK : errors_3D_VK){
      if(s_id_VK.first > out_3D_thresh){
	sps[s_id_VK.second].quality = 0.0;
	++out_3D;
      }
    }
    std::cout << "INFO: Calibrator::postFilterSamples 3D post filter samples and discard samples with << "
	      << post_filter_error_sd << " << more error than standard deviation: " << out_3D << std::endl;

    unsigned out_2D = 0;
    const float out_2D_thresh = mean2D + post_filter_error_sd * sd2D;
    for(const auto& s_id_VK : errors_2D_VK){
      if(s_id_VK.first > out_2D_thresh){
	sps[s_id_VK.second].quality = 0.0;
	++out_2D;
      }
    }
    std::cout << "INFO: Calibrator::postFilterSamples 2D post filter samples and discard samples with << "
	      << post_filter_error_sd << " << more error than standard deviation: " << out_2D << std::endl;
  }

  if(post_filter_percentile > 0.0){
    std::sort(errors_3D_VK.begin(), errors_3D_VK.end());
    std::reverse(errors_3D_VK.begin(),errors_3D_VK.end());
    std::sort(errors_2D_VK.begin(), errors_2D_VK.end());
    std::reverse(errors_2D_VK.begin(),errors_2D_VK.end());
    const unsigned last_outlier = std::min(sps.size(), size_t(sps.size() * post_filter_percentile));
    std::cout << "INFO: Calibrator::postFilterSamples post filter samples, order by error (both, 3D and 2D) and discard " << post_filter_percentile << " percentile: " << last_outlier << std::endl;
    for(unsigned o_id = 0; o_id < last_outlier; ++o_id){
      sps[errors_3D_VK[o_id].second].quality = 0.0;
      sps[errors_2D_VK[o_id].second].quality = 0.0;
    }
  }
}

void
Calibrator::applySamples(CalibVolume* cv, const std::vector<samplePoint>& sps, const RGBDConfig& cfg, unsigned idwneighbours, const char* basefilename, RGBDSensor* sensor, const glm::mat4* eye_d_to_world){

  auto start_time = std::chrono::system_clock::now();
  //CGAL : build Tree, search 100 neighbors, try NNI of neighbourhood, fallback to IDW small neighborhood
  std::cout << "INFO: Calibrator::applySamples applying " << sps.size()
	    << " for volume using on-the-fly initial calibration: " << (sensor != 0 ? "yes" : "no") << std::endl;

  std::vector<nniSample> nnisamples;

  for(unsigned s = 0; s < sps.size(); ++s){

    if( !(sps[s].quality > 0.0) ){
      std::cout << "INFO: calibrator.cpp: skipping sample because quality is zero: " << sps[s] << std::endl;
      continue;
    }
    nniSample nnis;
    nnis.quality = sps[s].quality;
    nnis.s_pos_cs  = sps[s].pos_real;

    nnis.s_pos.x = cv->width *  ( sps[s].tex_depth.u)/ cfg.size_d.x;
    nnis.s_pos.y = cv->height *  ( sps[s].tex_depth.v)/ cfg.size_d.y;
    nnis.s_pos.z = cv->depth  *  (sps[s].depth - cv->min_d) / (cv->max_d - cv->min_d);
    if( !((sps[s].depth > cv->min_d) && (sps[s].depth < cv->max_d)) ){
      std::cerr << "ERROR: calibrator.cpp: skipping sample because invalid depth: " << sps[s] << std::endl;
      continue;
    }

    // here we will introduce a small error because we have a tri-linear interpolation already in the first stage
    nnis.s_pos_off = sps[s/*sample point*/].pos_offset; // in world coordinates (metric)
    nnis.s_tex_off = sps[s/*sample point*/].tex_offset; // normalized between 0...1

    if(0 && sensor && eye_d_to_world /*use on the fly offset computation from initial calibration to avoid above error*/){

      const float depth = sps[s].depth;
      const float xd = sps[s].tex_depth.u;
      const float yd = sps[s].tex_depth.v;

      glm::vec3 pos3D_local = sensor->calc_pos_d(xd, yd, depth);
      glm::vec2 pos2D_rgb   = sensor->calc_pos_rgb(pos3D_local);
      pos2D_rgb.x /= sensor->config.size_rgb.x;
      pos2D_rgb.y /= sensor->config.size_rgb.y;

      glm::vec4 pos3D_world = (*eye_d_to_world) * glm::vec4(pos3D_local.x, pos3D_local.y, pos3D_local.z, 1.0);

      xyz pos3D;
      pos3D.x = pos3D_world.x;
      pos3D.y = pos3D_world.y;
      pos3D.z = pos3D_world.z;

      uv posUV;
      posUV.u = pos2D_rgb.x;
      posUV.v = pos2D_rgb.y;
      std::cout << "INFO: s_pos_off before: " << nnis.s_pos_off << std::endl;
#if 0
      xyz pos_tmp;
      xyz pos_tmp_tri = getTrilinear(cv->cv_xyz, cv->width, cv->height, cv->depth,
				     nnis.s_pos.x ,nnis.s_pos.y , nnis.s_pos.z );
      pos_tmp.x = sps[s/*sample point*/].pos_real[0] - pos_tmp_tri.x;
      pos_tmp.y = sps[s/*sample point*/].pos_real[1] - pos_tmp_tri.y;
      pos_tmp.z = sps[s/*sample point*/].pos_real[2] - pos_tmp_tri.z;
#endif
      //std::cout << "INFO: s_pos_off with Trilinear: " << pos_tmp << std::endl;
      //std::cout << "INFO: pos_tmp_tri: " << pos_tmp_tri << std::endl;
      //std::cout << "INFO: pos3D_onthefly: " << pos3D << std::endl;

      nnis.s_pos_off.x = sps[s/*sample point*/].pos_real[0] - pos3D.x;
      nnis.s_pos_off.y = sps[s/*sample point*/].pos_real[1] - pos3D.y;
      nnis.s_pos_off.z = sps[s/*sample point*/].pos_real[2] - pos3D.z;
      std::cout << "INFO: s_pos_off after: " << nnis.s_pos_off << std::endl;
      uv sps_tex_color_norm = sps[s/*sample point*/].tex_color;
      sps_tex_color_norm.u /= sensor->config.size_rgb.x;
      sps_tex_color_norm.v /= sensor->config.size_rgb.y;
      std::cout << "INFO: s_tex_off before: " << nnis.s_tex_off << std::endl;
      nnis.s_tex_off = sps_tex_color_norm - posUV;
      std::cout << "INFO: s_tex_off after: " << nnis.s_tex_off << std::endl;
    }

    //std::cerr << s << " " << nnis << std::endl;
    nnisamples.push_back(nnis);
  }

  Resampler rsa;
  rsa.resampleGridBased(nnisamples, cv,	basefilename);

#if 0
  std::cout << "INFO: initializing nearest neighbor search for border fill " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns_before_border_fill(nnisamples);
  rsa.fillBorder(nnisamples, cv, &nns_before_border_fill, idwneighbours, basefilename);
  //exit(0);
#endif

  std::cout << "INFO: initializing nearest neighbor search for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns(nnisamples);

  // init calib volume for natural neighbor interpolation
  NaturalNeighbourInterpolator* nnip = 0;
  CalibVolume* cv_nni = 0;
  if(using_nni){
    if(m_nni_possible == 0){
      m_nni_possible = new unsigned char [cv->width * cv->height * cv->depth];
    }
    memset(m_nni_possible, 0, cv->width * cv->height * cv->depth);
    cv_nni = new CalibVolume(cv->width, cv->height, cv->depth, cv->min_d, cv->max_d);
    std::cout << "INFO: initializing natural neighbor interpolation for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
    std::shuffle(std::begin(nnisamples), std::end(nnisamples), std::default_random_engine());
    nnip =   new NaturalNeighbourInterpolator(nnisamples);
  }

  const unsigned numthreads = 32;
  std::cout << "INFO: start interpolation per thread for " << numthreads << " threads." << std::endl;
  boost::thread_group threadGroup;
  for (unsigned tid = 0; tid < numthreads; ++tid){
    threadGroup.create_thread(boost::bind(&Calibrator::applySamplesPerThread, this, cv, &nns, tid, numthreads, idwneighbours, cv_nni, nnip));
  }
  threadGroup.join_all();

  if(using_nni){
    blendIDW2NNI(cv, cv_nni, basefilename);
  }

  auto end_time = std::chrono::system_clock::now();
  std::cout << "INFO: finished interpolation in "
	    <<  std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time).count()
	    << " seconds" << std::endl;

}

void
Calibrator::blendIDW2NNI(CalibVolume* cv, CalibVolume* cv_nni, const char* basefilename){

  std::cerr << "USING NNI therefore blending IDW into NNI" << std::endl;

  // print stats for nni_possible and write to volume file
  size_t possible = 0;
  for(size_t idx = 0; idx < (cv->width * cv->height * cv->depth); ++idx){
    possible += m_nni_possible[idx] > 0 ? 1 : 0;
  }
  std::cerr << "natural_neighbor_interpolation_was_possible for: "
	    << possible << " out of: " << (cv->width * cv->height * cv->depth)
	    << std::endl;

  // 1st pass detect border
  unsigned char* nni_border = new unsigned char[cv->width * cv->height * cv->depth];
  memset(nni_border, 0, cv->width * cv->height * cv->depth);
  const int border_size = 1;
  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  bool border = false;
	  for(int z_l = std::max(0 , (int(z) - border_size));
	      z_l < std::min(int(cv->depth), (int(z) + border_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - border_size));
		y_l < std::min(int(cv->height), (int(y) + border_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - border_size));
		  x_l < std::min(int(cv->width), (int(x) + border_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(0 == m_nni_possible[cv_index_l]){
		  border = true;
		}
	      }
	    }
	  }
	  nni_border[cv_index] = border ? 255 : 0;
	}
      }
    }
  }

  // 2nd pass blend between NNI and IDW
  unsigned char* nni_percentage = new unsigned char [cv->width * cv->height * cv->depth];
  memset(nni_percentage, 0, cv->width * cv->height * cv->depth);
  const int kernel_size = 8;
  const float min_dist = 1.0f;
  const float max_dist = 6.0f;

  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  float distance_to_border = std::numeric_limits<float>::max();
	  glm::vec3 voxel_curr(1.0f * x, 1.0f * y, 1.0f * z);
	  for(int z_l = std::max(0 , (int(z) - kernel_size));
	      z_l < std::min(int(cv->depth), (int(z) + kernel_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - kernel_size));
		y_l < std::min(int(cv->height), (int(y) + kernel_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - kernel_size));
		  x_l < std::min(int(cv->width), (int(x) + kernel_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(nni_border[cv_index_l] > 0){
		  distance_to_border = std::min(distance_to_border,
						glm::length(voxel_curr - glm::vec3(1.0f * x_l, 1.0f * y_l, 1.0f * z_l)));
		}
	      }
	    }
	  }

	  const float t_blend = std::min( max_dist, std::max(min_dist, distance_to_border)) / max_dist;
	  if(t_blend > 1.0f || t_blend < 0.0f){
	    std::cerr << "ERROR: invalid t_blend: " << t_blend << std::endl;
	    exit(0);
	  }

	  nni_percentage[cv_index] = (unsigned char) std::max(0.0f, std::min(255.0f, 255.0f * t_blend));
	  const xyz xyz_idw = cv->cv_xyz[cv_index];
	  const xyz xyz_nni = cv_nni->cv_xyz[cv_index];

	  const uv uv_idw = cv->cv_uv[cv_index];
	  const uv uv_nni = cv_nni->cv_uv[cv_index];

	  cv->cv_xyz[cv_index] = interpolate(xyz_idw, xyz_nni, t_blend);
	  cv->cv_uv[cv_index]  = interpolate(uv_idw , uv_nni , t_blend);

	}
      }
    }
  }

  std::cerr << "writing " << (std::string(basefilename) + "_nnistats").c_str() << std::endl;
  FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "wb");
  fwrite(m_nni_possible, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
  fclose(f_nni_stats);

  std::cerr << "writing " << (std::string(basefilename) + "_nniborder").c_str() << std::endl;
  FILE* f_nni_border = fopen((std::string(basefilename) + "_nniborder").c_str(), "wb");
  fwrite(nni_border, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_border);
  fclose(f_nni_border);

  std::cerr << "writing " << (std::string(basefilename) + "_percentage").c_str() << std::endl;
  FILE* f_nni_percentage = fopen((std::string(basefilename) + "_percentage").c_str(), "wb");
  fwrite(nni_percentage, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_percentage);
  fclose(f_nni_percentage);

  delete [] nni_border;
  delete [] nni_percentage;

}

void
Calibrator::evaluateSamples(CalibVolume* cv, std::vector<samplePoint>& sps, const RGBDConfig& cfg, const char* basefilename, bool isnni, bool create_error_vis){

  unsigned char* cv_nnistats = 0;
  unsigned nni_valids = 0;
  if(isnni){
    cv_nnistats = new unsigned char [cv->width * cv->height * cv->depth];
    // load .cv_nnistats if nni has to be evaluated
    FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "rb");
    fread(cv_nnistats, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
    fclose(f_nni_stats);
  }

  std::vector<nniSample> nnisamples_error_vol;

  std::vector<float> errors_3D;
  std::vector<float> errors_2D;
  float max_3D = std::numeric_limits<float>::lowest();
  float max_2D = std::numeric_limits<float>::lowest();

  // ranges in depth 0.5-1.25 1.25-2.0 2.0-3.0
  const float range_A_start = 0.5;
  const float range_A_end   = 1.25;
  float range_A_max_3D = std::numeric_limits<float>::lowest();
  float range_A_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_A_errors_3D;
  std::vector<float> range_A_errors_2D;

  const float range_B_start = 1.25;
  const float range_B_end   = 2.0;
  float range_B_max_3D = std::numeric_limits<float>::lowest();
  float range_B_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_B_errors_3D;
  std::vector<float> range_B_errors_2D;

  const float range_C_start = 2.0;
  const float range_C_end   = 3.0;
  float range_C_max_3D = std::numeric_limits<float>::lowest();
  float range_C_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_C_errors_3D;
  std::vector<float> range_C_errors_2D;

  unsigned num_skipped_due_quality = 0;
  for(unsigned i = 0; i < sps.size(); ++i){

    if(!(sps[i].quality > 0.0)){
      ++num_skipped_due_quality;
      continue;
    }

    const unsigned cv_width = cv->width;
    const unsigned cv_height = cv->height;
    const unsigned cv_depth = cv->depth;

    const float x = cv_width  *  ( sps[i].tex_depth.u) / cfg.size_d.x;
    const float y = cv_height *  ( sps[i].tex_depth.v)/ cfg.size_d.y;
    const float z = cv_depth  *  ( sps[i].depth - cv->min_d)/(cv->max_d - cv->min_d);

    if(isnni){
      const unsigned cv_index000 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index001 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index010 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index011 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      const unsigned cv_index100 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index101 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index110 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index111 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      bool nni_valid  = ( cv_nnistats[cv_index000]
			  && cv_nnistats[cv_index001]
			  && cv_nnistats[cv_index010]
			  && cv_nnistats[cv_index011]
			  && cv_nnistats[cv_index100]
			  && cv_nnistats[cv_index101]
			  && cv_nnistats[cv_index110]
			  && cv_nnistats[cv_index111]);
      if(!nni_valid){
	continue;
      }
      else{
	++nni_valids;
      }
    }

    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
    uv  tex = getTrilinear(cv->cv_uv,  cv_width, cv_height, cv_depth, x , y , z );

    sps[i].pos_offset.x = sps[i].pos_real[0] - pos.x;
    sps[i].pos_offset.y = sps[i].pos_real[1] - pos.y;
    sps[i].pos_offset.z = sps[i].pos_real[2] - pos.z;

    sps[i].tex_offset.u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
    sps[i].tex_offset.v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;

    sps[i].quality = 1.0f;

    const float err_3D(glm::length(glm::vec3(sps[i].pos_offset.x,sps[i].pos_offset.y,sps[i].pos_offset.z)));
    const float err_2D(glm::length(glm::vec3(sps[i].tex_offset.u * cfg.size_rgb.x,
					      sps[i].tex_offset.v * cfg.size_rgb.y,0.0)));

    // only track if sample is inside sweet bbx
    const glm::vec3 sweet_min(-0.6, 0.0, -0.6);
    const glm::vec3 sweet_max( 0.6, 1.9,  0.6);
    if(sps[i].pos_real[0] > sweet_min[0] &&
       sps[i].pos_real[1] > sweet_min[1] &&
       sps[i].pos_real[2] > sweet_min[2] &&

       sps[i].pos_real[0] < sweet_max[0] &&
       sps[i].pos_real[1] < sweet_max[1] &&
       sps[i].pos_real[2] < sweet_max[2]){

      errors_3D.push_back(err_3D);
      errors_2D.push_back(err_2D);

      max_3D = std::max(max_3D, err_3D);
      max_2D = std::max(max_2D, err_2D);

      // track local error for volume vis
      nniSample nnis;
      nnis.s_tex_off.u = err_3D;
      nnis.s_tex_off.v = err_2D;

      nnis.s_pos.x = x;
      nnis.s_pos.y = y;
      nnis.s_pos.z = z;

      nnisamples_error_vol.push_back(nnis);
    }

    if(range_A_start < sps[i].depth &&
       range_A_end >=  sps[i].depth){
      range_A_errors_3D.push_back(err_3D);
      range_A_errors_2D.push_back(err_2D);
      range_A_max_3D = std::max(range_A_max_3D, err_3D);
      range_A_max_2D = std::max(range_A_max_2D, err_2D);
    }
    else if(range_B_start < sps[i].depth &&
       range_B_end >=  sps[i].depth){
      range_B_errors_3D.push_back(err_3D);
      range_B_errors_2D.push_back(err_2D);
      range_B_max_3D = std::max(range_B_max_3D, err_3D);
      range_B_max_2D = std::max(range_B_max_2D, err_2D);
    }
    else if(range_C_start < sps[i].depth &&
       range_C_end >=  sps[i].depth){
      range_C_errors_3D.push_back(err_3D);
      range_C_errors_2D.push_back(err_2D);
      range_C_max_3D = std::max(range_C_max_3D, err_3D);
      range_C_max_2D = std::max(range_C_max_2D, err_2D);
    }
    else{
      ;//std::cout << "ERROR: not in range: " << sps[i].depth << std::endl;
    }

  }

  std::cout << "Calibrator::evaluateSamples INFO: skipped due to quality is zero: " << num_skipped_due_quality << std::endl;

  double mean3D, mean2D, sd3D, sd2D;
  calcMeanSD(errors_3D, mean3D, sd3D);
  calcMeanSD(errors_2D, mean2D, sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "Evaluation of ground truth samples: " << errors_3D.size() << std::endl;
  std::cout << "mean_error_3D: " << mean3D * 1000 << " (" << sd3D * 1000 << ") [" << max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "mean_error_2D: " << mean2D << " (" << sd2D << ") [" << max_2D << "] (in pixels)" << std::endl;
  if(isnni){
    std::cout << "Calibrator::evaluateSamples INFO: could evaluate based on natural neighbour interpolation: " << nni_valids << " samples from " << sps.size() << std::endl;
  }
  else{
    std::cout << "Calibrator::evaluateSamples INFO: natural neighbour based evalution is turned off" << std::endl;
  }

  if(create_error_vis){
    createErrorVis(nnisamples_error_vol, cv->width, cv->height, cv->depth, basefilename, isnni);
  }

  double range_A_mean3D, range_A_mean2D, range_A_sd3D, range_A_sd2D;
  calcMeanSD(range_A_errors_3D, range_A_mean3D, range_A_sd3D);
  calcMeanSD(range_A_errors_2D, range_A_mean2D, range_A_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_A_start << "m, " << range_A_end << "m]" << std::endl;
  std::cout << "Evaluation of ground truth samples range_A: " << range_A_errors_3D.size() << std::endl;
  std::cout << "range_A_mean_error_3D: " << range_A_mean3D * 1000 << " (" << range_A_sd3D * 1000 << ") [" << range_A_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_A_mean_error_2D: " << range_A_mean2D << " (" << range_A_sd2D << ") [" << range_A_max_2D << "] (in pixels)" << std::endl;

  double range_B_mean3D, range_B_mean2D, range_B_sd3D, range_B_sd2D;
  calcMeanSD(range_B_errors_3D, range_B_mean3D, range_B_sd3D);
  calcMeanSD(range_B_errors_2D, range_B_mean2D, range_B_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_B_start << "m, " << range_B_end << "m]" << std::endl;
  std::cout << "Evaluation of ground truth samples range_B: " << range_B_errors_3D.size() << std::endl;
  std::cout << "range_B_mean_error_3D: " << range_B_mean3D * 1000 << " (" << range_B_sd3D * 1000 << ") [" << range_B_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_B_mean_error_2D: " << range_B_mean2D << " (" << range_B_sd2D << ") [" << range_B_max_2D << "] (in pixels)" << std::endl;

  double range_C_mean3D, range_C_mean2D, range_C_sd3D, range_C_sd2D;
  calcMeanSD(range_C_errors_3D, range_C_mean3D, range_C_sd3D);
  calcMeanSD(range_C_errors_2D, range_C_mean2D, range_C_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_C_start << "m, " << range_C_end << "m]" << std::endl;
  std::cout << "Evaluation of ground truth samples range_C: " << range_C_errors_3D.size() << std::endl;
  std::cout << "range_C_mean_error_3D: " << range_C_mean3D * 1000 << " (" << range_C_sd3D * 1000 << ") [" << range_C_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_C_mean_error_2D: " << range_C_mean2D << " (" << range_C_sd2D << ") [" << range_C_max_2D << "] (in pixels)" << std::endl;

  std::cout << "---------------------------------------------------------" << std::endl;

#if 0 // this is used to print for automatic gnu plot stuff related to paper evaluation
  // 0.0 (0.0)[0.0] & 0.0 (0.0)[0.0]
  std::cout << std::setprecision(3) << mean3D * 1000 << " (" << sd3D * 1000 << ") [" << max_3D * 1000 << "] & " << mean2D << " (" << sd2D << ") [" << max_2D << "]" << std::endl;
  std::cout << (range_A_errors_3D.size() > 0 ? range_A_mean3D * 1000 : 0.0) << std::endl;
  std::cout << (range_A_errors_3D.size() > 0 ? range_A_sd3D * 1000 : 0.0) << std::endl;
  std::cout << (range_A_errors_2D.size() > 0 ? range_A_mean2D : 0.0) << std::endl;
  std::cout << (range_A_errors_2D.size() > 0 ? range_A_sd2D : 0.0) << std::endl;

  std::cout << (range_B_errors_3D.size() > 0 ? range_B_mean3D * 1000 : 0.0) << std::endl;
  std::cout << (range_B_errors_3D.size() > 0 ? range_B_sd3D * 1000 : 0.0) << std::endl;
  std::cout << (range_B_errors_2D.size() > 0 ? range_B_mean2D : 0.0) << std::endl;
  std::cout << (range_B_errors_2D.size() > 0 ? range_B_sd2D : 0.0) << std::endl;

  std::cout << (range_C_errors_3D.size() > 0 ? range_C_mean3D * 1000 : 0.0) << std::endl;
  std::cout << (range_C_errors_3D.size() > 0 ? range_C_sd3D * 1000 : 0.0) << std::endl;
  std::cout << (range_C_errors_2D.size() > 0 ? range_C_mean2D : 0.0) << std::endl;
  std::cout << (range_C_errors_2D.size() > 0 ? range_C_sd2D : 0.0) << std::endl;
#endif

}

void
Calibrator::createErrorVis(const std::vector<nniSample>& sps, const unsigned width, const unsigned height, const unsigned depth, const char* basefilename, bool isnni){

  // create volumes for world and texture coordinates
  unsigned char* error_vol_3D = new unsigned char [width * height * depth];
  memset(error_vol_3D, 0, width * height * depth);

  unsigned char* error_vol_2D = new unsigned char [width * height * depth];
  memset(error_vol_2D, 0, width * height * depth);

  unsigned char* error_vol_3D_nni = new unsigned char [width * height * depth];
  memset(error_vol_3D_nni, 0, width * height * depth);

  unsigned char* error_vol_2D_nni = new unsigned char [width * height * depth];
  memset(error_vol_2D_nni, 0, width * height * depth);

  NearestNeighbourSearch nns(sps);
  std::vector<nniSample> sps_tmp(sps);
  std::shuffle(std::begin(sps_tmp), std::end(sps_tmp), std::default_random_engine());
  NaturalNeighbourInterpolator nnip(sps_tmp);

  // boost threads here
  const unsigned numthreads = 32;
  //std::cerr << "start interpolation of error visualization volumes using " << numthreads << " threads." << std::endl;
  boost::thread_group threadGroup;
  for (unsigned tid = 0; tid < numthreads; ++tid){
    //threadGroup.create_thread(boost::bind(&Calibrator::applyErrorVisPerThread, this, width, height, depth,
    //					  error_vol_3D, error_vol_2D, error_vol_3D_nni, error_vol_2D_nni, &nns, &nnip, tid, numthreads));
    threadGroup.create_thread([width, height, depth,
			       error_vol_3D, error_vol_2D, error_vol_3D_nni, error_vol_2D_nni, &nns, &nnip, tid, numthreads, this](){
				this->applyErrorVisPerThread(width, height, depth,
							     error_vol_3D, error_vol_2D, error_vol_3D_nni, error_vol_2D_nni, &nns, &nnip, tid, numthreads);
			      });
  }
  threadGroup.join_all();

  //std::cout << "INFO: writing " << (std::string(basefilename) + "_error3D_idw").c_str()<< std::endl;
  FILE* f_error_vol_3D = fopen((std::string(basefilename) + "_error3D_idw").c_str(), "wb");
  fwrite(error_vol_3D, sizeof(unsigned char), (width * height * depth), f_error_vol_3D);
  fclose(f_error_vol_3D);

  //std::cout << "INFO: writing " << (std::string(basefilename) + "_error2D_idw").c_str()<< std::endl;
  FILE* f_error_vol_2D = fopen((std::string(basefilename) + "_error2D_idw").c_str(), "wb");
  fwrite(error_vol_2D, sizeof(unsigned char), (width * height * depth), f_error_vol_2D);
  fclose(f_error_vol_2D);

  if(isnni){
    //std::cout << "INFO: writing " << (std::string(basefilename) + "_error3D_nni").c_str()<< std::endl;
    FILE* f_error_vol_3Dnni = fopen((std::string(basefilename) + "_error3D_nni").c_str(), "wb");
    fwrite(error_vol_3D_nni, sizeof(unsigned char), (width * height * depth), f_error_vol_3Dnni);
    fclose(f_error_vol_3Dnni);

    //std::cout << "INFO: writing " << (std::string(basefilename) + "_error2D_nni").c_str()<< std::endl;
    FILE* f_error_vol_2Dnni = fopen((std::string(basefilename) + "_error2D_nni").c_str(), "wb");
    fwrite(error_vol_2D_nni, sizeof(unsigned char), (width * height * depth), f_error_vol_2Dnni);
    fclose(f_error_vol_2Dnni);
  }
}

void
Calibrator::applyErrorVisPerThread(const unsigned width, const unsigned height, const unsigned depth,
				   unsigned char* error_vol_3D, unsigned char* error_vol_2D,
				   unsigned char* error_vol_3D_nni, unsigned char* error_vol_2D_nni,
				   const NearestNeighbourSearch* nns, const NaturalNeighbourInterpolator* nnip,
				   const unsigned tid, const unsigned numthreads){

  const unsigned idwneighbours = 20;
  const glm::vec3 diameter(width, height, depth);
  const float max_influence_dist = glm::length(diameter);
  const float max_error_3D_vol = 0.01; // 5cm
  const float max_error_2D_vol = 2.0; // 5 pixel

  for(unsigned z = tid; z < depth; z += numthreads){
    for(unsigned y = 0; y < height; ++y){
      for(unsigned x = 0; x < width; ++x){

	const unsigned cv_index = (z * width * height) + (y * width) + x;

	nniSample ipolant;
	ipolant.s_pos.x = x;
	ipolant.s_pos.y = y;
	ipolant.s_pos.z = z;

	ipolant.s_pos_off.x = 0.0;
	ipolant.s_pos_off.y = 0.0;
	ipolant.s_pos_off.z = 0.0;

	ipolant.s_tex_off.u = 0.0;
	ipolant.s_tex_off.v = 0.0;

	std::vector<nniSample> neighbours = nns->search(ipolant,idwneighbours);
	if(neighbours.empty()){
	  //std::cerr << "ERROR in Calibrator::applySamplesPerThread -> no neighbours found, skipping voxel at pos " << ipolant.s_pos << std::endl;
	  continue;
	}
	idw_interpolate(neighbours, idwneighbours, ipolant, max_influence_dist);

	// s_tex_off.u -> 3D error
	error_vol_3D[cv_index] = (unsigned char) (std::max(0.0f,
							   std::min(ipolant.s_tex_off.u, max_error_3D_vol))
						  * 255.0f/max_error_3D_vol);
	// s_tex_off.v -> 2D error
	error_vol_2D[cv_index] = (unsigned char) (std::max(0.0f,
							   std::min(ipolant.s_tex_off.v, max_error_2D_vol))
						  * 255.0f/max_error_2D_vol);

	{
	  nniSample ipolant_nni;
	  ipolant_nni.s_pos.x = x;
	  ipolant_nni.s_pos.y = y;
	  ipolant_nni.s_pos.z = z;

	  ipolant_nni.s_pos_off.x = 0.0;
	  ipolant_nni.s_pos_off.y = 0.0;
	  ipolant_nni.s_pos_off.z = 0.0;

	  ipolant_nni.s_tex_off.u = 0.0;
	  ipolant_nni.s_tex_off.v = 0.0;

	  bool nni_valid = nnip->interpolate(ipolant_nni);
	  if(nni_valid){
	    // s_tex_off.u -> 3D error
	    error_vol_3D_nni[cv_index] = (unsigned char) (std::max(0.0f,
								   std::min(ipolant_nni.s_tex_off.u, max_error_3D_vol))
							  * 255.0f/max_error_3D_vol);
	    // s_tex_off.v -> 2D error
	    error_vol_2D_nni[cv_index] = (unsigned char) (std::max(0.0f,
								   std::min(ipolant_nni.s_tex_off.v, max_error_2D_vol))
							  * 255.0f/max_error_2D_vol);
	  }

	}

      }
    }
  }

}

double
Calibrator::evaluatePlanes(CalibVolume* cv, ChessboardSampling* cbs, const RGBDConfig& cfg, unsigned stride){

  const unsigned cv_width = cv->width;
  const unsigned cv_height = cv->height;
  const unsigned cv_depth = cv->depth;

  std::vector<double> plane_qualities;
  const std::vector<ChessboardRange>& valid_ranges = cbs->getValidRanges();
  const std::vector<ChessboardViewIR>& cb_irs = cbs->getIRs();
  for(const auto& r : valid_ranges){
    for(unsigned cb_id = r.start; cb_id != r.end; ++cb_id){
      if((cb_id % stride) == 0){
	std::vector<xyz> world_space_corners;
	for(unsigned idx = 0; idx < (CB_WIDTH * CB_HEIGHT); ++idx){
	  if(cb_irs[cb_id].quality[idx] > 0.0){
	    const xyz corner = cb_irs[cb_id].corners[idx];
	    const float x = cv_width  *  ( corner.x)/ cfg.size_d.x;
	    const float y = cv_height *  ( corner.y)/ cfg.size_d.y;
	    const float z = cv_depth  *  ( corner.z - cv->min_d)/(cv->max_d - cv->min_d);
	    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );

	    pos.x *= 10;
	    pos.y *= 10;
	    pos.z *= 10;

	    world_space_corners.push_back(pos);
	  }
	}

	const auto pq = detectPlaneQuality(world_space_corners);
	//std::cout << "cb_id: " << cb_id << " -> " << pq << std::endl;
	plane_qualities.push_back(pq);
      }

    }
  }

  double mean;
  double sd;
  calcMeanSD(plane_qualities, mean, sd);

  return mean;
}

double
Calibrator::evaluate3DError(CalibVolume* cv, ChessboardSampling* cbs, const Checkerboard* cb, const RGBDConfig& cfg, float delta_t_pose, unsigned stride){

  const unsigned cv_width = cv->width;
  const unsigned cv_height = cv->height;
  const unsigned cv_depth = cv->depth;

  std::vector<double> errors_3D;
  const std::vector<ChessboardRange>& valid_ranges = cbs->getValidRanges();
  const std::vector<ChessboardViewIR>& cb_irs = cbs->getIRs();

  for(const auto& r : valid_ranges){
    for(unsigned cb_id = r.start; cb_id != r.end; ++cb_id){

      if((cb_id % stride) == 0){

	const double time = cb_irs[cb_id].time;
	// retrieve chessboards_pose
	bool valid_pose = false;
	glm::mat4 cb_transform = cbs->interpolatePose(time + delta_t_pose, valid_pose);
	if(!valid_pose){
	  continue;
	}

	std::vector<double> cb_errors_3D;
	for(unsigned idx = 0; idx < (CB_WIDTH * CB_HEIGHT); ++idx){

	  // calculate calibrated position
	  if(cb_irs[cb_id].quality[idx] > 0.0){
	    const xyz corner = cb_irs[cb_id].corners[idx];
	    const float x = cv_width  *  ( corner.x)/ cfg.size_d.x;
	    const float y = cv_height *  ( corner.y)/ cfg.size_d.y;
	    const float z = cv_depth  *  ( corner.z - cv->min_d)/(cv->max_d - cv->min_d);
	    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
	    glm::vec3 pos_calib(pos.x,pos.y,pos.z);
	    // calculate ground truth position
	    glm::vec4 pos_realH = (cb->pose_offset * cb_transform) * glm::vec4(cb->points_local[idx].x,
									       cb->points_local[idx].y,
									       cb->points_local[idx].z, 1.0f);
	    glm::vec3 pos_gt = glm::vec3(pos_realH.x, pos_realH.y, pos_realH.z);

	    cb_errors_3D.push_back(glm::length(pos_calib - pos_gt));
	  }
	}

	double cb_mean;
	double cb_sd;
	calcMeanSD(cb_errors_3D, cb_mean, cb_sd);
	errors_3D.push_back(cb_mean);
      }

    }
  }

  double mean;
  double sd;
  calcMeanSD(errors_3D, mean, sd);

  return mean;

}

double
Calibrator::evaluate2DError(CalibVolume* cv, ChessboardSampling* cbs, const RGBDConfig& cfg, float delta_t_color, unsigned stride){

  const unsigned cv_width = cv->width;
  const unsigned cv_height = cv->height;
  const unsigned cv_depth = cv->depth;

  std::vector<double> errors_2D;
  const std::vector<ChessboardRange>& valid_ranges = cbs->getValidRanges();
  const std::vector<ChessboardViewIR>& cb_irs = cbs->getIRs();

  for(const auto& r : valid_ranges){
    for(unsigned cb_id = r.start; cb_id != r.end; ++cb_id){

      if((cb_id % stride) == 0){

	const double time = cb_irs[cb_id].time;
	// retrieve color chessboard view
	bool valid_color = false;
	ChessboardViewRGB cb_rgb_i = cbs->interpolateRGB(time + delta_t_color, valid_color);
	if(!valid_color){
	  continue;
	}

	std::vector<double> cb_errors_2D;
	for(unsigned idx = 0; idx < (CB_WIDTH * CB_HEIGHT); ++idx){

	  // look up calibrated color coordinate
	  if(cb_irs[cb_id].quality[idx] > 0.0){
	    const xyz corner = cb_irs[cb_id].corners[idx];
	    const float x = cv_width  *  ( corner.x)/ cfg.size_d.x;
	    const float y = cv_height *  ( corner.y)/ cfg.size_d.y;
	    const float z = cv_depth  *  ( corner.z - cv->min_d)/(cv->max_d - cv->min_d);

	    // normalized
	    uv  cc_calib = getTrilinear(cv->cv_uv, cv_width, cv_height, cv_depth, x , y , z );
	    // renormalize
	    glm::vec2 cc_calib_pixel(cc_calib.u * cfg.size_rgb.x, cc_calib.v * cfg.size_rgb.y);

	    // retrieve ground truth color coordinate, already in pixels
	    uv  cc_gt(cb_rgb_i.corners[idx]);
	    glm::vec2 cc_gt_pixel(cc_gt.u, cc_gt.v);
#if 0
	    std::cerr << "cb_id: " << cb_id << " corner idx: " << idx
		      << " cc_calib_pixel -> cc_gt_pixel: "
		      << cc_calib_pixel[0] << ", " << cc_calib_pixel[1] << " -> "
		      << cc_gt_pixel[0] << ", " << cc_gt_pixel[1] << std::endl;
#endif
	    cb_errors_2D.push_back(glm::length(cc_calib_pixel - cc_gt_pixel));
	  }
	}

	double cb_mean;
	double cb_sd;
	calcMeanSD(cb_errors_2D, cb_mean, cb_sd);
	errors_2D.push_back(cb_mean);
      }

    }
  }

  double mean;
  double sd;
  calcMeanSD(errors_2D, mean, sd);

  return mean;

}

void
Calibrator::applySamplesPerThread(CalibVolume* cv, const NearestNeighbourSearch* nns, unsigned tid, unsigned numthreads, unsigned idwneighbours, CalibVolume* cv_nni, const NaturalNeighbourInterpolator* nnip){

  const glm::vec3 diameter(cv->width, cv->height, cv->depth);
  const float max_influence_dist = glm::length(diameter);

  const unsigned cv_width  = cv->width;
  const unsigned cv_height = cv->height;
  const unsigned cv_depth  = cv->depth;
    //unsigned having = 0;
    for(unsigned z = tid; z < cv_depth; z += numthreads){
      //std::cerr << "tid: having " << ++having << " from " << cv_depth / numthreads <<  std::endl;
      for(unsigned y = 0; y < cv_height; ++y){
	for(unsigned x = 0; x < cv_width; ++x){

	  const unsigned cv_index = (z * cv_width * cv_height) + (y * cv_width) + x;

	  nniSample ipolant;
	  ipolant.s_pos.x = x;
	  ipolant.s_pos.y = y;
	  ipolant.s_pos.z = z;

	  ipolant.s_pos_off.x = 0.0;
	  ipolant.s_pos_off.y = 0.0;
	  ipolant.s_pos_off.z = 0.0;

	  ipolant.s_tex_off.u = 0.0;
	  ipolant.s_tex_off.v = 0.0;

	  std::vector<nniSample> neighbours = nns->search(ipolant,idwneighbours);
	  if(neighbours.empty()){
	    std::cerr << "ERROR in Calibrator::applySamplesPerThread -> no neighbours found, skipping voxel at pos " << ipolant.s_pos << std::endl;
	    continue;
	  }
	  idw_interpolate(neighbours, idwneighbours, ipolant, max_influence_dist);

	  const xyz xyz_curr = cv->cv_xyz[cv_index];
	  const uv  uv_curr  = cv->cv_uv[cv_index];

	  cv->cv_xyz[cv_index] = cv->cv_xyz[cv_index] + ipolant.s_pos_off;
	  cv->cv_uv[cv_index]  = cv->cv_uv[cv_index]  + ipolant.s_tex_off;

	  if(using_nni){
	    nniSample ipolant_nni;
	    ipolant_nni.s_pos.x = x;
	    ipolant_nni.s_pos.y = y;
	    ipolant_nni.s_pos.z = z;

	    ipolant_nni.s_pos_off.x = 0.0;
	    ipolant_nni.s_pos_off.y = 0.0;
	    ipolant_nni.s_pos_off.z = 0.0;

	    ipolant_nni.s_tex_off.u = 0.0;
	    ipolant_nni.s_tex_off.v = 0.0;
	    bool nni_valid = nnip->interpolate(ipolant_nni);
	    m_nni_possible[cv_index] = nni_valid ? 255 : 0;
	    if(nni_valid){
	      cv_nni->cv_xyz[cv_index] = xyz_curr + ipolant_nni.s_pos_off;
	      cv_nni->cv_uv[cv_index]  = uv_curr  + ipolant_nni.s_tex_off;
	      // STEPPO REMOVE
	      //std::cerr << "IDW: " << ipolant << std::endl;
	      //std::cerr << "NNI: " << ipolant_nni << std::endl;
	      //glm::vec3 pos_glm(ipolant_nni.s_pos_off.x,ipolant_nni.s_pos_off.y,ipolant_nni.s_pos_off.z);
	      //const float od = glm::length(pos_glm);
	      //if(od > 0.1){
	      //	std::cerr << tid << " ERROR: outside offset_too_large at " << ipolant_nni.s_pos_off << " -> " << od << std::endl;
	      //}

	    }
	  }

	}
      }
    }

}

/*static*/ void
Calibrator::idw_interpolate(const std::vector<nniSample>& neighbours, const unsigned idw_neigbours, nniSample& ipolant, const float max_influence_dist){

  const float sigma = max_influence_dist * 1.0/3.3;
  const float mean = 0;
  const float norm = 1.0f/gauss(0.0f, sigma, mean);

  double weight_d = 0.0;
  xyz_d pos_offset;
  pos_offset.x = 0.0;pos_offset.y = 0.0;pos_offset.z = 0.0;
  uv_d tex_offset;
  tex_offset.u = 0.0;tex_offset.v = 0.0;
  glm::vec3 ipolant_pos(ipolant.s_pos.x,ipolant.s_pos.y,ipolant.s_pos.z);

  for(unsigned i = 0; i < neighbours.size() && i < idw_neigbours; ++i){
    nniSample s = neighbours[i];
    //std::cerr << "s.quality: " << s.quality <<  std::endl;
    glm::vec3 s_pos(s.s_pos.x,s.s_pos.y,s.s_pos.z);
    const float influence_dist = std::min(max_influence_dist, glm::length(ipolant_pos - s_pos));
    //std::cerr << "influence dist: " << influence_dist << " of " << max_influence_dist <<  std::endl;
    const double s_weight = /*double(s.quality) * */gauss(influence_dist, sigma, mean) * norm;
    //std::cerr << "s_weight: " << s_weight << std::endl;

    weight_d += s_weight;
    pos_offset = pos_offset + s_weight * s.s_pos_off;
    tex_offset = tex_offset + s_weight * s.s_tex_off;

  }

  if(weight_d > 0.00001){

    ipolant.s_pos_off.x = pos_offset.x/weight_d;
    ipolant.s_pos_off.y = pos_offset.y/weight_d;
    ipolant.s_pos_off.z = pos_offset.z/weight_d;

    ipolant.s_tex_off.u = tex_offset.u/weight_d;
    ipolant.s_tex_off.v = tex_offset.v/weight_d;
  }
  else{
    std::cerr << "ERROR in Calibrator::idw_interpolate!!!! weight to low (weight_d < 0.0001) " << std::endl;
  }

}


/* -----------------------------------------------------------------
 * File:    Recipe.cpp
 * Author:  Michael Gharbi <gharbi@mit.edu>
 * Created: 2015-07-24
 * -----------------------------------------------------------------
 *
 *
 *
 * ---------------------------------------------------------------*/

#include <cmath>
#include <algorithm>

#ifndef __ANDROID__
    #include <armadillo>
    #include "utils/image_io.h"
#endif

// HALIDE
#include "utils/static_image.h"

#include "recipe/hl_reconstruct.h"
#include "recipe/hl_lowpass.h"
#include "recipe/hl_highpass.h"
#include "recipe/hl_compute_features.h"
#include "recipe/hl_precompute_pyramid.h"

#include "print_helper.h"
#include "perf_measure.h"

#include "recipe/Recipe.h"

namespace xform {

Recipe::Recipe( Image<uint32_t>& unprocessed ) {
    m_model_width  = static_cast<int>(ceil(2.0f*unprocessed.width()/XFORM_WSIZE));
    m_model_height = static_cast<int>(ceil(2.0f*unprocessed.height()/XFORM_WSIZE));
    m_lp_width     = unprocessed.width()/XFORM_WSIZE;
    m_lp_height    = unprocessed.height()/XFORM_WSIZE;
    m_unprocessed_channels = 3;
    m_processed_channels   = 3;
}

#ifndef __ANDROID__
Recipe::Recipe( Image<uint32_t>& unprocessed, Image<uint32_t>& processed ):
    Recipe(unprocessed)
{
    fit(unprocessed, processed);
}
#endif

#ifndef __ANDROID__
void Recipe::fit( Image<uint32_t>& unprocessed, Image<uint32_t>& processed )
{
    PRINT("Fit recipe\n");
    m_lp_residual = std::make_shared<Image<uint32_t> >(m_lp_width,m_lp_height,m_processed_channels);
    m_hp_coefs    = std::make_shared<Image<float> >(m_model_width,m_model_height,nCoefMaps());
    m_qtable      = std::vector<float>(2*nCoefMaps());

    int width  = unprocessed.width();
    int height = unprocessed.height();

    Image<float> processed_hp(width, height, 3);

    // Compute and store lowpass of the processed image
    auto start = get_time();
    hl_lowpass(processed, *m_lp_residual);
    PRINT("  - lowpass:\t %ldms\n", get_duration(start,get_time()));

    // Compute highpass of the output
    start = get_time();
    hl_highpass(processed, *m_lp_residual, processed_hp);
    PRINT("  - highpass:\t %ldms\n", get_duration(start,get_time()));

    // Features
    start = get_time();
    Image<float> features(width, height, nFeatureChannels());
    hl_compute_features(unprocessed, features);
    PRINT("  - features:\t %ldms\n", get_duration(start,get_time()));

    start = get_time();
    regression(features, processed_hp);
    PRINT("  - regression:\t %ldms\n", get_duration(start,get_time()));

    quantize();
}
#endif

void Recipe::precompute_features( const Image<uint32_t>& unprocessed)
{
    m_lp_unprocessed = Image<uint32_t>(m_lp_width, m_lp_height);
    m_hp_unprocessed = Image<float>(unprocessed.width(), unprocessed.height(), 3);

    auto start = get_time();
    hl_lowpass(unprocessed, m_lp_unprocessed);
    PRINT("  - lowpass : %ldms\n", get_duration(start,get_time()));

    start = get_time();
    hl_highpass(unprocessed, m_lp_unprocessed, m_hp_unprocessed);
    PRINT("  - highpass : %ldms\n", get_duration(start,get_time()));

    start = get_time();
    m_pyramid_unprocessed = Image<float>(unprocessed.width(), unprocessed.height(), nPyrLevels()-1);
    hl_precompute_pyramid(unprocessed,m_pyramid_unprocessed);
    PRINT("  - precompute pyramid : %ldms\n", get_duration(start,get_time()));
}

void Recipe::reconstruct_with_features( Image<uint32_t> &output )
{
    dequantize();

    output = Image<uint32_t>(m_pyramid_unprocessed.width(),
            m_pyramid_unprocessed.height(), m_processed_channels);

    auto start = get_time();
    hl_reconstruct(
            m_pyramid_unprocessed,
            m_hp_unprocessed,
            *m_hp_coefs,
            *m_lp_residual,
            output
    );
    PRINT("  - reconstruct precomputed: %ldms\n", get_duration(start,get_time()));
}

void Recipe::reconstruct_image( const Image<uint32_t>& unprocessed, Image<uint32_t> &output )
{
    precompute_features(unprocessed);
    reconstruct_with_features(output);
}

#ifndef __ANDROID__
void Recipe::regression( Image<float>& features, Image<float>& target)
{
    const int h_mdl           = m_model_height;
    const int w_mdl           = m_model_width;
    const int h               = target.height();
    const int w               = target.width();
    const int n_targets       = target.channels();
    const int n_lumCoefs      = nLumCoefs();
    const int n_chromCoefs    = nChromCoefs();
    const int n_features      = features.channels();

    float* pDataFeat   = features.data();
    float* pDataTarget = target.data();
    float* pLuminCoef  = m_hp_coefs->data();
    float* pChromCoef  = pLuminCoef + h_mdl*w_mdl*n_lumCoefs;

    int wSize = XFORM_WSIZE;
    int step  = XFORM_WSIZE/2;

    #pragma omp parallel for
    for (int patch_idx = 0; patch_idx < h_mdl*w_mdl; ++patch_idx)
    {
        // temporary storage for the current patch
        float* X = new float[wSize*wSize*n_lumCoefs]();
        float* Y = new float[wSize*wSize*n_targets]();

        // Patch indices (in the recipe)
        int patch_x = patch_idx % w_mdl;
        int patch_y = patch_idx / w_mdl;

        // Pixel indices (in the image)
        int x_min   = patch_x*step;
        int x_max   = min(x_min+wSize, w);
        int y_min   = patch_y*step;
        int y_max   = min(y_min+wSize, h);
        int n_samples = (y_max-y_min)*(x_max-x_min);

        // Copy patch features
        int idx = 0;
        for (int f = 0; f < n_features; ++f)
        for (int y = y_min; y < y_max; ++y)
        for (int x = x_min; x < x_max; ++x)
        {
            X[idx] = pDataFeat[x+y*w+f*h*w];
            ++idx;
        }

        // Make features for the non-linear luminance curve
        int first_curve = idx;
        auto minmax     = std::minmax_element(X,X + n_samples);
        if(!XFORM_ADAPTIVE_LUMA_RANGE) {
            *minmax.first = 0.0f;
            *minmax.second = 1.0f;
        }
        float step_sz   = (*minmax.second-*minmax.first)/XFORM_LUMA_BANDS;
        float mini      = *minmax.first;
        for (int s = 0; s < XFORM_LUMA_BANDS-1; ++s){
            float thresh = mini+(s+1)*step_sz;
            for (idx = 0; idx < n_samples; ++idx)
            {
                X[first_curve + n_samples*s + idx] = std::max(X[idx]-thresh, 0.0f);
            }
        }

        // Copy target variables
        idx = 0;
        for (int f = 0; f < n_targets; ++f)
        for (int y = y_min; y < y_max; ++y)
        for (int x = x_min; x < x_max; ++x)
        {
            Y[idx] = pDataTarget[x+y*w+f*h*w];
            ++idx;
        }

        // Solve least-square regression
        arma::Mat<float> matX(X, n_samples,n_lumCoefs, false);
        arma::Mat<float> matY(Y, n_samples,n_targets, false);

        // For the luminance
        arma::Mat<float> result_lumin;
        arma::Mat<float> Xview = matX.cols(0,n_lumCoefs-1);
        arma::Mat<float> Yview = matY.cols(0,0);
        arma::Mat<float> regularizer = arma::eye<arma::Mat<float>>(n_lumCoefs, n_lumCoefs);
        regularizer(3,3) = 0.0f; //do not regularize affine offset
        arma::Mat<float> lhs = arma::trans(Xview) * Xview + XFORM_EPSILON*regularizer;
        arma::Mat<float> rhs = arma::trans(Xview) * Yview;
        arma::solve(result_lumin,lhs,rhs);

        // For the chrominance
        arma::Mat<float> result_chrom;
        Xview = matX.cols(0,n_chromCoefs-1);
        Yview = matY.cols(1,2);
        regularizer = arma::eye<arma::Mat<float>>(n_chromCoefs, n_chromCoefs);
        regularizer(3,3) = 0.0f; //do not regularize affine offset
        lhs = arma::trans(Xview) * Xview + XFORM_EPSILON*regularizer;
        rhs = arma::trans(Xview) * Yview;
        arma::solve(result_chrom,lhs,rhs);

        // Fill-in the luminance regression coefficients
        float* coefs = result_lumin.memptr();
        idx = 0;
        for (int f = 0; f < n_lumCoefs; ++f)
        {
            pLuminCoef[patch_x+patch_y*w_mdl+f*h_mdl*w_mdl] = coefs[idx];
            ++idx;
        }

        // Fill in the chrominance regression coefficients
        coefs = result_chrom.memptr();
        idx = 0;
        for (int c = 0; c < 2; ++c)
        for (int f = 0; f < n_chromCoefs; ++f)
        {
            pChromCoef[patch_x + patch_y*w_mdl + f*h_mdl*w_mdl + c*n_chromCoefs*h_mdl*w_mdl] = coefs[idx];
            ++idx;
        }
        coefs = nullptr;

        delete[] X; X = nullptr;
        delete[] Y; Y = nullptr;
    }
    pDataTarget = nullptr;
    pDataFeat   = nullptr;
}
#endif

void Recipe::quantize()
{
    const int h_mdl   = m_model_height;
    const int w_mdl   = m_model_width;
    const int n_feats = nCoefMaps();

    // Get values range per regression channel
    float* pCoef = m_hp_coefs->data();

    // Compute min and max for each channel
    for (int f = 0; f < n_feats; ++f){
        m_qtable[2*f]   = pCoef[f*h_mdl*w_mdl];
        m_qtable[2*f+1] = pCoef[f*h_mdl*w_mdl];
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x)
        {
            float val = pCoef[x + y*w_mdl + f*h_mdl*w_mdl];
            // min
            m_qtable[2*f] = min(m_qtable[2*f],val);
            // max
            m_qtable[2*f+1] = max(m_qtable[2*f+1],val);
        }
    }

    // Quantize
    for (int f = 0; f < n_feats; ++f)
    {
        float min = (m_qtable)[2*f];
        float max = (m_qtable)[2*f+1];
        float inv_rng = 1.0f/(max-min);
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x)
        {
            float *val = pCoef + (x+y*w_mdl+f*h_mdl*w_mdl);
            *val -= min;
            *val *= inv_rng;
            *val = floor(255.0f*(*val)+0.5f);
        }
    }
}

void Recipe::dequantize()
{
    const int h_mdl        = m_model_height;
    const int w_mdl        = m_model_width;
    const int n_feats = nCoefMaps();

    float* pCoef  = m_hp_coefs->data();

    // Dequantize
    for (int f = 0; f < n_feats; ++f)
    {
        float min = m_qtable[2*f];
        float max = m_qtable[2*f+1];
        float rng = (max-min)/255.0f;
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x) {
            float *val = pCoef + (x+y*w_mdl+f*h_mdl*w_mdl);
            *val *= rng;
            *val += min;
        }
    }
}

} //namespace xform


/// \file   python_module.cpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2020-08-30
/// \copyright  Copyright 2017-2020 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#include <esl/mathematics/python_module_mathematics.hpp>

#ifdef WITH_PYTHON

#include <esl/mathematics/variable.hpp>

#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include <boost/python.hpp>
using namespace boost::python;

#include <adept/Stack.h>

typedef adept::internal::BinaryOperation<double, adept::Active<double>, adept::internal::Multiply, adept::Active<double> > binary_operation_t;

using namespace esl;

BOOST_PYTHON_MODULE(_mathematics)
{
    class_<variable>( "variable", init<>())
        .def(init<double>())
        .def(self == self)
        .def(self != self)
        .def(self < self)
        .def(self <= self)
        .def(self > self)
        .def(self >= self)
        .def(self + self)
        .def(self += self)
        .def(self - self)
        .def(self -= self)
        .def(self * self)
        .def(self *= self)
        .def(self / self)
        .def(self /= self)
        //.def("value", &variable::value)
        ;

    class_<adept::Stack>("stack", init<>())
        .def("pause_recording", &adept::Stack::pause_recording)
        .def("continue_recording", &adept::Stack::continue_recording)
        .def("new_recording", &adept::Stack::new_recording)
        .def("compute_adjoint", &adept::Stack::compute_adjoint)

    ;

    // TODO: verify that all opeartions on this are exported
    class_<binary_operation_t>("binary_operation_t", no_init)
        .def(self == self)
        .def(self != self)
        .def(self < self)
        .def(self <= self)
        .def(self > self)
        .def(self >= self)
        .def(self + self)
       // .def(self + variable())
        .def(self - self)
     //   .def(self - variable())
        .def(self * self)
      //  .def(self * variable())
        .def(self / self)
    //    .def(self / variable())
        ;

// TODO: export these
//    def("get_gradients", &adept::get_gradients);
//    stack_.independent(&active_[0], active_.size());
//    stack_.dependent(&values_[0], values_.size());
//    stack_.jacobian(jacobian);

}

#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
/*! \file BlockCSRMatrix.hpp
Definition of a compressed row sparse block matrix of SiconosMatrix*
*/

#ifndef BLOCKCSRMATRIX_H
#define BLOCKCSRMATRIX_H

#include "NumericsFwd.h"  // Numerics forward declarations
#include "SimulationTypeDef.hpp"
#include "SiconosSerialization.hpp" // for ACCEPT_SERIALIZATION
#include <boost/numeric/ublas/fwd.hpp> // Boost forward declarations

/* with signed int typedef  boost::numeric::ublas::compressed_matrix<double*> CompressedRowMat; */
/* cf http://boost.2283326.n4.nabble.com/LU-decomposition-of-compressed-matrix-td3417929.html */
typedef boost::numeric::ublas::compressed_matrix <
double*, boost::numeric::ublas::basic_row_major<unsigned int>, 0, boost::numeric::ublas::unbounded_array<std::size_t> >  CompressedRowMat;
TYPEDEF_SPTR(CompressedRowMat)
TYPEDEF_SPTR(SparseBlockStructuredMatrix)

/** Definition of a compressed sparse row matrix of SiconosMatrix,
 * used in OneStepNSProblem to store the M matrix.
 *
 * This class defines a specific compressed row sparse storage for
 * blocks matrices, each block being a SiconosMatrix*.
 *
 * It handles:
 * - a SparseMat (boost-ublas) of SiconosMatrix*
 * - a vector<SiconosMatrix*> which handles the non-null blocks

 * - three vector<int> (IndexInt) to save non-null blocks position in
     row, columns and the list of the sizes of diagonal blocks.

 * - two int, the number of blocks in a row and the number of non null blocks.
 *
 * Each block of the current object represents the connection between
 * two coupled Interactions, \n (for example for Lagrangian
 * systems, a single \f$ H W^{-1} H^t \f$ block or for first order
 * systems \f$ hCW^{-1}B \f$ ...) \n
 *
 * This objects is built using an index set of SP::Interaction,
 * that represents the "active" constraints in the OSNS problem and a
 * map<SP::Interaction u1, <SP::Interaction u2, SP::SiconosMatrix block> >, block being the link
 * between u1 and u2. Only Interaction present in the index set are picked out
 * in the map.
 *
 *  A convert method is also implemented to create a
 *  SparseBlockStructuredMatrix which is Numerics-readable.
 *
 * As an example, consider the index set I={u1, u3, u5, u8} and the
 * map where non null blocks are (ui,ui), (u1,u3), (u1,u8), (u3,u1),
 * (u8,u1).\n Each block being a pointer to a 3x3 matrix.\n Then the
 * resulting matrix has 4 X 4 blocks, with 8 non-null blocks and looks
 * like:
 *
 \rst

 .. math::
    :nowrap:

    M=\left\lbrace\begin{array}{cccc}
    b11 & b13 & 0 & b18 \\
    b31 & b22 & 0 & 0 \\
    0   & 0   & b33&0 \\
    b81 & 0   & 0 & b44
    \end{array}\right.

 \endrst
 *
 * with nc = 4, nbNonNullBlocks = 8, RowPos = [0 0 0 1 1 2 3 3],
 * RowCol = [0 1 3 0 1 2 0 3]\n and _diagsize0 = [3 6 9 12].
 *
 * We use stl::vector (which may seems redundent with the double* of
 * the numerics SparseBlockStructuredMatrix) because memory can be
 * reserved during construction or initialized and then vectors are
 * resized when the object is filled in. This avoid some call to
 * malloc/free at each iteration.
 *
 */
class BlockCSRMatrix
{
private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(BlockCSRMatrix);

  /** Number of blocks rows (first dimension of the block matrix)*/
  unsigned int _nr;

  /** Number of blocks columns (second dimension of the block matrix)*/
  unsigned int _nc;

  /** Sparse-Block Boost Matrix. Each block is a SiconosMatrix**/
  SP::CompressedRowMat _blockCSR;

  /** Specific structure required when a (Numerics) solver block is used */
  SP::SparseBlockStructuredMatrix _sparseBlockStructuredMatrix;

  /** Vector used to save the sum of rows of diagonal blocks of M:
      _diagsize0[i] = _diagsize0[i-1] + ni, ni being the size of the
      diagonal block at row(block) i */
  SP::IndexInt _diagsize0;

  /** Vector used to save the sum of dim of diagonal blocks of M:
      _diagsize0[i] = _diagsize0[i-1] + ni, ni being the size of the
      diagonal block at row(block) i */
  SP::IndexInt _diagsize1;

  /** List of non null blocks positions (in row) */
  SP::IndexInt rowPos;

  /** List of non null blocks positions (in col) */
  SP::IndexInt colPos;

  /** Private copy constructor => no copy nor pass by value */
  BlockCSRMatrix(const BlockCSRMatrix&);

  /** Private assignment -> forbidden
   * \return  BlockCSRMatrix&
   */
  BlockCSRMatrix& operator=(const BlockCSRMatrix&);

public:

  /** Default constructor -> empty matrix
   */
  BlockCSRMatrix();

  /** Constructor with dimension (number of blocks)
      \param n number of blocks in a row/column (only square matrices allowed)
  */
  BlockCSRMatrix(unsigned int n);

  /** Constructor from index set
      \param indexSet the index set of the active constraints
  */
  BlockCSRMatrix(InteractionsGraph& indexSet);

  /** destructor
   */
  ~BlockCSRMatrix();

  /** get size (in block-components)
   * \return unsigned int NumberOfBlocksInARow
   */
  inline unsigned int numberOfBlocksInARow() const
  {
    return _nr;
  };

  /** get total number of non-null blocks
   * \return unsigned int
   */
  unsigned int getNbNonNullBlocks() const;

  /** get the numerics-readable structure
   * \return SP::SparseBlockStructuredMatrix
   */
  inline SP::SparseBlockStructuredMatrix getNumericsMatSparse()
  {
    return _sparseBlockStructuredMatrix;
  };

  /** get the ublas sparse mat
   * \return SP::CompressedRowMat
   */
  inline SP::CompressedRowMat getMSparse()
  {
    return _blockCSR;
  };

  /** get the dimension of the square-diagonal block number num
   * \param i block position
   * \return unsigned int
  */
  IndexInt::value_type getSizeOfDiagonalBlock(int i) const
  {
    if (i == 0) return _diagsize0->at(0);
    else return (_diagsize0->at(i) - _diagsize0->at(i - 1));
  };

  /** get the index of blocks position (i=0 -> rows, i=1 -> columns)
   * \param i unsigned int, 0 for rows, 1 for columns
   * \return SP::IndexInt
   */
  inline SP::IndexInt getPositionsIndex(bool i)
  {
    if (i) return rowPos;
    else return colPos;
  };

  /** fill the current class using an index set
   *  \param indexSet set of the active constraints
   */
  void fill(InteractionsGraph& indexSet);

  /** fill the matrix with the Mass matrix
   * \warning only for NewtonEulerDS
   * \param indexSet of the active constraints
   */
  void fillM(InteractionsGraph& indexSet);

  /** fill the matrix with the H matrix
   * \warning only for NewtonEuler3DR
   * \param indexSet of the active constraints
   */
  void fillH(InteractionsGraph& indexSet);

  /** fill the numerics structure _sparseBlockStructuredMatrix using _blockCSR */
  void convert();

  /** display the current matrix
   */
  void display() const;
};

#endif


#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Sundials.h"

namespace Upp {

using namespace Eigen;

#include <kinsol/kinsol.h>           		  /* access to KINSOL func., consts. */
#include <ida/ida.h>                          /* prototypes for IDA fcts., consts.    */
#include <nvector/nvector_serial.h>           /* access to serial N_Vector            */
#include <sunmatrix/sunmatrix_dense.h>        /* access to dense SUNMatrix            */
#include <sunlinsol/sunlinsol_dense.h>        /* access to dense SUNLinearSolver      */
#include <sunnonlinsol/sunnonlinsol_newton.h> /* access to Newton SUNNonlinearSolver  */
#include <sundials/sundials_types.h>          /* defs. of realtype, sunindextype      */
#include <sundials/sundials_math.h>           /* defs. of SUNRabs, SUNRexp, etc.      */

static void CheckMem(void *returnvalue, const char *funcname) {
	if (returnvalue == NULL)
		throw Exc(Format(t_("SUNDIALS_ERROR: %s() failed - returned NULL pointer"), funcname));
}

static void CheckRet(int returnvalue, const char *funcname) {
	if (returnvalue < 0)
	  	throw Exc(Format(t_("SUNDIALS_ERROR: %s() failed with retval = %d"), funcname, returnvalue));
}

static void PrintFinalStats(void *mem) {
	int retval;
	long int nst, nni, nje, nre, nreLS, netf, ncfn, nge;

	retval = IDAGetNumSteps(mem, &nst);
	CheckRet(retval, "IDAGetNumSteps");
	retval = IDAGetNumResEvals(mem, &nre);
	CheckRet(retval, "IDAGetNumResEvals");
	retval = IDAGetNumJacEvals(mem, &nje);
	CheckRet(retval, "IDAGetNumJacEvals");
	retval = IDAGetNumNonlinSolvIters(mem, &nni);
	CheckRet(retval, "IDAGetNumNonlinSolvIters");
	retval = IDAGetNumErrTestFails(mem, &netf);
	CheckRet(retval, "IDAGetNumErrTestFails");
	retval = IDAGetNumNonlinSolvConvFails(mem, &ncfn);
	CheckRet(retval, "IDAGetNumNonlinSolvConvFails");
	retval = IDAGetNumLinResEvals(mem, &nreLS);
	CheckRet(retval, "IDAGetNumLinResEvals");
	retval = IDAGetNumGEvals(mem, &nge);
	CheckRet(retval, "IDAGetNumGEvals");

	printf("\nFinal Run Statistics: \n\n");
	printf("Number of steps                    = %ld\n", nst);
	printf("Number of residual evaluations     = %ld\n", nre+nreLS);
	printf("Number of Jacobian evaluations     = %ld\n", nje);
	printf("Number of nonlinear iterations     = %ld\n", nni);
	printf("Number of error test failures      = %ld\n", netf);
	printf("Number of nonlinear conv. failures = %ld\n", ncfn);
	printf("Number of root fn. evaluations     = %ld\n", nge);
}

static void PrintIteration(realtype t, realtype *y, int neq) {
	Cout() << "\n" << Format("%7.3f ", t);
	for (int i = 0; i < neq; ++i)
		Cout() << Format("%12.4e ", y[i]);
}

static void PrintRootInfo(int *roots, int nroots) {
	Cout() << "\nRoots found[] = ";
	for (int i = 0; i < nroots; ++i)
		Cout() << roots[i] << " ";
}

static String GetIdaErrorMsg(int ret) {
	switch (ret) {
	case IDA_TOO_MUCH_WORK:	return "IDASolve: IDA_TOO_MUCH_WORK";
	case IDA_TOO_MUCH_ACC:  return "IDASolve: IDA_TOO_MUCH_ACC";
	case IDA_ERR_FAIL:      return "IDASolve: IDA_ERR_FAIL";
	case IDA_MEM_NULL:		return "ida_mem is NULL";
	case IDA_NO_MALLOC:		return "ida_mem was not allocated";
	case IDA_ILL_INPUT:		return "bad value for icopt, tout1, or id";
	case IDA_LINIT_FAIL:	return "the linear solver linit routine failed";
 	case IDA_BAD_EWT:		return "zero value of some component of ewt";
 	case IDA_RES_FAIL:		return "res had a non-recoverable error";
 	case IDA_FIRST_RES_FAIL:return "res failed recoverably on the first call";
 	case IDA_LSETUP_FAIL:	return "lsetup had a non-recoverable error";
 	case IDA_LSOLVE_FAIL:	return "lsolve had a non-recoverable error";
 	case IDA_NO_RECOVERY:	return "res, lsetup, or lsolve had a recoverable error, but IDACalcIC could not recover";
 	case IDA_CONSTR_FAIL:	return "the inequality constraints could not be met";
 	case IDA_LINESEARCH_FAIL:return"the linesearch failed (either on steptol test or on the maxbacks test)";
 	case IDA_CONV_FAIL:		return "the Newton iterations failed to converge";
	default:				return Format("Unknown Sundials error %d", ret);
	}
}

void SolveDAE(const VectorXd &y, const VectorXd &dy, double dt, double maxt, Upp::Array<VectorXd> &res,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>ResidualZero) {
	ASSERT(y.size() == dy.size());
	Eigen::Index numEq = y.size();
	res.SetCount(int(numEq));
	for (int i = 0; i < numEq; ++i) {
		res[i].resize(int(maxt/dt)+1);
		res[i](0) = y[i];
	}
	SolveDAE(y.data(), dy.data(), int(numEq), dt, maxt, Residual, numZero, ResidualZero, [&](double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)->int {
		for (int i = 0; i < int(numEq); ++i)
			res[i](iiter) = y[i];
		return true;
	});
}

void SolveDAE(const VectorXd &y, const VectorXd &dy, double dt, double maxt,
		Upp::Array<VectorXd> &res, Upp::Array<VectorXd> &dres,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>ResidualZero) {
	ASSERT(y.size() == dy.size());
	Eigen::Index numEq = y.size();
	res.SetCount(int(numEq));
	dres.SetCount(int(numEq));
	for (int i = 0; i < numEq; ++i) {
		res[i].resize(int(maxt/dt)+1);
		res[i](0) = y[i];
		dres[i].resize(int(maxt/dt)+1);
		dres[i](0) = dy[i];
	}
	SolveDAE(y.data(), dy.data(), int(numEq), dt, maxt, Residual, numZero, ResidualZero, [&](double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)->int {
		for (int i = 0; i < int(numEq); ++i) {
			res[i](iiter) = y[i];
			dres[i](iiter) = dy[i];
		}
		return true;
	});
}

void SolveDAE(const double y[], const double dy[], int numEq, double dt, double maxt,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> ResidualZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)>OnIteration) {
	int retval, retvalr;
	Buffer<int> rootsfound(numEq);
	SUNNonlinearSolver NLS = NULL;
	SUNLinearSolver LS = NULL;
	SUNMatrix A = NULL;
	void *mem = NULL;
	N_Vector avtol = NULL, yy = NULL, yp = NULL;

	Exc error;
	try {
		yy = N_VNew_Serial(numEq);
		CheckMem((void *)yy, "N_VNew_Serial");
		yp = N_VNew_Serial(numEq);
		CheckMem((void *)yp, "N_VNew_Serial");
		avtol = N_VNew_Serial(numEq);
		CheckMem((void *)avtol, "N_VNew_Serial");

	  	/* Create and initialize  y, y', and absolute tolerance vectors. */
		memcpy(N_VGetArrayPointer(yy), y,  numEq*sizeof(double));
		memcpy(N_VGetArrayPointer(yp), dy, numEq*sizeof(double));

		realtype rtol = 1.0e-5;

		realtype *atval = N_VGetArrayPointer(avtol);
		for (int i = 0; i < numEq; ++i)
			atval[i] = 1.0e-8;

	  	double t0 = 0;

		struct UserData {
			Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> Residual;
			Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> ResidualZero;
			int iiter = 0;
		} userData;

		userData.Residual = Residual;
		userData.ResidualZero = ResidualZero;

	    auto ResFun = [](realtype t, N_Vector _y, N_Vector _dy, N_Vector _res, void *user_data) {
			realtype *y = N_VGetArrayPointer(_y);
			realtype *dy = N_VGetArrayPointer(_dy);
			realtype *res = N_VGetArrayPointer(_res);

			UserData &userData = *(UserData *)user_data;

			return userData.Residual(t, userData.iiter, y, dy, res) ? 0 : -1;
		};
	    auto ResZeroFun = [](realtype t, N_Vector _y, N_Vector _dy, realtype *res, void *user_data) {
			realtype *y = N_VGetArrayPointer(_y);
			realtype *dy = N_VGetArrayPointer(_dy);

			UserData &userData = *(UserData *)user_data;

			return userData.ResidualZero(t, userData.iiter, y, dy, res) ? 0 : -1;
		};

		/* Call IDACreate and IDAInit to initialize IDA memory */
		mem = IDACreate();
		CheckMem((void *)mem, "IDACreate");

		retval = IDASetUserData(mem, (void *)&userData);
		CheckRet(retval, "IDASetUserData");

		retval = IDAInit(mem, ResFun, t0, yy, yp);
		CheckRet(retval, "IDAInit");

		IDASetErrHandlerFn(mem, [](int error_code, const char *module, const char *function,
                                char *msg, void *user_data) {
        	throw Exc(Format(t_("Sundials %s error (%s): %s"), module, function, msg));
                                }, nullptr);

		/* Call IDASVtolerances to set tolerances */
		retval = IDASVtolerances(mem, rtol, avtol);
		CheckRet(retval, "IDASVtolerances");

		/* Call IDARootInit to specify the root function grob with 2 components */
		if (ResidualZero && numZero > 0) {
			retval = IDARootInit(mem, numZero, ResZeroFun);
			CheckRet(retval, "IDARootInit");
		}

		/* Create dense SUNMatrix for use in linear solver */
		A = SUNDenseMatrix(numEq, numEq);
		CheckMem((void *)A, "SUNDenseMatrix");

		/* Create dense SUNLinearSolver object */
		LS = SUNLinSol_Dense(yy, A);
		CheckMem((void *)LS, "SUNLinSol_Dense");

		/* Attach the matrix and linear solver */
		retval = IDASetLinearSolver(mem, LS, A);
		CheckRet(retval, "IDASetLinearSolver");

		retval = IDASetMaxNumSteps(mem, 20000);
		CheckRet(retval, "IDASetLinearSolver");

		/* Create Newton SUNNonlinearSolver object. IDA uses a
		* Newton SUNNonlinearSolver by default, so it is unnecessary
		* to create it and attach it. It is done in this example code
		* solely for demonstration purposes. */
		NLS = SUNNonlinSol_Newton(yy);
		CheckMem((void *)NLS, "SUNNonlinSol_Newton");

		/* Attach the nonlinear solver */
		retval = IDASetNonlinearSolver(mem, NLS);
		CheckRet(retval, "IDASetNonlinearSolver");

		userData.iiter = 0;
		double tnext = 0.000001;
		double titer;
		while(tnext <= maxt && userData.iiter < int(maxt/dt)+1) {
			retval = IDASolve(mem, tnext, &titer, yy, yp, IDA_NORMAL);
			CheckRet(retval, "IDASolve");

			realtype *y  = N_VGetArrayPointer(yy);
			realtype *dy = N_VGetArrayPointer(yp);

			//PrintIteration(titer, y, numEq);

			if (retval == IDA_ROOT_RETURN) {
				retvalr = IDAGetRootInfo(mem, rootsfound);
				CheckRet(retvalr, "IDAGetRootInfo");
				//PrintRootInfo(rootsfound, numEq);
			} else if (retval == IDA_SUCCESS)
				;
			else
				throw Exc(Format(t_("Sundials IDA error: %s"), GetIdaErrorMsg(retval)));

			if(OnIteration && !OnIteration(titer, userData.iiter, y, dy, retval == IDA_ROOT_RETURN, rootsfound))
				break;

			if (retval == IDA_SUCCESS) {
				userData.iiter++;
				tnext = userData.iiter*dt;
			}
	  	}
	  	//PrintFinalStats(mem);
	} catch(Exc err) {
		error = err;
	}
	IDAFree(&mem);
	SUNNonlinSolFree(NLS);
	SUNLinSolFree(LS);
	SUNMatDestroy(A);
	N_VDestroy(avtol);
	N_VDestroy(yy);
	N_VDestroy(yp);

	if (!error.IsEmpty())
		throw Exc(error);
}

/*
0 	then no constraint is imposed on ui.
1 	then ui will be constrained to be ui 0:0.
-1 	then ui will be constrained to be ui 0:0.
2 	then ui will be constrained to be ui > 0:0.
-2 	then ui will be constrained to be ui < 0:0.
*/
void SolveNonLinearEquationsSun(double y[], int numEq,
			Function <bool(const double b[], double residuals[])> Residual, int constraints[]) {
	N_Vector s = NULL;
	void *kmem = NULL;

  	Exc error;
  	try {
  		N_Vector u = N_VNew_Serial(numEq);
		CheckMem((void *)u, "N_VNew_Serial");
  		realtype *udata = NV_DATA_S(u);
 		memcpy(udata, y, numEq*sizeof(double));

		s = N_VNew_Serial(numEq);
	  	CheckMem((void *)s, "N_VNew_Serial");

		N_VConst_Serial(1. ,s); /* no scaling */

		double fnormtol = 1.e-6;
		double scsteptol = 1.e-6;
		int numIterations = 500;

	  	kmem = KINCreate();
	  	CheckMem((void *)kmem, "KINCreate");

	  	auto ErrorFun = [](int error_code, const char *module, const char *function, char *msg, void *data) {
		  	String serror = Format("%d, module %s, function %s", error_code, module, function);
		  	if (error_code == KIN_WARNING)
		    	Cout() << Format(t_("Kinsol Warning (%s): %s"), serror, msg);
		  	else {
		  		char *str = KINGetReturnFlagName(error_code);
		  		String cerror(str);
		  		free(str);
		    	throw Exc(Format(t_("Kinsol Error %s (%s): %s"), cerror, serror, msg));
		  	}
		};

	  	int flag;

		flag = KINSetErrHandlerFn(kmem, ErrorFun, NULL);
		CheckRet(flag, "KINSetErrHandlerFn");

	  	struct UserData {
			Function <bool(const double y[], double residual[])> Residual;
		} userData;

		userData.Residual = Residual;

	  	flag = KINSetUserData(kmem, &userData);
	  	CheckRet(flag, "KINSetUserData");
		flag = KINSetFuncNormTol(kmem, fnormtol);
		CheckRet(flag, "KINSetFuncNormTol");
		flag = KINSetScaledStepTol(kmem, scsteptol);
		CheckRet(flag, "KINSetScaledStepTol");
		flag = KINSetNumMaxIters(kmem, numIterations);
	  	CheckRet(flag, "KINSetNumMaxIters");

	  	auto ResFun = [](N_Vector u, N_Vector f, void *user_data) {
			realtype *y = NV_DATA_S(u);
			realtype *res = NV_DATA_S(f);

			UserData &userData = *(UserData *)user_data;

			return userData.Residual(y, res) ? 0 : 1;
		};

		flag = KINInit(kmem, ResFun, u);
		CheckRet(flag, "KINInit");

		if (constraints != nullptr) {
			N_Vector constr = N_VNew_Serial(numEq);
			CheckMem((void *)constr, "N_VNew_Serial");
			realtype *constrdata = NV_DATA_S(constr);
			for (int i = 0; i < numEq; ++i)
				constrdata[i] = constraints[i];

		 	flag = KINSetConstraints(kmem, constr);
		  	CheckRet(flag, "KINSetConstraints");

			N_VDestroy_Serial(constr);
		}

		/* Create dense SUNMatrix */
		SUNMatrix J = SUNDenseMatrix(numEq, numEq);
		CheckMem((void *)J, "SUNDenseMatrix");

		/* Create dense SUNLinearSolver object */
		SUNLinearSolver LS = SUNLinSol_Dense(u, J);
		CheckMem((void *)LS, "SUNLinSol_Dense");

		/* Attach the matrix and linear solver to KINSOL */
		flag = KINSetLinearSolver(kmem, LS, J);
		CheckRet(flag, "KINSetLinearSolver");

		int glstr = KIN_NONE;	// KIN_NONE 		KIN_LINESEARCH	Using line search
		int mset = 1;			// 1 Exact Newton	0 Modified Newton

		flag = KINSetMaxSetupCalls(kmem, mset);
		CheckRet(flag, "KINSetMaxSetupCalls");

		flag = KINSol(kmem, u, glstr, s, s);
		CheckRet(flag, "KINSol");

		for (int i = 0; i < numEq; ++i)
			if (udata[i] != udata[i])
				throw Exc(Format(t_("Obtained NaN in value %d"), i));
		 memcpy(y, udata, numEq*sizeof(double));

  	} catch(Exc err) {
  		error = err;
  	}
 	N_VDestroy_Serial(s);
  	KINFree(&kmem);
  	if (!error.IsEmpty())
  		throw Exc(error);
}

}