//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

#include <boost/simd/function/any.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/two.hpp>

#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

 BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) BOOST_NOEXCEPT
    {
      return musl_(log1p)(a0);
    }
  };

 BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) BOOST_NOEXCEPT
    {
      return plain_(log1p)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::musl_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      uiA0 iu = bitwise_cast<uiA0>(uf);
      iu += 0x3f800000 - 0x3f3504f3;
      iA0 k = bitwise_cast<iA0>(iu>>23) - 0x7f;
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iu = (iu&0x007fffff) + 0x3f3504f3;
      A0 f =  dec(bitwise_cast<A0>(iu));
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
      A0 dk = tofloat(k);
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::musl_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      uiA0 hu = bitwise_cast<uiA0>(uf)>>32;
      hu += 0x3ff00000 - 0x3fe6a09e;
      iA0 k = bitwise_cast<iA0>(hu>>20) - 0x3ff;
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c =  if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;
      hu =  (hu&0x000fffff) + 0x3fe6a09e;
      A0 f = bitwise_cast<A0>( bitwise_cast<uiA0>(hu<<32) | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(uf))));
      f = dec(f);

      A0 hfsq = Half<A0>()*sqr(f);
      A0 s = f/(2.0 + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };
//=================================================================================================================
  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::plain_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

     /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 k;
      A0 x;
      std::tie(x, k) = ifrexp(uf);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() >  x);
      k += bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 k;
      A0 x;
      std::tie(x, k) = ifrexp(uf);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() >  x);
      k += bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;

      A0 hfsq = Half<A0>()*sqr(f);
      A0 s = f/(2.0 + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

} } }

#endif


#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <limits.h>
#include <Eigen/Eigen>
#include <opengv/triangulation/methods.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/math/cayley.hpp>
#include <sstream>
#include <fstream>
#include <boost/concept_check.hpp>

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace Eigen;
using namespace opengv;

int main( int argc, char** argv )
{
	//initialize random seed
	initializeRandomSeed();

	//set experiment parameters
	double noise = 2.5;
	double outlierFraction = 0.3;
	size_t numberPoints = 200;

	//generate a random pose for viewpoint 1
	translation_t position1 = Eigen::Vector3d::Zero();
	rotation_t rotation1 = Eigen::Matrix3d::Identity();

	//generate a random pose for viewpoint 2
	translation_t position2 = generateRandomTranslation(2.0);
	rotation_t rotation2 = generateRandomRotation(0.25);

	//create a fake central camera
	translations_t camOffsets;
	rotations_t camRotations;
	generateCentralCameraSystem( camOffsets, camRotations );

	//derive correspondences based on random point-cloud
	bearingVectors_t bearingVectors1;
	bearingVectors_t bearingVectors2;
	std::vector<int> camCorrespondences1; //unused in the central case
	std::vector<int> camCorrespondences2; //unused in the central case
	Eigen::MatrixXd gt(3,numberPoints);
	generateRandom2D2DCorrespondences(
		position1, rotation1, position2, rotation2,
		camOffsets, camRotations, numberPoints, noise, outlierFraction,
		bearingVectors1, bearingVectors2,
		camCorrespondences1, camCorrespondences2, gt );

	std::cout << "*************************************************" << std::endl;
	std::cout << "		Ground Truth" << std::endl;
	std::cout << "*************************************************" << std::endl;

	//print the experiment characteristics
	printExperimentCharacteristics(
		position2 / position2.norm(), rotation2, noise, outlierFraction );

	/*********************************
	 * 		Compute Relative Pose	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Computing relative Pose between View 0 and View 1" << std::endl;
	std::cout << "*************************************************" << std::endl;

	relative_pose::CentralRelativeAdapter rel_adapter(
		bearingVectors1,
		bearingVectors2);

	sac::Ransac<
	sac_problems::relative_pose::CentralRelativePoseSacProblem> rel_ransac;
	std::shared_ptr<
	sac_problems::relative_pose::CentralRelativePoseSacProblem> relposeproblem_ptr(
		new sac_problems::relative_pose::CentralRelativePoseSacProblem(
			rel_adapter,
			sac_problems::relative_pose::CentralRelativePoseSacProblem::NISTER));
	rel_ransac.sac_model_ = relposeproblem_ptr;
	rel_ransac.threshold_ = 2.*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	rel_ransac.max_iterations_ = 1000;

	//Run the experiment
	struct timeval tic;
	struct timeval toc;
	gettimeofday( &tic, 0 );
	rel_ransac.computeModel();
	gettimeofday( &toc, 0 );
	double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print results for ransac 1
	std::cout << "the ransac threshold is: " << rel_ransac.threshold_ << std::endl;
	std::cout << "the normalized translation result is: " << std::endl;
	std::cout << rel_ransac.model_coefficients_.col(3)/
	rel_ransac.model_coefficients_.col(3).norm() << std::endl << std::endl;
	std::cout << "Ransac needed " << rel_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << rel_ransac.inliers_.size() << " out of " << numberPoints;;
	std::cout << std::endl << std::endl;

	translation_t t = rel_ransac.model_coefficients_.col(3) / rel_ransac.model_coefficients_.col(3).norm();
	rotation_t R = rel_ransac.model_coefficients_.block<3,3>(0,0);

	bearingVectors1.clear();
	bearingVectors2.clear();

	for(size_t i = 0 ; i < rel_ransac.inliers_.size() ; ++i)
	{
		bearingVectors1.push_back(rel_adapter.getBearingVector1(rel_ransac.inliers_[i]));
		bearingVectors2.push_back(rel_adapter.getBearingVector2(rel_ransac.inliers_[i]));
	}

	relative_pose::CentralRelativeAdapter adapter(
		bearingVectors1,
		bearingVectors2,
		rel_adapter.gett12() / rel_adapter.gett12().norm(),
		rel_adapter.getR12());

	/*********************************
	 * 		Triangulate Points		 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Triangulating Points between View 0 and View 1" << std::endl;
	std::cout << "*************************************************" << std::endl;

	std::cout << "running triangulation algorithm 2" << std::endl << std::endl;
	MatrixXd triangulate2_results(3,(int) rel_ransac.inliers_.size());
	gettimeofday( &tic, 0 );

	for(size_t j = 0; j < rel_ransac.inliers_.size(); j++)
	{
		triangulate2_results.block<3,1>(0,j) = triangulation::triangulate2(adapter,j);
	}

	gettimeofday( &toc, 0 );

	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	/*
	std::cout << "triangulation truth: " << std::endl;
	std::cout << gt << std::endl;
	*/

	std::cout << "triangulation result: " << std::endl;
	std::cout << triangulate2_results.col(0) << std::endl << std::endl;

	std::cout << " in " << ransac_time << " seconds" << std::endl << std::endl;

	/*
	MatrixXd error(1,numberPoints);
	for(size_t i = 0; i < numberPoints; i++)
	{
		Vector3d singleError = triangulate2_results.col(i) - gt.col(i);
		error(0,i) = singleError.norm();
	}
	std::cout << "triangulation error is: " << std::endl << error << std::endl;
	*/

	/*********************************
	 * 		Compute Absolute Pose	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Computing absolute Pose of View 1 from 3D points" << std::endl;
	std::cout << "*************************************************" << std::endl;

	points_t points;

	for(size_t i = 0 ; i < rel_ransac.inliers_.size() ; ++i)
	{
		//store the point
		points.push_back(triangulate2_results.col(i));
	}

	//create a central absolute adapter
	absolute_pose::CentralAbsoluteAdapter abs_adapter(
		bearingVectors2,
		points);

	transformation_t optimized_pose;

	//Create an AbsolutePoseSac problem and Ransac
	//The method can be set to KNEIP, GAO or EPNP
	sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> abs_ransac;
	std::shared_ptr<
	sac_problems::absolute_pose::AbsolutePoseSacProblem> abspose_kneip_ptr(
		new sac_problems::absolute_pose::AbsolutePoseSacProblem(
			abs_adapter,
			sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
	abs_ransac.sac_model_ = abspose_kneip_ptr;
	abs_ransac.threshold_ = 1.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	abs_ransac.max_iterations_ = 1000;

	//Run the experiment
	gettimeofday( &tic, 0 );
	abs_ransac.computeModel();
	gettimeofday( &toc, 0 );
	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print the results
	std::cout << "KNEIP: " << std::endl;
	std::cout << "the ransac results is: " << std::endl;
	std::cout << abs_ransac.model_coefficients_ << std::endl << std::endl;
	std::cout << "Ransac needed " << abs_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << abs_ransac.inliers_.size() << " out of " << rel_ransac.inliers_.size();
	std::cout << std::endl << std::endl;

	abs_ransac.sac_model_->optimizeModelCoefficients(abs_ransac.inliers_, abs_ransac.model_coefficients_, optimized_pose);

	std::cout << "the sac optimized ransac results is: " << std::endl;
	std::cout <<  optimized_pose << std::endl << std::endl;

	t = abs_ransac.model_coefficients_.col(3);
	R = abs_ransac.model_coefficients_.block<3,3>(0,0);

	abs_adapter.sett(t);
	abs_adapter.setR(R);

	transformation_t nonlinear_transformation =
	absolute_pose::optimize_nonlinear(abs_adapter);

	std::cout << "results from nonlinear algorithm with every points:" << std::endl;
	std::cout << nonlinear_transformation << std::endl << std::endl;

	std::cout << "Truth is: " << std::endl  << std::endl;
	std::cout << position2 / position2.norm() << std::endl << std::endl;
	std::cout << rotation2 << std::endl << std::endl;

	/*********************************
	 * 		Simulation of view 3	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "		Simulating a 3rd View " << std::endl;
	std::cout << "*************************************************" << std::endl;

	//generate a random pose for viewpoint 3
	translation_t position3 = position2 + generateRandomTranslation(5.0);
	rotation_t rotation3 = generateRandomRotation(0.25);

	//derive correspondences based on random point-cloud
	bearingVectors_t bearingVectors3;
	std::vector<int> camCorrespondences22; //unused in the central case
	std::vector<int> camCorrespondences33; //unused in the central case

	for( size_t i = 0; i < points.size(); i++ )
	{
		//get the point in viewpoint 3
		point_t bodyPoint3 = rotation3.transpose()*(points[i] - position3);

		//get the point in the camera in viewpoint 2
		bearingVectors3.push_back(bodyPoint3);

		//normalize the bearing-vectors
		bearingVectors3[i] = bearingVectors3[i] / bearingVectors3[i].norm();

		//add noise
		bearingVectors3[i] = addNoise(2.5,bearingVectors3[i]);
	}

	//print the experiment characteristics
	printExperimentCharacteristics(
		position3, rotation3, noise, outlierFraction );

	//create a central absolute adapter
	absolute_pose::CentralAbsoluteAdapter next_abs_adapter(
		bearingVectors3,
		points);

	//Create an AbsolutePoseSac problem and Ransac
	//The method can be set to KNEIP, GAO or EPNP
	sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> next_abs_ransac;
	std::shared_ptr<
	sac_problems::absolute_pose::AbsolutePoseSacProblem> next_abspose_kneip_ptr(
		new sac_problems::absolute_pose::AbsolutePoseSacProblem(
			next_abs_adapter,
			sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
	next_abs_ransac.sac_model_ = next_abspose_kneip_ptr;
	next_abs_ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	next_abs_ransac.max_iterations_ = 1000;

	//Run the experiment
	gettimeofday( &tic, 0 );
	next_abs_ransac.computeModel();
	gettimeofday( &toc, 0 );
	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print the results
	std::cout << "KNEIP: " << std::endl;
	std::cout << "the ransac results is: " << std::endl;
	std::cout << next_abs_ransac.model_coefficients_ << std::endl << std::endl;
	std::cout << "Ransac needed " << next_abs_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << next_abs_ransac.inliers_.size() << " out of " << rel_ransac.inliers_.size();
	std::cout << std::endl << std::endl;

	next_abs_ransac.sac_model_->optimizeModelCoefficients(next_abs_ransac.inliers_, next_abs_ransac.model_coefficients_, optimized_pose);

	std::cout << "the sac optimized ransac results is: " << std::endl;
	std::cout <<  optimized_pose << std::endl << std::endl;

	t = next_abs_ransac.model_coefficients_.col(3);
	R = next_abs_ransac.model_coefficients_.block<3,3>(0,0);

	next_abs_adapter.sett(t);
	next_abs_adapter.setR(R);

	nonlinear_transformation =
	absolute_pose::optimize_nonlinear(next_abs_adapter);

	std::cout << "results from nonlinear algorithm with every points:" << std::endl;
	std::cout << nonlinear_transformation << std::endl << std::endl;

	std::cout << "Truth is: " << std::endl  << std::endl;
	std::cout << position3 << std::endl << std::endl;
	std::cout << rotation3 << std::endl << std::endl;

}


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <string>
#include <utility>
#include <Eigen/Core>
#include <limits>
#include <cassert>
#include "metro/regression/LogLikelihood.hpp"
#include "metro/ModifiedCholesky.hpp"
#include "metro/ModifiedNewtonRaphson.hpp"
#include "metro/CholeskyStepper.hpp"

#define DEBUG 1

namespace metro {
	CholeskyStepper::CholeskyStepper( double tolerance, int max_iterations, Tracer tracer ):
		m_tolerance( tolerance ),
		m_max_iterations( max_iterations ),
		m_tracer( tracer ),
		m_iteration( -1 ),
		m_target_ll( -std::numeric_limits< double >::infinity() )
	{
		assert( tolerance > 0 ) ;
	}

	bool CholeskyStepper::step( Function& function, Vector* result ) {
		++m_iteration ;
		//std::cerr << "CholeskyStepper::step(): calling evaluate()...\n" ;
		function.evaluate( 2 ) ;
		double const ll = function.get_value_of_function() ;
		m_solver.compute( -function.get_value_of_second_derivative() ) ;

		// Different possible stopping conditions are possible.
		// One is to stop when the predicted improvement in function
		// value is small.  This amount is encoded by the directional derivative.
		// However, for statistical model fitting we would like a rule that is invariant to
		// various types of rescaling the function.  For example
		// - scaling parameters (which scales the derivative)
		// - multiplying the function by a constant (which is a bit like adding more data)
		// A rule like directional_derivative < tolerance does not have these properties.

		// Instead, we compute the derivative normalised to 'standard Gaussian' space.
		// Theory: if the function 2nd derivative H = LLᵗ then Σ=(Lᵗ)⁻¹L⁻¹.
		// We assume the log-likelihood looks like this:
		// f(x) = s(Lᵗ(x-x₀)) + O((x-x₀)³)
		// near the true maximum x₀, where s is the standard multivariate normal
		// log-density.  Then
		// f'(x) = -L Lᵗ (x-x₀) + O((x-x₀)²)   (expressed as a column vector).
		// We put a convergence condition on the corresponding derivative in s-space, given by
		// z = L⁻¹ f'(x) = -Lᵗ(x-x₀) + O((x-x₀)²)
		// This has the nice interpretation of being interpretable as a distance
		// to the maximum in the uncorrelated-variable space that is the domain of s.

		double const derivativeOneNorm = function.get_value_of_first_derivative().array().abs().maxCoeff() ;
		Vector step = m_solver.solve( function.get_value_of_first_derivative() ) ;
		//Vector const sqrtStep = m_solver.halfSolve( function.get_value_of_first_derivative() ) ;
		double directional_derivative = function.get_value_of_first_derivative().transpose() * step ;

		bool converged = (
			(derivativeOneNorm < m_tolerance)
			&& ( ll >= ( m_target_ll - m_tolerance ) )
			//&& (sqrtStep.array().abs().maxCoeff() < m_tolerance )
			&& (directional_derivative < m_tolerance )
		) ;

		if( m_tracer ) {
			m_tracer( m_iteration, ll, m_target_ll, function.parameters(), function.get_value_of_first_derivative(), step, converged ) ;
		}

		if(
			converged || (m_iteration) >= m_max_iterations
		) {
			return false ;
		} else {
			(*result) = step ;
			m_target_ll = std::max( m_target_ll, ll ) ;
			return true ;
		}
	}

	bool CholeskyStepper::diverged() const {
		return m_iteration >= m_max_iterations || m_target_ll != m_target_ll ;
	}

	std::size_t CholeskyStepper::number_of_iterations() const {
		return m_iteration ;
	}

	void CholeskyStepper::reset() {
		m_iteration = -1 ;
		m_target_ll = -std::numeric_limits< double >::infinity() ;
	}
}


#ifndef CY_NNQS_JASTROW_HPP
#define CY_NNQS_JASTROW_HPP
#include <random>
#include <Eigen/Dense>
#include "Utilities/type_traits.hpp"
#include <nlohmann/json.hpp>
namespace yannq
{
template<typename T>
class Jastrow
{
public:
	using Scalar=T;
	typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	typedef Eigen::Matrix<T, Eigen::Dynamic, 1>  Vector;

private:
	int n_;

	Vector a_;
	Matrix J_;

public:

	nlohmann::json desc() const
	{
		return nlohmann::json
		{
			{"name", "Jastrow"},
			{"n", n_},
		};
	}

	inline int getN() const
	{
		return n_;
	}

	Jastrow(int n)
		: n_(n), a_(n_), J_(n,n)
	{
	}

	void resize(int n)
	{
		n_ = n;
		a_.resize(n);
		J_.resize(n,n);
	}

	int getDim() const
	{
		return n_ + n_*(n_-1)/2;
	}

	Vector getA() const
	{
		return a_;
	}
	Matrix getJ() const
	{
		return J_;
	}

	void setA(const Vector& a) const
	{
		a_ = a;
	}

	void setJ(const Matrix& J) const
	{
		J_ = J;
	}

	T A(int i) const
	{
		return a_(i);
	}
	T J(int i, int j) const
	{
		return J_(i,j);
	}

	template <typename RandomEngine, class U=T,
               typename std::enable_if < !is_complex_type<U>::value, int >::type = 0 >
	void initializeRandom(RandomEngine& re, T weight = 0.001)
	{
		std::normal_distribution<double> nd{};

		for(int i = 0; i < n_; i++)
		{
			a_(i) = weight*nd(re);
		}
		J_.setZero();
		for(int j = n_-1; j >=0; --j)
		{
			for(int i = 0; i < j; i++)
			{
				J_(i, j) = weight*nd(re);
			}
		}
	}
	template <typename RandomEngine, class U=T,
               typename std::enable_if < is_complex_type<U>::value, int >::type = 0 >
	void initializeRandom(RandomEngine& re, T weight = 0.001)
	{
		std::normal_distribution<double> nd{};

		for(int i = 0; i < n_; i++)
		{
			a_(i) = weight*T(nd(re),nd(re));
		}
		J_.setZero();
		for(int j = n_-1; j >=0; --j)
		{
			for(int i = 0; i < j; i++)
			{
				J_(i, j) = weight*T(nd(re),nd(re));
			}
		}
	}

	T calcTheta(const Eigen::VectorXi& sigma) const
	{
		Vector s = sigma.cast<T>();
		T res = a_.transpose()*s;
		res += T(s.transpose()*J_*s);
		return res;
	}

	std::tuple<Eigen::VectorXi, T> makeData(const Eigen::VectorXi& sigma) const
	{
		return std::make_tuple(sigma, calcTheta(sigma));
	}

	Vector logDeriv(const std::tuple<Eigen::VectorXi, T>& t) const
	{
		Vector res(getDim());
		for(int i = 0; i < n_; i++)
		{
			res(i) = std::get<0>(t)(i);
		}
		for(int i = 0; i < n_; i++)
		{
			for(int j = i+1; i < n_; j++)
			{
				res(i*n_+j+n_) = std::get<0>(t)(i)*std::get<0>(t)(j);
			}
		}
		return res;
	}

	void updateParams(const Vector& u)
	{
		a_ += u.segment(0, n_);
		for(int i = 0; i < n_; i++)
		{
			for(int j = i+1; i < n_; j++)
			{
				J_(i, j) += u(i*n_ + j + n_);
			}
		}
	}

};
}
#endif//CY_NNQS_JASTROW_HPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2013 Paul Bristow
//  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_POLYGAMMA_DETAIL_2013_07_30_HPP_
  #define _BOOST_POLYGAMMA_DETAIL_2013_07_30_HPP_

#include <cmath>
  #include <limits>
  #include <boost/cstdint.hpp>
  #include <boost/math/policies/policy.hpp>
  #include <boost/math/special_functions/bernoulli.hpp>
  #include <boost/math/special_functions/trunc.hpp>
  #include <boost/math/special_functions/zeta.hpp>
  #include <boost/math/special_functions/digamma.hpp>
  #include <boost/math/special_functions/sin_pi.hpp>
  #include <boost/math/special_functions/cos_pi.hpp>
  #include <boost/math/special_functions/pow.hpp>
  #include <boost/mpl/if.hpp>
  #include <boost/mpl/int.hpp>
  #include <boost/static_assert.hpp>
  #include <boost/type_traits/is_convertible.hpp>

#ifdef _MSC_VER
#pragma once
#pragma warning(push)
#pragma warning(disable:4702) // Unreachable code (release mode only warning)
#endif

namespace boost { namespace math { namespace detail{

  template<class T, class Policy>
  T polygamma_atinfinityplus(const int n, const T& x, const Policy& pol, const char* function) // for large values of x such as for x> 400
  {
     // See http://functions.wolfram.com/GammaBetaErf/PolyGamma2/06/02/0001/
     BOOST_MATH_STD_USING
     //
     // sum       == current value of accumulated sum.
     // term      == value of current term to be added to sum.
     // part_term == value of current term excluding the Bernoulli number part
     //
     if(n + x == x)
     {
        // x is crazy large, just concentrate on the first part of the expression and use logs:
        if(n == 1) return 1 / x;
        T nlx = n * log(x);
        if((nlx < tools::log_max_value<T>()) && (n < (int)max_factorial<T>::value))
           return ((n & 1) ? 1 : -1) * boost::math::factorial<T>(n - 1) * pow(x, -n);
        else
         return ((n & 1) ? 1 : -1) * exp(boost::math::lgamma(T(n), pol) - n * log(x));
     }
     T term, sum, part_term;
     T x_squared = x * x;
     //
     // Start by setting part_term to:
     //
     // (n-1)! / x^(n+1)
     //
     // which is common to both the first term of the series (with k = 1)
     // and to the leading part.
     // We can then get to the leading term by:
     //
     // part_term * (n + 2 * x) / 2
     //
     // and to the first term in the series
     // (excluding the Bernoulli number) by:
     //
     // part_term n * (n + 1) / (2x)
     //
     // If either the factorial would overflow,
     // or the power term underflows, this just gets set to 0 and then we
     // know that we have to use logs for the initial terms:
     //
     part_term = ((n > (int)boost::math::max_factorial<T>::value) && (T(n) * n > tools::log_max_value<T>()))
        ? T(0) : static_cast<T>(boost::math::factorial<T>(n - 1, pol) * pow(x, -n - 1));
     if(part_term == 0)
     {
        // Either n is very large, or the power term underflows,
        // set the initial values of part_term, term and sum via logs:
        part_term = static_cast<T>(boost::math::lgamma(n, pol) - (n + 1) * log(x));
        sum = exp(part_term + log(n + 2 * x) - boost::math::constants::ln_two<T>());
        part_term += log(T(n) * (n + 1)) - boost::math::constants::ln_two<T>() - log(x);
        part_term = exp(part_term);
     }
     else
     {
        sum = part_term * (n + 2 * x) / 2;
        part_term *= (T(n) * (n + 1)) / 2;
        part_term /= x;
     }
     //
     // If the leading term is 0, so is the result:
     //
     if(sum == 0)
        return sum;

     for(unsigned k = 1;;)
     {
        term = part_term * boost::math::bernoulli_b2n<T>(k, pol);
        sum += term;
        //
        // Normal termination condition:
        //
        if(fabs(term / sum) < tools::epsilon<T>())
           break;
        //
        // Increment our counter, and move part_term on to the next value:
        //
        ++k;
        part_term *= T(n + 2 * k - 2) * (n - 1 + 2 * k);
        part_term /= (2 * k - 1) * 2 * k;
        part_term /= x_squared;
        //
        // Emergency get out termination condition:
        //
        if(k > policies::get_max_series_iterations<Policy>())
        {
           return policies::raise_evaluation_error(function, "Series did not converge, closest value was %1%", sum, pol);
        }
     }

     if((n - 1) & 1)
        sum = -sum;

     return sum;
  }

  template<class T, class Policy>
  T polygamma_attransitionplus(const int n, const T& x, const Policy& pol, const char* function)
  {
    // See: http://functions.wolfram.com/GammaBetaErf/PolyGamma2/16/01/01/0017/

    // Use N = (0.4 * digits) + (4 * n) for target value for x:
    BOOST_MATH_STD_USING
    const int d4d  = static_cast<int>(0.4F * policies::digits_base10<T, Policy>());
    const int N = d4d + (4 * n);
    const int m    = n;
    const int iter = N - itrunc(x);

    if(iter > (int)policies::get_max_series_iterations<Policy>())
       return policies::raise_evaluation_error<T>(function, ("Exceeded maximum series evaluations evaluating at n = " + boost::lexical_cast<std::string>(n) + " and x = %1%").c_str(), x, pol);

    const int minus_m_minus_one = -m - 1;

    T z(x);
    T sum0(0);
    T z_plus_k_pow_minus_m_minus_one(0);

    // Forward recursion to larger x, need to check for overflow first though:
    if(log(z + iter) * minus_m_minus_one > -tools::log_max_value<T>())
    {
       for(int k = 1; k <= iter; ++k)
       {
          z_plus_k_pow_minus_m_minus_one = pow(z, minus_m_minus_one);
          sum0 += z_plus_k_pow_minus_m_minus_one;
          z += 1;
       }
       sum0 *= boost::math::factorial<T>(n);
    }
    else
    {
       for(int k = 1; k <= iter; ++k)
       {
          T log_term = log(z) * minus_m_minus_one + boost::math::lgamma(T(n + 1), pol);
          sum0 += exp(log_term);
          z += 1;
       }
    }
    if((n - 1) & 1)
       sum0 = -sum0;

    return sum0 + polygamma_atinfinityplus(n, z, pol, function);
  }

  template <class T, class Policy>
  T polygamma_nearzero(int n, T x, const Policy& pol, const char* function)
  {
     BOOST_MATH_STD_USING
     //
     // If we take this expansion for polygamma: http://functions.wolfram.com/06.15.06.0003.02
     // and substitute in this expression for polygamma(n, 1): http://functions.wolfram.com/06.15.03.0009.01
     // we get an alternating series for polygamma when x is small in terms of zeta functions of
     // integer arguments (which are easy to evaluate, at least when the integer is even).
     //
     // In order to avoid spurious overflow, save the n! term for later, and rescale at the end:
     //
     T scale = boost::math::factorial<T>(n, pol);
     //
     // "factorial_part" contains everything except the zeta function
     // evaluations in each term:
     //
     T factorial_part = 1;
     //
     // "prefix" is what we'll be adding the accumulated sum to, it will
     // be n! / z^(n+1), but since we're scaling by n! it's just
     // 1 / z^(n+1) for now:
     //
     T prefix = pow(x, n + 1);
     if(prefix == 0)
        return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     prefix = 1 / prefix;
     //
     // First term in the series is necessarily < zeta(2) < 2, so
     // ignore the sum if it will have no effect on the result anyway:
     //
     if(prefix > 2 / policies::get_epsilon<T, Policy>())
        return ((n & 1) ? 1 : -1) *
         (tools::max_value<T>() / prefix < scale ? policies::raise_overflow_error<T>(function, 0, pol) : prefix * scale);
     //
     // As this is an alternating series we could accelerate it using
     // "Convergence Acceleration of Alternating Series",
     // Henri Cohen, Fernando Rodriguez Villegas, and Don Zagier, Experimental Mathematics, 1999.
     // In practice however, it appears not to make any difference to the number of terms
     // required except in some edge cases which are filtered out anyway before we get here.
     //
     T sum = prefix;
     for(unsigned k = 0;;)
     {
        // Get the k'th term:
        T term = factorial_part * boost::math::zeta(T(k + n + 1), pol);
        sum += term;
        // Termination condition:
        if(fabs(term) < fabs(sum * boost::math::policies::get_epsilon<T, Policy>()))
           break;
        //
        // Move on k and factorial_part:
        //
        ++k;
        factorial_part *= (-x * (n + k)) / k;
        //
        // Last chance exit:
        //
        if(k > policies::get_max_series_iterations<Policy>())
           return policies::raise_evaluation_error<T>(function, "Series did not converge, best value is %1%", sum, pol);
     }
     //
     // We need to multiply by the scale, at each stage checking for oveflow:
     //
     if(boost::math::tools::max_value<T>() / scale < sum)
        return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     sum *= scale;
     return n & 1 ? sum : T(-sum);
  }

  //
  // Helper function which figures out which slot our coefficient is in
  // given an angle multiplier for the cosine term of power:
  //
  template <class Table>
  typename Table::value_type::reference dereference_table(Table& table, unsigned row, unsigned power)
  {
     return table[row][power / 2];
  }

  template <class T, class Policy>
  T poly_cot_pi(int n, T x, T xc, const Policy& pol, const char* function)
  {
     BOOST_MATH_STD_USING
     // Return n'th derivative of cot(pi*x) at x, these are simply
     // tabulated for up to n = 9, beyond that it is possible to
     // calculate coefficients as follows:
     //
     // The general form of each derivative is:
     //
     // pi^n * SUM{k=0, n} C[k,n] * cos^k(pi * x) * csc^(n+1)(pi * x)
     //
     // With constant C[0,1] = -1 and all other C[k,n] = 0;
     // Then for each k < n+1:
     // C[k-1, n+1]  -= k * C[k, n];
     // C[k+1, n+1]  += (k-n-1) * C[k, n];
     //
     // Note that there are many different ways of representing this derivative thanks to
     // the many trigomonetric identies available.  In particular, the sum of powers of
     // cosines could be replaced by a sum of cosine multiple angles, and indeed if you
     // plug the derivative into Mathematica this is the form it will give.  The two
     // forms are related via the Chebeshev polynomials of the first kind and
     // T_n(cos(x)) = cos(n x).  The polynomial form has the great advantage that
     // all the cosine terms are zero at half integer arguments - right where this
     // function has it's minumum - thus avoiding cancellation error in this region.
     //
     // And finally, since every other term in the polynomials is zero, we can save
     // space by only storing the non-zero terms.  This greatly complexifies
     // subscripting the tables in the calculation, but halves the storage space
     // (and complexity for that matter).
     //
     T s = fabs(x) < fabs(xc) ? boost::math::sin_pi(x, pol) : boost::math::sin_pi(xc, pol);
     T c = boost::math::cos_pi(x, pol);
     switch(n)
     {
     case 1:
        return -constants::pi<T, Policy>() / (s * s);
     case 2:
     {
        return 2 * constants::pi<T, Policy>() * constants::pi<T, Policy>() * c / boost::math::pow<3>(s, pol);
     }
     case 3:
     {
        int P[] = { -2, -4 };
        return boost::math::pow<3>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<4>(s, pol);
     }
     case 4:
     {
        int P[] = { 16, 8 };
        return boost::math::pow<4>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<5>(s, pol);
     }
     case 5:
     {
        int P[] = { -16, -88, -16 };
        return boost::math::pow<5>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<6>(s, pol);
     }
     case 6:
     {
        int P[] = { 272, 416, 32 };
        return boost::math::pow<6>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<7>(s, pol);
     }
     case 7:
     {
        int P[] = { -272, -2880, -1824, -64 };
        return boost::math::pow<7>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<8>(s, pol);
     }
     case 8:
     {
        int P[] = { 7936, 24576, 7680, 128 };
        return boost::math::pow<8>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<9>(s, pol);
     }
     case 9:
     {
        int P[] = { -7936, -137216, -185856, -31616, -256 };
        return boost::math::pow<9>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<10>(s, pol);
     }
     case 10:
     {
        int P[] = { 353792, 1841152, 1304832, 128512, 512 };
        return boost::math::pow<10>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<11>(s, pol);
     }
     case 11:
     {
        int P[] = { -353792, -9061376, -21253376, -8728576, -518656, -1024};
        return boost::math::pow<11>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<12>(s, pol);
     }
     case 12:
     {
        int P[] = { 22368256, 175627264, 222398464, 56520704, 2084864, 2048 };
        return boost::math::pow<12>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<13>(s, pol);
     }
#ifndef BOOST_NO_LONG_LONG
     case 13:
     {
        long long P[] = { -22368256LL, -795300864LL, -2868264960LL, -2174832640LL, -357888000LL, -8361984LL, -4096 };
        return boost::math::pow<13>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<14>(s, pol);
     }
     case 14:
     {
        long long P[] = { 1903757312LL, 21016670208LL, 41731645440LL, 20261765120LL, 2230947840LL, 33497088LL, 8192 };
        return boost::math::pow<14>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<15>(s, pol);
     }
     case 15:
     {
        long long P[] = { -1903757312LL, -89702612992LL, -460858269696LL, -559148810240LL, -182172651520LL, -13754155008LL, -134094848LL, -16384 };
        return boost::math::pow<15>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<16>(s, pol);
     }
     case 16:
     {
        long long P[] = { 209865342976LL, 3099269660672LL, 8885192097792LL, 7048869314560LL, 1594922762240LL, 84134068224LL, 536608768LL, 32768 };
        return boost::math::pow<16>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<17>(s, pol);
     }
     case 17:
     {
        long long P[] = { -209865342976LL, -12655654469632LL, -87815735738368LL, -155964390375424LL, -84842998005760LL, -13684856848384LL, -511780323328LL, -2146926592LL, -65536 };
        return boost::math::pow<17>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<18>(s, pol);
     }
     case 18:
     {
        long long P[] = { 29088885112832LL, 553753414467584LL, 2165206642589696LL, 2550316668551168LL, 985278548541440LL, 115620218667008LL, 3100738912256LL, 8588754944LL, 131072 };
        return boost::math::pow<18>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<19>(s, pol);
     }
     case 19:
     {
        long long P[] = { -29088885112832LL, -2184860175433728LL, -19686087844429824LL, -48165109676113920LL, -39471306959486976LL, -11124607890751488LL, -965271355195392LL, -18733264797696LL, -34357248000LL, -262144 };
        return boost::math::pow<19>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<20>(s, pol);
     }
     case 20:
     {
        long long P[] = { 4951498053124096LL, 118071834535526400LL, 603968063567560704LL, 990081991141490688LL, 584901762421358592LL, 122829335169859584LL, 7984436548730880LL, 112949304754176LL, 137433710592LL, 524288 };
        return boost::math::pow<20>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<21>(s, pol);
     }
#endif
     }

     //
     // We'll have to compute the coefficients up to n,
     // complexity is O(n^2) which we don't worry about for now
     // as the values are computed once and then cached.
     // However, if the final evaluation would have too many
     // terms just bail out right away:
     //
     if((unsigned)n / 2u > policies::get_max_series_iterations<Policy>())
        return policies::raise_evaluation_error<T>(function, "The value of n is so large that we're unable to compute the result in reasonable time, best guess is %1%", 0, pol);
#ifdef BOOST_HAS_THREADS
     static boost::detail::lightweight_mutex m;
     boost::detail::lightweight_mutex::scoped_lock l(m);
#endif
     static int digits = tools::digits<T>();
     static std::vector<std::vector<T> > table(1, std::vector<T>(1, T(-1)));

     int current_digits = tools::digits<T>();

     if(digits != current_digits)
     {
        // Oh my... our precision has changed!
        table = std::vector<std::vector<T> >(1, std::vector<T>(1, T(-1)));
        digits = current_digits;
     }

     int index = n - 1;

     if(index >= (int)table.size())
     {
        for(int i = (int)table.size() - 1; i < index; ++i)
        {
           int offset = i & 1; // 1 if the first cos power is 0, otherwise 0.
           int sin_order = i + 2;  // order of the sin term
           int max_cos_order = sin_order - 1;  // largest order of the polynomial of cos terms
           int max_columns = (max_cos_order - offset) / 2;  // How many entries there are in the current row.
           int next_offset = offset ? 0 : 1;
           int next_max_columns = (max_cos_order + 1 - next_offset) / 2;  // How many entries there will be in the next row
           table.push_back(std::vector<T>(next_max_columns + 1, T(0)));

           for(int column = 0; column <= max_columns; ++column)
           {
              int cos_order = 2 * column + offset;  // order of the cosine term in entry "column"
              BOOST_ASSERT(column < (int)table[i].size());
              BOOST_ASSERT((cos_order + 1) / 2 < (int)table[i + 1].size());
              table[i + 1][(cos_order + 1) / 2] += ((cos_order - sin_order) * table[i][column]) / (sin_order - 1);
              if(cos_order)
                table[i + 1][(cos_order - 1) / 2] += (-cos_order * table[i][column]) / (sin_order - 1);
           }
        }

     }
     T sum = boost::math::tools::evaluate_even_polynomial(&table[index][0], c, table[index].size());
     if(index & 1)
        sum *= c;  // First coeffient is order 1, and really an odd polynomial.
     if(sum == 0)
        return sum;
     //
     // The remaining terms are computed using logs since the powers and factorials
     // get real large real quick:
     //
     T power_terms = n * log(boost::math::constants::pi<T>());
     if(s == 0)
        return sum * boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     power_terms -= log(fabs(s)) * (n + 1);
     power_terms += boost::math::lgamma(T(n));
     power_terms += log(fabs(sum));

     if(power_terms > boost::math::tools::log_max_value<T>())
        return sum * boost::math::policies::raise_overflow_error<T>(function, 0, pol);

     return exp(power_terms) * ((s < 0) && ((n + 1) & 1) ? -1 : 1) * boost::math::sign(sum);
  }

  template <class T, class Policy>
  struct polygamma_initializer
  {
     struct init
     {
        init()
        {
           // Forces initialization of our table of coefficients and mutex:
           boost::math::polygamma(30, T(-2.5f), Policy());
        }
        void force_instantiate()const{}
     };
     static const init initializer;
     static void force_instantiate()
     {
        initializer.force_instantiate();
     }
  };

  template <class T, class Policy>
  const typename polygamma_initializer<T, Policy>::init polygamma_initializer<T, Policy>::initializer;

  template<class T, class Policy>
  inline T polygamma_imp(const int n, T x, const Policy &pol)
  {
    BOOST_MATH_STD_USING
    static const char* function = "boost::math::polygamma<%1%>(int, %1%)";
    polygamma_initializer<T, Policy>::initializer.force_instantiate();
    if(n < 0)
       return policies::raise_domain_error<T>(function, "Order must be >= 0, but got %1%", static_cast<T>(n), pol);
    if(x < 0)
    {
       if(floor(x) == x)
       {
          //
          // Result is infinity if x is odd, and a pole error if x is even.
          //
          if(lltrunc(x) & 1)
             return policies::raise_overflow_error<T>(function, 0, pol);
          else
             return policies::raise_pole_error<T>(function, "Evaluation at negative integer %1%", x, pol);
       }
       T z = 1 - x;
       T result = polygamma_imp(n, z, pol) + constants::pi<T, Policy>() * poly_cot_pi(n, z, x, pol, function);
       return n & 1 ? T(-result) : result;
    }
    //
    // Limit for use of small-x-series is chosen
    // so that the series doesn't go too divergent
    // in the first few terms.  Ordinarily this
    // would mean setting the limit to ~ 1 / n,
    // but we can tolerate a small amount of divergence:
    //
    T small_x_limit = (std::min)(T(T(5) / n), T(0.25f));
    if(x < small_x_limit)
    {
      return polygamma_nearzero(n, x, pol, function);
    }
    else if(x > 0.4F * policies::digits_base10<T, Policy>() + 4.0f * n)
    {
      return polygamma_atinfinityplus(n, x, pol, function);
    }
    else if(x == 1)
    {
       return (n & 1 ? 1 : -1) * boost::math::factorial<T>(n, pol) * boost::math::zeta(T(n + 1), pol);
    }
    else if(x == 0.5f)
    {
       T result = (n & 1 ? 1 : -1) * boost::math::factorial<T>(n, pol) * boost::math::zeta(T(n + 1), pol);
       if(fabs(result) >= ldexp(tools::max_value<T>(), -n - 1))
          return boost::math::sign(result) * policies::raise_overflow_error<T>(function, 0, pol);
       result *= ldexp(T(1), n + 1) - 1;
       return result;
    }
    else
    {
      return polygamma_attransitionplus(n, x, pol, function);
    }
  }

} } } // namespace boost::math::detail

#ifdef _MSC_VER
#pragma warning(pop)
#endif

#endif // _BOOST_POLYGAMMA_DETAIL_2013_07_30_HPP_


#include <string>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <cassert>
#include <vector>
#include <map>
#include <algorithm>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>

#include <rstbx/indexing_api/indexing_api.h>
#include <rstbx/dps_core/direction.h>
#include <cctbx/math/mod.h>

namespace af = scitbx::af;
namespace constants = scitbx::constants;
using rstbx::Direction;
using rstbx::Directional_FFT;
using rstbx::kvalcmp;

af::shared<int>
rstbx::indexing_api::cpp_absence_test(af::shared<cctbx::miller::index<> > hkl,
                     const int& mod, cctbx::miller::index<> vecrep){
  af::shared<int> cum;
  for (std::size_t i=0; i<mod; ++i) {cum.push_back(0);}

  typedef af::shared<cctbx::miller::index<> >::const_iterator For;
  For b = hkl.begin();
  For e = hkl.end();
  for (;b!=e;++b){
    int pattern_sum = (*b)[0]*vecrep[0] + (*b)[1]*vecrep[1] + (*b)[2]*vecrep[2];
    cum[cctbx::math::mod_positive(pattern_sum, mod)]+=1;
  }

  return cum;
}

rstbx::indexing_api::dps_extended::dps_extended():rstbx::dps_core(){}

void
rstbx::indexing_api::dps_extended::setData(const pointlist& raw){
  rawdata=pointlistmm();
  rawdata.reserve(raw.size());
  for (std::size_t i = 0; i< raw.size(); ++i) {
      rawdata.push_back(raw[i]);
  }
}

rstbx::Direction
rstbx::indexing_api::dps_extended::refine_direction(const rstbx::Direction& candidate,
                               const double& current_grid,
                               const double& target_grid) const {
  double incr = current_grid/4.0; //recursively smaller grid with neighbor overlap
  if (incr < target_grid) { return candidate; }
  rstbx::Direction direction0(candidate.dvec);
  rstbx::Direction direction1(candidate.psi + incr, candidate.phi);
  scitbx::vec3<double> rotaxisi = direction0.dvec.cross(direction1.dvec).normalize();
  scitbx::vec3<double> rotaxisj = rotaxisi.cross(direction1.dvec).normalize();

  rstbx::Direction best_refined = candidate;
  for (int i = -2; i < 3; ++i) {
    scitbx::vec3<double> rotate1 = direction0.dvec.unit_rotate_around_origin(rotaxisi,-i*incr);
    for (int j = -2; j < 3; ++j) {
      scitbx::vec3<double> rotate2 = rotate1.unit_rotate_around_origin(rotaxisj,-j*incr);
      SCITBX_ASSERT(std::abs(1.0 - rotate2.length())<0.00001); //rotate2 is unit vector
      rstbx::Direction new_candidate(rotate2);
      rstbx::fftptr dfft( fft_factory(new_candidate) );

      if ( dfft->kval() > best_refined.kval ) {
        new_candidate.extract_directional_properties(dfft);
        best_refined=new_candidate;
      }
    }
  }
  return refine_direction(best_refined,incr,target_grid);
}

double
rstbx::indexing_api::dps_extended::high()const{
  //Go through all film coordinates and get high resolution limit
  // This is NOT a safe function if rawdata.size()==0 due to
  // dereferencing s.end().  This bug causes a painful infinite loop
  // in some runtime environments.  See the protecting test in classify_spots().
  // The real fix will be to split this
  // class into components, with the high() function being a member
  // of the class containing rawdata.

  SCITBX_ASSERT (xyzdata.size() > 0);
  af::shared<double> s(xyzdata.size());
  for (std::size_t i = 0; i<xyzdata.size(); ++i) {
    s[i] =         1./xyzdata[i].length();
  }
  double dmax = *(std::max_element<af::shared<double>::const_iterator>(s.begin(),s.end()));
  //dmax is maximum squared distance (mm^2) from the beam center
  return dmax;
}

af::shared< scitbx::vec3<double> >
rstbx::indexing_api::raw_spot_positions_mm_to_reciprocal_space_xyz(
  pointlist raw_spot_input,
  dxtbx::model::Detector const& detector, double const& inverse_wave,
  scitbx::vec3<double> const& S0_vector, scitbx::vec3<double> const& axis, af::shared<int> panelID
){

  // it is not clear how we will plug in to this function if and when
  // we need to use a derived detector class with a different implemented
  // behavior of get_lab_coord().

  af::shared< scitbx::vec3<double> > reciprocal_space_vectors;
  // with a single panel only, all the convenience functions work for us
  for (int n = 0; n != raw_spot_input.size(); ++n) {
    int pid = panelID[n];

    // tile surface to laboratory transformation
    scitbx::vec2<double> raw_spot(raw_spot_input[n][0],raw_spot_input[n][1]);
    scitbx::vec3<double> lab_direct = detector[pid].get_lab_coord(raw_spot);

    // laboratory direct to reciprocal space xyz transformation
    scitbx::vec3<double> lab_recip = (lab_direct.normalize() * inverse_wave) - S0_vector;

      // raw_spot_input[n][2] MUST be given in degrees.
    reciprocal_space_vectors.push_back( lab_recip.rotate_around_origin(
      axis, -raw_spot_input[n][2] * scitbx::constants::pi_180) );
  }
  return reciprocal_space_vectors;

}

af::shared< scitbx::vec3<double> >
rstbx::indexing_api::raw_spot_positions_mm_to_reciprocal_space_xyz(
  pointlist raw_spot_input,
  dxtbx::model::Detector const& detector, double const& inverse_wave,
  scitbx::vec3<double> const& S0_vector, af::shared<int> panelID
){

  // it is not clear how we will plug in to this function if and when
  // we need to use a derived detector class with a different implemented
  // behavior of get_lab_coord().

  af::shared< scitbx::vec3<double> > reciprocal_space_vectors;
  // with a single panel only, all the convenience functions work for us
  for (int n = 0; n != raw_spot_input.size(); ++n) {
    int pid = panelID[n];

    // tile surface to laboratory transformation
    scitbx::vec2<double> raw_spot(raw_spot_input[n][0],raw_spot_input[n][1]);
    scitbx::vec3<double> lab_direct = detector[pid].get_lab_coord(raw_spot);

    // laboratory direct to reciprocal space xyz transformation
    scitbx::vec3<double> lab_recip = (lab_direct.normalize() * inverse_wave) - S0_vector;

      // raw_spot_input[n][2] MUST be given in degrees.
    reciprocal_space_vectors.push_back(lab_recip);
  }
  return reciprocal_space_vectors;

}


#include <stdint.h>
#include <iostream>
#include <math.h>
#include <tgmath.h>
#include <string.h>
#include <fstream>
#include <stdlib.h>
using namespace std;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"

//Un des deux ou aucun des deux
#define DRAWSTARS 1 //Si 1 etoiles sur le fond celeste, sinon rien
#define DRAWGRID 0  //Si 1 grille sur le fond celeste

//Un des deux ou aucun des deux
#define ADISK_NORMAL 1 //Si 1, texture + temperature, sinon rien
#define ADISK_GRID 0   //Si 1, grille, sinon rien

#include "fct.h"
#include "raytracing.h"
#include "par_for.h"

#define CHANNEL_NUM 3

#define MAXITER 500000

static const int Neq = 6; //Nombre de variables pour l'integration numerique

struct Rendu rdr;
struct Scene scn;
struct Blackhole bh;
struct Disk disk;

int adisk_width, adisk_height, adisk_bpp;
uint8_t *adisk = stbi_load("adisk_upscaled.png", &adisk_width, &adisk_height, &adisk_bpp, 3);

static const int SpectrumSampleSize = 76; //nombre d'échantillons
float deltaWaveLength = 4.;               //(En nm, espacement des échantillons)
float wavelengthSamples[SpectrumSampleSize], wavelengthSamples5[SpectrumSampleSize], sensitivitySamplesR[SpectrumSampleSize], sensitivitySamplesG[SpectrumSampleSize], sensitivitySamplesB[SpectrumSampleSize];

void readSensitivityData(const char *filename, float *wavelengthSamples, float *wavelengthSamples5, float *sensitivitySamplesR, float *sensitivitySamplesG, float *sensitivitySamplesB)
{
    int cnt = 0;
    ifstream source;       // build a read-Stream
    source.open(filename); // open data

    for (std::string line; std::getline(source, line);) //read stream line by line
    {
        std::istringstream in(line); //make a stream for the line itself

        in >> wavelengthSamples[cnt] >> sensitivitySamplesR[cnt] >> sensitivitySamplesG[cnt] >> sensitivitySamplesB[cnt]; //now read the whitespace-separated floats

        wavelengthSamples5[cnt] = pow(wavelengthSamples[cnt], 5);
        cnt++;
    }
}

void getBodyColor(float *rgbR, float *rgbG, float *rgbB, float temperature, float brightness)
{
    float I;

    for (int l = 0; l < SpectrumSampleSize; ++l)
    {

        I = (6.e14 / wavelengthSamples5[l]) / exp(1.43913e7 / (wavelengthSamples[l] * temperature) - 1.) * brightness; //6e14 pour renormaliser les composante des pixels

        I *= deltaWaveLength;
        *rgbR += sensitivitySamplesR[l] * I;
        *rgbG += sensitivitySamplesG[l] * I;
        *rgbB += sensitivitySamplesB[l] * I;
    }
}

//Affiche une grille sur le disque
void getDiskColorGrid(float phi, float r, float *rgbR, float *rgbG, float *rgbB)
{
    phi = (phi + M_PI) / (2. * M_PI);

    bool a = int((100 * phi)) % 2; //100 alternances de couleur par tour

    bool b = ((r - disk.R_min) / (disk.R_max - disk.R_min) > .5); //Séparer le disque en 2 radialement

    if (a ^ b) //Ou exclusif
    {
        *rgbR = 255.;
        *rgbG = 0.;
    }
    else
    {
        *rgbR = 0.;
        *rgbG = 255.;
    }
}

//Passage aux coordonnees de Boyer Lindquist
//Pour l'instant juste passage au coordonnées sphériques (plus simple)
//Utilisé uniquement pour trouver la pos initiale de la camera, pas important
void cartesianToBl(float x, float y, float z, float *r, float *theta, float *phi)
{

    float r2 = x * x + y * y + z * z;
    *r = sqrt(r2);

    *phi = atan2(y, x);
    *theta = acos(z / (*r));
}

//Passage des coordonnées de Boyer Lindquist au coordonnées cartésienne (exacte)
void blToCartesian(float r, float theta, float phi, float *x, float *y, float *z)
{

    float sintheta = sin(theta);
    float costheta = cos(theta);
    float cosphi = cos(phi);
    float sinphi = sin(phi);
    float temp = sintheta * sqrt(r * r + bh.a2);

    *x = temp * cosphi;
    *y = temp * sinphi;
    *z = r * costheta;
}

/* Fonction utilisé pour l'intégration */
void geodesic(float L, float kappa, float *y, float *dydx)
{
    float r, theta, pr, ptheta;

    r = y[0];
    theta = y[1];
    pr = y[4];
    ptheta = y[5];

    float r2 = r * r;
    float twor = 2.0 * r;

    float sintheta, costheta;
    sintheta = sin(theta);
    costheta = cos(theta);

    float costheta2 = costheta * costheta;
    float sintheta2 = sintheta * sintheta;

    float sigma = r2 + bh.a2 * costheta2;
    float delta = r2 - twor + bh.a2;
    float sd = sigma * delta;
    float siginv = 1.0 / sigma;
    float bot = 1.0 / sd;

    /* Prevent problems with the axis */
    if (sintheta < 1e-8)
    {
        sintheta = 1e-8;
        sintheta2 = 1e-16;
    }

    dydx[0] = -pr * delta * siginv;
    dydx[1] = -ptheta * siginv;
    dydx[2] = -(twor * bh.a + (sigma - twor) * L / sintheta2) * bot;
    dydx[3] = -(1.0 + (twor * (r2 + bh.a2) - twor * bh.a * L) * bot);
    dydx[4] = -(((r - 1.0) * (-kappa) + twor * (r2 + bh.a2) - 2.0 * bh.a * L) * bot - 2.0 * pr * pr * (r - 1.0) * siginv);
    dydx[5] = -sintheta * costheta * (L * L / (sintheta2 * sintheta2) - bh.a2) * siginv;
}

/* Conditions initiales pour un rayon */
void initial(float r0, float theta0, float *L, float *kappa, float *y0, float *ydot0, float x, float y)
{
    y0[0] = r0;
    y0[1] = theta0;
    y0[2] = 0;
    y0[3] = 0;
    y0[4] = cos(y) * cos(x);
    y0[5] = sin(y) / r0;

    float sintheta, costheta;
    sintheta = sin(theta0);
    costheta = cos(theta0);
    float costheta2 = costheta * costheta;
    float sintheta2 = sintheta * sintheta;

    float rdot0 = y0[4];
    float thetadot0 = y0[5];

    float r2 = r0 * r0;
    float sigma = r2 + bh.a2 * costheta2;
    float delta = r2 - 2.0 * r0 + bh.a2;
    float s1 = sigma - 2.0 * r0;

    y0[4] = rdot0 * sigma / delta;
    y0[5] = thetadot0 * sigma;

    ydot0[0] = rdot0;
    ydot0[1] = thetadot0;
    ydot0[2] = cos(y) * sin(x) / (r0 * sin(theta0));

    float phidot0 = ydot0[2];
    float energy2 = s1 * (rdot0 * rdot0 / delta + thetadot0 * thetadot0) + delta * sintheta2 * phidot0 * phidot0;

    float energy = sqrt(energy2);

    /* Energie de 1 */
    y0[4] = y0[4] / energy;
    y0[5] = y0[5] / energy;

    /* Angular Momentum with E = 1 */
    *L = ((sigma * delta * phidot0 - 2.0 * bh.a * r0 * energy) * sintheta2 / s1) / energy;

    *kappa = y0[5] * y0[5] + bh.a2 * sintheta2 + (*L) * (*L) / sintheta2;

    /* Hack - make sure everything is normalized correctly by a call to geodesic */

    geodesic(*L, *kappa, y0, ydot0);
}

//Obtenir la direction du rayon à partir des coord de BL (y) et de leurs dérivées (dydx)
//Retourne un vecteur normé
void getDirection(float *y, float *dydx, float *xp, float *yp, float *zp)
{

    float costheta = cos(y[1]);
    float sintheta = sin(y[1]);
    float cosphi = cos(y[2]);
    float sinphi = sin(y[2]);
    float r2 = y[0] * y[0];
    float R2 = r2 + bh.a2;
    float R = sqrt(r2 + bh.a2);

    *xp = R * dydx[1] * cosphi * costheta - R * dydx[2] * sinphi * sintheta + sintheta * cosphi * y[0] * dydx[0] / R;
    *yp = R * dydx[1] * sinphi * costheta + R * dydx[2] * cosphi * sintheta + sintheta * sinphi * y[0] * dydx[0] / R;
    *zp = -y[0] * sintheta * dydx[1] + dydx[0] * costheta;
    normalise(xp, yp, zp);
}

//Simulation complete d'un rayon (trajectoire+collision)
void sim(float r0, float theta0, float phi0, float xpixel, float ypixel, float *xp, float *yp, float *zp, float *pixel_transpr, float *pixel_transpg, float *pixel_transpb, float *canalAlpha, bool *ReachedInfinity, int *nbCollision)
{
    int N = 0; //Nombre d'itérations
    int k = 0; //nombre de collision

    float oldtheta; //pour detecter le passage à travers le plan z=0 (pour tracer le disque)

    bool zSignChange, diskDistance, diskCollision;

    //Euler

    //float y[Neq];
    //float dydx[Neq];

    //RK4
    float y[Neq];
    float ak[Neq];
    float dydx1[Neq], dydx2[Neq], dydx3[Neq], dydx4[Neq];
    float ytemp[Neq];

    float L, kappa;

    initial(r0, theta0, &L, &kappa, y, ak, xpixel, ypixel);

    float currentStep = rdr.step(y[0]);

    int l = 0; //Juste pour effectuer des boucles sans redeclarer de var

    while ((N < MAXITER) && (rdr.R_min < y[0]) && (y[0] < rdr.R_inf))
    {

        N += 1;

        oldtheta = y[1];

        currentStep = rdr.step(y[0]);

        //Euler
        /*for (int l = 0; l < Neq; l++)
	    {
		    float hdydx = currentStep * dydx[l];
		    y[l] = y[l] + hdydx;
	    }

	    geodesic(L,kappa,y, dydx);*/

        //RK4
        geodesic(L, kappa, y, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx1[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx2[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx3[l] = ak[l];
            ytemp[l] = y[l] + currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx4[l] = ak[l];
            dydx1[l] = currentStep / 6. * (dydx1[l] + 2. * dydx2[l] + 2. * dydx3[l] + dydx4[l]); //Recuperer une bonne approx de la derivée dans dydx1
            y[l] = y[l] + dydx1[l];
        }

        zSignChange = (oldtheta > M_PI_2) != (y[1] > M_PI_2);      //on traverse le plan z=0
        diskDistance = (y[0] < disk.R_max) && (y[0] > disk.R_min); //On l'a traversé la ou est le disque
        diskCollision = zSignChange && diskDistance;

        if (diskCollision)
        {
            float xpos, ypos, zpos;
            blToCartesian(y[0], y[1], y[2], &xpos, &ypos, &zpos); //Obtenir la position en coord cartesiennes
            getDirection(y, dydx1, xp, yp, zp);                   //Obtenir la direction (en coord cartesiennes)

            //Trouver le point de colision (en allant tout droit entre les deux point au dessus et en dessous du disque (valable si le pas est assez petit))
            float lambda = -zpos / (*zp);
            float coll_x = xpos + lambda * (*xp);
            float coll_y = ypos + lambda * (*yp);
            float coll_z = zpos + lambda * (*zp);
            float r = norm(coll_x, coll_y, coll_z);

            diskCollision = (r < disk.R_max) && (r > disk.R_min); //#reverification plus précise
            if (diskCollision)
            {
                if (k < rdr.maxtransparency)
                {

                    float phi = atan2(coll_y / r, coll_x / r); //Coordonné du point d'impact (en sphérique)

#if ADISK_NORMAL == 1

                    float diskspeed_x, diskspeed_y, diskspeed_z;

                    //rotation prograde du disque

                    sphericalToCartesian(M_PI / 2., phi + M_PI / 2., &diskspeed_x, &diskspeed_y, &diskspeed_z); //Direction de la vitesse en ce point, sens trigo,mvt circulaire

                    float beta = disk.RotationSpeed(r);                                                         //Obtenir la norme de la vitesse des poussières en ce point
                    float costhetadoppler = -((*xp) * diskspeed_x + (*yp) * diskspeed_y + (*zp) * diskspeed_z); //Obtenir le cosinus de l'angle entre le rayon et la vitesse des particules (Les vecteurs sont normés)
                    //- à cause du sens des rayons

                    //Temperature du disque à cet endroit
                    float Temp = disk.Temp(r);

                    //Décalage en fréquence égal à décalage en temperature, valable aussi pour l'intensité

                    Temp = Temp * (1 + beta * costhetadoppler) / sqrt(1 - beta * beta); //Effet doppler relativiste

                    //calcul du redshift gravitationnel
                    float gtt = 1. - 1. / r;
                    float gtphi = bh.a / r;
                    float gphiphi = -(r * r + bh.a2 + bh.a2 / r);
                    float omega = beta / r;                                                //Vitesse angulaire
                    Temp = Temp * sqrt(gtt + 2 * gtphi * omega + gphiphi * omega * omega); //Redshift gravitationnel

                    //Recuperer la texture du disque à cet endroit, qu'on utilise uniquemenet pour obtenir la transparence du disque
                    int cx = int((r - disk.R_min) * (adisk_height - 1) / (disk.R_max - disk.R_min));
                    int cy = int((adisk_width - 1) * mod(disk.texture_rep * phi - M_PI, 2. * M_PI) / (2. * M_PI));

                    int loc = (cx * adisk_width + cy) * CHANNEL_NUM;

                    pixel_transpr[k] = 0.;
                    pixel_transpg[k] = 0.;
                    pixel_transpb[k] = 0.;

                    getBodyColor(&pixel_transpr[k], &pixel_transpg[k], &pixel_transpb[k], Temp, 1.); //Obtenir la couleur

                    canalAlpha[k] = (float)adisk[loc + 1] / 255.; //Choix: on utilise la composante verte (le +1) pour reconstituer la transparence

#endif

#if ADISK_GRID == 1

                    getDiskColorGrid(phi, r, &pixel_transpr[k], &pixel_transpg[k], &pixel_transpb[k]); //Motif de grille sur le disque et calcul de la transparence
                    canalAlpha[0] = 1.;
#endif

                    k++;
                }
            }
        }
    }

    *ReachedInfinity = (y[0] >= rdr.R_inf);
    *nbCollision = k;

    if (*ReachedInfinity)
    {
        //Pour visualiser quels rayons ont atteints R_inf
        //*nbCollision=1;
        //pixel_transpr[0]=255;
        //canalAlpha[0]=.5;

        getDirection(y, dydx1, xp, yp, zp);
        normalise(xp, yp, zp); //Ne pas effectuer inutilement cette opération
    }
}

//Simulation d'un rayon (trajectoire uniquement)
void sim_opt(float r0, float theta0, float phi0, float xpixel, float ypixel, float *xp, float *yp, float *zp, bool *ReachedInfinity)
{

    int N = 0;

    //Euler
    //float y[Neq];
    //float dydx[Neq];

    //RK4
    float y[Neq], ak[Neq];
    float dydx1[Neq], dydx2[Neq], dydx3[Neq], dydx4[Neq];
    float ytemp[Neq];

    float L, kappa;

    initial(r0, theta0, &L, &kappa, y, ak, xpixel, ypixel); //CI

    float currentStep = rdr.step(y[0]);

    int l;

    while ((N < MAXITER) && (rdr.R_min < y[0]) && (y[0] < rdr.R_inf))
    {

        N += 1;

        currentStep = rdr.step(y[0]);

        //Euler
        /*for (int l = 0; l < Neq; l++)
	    {
		    float hdydx = currentStep * dydx[l];
		    y[l] = y[l] + hdydx;
	    }
	    geodesic(L,kappa,y, dydx);*/

        //RK4

        geodesic(L, kappa, y, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx1[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx2[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx3[l] = ak[l];
            ytemp[l] = y[l] + currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx4[l] = ak[l];
            dydx1[l] = currentStep / 6. * (dydx1[l] + 2. * dydx2[l] + 2. * dydx3[l] + dydx4[l]); //Recuperer une bonne aprrox de la derivée
            y[l] = y[l] + dydx1[l];
        }
    }

    *ReachedInfinity = (y[0] >= rdr.R_inf);

    if (*ReachedInfinity)
    { //Ne pas effectuer inutilement cette opération
        getDirection(y, dydx1, xp, yp, zp);
        normalise(xp, yp, zp);
    }
}

//Simulation d'un faisceu de rayons (uniquement dans le cas ou on dessine des étoiles)
void sim_bundle(int i, int j, float x, float y, float z, float *pixelr, float *pixelg, float *pixelb)
{ //arguments: i et j position du pixel, x,y,z position initiale du rayon dans l'espace

    float r0, theta0, phi0;
    cartesianToBl(x, y, z, &r0, &theta0, &phi0); //Approximativement (compliqué de passer de cartesien à BL), mais pas important

    float xpcentre0, ypcentre0, zpcentre0;
    float xpcentref, ypcentref, zpcentref;
    float xp0, yp0, zp0;
    float xpf, ypf, zpf;

    bool ReachedInfinity = true;
    int nbCollision;

    float maxdist2 = 0.;
    float maxdist2_0 = 0.;
    //float mindist2=1000.;
    //float mindist2_0=1000.;

    /*float Distances2[4];
    float Distances2_0[4];
    float dilatation=1.;*/

    float pixel_transpr[rdr.maxtransparency], pixel_transpg[rdr.maxtransparency], pixel_transpb[rdr.maxtransparency];
    float canalAlpha[rdr.maxtransparency];

    xpcentref = xpcentre0;
    ypcentref = ypcentre0;
    zpcentref = zpcentre0;

    float range = 0.075 * 20. / (rdr.width - 1.0); //FOV
    //float range = 0.1 * 20. / (rdr.width - 1.0);

    float xpixel = -(i - (rdr.width + 1.0) / 2) * range;
    float ypixel = -(j - (rdr.height + 1.0) / 2) * range;

    sim(r0, theta0, phi0, xpixel, ypixel, &xpcentref, &ypcentref, &zpcentref, pixel_transpr, pixel_transpg, pixel_transpb, canalAlpha, &ReachedInfinity, &nbCollision); //Simulation du rayon principal

#if DRAWSTARS == 1

    int Nray = 0;

    while (Nray < 4 && ReachedInfinity)
    {

        if (Nray == 0)
        {
            xpixel = -(i + rdr.delta - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 1)
        {
            xpixel = -(i - rdr.delta - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 2)
        {
            xpixel = -(i - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j + rdr.delta - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 3)
        {
            xpixel = -(i - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - rdr.delta - (rdr.height + 1.0) / 2) * range;
        }

        Nray++;

        xpf = xp0;
        ypf = yp0;
        zpf = zp0;

        sim_opt(r0, theta0, phi0, xpixel, ypixel, &xpf, &ypf, &zpf, &ReachedInfinity); //Simulation des rayons secondaires

        if (ReachedInfinity)
        {
            //Calcule de la taille de la zone d'impact des differents rayons (zone en terme de direction)
            //On trouve le rayon maxdist du cercle centré sur la direction du rayon principal, qui contient les autres rayons (tjrs en terme de direction)
            //Calcul en coord cartesienne pour éviter les discontinuités liées au modulo 2PI

            //float dx0=xpcentre0-xp0;
            //float dy0=ypcentre0-yp0;
            //float dz0=zpcentre0-zp0;

            //float dist2_0=sqrnorm(dx0,dy0,dz0);

            float dx = xpcentref - xpf;
            float dy = ypcentref - ypf;
            float dz = zpcentref - zpf;

            float dist2 = sqrnorm(dx, dy, dz);

            //if (maxdist2_0<dist2_0){ maxdist2_0=dist2_0; }
            if (maxdist2 < dist2)
            {
                maxdist2 = dist2;
            }
        }
    }

    if (ReachedInfinity)
    { //Si tous les rayons se sont échappés, on regarde quels étoiles sont dans le cercle formé par le faisceau sur le ciel

        float maxdist = sqrt(maxdist2);
        float sqrdistanceToStar; //sqrnorm(scn.starx[i]-xpcentref,scn.stary[i]-ypcentref,scn.starz[i]-zpcentref);

        float c1, c2, c3;

        for (int i = 0; i < Nstar; ++i)
        {

            c1 = abs(scn.starx[i] - xpcentref);

            if (c1 < maxdist)
            {
                c2 = abs(scn.stary[i] - ypcentref);
                if (c2 < maxdist)
                {
                    c3 = abs(scn.starz[i] - zpcentref);
                    if (c3 < maxdist)
                    {
                        sqrdistanceToStar = sqrnorm(c1, c2, c3);
                        if (sqrdistanceToStar < maxdist2)
                        {
                            getBodyColor(pixelr, pixelg, pixelb, scn.starTemp[i], scn.starBrightness[i] * 12.); //(Récupere la couleur et effectue l'addition composante par composante)
                        }
                    }
                }
            }
        }

        float b = 1e-7 / maxdist2; //Angle solide initial environ constant égal à 1e-7
        *pixelr *= b;
        *pixelg *= b;
        *pixelb *= b;
    }
#endif
#if DRAWGRID == 0
    getFinalColor(pixelr, pixelg, pixelb, pixel_transpr, pixel_transpg, pixel_transpb, canalAlpha, nbCollision); // Calcul de transparence
#else
    getFinalColorGrid(xpcentref, ypcentref, zpcentref, pixelr, pixelg, pixelb, pixel_transpr, pixel_transpg, pixel_transpb, nbCollision, ReachedInfinity); // Calcul de la transparence, et calcul de la couleur de la grille
#endif
}

void render(const char *name)
{
    float *image = new float[rdr.width * rdr.height * CHANNEL_NUM]; //Tableau qui va contenir les données

    int Chunkcomputed = 0;
    pl::async_par_for(0, rdr.TotalChunknumber, [&](unsigned h) {
        //for (int h=0;h<rdr.TotalChunknumber;++h){ //A utiliser si jamais pl async ne marche pas
        printf("file %s - Chunk %04d/%04d started - %.2f pourcents  \n", name, h, rdr.TotalChunknumber, 100. * (float)Chunkcomputed / ((float)rdr.TotalChunknumber));

        int j0 = h/rdr.NChunkWidth;
        int i0 = h % rdr.NChunkWidth;

        i0*=rdr.ChunkSizeWidth;
        j0*=rdr.ChunkSizeHeight;
        int i1=i0 + rdr.ChunkSizeWidth;
        int j1=j0 + rdr.ChunkSizeHeight;

        for (int j = j0; j < j1; ++j)
        {

            for (int i = i0; i < i1; ++i)
            {

                float pixelr, pixelg, pixelb;

                pixelr = 0.;
                pixelg = 0.;
                pixelb = 0.;

                sim_bundle(i, j, scn.camera.x, scn.camera.y, scn.camera.z, &pixelr, &pixelg, &pixelb);

                int loc = (rdr.width * j + i) * CHANNEL_NUM;

                image[loc] = pixelr;
                image[loc + 1] = pixelg;
                image[loc + 2] = pixelb;

            }
        }
        Chunkcomputed++;

    });
    //}

    //Postprocess possible ici
    int index = 0;

    uint8_t *image_byte = new uint8_t[rdr.width * rdr.height * CHANNEL_NUM]; //Image finale

    std::cout << "conversion" << endl;

    for (int j = 0; j < rdr.height; ++j)
    {

        for (int i = 0; i < rdr.width; ++i)
        {

            image_byte[index] = char(image[index]);
            index++;

            image_byte[index] = char(image[index]);
            index++;

            image_byte[index] = char(image[index]);
            index++;
        }
    }

    stbi_write_png(name, rdr.width, rdr.height, CHANNEL_NUM, image_byte, rdr.width * CHANNEL_NUM);
}

int image(string fichier)//int image()
{
    ifstream monFlux;
    monFlux.open(fichier.c_str()) ;

    if (monFlux)
    {
        monFlux >> bh.a >> disk.R_max >> scn.camera.x >> scn.camera.y >> scn.camera.z >> rdr.R_inf >> rdr.width >> rdr.height;
        cout<<"Les paramètres ont été initialisés"<<endl;
        if (adisk == NULL)
        {
            cout<<"L'image de départ n'a pas été trouvée"<< endl;
            exit(0);
        }

        /*rdr.height = 1080 / 8;
        rdr.width = 1920 / 8;
        rdr.R_inf = 21.5; //distance à partir de laquelle on considere etre a l'infini */

        rdr.ChunkSizeHeight = 15; //Blocs de 15 par 15 pixels traités en parallele
        rdr.ChunkSizeWidth = 15; //Blocs de 15 par 15 pixels traités en parallele

        //Euler
        /*rdr.stepmax=0.0035;
    rdr.stepmin=0.002; //0.001 min sinon erreurs arrondi ?*/

        //RK4
        rdr.stepmax = 0.02 * 15;
        rdr.stepmin = 0.007 * 15;

        rdr.delta = .5; //Ecart angulaire (en pixel) entre les rayons d'un même faisceau

        //bh.a = 0.5;   //spin (adimensionné,entre 0 et 1)
        bh.precalc(); //A executer avant inner orbit

        disk.R_min = bh.inner_orbit(); //Trouver l'orbite la plus proche du trou noir encore stable (prograde)

        //disk.R_max = 16.;
        disk.betamax = 0.65; //Vitesse du disque au plus proche du trou noir (c'est la qu'est la vitesse max pour un profil de vitesse en r^-1/2)
        disk.TMax = 17500.;  //temperature du disque au plus proche du trou noir

        //scn.FOV=40.;

        disk.texture_rep = 8.; //Répéter la texture du dique (en longeur pour ne pas qu'elle soit pixelisée)

        rdr.precalc(bh.a2); //quelques calculs pour avoir les carrés de certaines qtité et le fov en radian etc..
        //scn.precalc(rdr.width);
        disk.precalc();

        scn.generateSky();

        //Rendre une image fixe:
        /*scn.camera.x = 20.;
        scn.camera.y = 0.;
        scn.camera.z = 1.2;*/

        /*scn.camera.theta=0.04;
    scn.camera.phi=0.01;*/

        readSensitivityData("sensitivity.txt", wavelengthSamples, wavelengthSamples5, sensitivitySamplesR, sensitivitySamplesG, sensitivitySamplesB);

        /*for (int l = 0; l < SpectrumSampleSize; ++l)
    {
        printf("WL:%f R:%f, G:%f B:%f \n",wavelengthSamples[l],sensitivitySamplesR[l],sensitivitySamplesG[l],sensitivitySamplesB[l]);
    }*/

        //float R,G,B;
        //getBodyColor(&R,&G,&B,6000,1.);
        //printf("R:%f G:%f, B:%f \n",R,G,B);

        cout << "Start" << endl;
        render("resultat.png");
        cout << "End" << endl;

        stbi_image_free(adisk);
    }
    else
    {
        cout << "Erreur le fichier ne s'est pas ouvert" << endl;
    }
    return 0;
}

/*int main()
{
    image("params.txt");
    return 0;
}*/

#include <boost/python.hpp>

BOOST_PYTHON_MODULE(kerr)
{
    using namespace boost::python;
    def("image", image);
}