#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
#include <iostream>
#include <fstream>
#include <map>
#include <boost/algorithm/string/trim.hpp>
#include <string.h>
#include <vector>
#include "Eigen/Core"
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

using namespace std;
template<class T> struct Point{
    template <typename NewType> Point<NewType> cast() const{
        return Point<NewType>();
    }
};
struct tt{
	int x;
	int y;
};
int main()
{
	Eigen::Quaterniond quat0(1, 2,0,0);
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	Eigen::Matrix3d mat0 = quat0.toRotationMatrix();
	std::cout<<"mat0 (from quat0): \n" <<mat0 <<"\n";
	Eigen::Quaterniond quat1=quat0.conjugate();
	std::cout<<"quat0 conjugate: " <<quat1.w()<<" "<<quat1.x()<<" "<<quat1.y()<<" "<<quat1.z()<<"\n";
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	quat0.normalize();
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	Eigen::Matrix3d mat1 = quat0.toRotationMatrix();
	std::cout<<"mat1 (quat0 normalized): \n" <<mat1 <<"\n";
	Eigen::Quaterniond quat(1, 0,0,0);
	std::string ss0;
	std::ofstream outFile("/home/student//Documents/cartographer/test_ceres_pcd/sweep4.txt");
	std::ostringstream ssout("~/Documents/cartographer/test_ceres_pcd/sweep4.txt");
	ssout<<"hello"<<2;
	ssout.str("");//reset
	ssout<<"newhello"<<2;
	outFile<<"hello"<<3<<"hello";
	outFile<<ssout.str();
	outFile<<1<<","<<2;
	std::cout<<ssout.str();

    std::cout<<"Eigen::Quaterniond: "<< quat.w() <<"\n";
	Eigen::Matrix3d mat3 = Eigen::Quaterniond(1, 0,0,0).toRotationMatrix();
    Eigen::Vector3d v3d(-1., 0., 0.);
    Eigen::Transform<double, 3, Eigen::Affine > pose_transform;

    pose_transform.translation()=v3d;
    pose_transform.rotate(Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX()));
    std::cout<<"Eigen::Transform: rotation"<< pose_transform.rotation() <<"\n";
    pose_transform.rotate(quat);
    pose_transform.linear()=mat3; //Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX());
    //pose_transform.rotate() =mat3;
    //pose_transform.block<3,3>(0,0) =mat3;
    std::cout<<"Eigen::Matrix3f: "<< mat3 <<"\n";
    std::cout<<"Eigen::Vector3d: "<< v3d <<v3d[0]<<"\n";
    std::cout<<"Eigen::Transform: "<< pose_transform.translation() <<"\n";
    std::cout<<"Eigen::Transform: rotation"<< pose_transform.rotation() <<"\n";
    Eigen::Matrix<float, 3, 1> translation0(2,2,2);
    Eigen::Matrix<double, 3, 1> translation1(translation0.cast<double>());

    Point<float> p1;
     Point<double> p2;
    tt t1{1,2};
    //tt t2(1,2); bad structure initialization
    std::cout<<"struct tt: " <<t1.x<<"\n";
    std::vector<int> v = { 7, 5, 16, 8 };
    std::vector<int> intvec1;
    std::cout <<"intvec1.size(): "<< intvec1.size();
    intvec1.push_back(3);
    std::cout <<"\nintvec1.front(): "<< intvec1.front()<<"\n";

    //std::cout<<p1 <<" "<<p2 <<"\n";
    p2 = p1.cast<double>();
	std::array<int,3> a1{{1,2,3}};
	std::cout<<a1.data()[2];
	std::cout<<a1.at(2);

    std::cout<<"hello";
    std::string filename="testopt2.txt";

    std::cout<<NULL<<"----\n";
std::ifstream stream(filename.c_str());
//    std::ifstream stream("testopt2.txt");
    std::string myoptstr= std::string((std::istreambuf_iterator<char>(stream)),
                     std::istreambuf_iterator<char>());
    std::cout<< myoptstr;
map<string, string> mymap;
char *token;
token = strtok(const_cast<char*>(myoptstr.c_str()), "\n");
    while (token != NULL) {
        string s(token);
        size_t pos = s.find(":");
        //mymap[s.substr(0, pos)] = boost::algorithm::trim(s);
	string svalue = s.substr(pos + 1, string::npos);
	boost::algorithm::trim(svalue);
        mymap[s.substr(0, pos)] = svalue;
	//boost::algorithm::trim(s.substr(pos + 1, string::npos));
        token = strtok(NULL, "\n");

	if (s.substr(0, pos)=="tst"){
	std::cout<<"test tst"<<std::endl;
		char * token2;
		//std::cout<<svalue<<std::endl;
		token2 = strtok(const_cast<char*>(svalue.c_str()), ",");
		while (token2 != NULL) {
			string ss(token2);
			std::cout<<token2 <<" ";
        		token2 = strtok(NULL, ",");
		}

	}
    }

    std::cout<<"\n*****************\ncout mymap:\n";
    for (auto keyval : mymap)
        cout << keyval.first << ":" << keyval.second << endl;

      pcl::PointCloud<pcl::PointXYZ> cloud;

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

  for (auto& point: cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);

    return 0;
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the solution of the system Ax=b when A is triangular
     * @param A Triangular matrix.  Specify upper or lower with TriView
     * being Eigen::Upper or Eigen::Lower.
     * @param b Right hand side matrix or vector.
     * @return x = A^-1 b, solution of the linear system.
     * @throws std::domain_error if A is not square or the rows of b don't
     * match the size of A.
     */
    template <int TriView, typename T1, typename T2,
              int R1, int C1, int R2, int C2>
    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  R1, C2>
    mdivide_left_tri(const Eigen::Matrix<T1, R1, C1> &A,
                     const Eigen::Matrix<T2, R2, C2> &b) {
      check_square("mdivide_left_tri", "A", A);
      check_multiplicable("mdivide_left_tri", "A", A, "b", b);
      return promote_common<Eigen::Matrix<T1, R1, C1>,
                            Eigen::Matrix<T2, R1, C1> >(A)
        .template triangularView<TriView>()
        .solve(promote_common<Eigen::Matrix<T1, R2, C2>,
               Eigen::Matrix<T2, R2, C2> >(b));
    }

    /**
     * Returns the solution of the system Ax=b when A is triangular and b=I.
     * @param A Triangular matrix.  Specify upper or lower with TriView
     * being Eigen::Upper or Eigen::Lower.
     * @return x = A^-1 .
     * @throws std::domain_error if A is not square
     */
    template<int TriView, typename T, int R1, int C1>
    inline
    Eigen::Matrix<T, R1, C1>
    mdivide_left_tri(const Eigen::Matrix<T, R1, C1> &A) {
      check_square("mdivide_left_tri", "A", A);
      int n = A.rows();
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
      b.setIdentity(n, n);
      A.template triangularView<TriView>().solveInPlace(b);
      return b;
    }

  }
}
#endif


//==================================================================================================
/*!
  @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_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/plain.hpp>

#include <boost/simd/function/scalar/inc.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/scalar/inc.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log1p)(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::log1p(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 ix = bitwise_cast<uiA0>(x);
      iA0 k = 1;
      A0 c = Zero<A0>(), f = x;
      if (ix < 0x3ed413d0 || ix>>31)               /* 1+x < sqrt(2)+  */
      {
        if (ix >= 0xbf800000)                       /* x <= -1.0 */
        {
          if (x == Mone<A0>())  return Minf<A0>();  /* log1p(-1)=-inf */
          return Nan<A0>();                         /* log1p(x<-1)=NaN */
        }
        if (ix<<1 < 0x33800000<<1)                  /* |x| < 2**-24 */
        {
          if ((ix&0x7f800000) == 0) return x;
        }
        if (ix <= 0xbe95f619)                       /* sqrt(2)/2- <= 1+x < sqrt(2)+ */
        {
          k = 0;
        }
      }
      else if (ix >= 0x7f800000)  return x;
      if (k)
      {
        /* reduce u into [sqrt(2)/2, sqrt(2)] */
        A0 uf =  inc(x);
        uiA0 iu = bitwise_cast<uiA0>(uf);
        iu += 0x3f800000 - 0x3f3504f3;
        k = bitwise_cast<iA0>(iu>>23) - 0x7f;
        /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
        if (k < 25)
        {
          c = k >= 2 ? oneminus(uf-x) : x-dec(uf);
          c /= uf;
        }

        /* reduce u into [sqrt(2)/2, sqrt(2)] */
        iu = (iu&0x007fffff) + 0x3f3504f3;
        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 = 0.5f*sqr(f);
      A0 dk = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k = 1;

      A0 c = Zero<A0>(), f = x;
      if (hx < 0x3fda827a || hx>>31)               /* 1+x < sqrt(2)+ */
      {
        if (hx >= 0xbff00000)                      /* x <= -1.0 */
        {
          if (x == Mone<A0>()) return Minf<A0>();  /* log1p(-1)=-inf */
          return Nan<A0>();                        /* log1p(x<-1)=NaN */
        }
        if (hx<<1 < 0x3ca00000<<1)                 /* |x| < 2**-53 */
        {
          if ((hx&0x7ff00000) == 0) return x;
        }
        if (hx <= 0xbfd2bec4)                      /* sqrt(2)/2- <= 1+x < sqrt(2)+ */
        {
          k = 0;
        }
      } else if (hx >= 0x7ff00000) return x;
      if (k)
      {
        /* reduce x into [sqrt(2)/2, sqrt(2)] */
        A0 uf =  inc(x);
        uiA0 hu = bitwise_cast<uiA0>(uf)>>32;
        hu += 0x3ff00000 - 0x3fe6a09e;
        k = (int)(hu>>20) - 0x3ff;
        /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
        if (k < 54)
        {
          c = k >= 2 ? oneminus(uf-x) : x-dec(uf);
          c /= uf;
        }
        hu =  (hu&0x000fffff) + 0x3fe6a09e;
        f = bitwise_cast<A0>( bitwise_cast<uiA0>(hu<<32) | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(uf))));
        f = dec(f);
      }

      A0 hfsq = 0.5*sqr(f);
      A0 s = f/(2.0f + 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 = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
    }
  };

    BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, A0 x) const BOOST_NOEXCEPT
    {
      return musl_(log1p)(x); //the "plain" version of the algorithm is never speedier than the "musl" version.
      // the call is here to allow a scalar fallback to simd calls
    }
  };

} } }

#endif


#include <cassert>
#include <boost/math/distributions/chi_squared.hpp>
#include <Eigen/Core>
#include "genfile/Error.hpp"
#include "genfile/VariantEntry.hpp"
#include "components/SNPSummaryComponent/DifferentialMissingnessComputation.hpp"
#include "metro/FishersExactTest.hpp"
#include "metro/likelihood/Multinomial.hpp"
#include "metro/likelihood/ProductOfMultinomials.hpp"

namespace stats {
	DifferentialMissingnessComputation::UniquePtr DifferentialMissingnessComputation::create( std::string const& stratification_name, StrataMembers const& strata_members ) {
		return DifferentialMissingnessComputation::UniquePtr(
			new DifferentialMissingnessComputation( stratification_name, strata_members )
		) ;
	}

	DifferentialMissingnessComputation::DifferentialMissingnessComputation( std::string const& stratification_name, StrataMembers const& strata_members, double threshhold ):
		m_stratification_name( stratification_name ),
		m_strata_members( strata_members ),
		m_strata_levels( compute_strata_levels( m_strata_members ) ),
		m_threshhold( threshhold )
	{
	#if 0
		if( strata_members.size() != 2 ) {
			throw genfile::BadArgumentError( "DifferentialMissingnessComputation::DifferentialMissingnessComputation()", "strata_members( size " + genfile::string_utils::to_string( strata_members.size() ) + ")" ) ;
		}
	#endif
	}

	std::vector< int > DifferentialMissingnessComputation::compute_strata_levels( StrataMembers const& strata_members ) const {
		std::vector< int > result ;
		int level = 0 ;
		for( StrataMembers::const_iterator i = strata_members.begin(); i != strata_members.end(); ++i, ++level ) {
			for( std::size_t j = 0; j < i->second.size(); ++j ) {
				std::size_t const index = i->second[j] ;
				result.resize( std::max( result.size(), index+1 ), -1 ) ;
				result[ index ] = level ;
			}
		}
		return result ;
	}

	void DifferentialMissingnessComputation::operator()( VariantIdentifyingData const& snp, Genotypes const& genotypes, Ploidy const&, genfile::VariantDataReader&, ResultCallback callback ) {
		// construct a table
		//
		//                 missing     not missing
		//   stratum 1       a              b
		//   stratum 2       c              d
		//
		// on which we can do a Fisher's exact test or a product of binomial-type test.
		//
		int const number_of_strata = 1 + *std::max_element( m_strata_levels.begin(), m_strata_levels.end() ) ;
		Eigen::MatrixXd table( number_of_strata, 2 ) ;
		table.setZero() ;
		for( int i = 0; i < genotypes.rows(); ++i ) {
			if( m_strata_levels[i] >= 0 ) {
				if( genotypes.row( i ).sum() < m_threshhold ) {
					table( m_strata_levels[i], 0 )++ ;
				}
				else {
					table( m_strata_levels[i], 1 )++ ;
				}
			}
		}

		{
			StrataMembers::const_iterator i = m_strata_members.begin() ;
			StrataMembers::const_iterator const end_i = m_strata_members.end() ;
			for( int level = 0; i != end_i; ++level, ++i ) {
				std::string tag = "[" + m_stratification_name + "=" + genfile::string_utils::to_string( i->first ) + "]" ;
				callback( "missing" + tag, table( level, 0 ) ) ;
				callback( "non_missing" + tag, table( level, 1 ) ) ;
			}
		}

		std::string const stub = "missingness_by_" + m_stratification_name ;
	//	callback( stub + "_sample_odds_ratio", table(0,0) * table(1,1) / ( table(0,1) * table(1,0) ) ) ;

		if( table.row(0).sum() > 0 && table.row(1).sum() > 0 ) {
			// perform the exact test.  For speed reasons this is only done if the chi-square approximation may be inaccurate.
			if( table.rows() == 2 ) {
				try {
					metro::FishersExactTest test( table ) ;
					callback( stub + "_exact_pvalue", test.get_pvalue( metro::FishersExactTest::eTwoSided ) )  ;
				}
				catch( std::exception const& e ) {
					callback( stub + "_exact_pvalue", genfile::MissingValue() ) ;
				}
			} else {
				callback( stub + "_exact_pvalue", genfile::MissingValue() ) ;
			}
			{
				metro::likelihood::Multinomial< double, Eigen::VectorXd, Eigen::MatrixXd > null_model( table.colwise().sum() ) ;
				null_model.evaluate_at( null_model.get_MLE() ) ;
				metro::likelihood::ProductOfMultinomials< double, Eigen::VectorXd, Eigen::MatrixXd > alternative_model( table ) ;
				alternative_model.evaluate_at( alternative_model.get_MLE() ) ;
				double likelihood_ratio_statistic = -2.0 * ( null_model.get_value_of_function() - alternative_model.get_value_of_function() ) ;

				if( likelihood_ratio_statistic != likelihood_ratio_statistic || likelihood_ratio_statistic < 0.0 ) {
					likelihood_ratio_statistic = std::numeric_limits< double >::quiet_NaN() ;
				}
				else {
					boost::math::chi_squared_distribution< double > chi_squared( table.rows() - 1 ) ;

					double p_value = boost::math::cdf(
						boost::math::complement(
							chi_squared,
							likelihood_ratio_statistic
						)
					) ;

					callback( stub + "_lrt_pvalue", p_value ) ;
					callback( stub + "_lrt_df", genfile::VariantEntry::Integer( table.rows() - 1 )) ;
				}
			}
		}
	}

	std::string DifferentialMissingnessComputation::get_summary( std::string const& prefix, std::size_t column_width ) const {
		return prefix + "DifferentialMissingnessComputation" ;
	}
}


// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved.
//
// 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.

#include "paddle/phi/kernels/qr_kernel.h"

#include <Eigen/Dense>

#include "paddle/phi/backends/cpu/cpu_context.h"
#include "paddle/phi/core/kernel_registry.h"
#include "paddle/phi/kernels/funcs/complex_functors.h"
#include "paddle/phi/kernels/funcs/parse_qr_mode.h"

namespace phi {

template <typename T, typename Context>
void QrKernel(const Context& ctx,
              const DenseTensor& x,
              const std::string& mode,
              DenseTensor* q,
              DenseTensor* r) {
  bool compute_q;
  bool reduced_mode;
  std::tie(compute_q, reduced_mode) = phi::funcs::ParseQrMode(mode);
  auto numel = x.numel();
  PADDLE_ENFORCE_GT(
      numel, 0, errors::PreconditionNotMet("The input of QR is empty."));
  auto x_dims = x.dims();
  int x_rank = x_dims.size();
  int m = x_dims[x_rank - 2];
  int n = x_dims[x_rank - 1];
  int min_mn = std::min(m, n);
  int k = reduced_mode ? min_mn : m;
  int batch_size = numel / (m * n);
  int x_stride = m * n;
  int q_stride = m * k;
  int r_stride = k * n;
  auto* x_data = x.data<phi::dtype::Real<T>>();
  T* q_data = nullptr;
  if (compute_q) {
    q_data = ctx.template Alloc<phi::dtype::Real<T>>(
        q, batch_size * m * k * sizeof(phi::dtype::Real<T>));
  }
  auto* r_data = ctx.template Alloc<phi::dtype::Real<T>>(
      r, batch_size * k * n * sizeof(phi::dtype::Real<T>));

  // Implement QR by calling Eigen
  for (int i = 0; i < batch_size; ++i) {
    const T* x_matrix_ptr = x_data + i * x_stride;
    T* r_matrix_ptr = r_data + i * r_stride;
    using EigenDynamicMatrix =
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    auto x_matrix = Eigen::Map<const EigenDynamicMatrix>(x_matrix_ptr, m, n);
    Eigen::HouseholderQR<EigenDynamicMatrix> qr(x_matrix);
    if (reduced_mode) {
      auto qr_top_matrix = qr.matrixQR().block(0, 0, min_mn, n);
      auto r_matrix_view =
          qr_top_matrix.template triangularView<Eigen::Upper>();
      auto r_matrix = EigenDynamicMatrix(r_matrix_view);
      memcpy(r_matrix_ptr, r_matrix.data(), r_matrix.size() * sizeof(T));
    } else {
      auto r_matrix_view =
          qr.matrixQR().template triangularView<Eigen::Upper>();
      auto r_matrix = EigenDynamicMatrix(r_matrix_view);
      memcpy(r_matrix_ptr, r_matrix.data(), r_matrix.size() * sizeof(T));
    }

    if (compute_q) {
      T* q_matrix_ptr = q_data + i * q_stride;
      if (reduced_mode) {
        auto q_matrix =
            qr.householderQ() * EigenDynamicMatrix::Identity(m, min_mn);
        q_matrix.transposeInPlace();
        memcpy(q_matrix_ptr, q_matrix.data(), q_matrix.size() * sizeof(T));
      } else {
        auto q_matrix = qr.householderQ() * EigenDynamicMatrix::Identity(m, m);
        q_matrix.transposeInPlace();
        memcpy(q_matrix_ptr, q_matrix.data(), q_matrix.size() * sizeof(T));
      }
    }
  }
}

}  // namespace phi

PD_REGISTER_KERNEL(qr, CPU, ALL_LAYOUT, phi::QrKernel, float, double) {}


#ifndef YQVMC_CI_BINDER_HPP
#define YQVMC_CI_BINDER_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/moment.hpp>

namespace yqvmc { namespace ci {
  class BinderCumulant {
  public:
    BinderCumulant() {}
    typedef double result_type;

    template <typename C>
    void measure(const C& conf, std::size_t stamp) {
      double m = 0.;
      for (bool spin : conf)
        m += spin ? +1. : -1.;
      m_acc(m);
    }

    result_type result() {
      double m4 = boost::accumulators::moment<4>(m_acc);
      double m2 = boost::accumulators::moment<2>(m_acc);
      return 1. - m4/3./m2/m2;
    }

  private:
    boost::accumulators::accumulator_set<double,
      boost::accumulators::stats<
        boost::accumulators::tag::moment<2>,
        boost::accumulators::tag::moment<4>
      >
    > m_acc;
  };
} }
#endif


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

/*
 Copyright (C) 2011 Peter Caspers

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

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

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

/*! \file markovfunctional.hpp
    \brief Markov Functional 1 Factor Model
*/

#ifndef quantlib_markovfunctional_hpp
#define quantlib_markovfunctional_hpp

#include <iostream>
#include <iomanip>

#include <boost/timer/timer.hpp>

#include <math.h>
#include <boost/math/special_functions/fpclassify.hpp>

#include <ql/models/model.hpp>
#include <ql/models/parameter.hpp>
#include <ql/math/interpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/solvers1D/brent.hpp>
#include <ql/math/errorfunction.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/time/date.hpp>
#include <ql/time/period.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvolstructure.hpp>
#include <ql/termstructures/volatility/optionlet/optionletvolatilitystructure.hpp>
#include <ql/termstructures/volatility/smilesection.hpp>
#include <ql/stochasticprocess.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/pricingengines/blackcalculator.hpp>
#include <ql/utilities/null.hpp>

#include <mfstateprocess.hpp>
#include <kahalesmilesection.hpp>

namespace QuantLib {

    //! One factor Markov Functional model class.
    /*! blah blah
		TODO use voltermstructure's timeFromReference() for std dev calculation of market digitals (or blackVariance()), also in swaption and caplet engine
			 however, is this then still consistent with the process x starting at yts's reference date? think about that and make it consistent ...
    */

    class MarkovFunctional : public TermStructureConsistentModel, public CalibratedModel  {

      public:

		struct ModelSettings {

			ModelSettings(const int xGridPoints,const Real xStdDevs,const int gaussHermitePoints,const Real digitalGap,const int arbitrageCheckGridPoints,
				const Real rateCutLeft, const Rate rateCutRight, const Rate upperRateBound, const bool adjustDigitals, const bool forceExactYtsFit, const int digitalInterpolationDegree):

					xGridPoints_(xGridPoints), xStdDevs_(xStdDevs), gaussHermitePoints_(gaussHermitePoints), digitalGap_(digitalGap),
					arbitrageCheckGridPoints_(arbitrageCheckGridPoints), rateCutLeft_(rateCutLeft), rateCutRight_(rateCutRight),
					upperRateBound_(upperRateBound), adjustDigitals_(adjustDigitals), forceExactYtsFit_(forceExactYtsFit), digitalInterpolationDegree_(digitalInterpolationDegree) {}

			void validate() const {

				QL_REQUIRE(xGridPoints_>0,"At least one grid point (" << xGridPoints_ << ") for the state process discretization must be given");
				QL_REQUIRE(xStdDevs_>0.0,"Multiple of standard deviations covered by state process discretization (" << xStdDevs_ << ") must be positive");
				QL_REQUIRE(gaussHermitePoints_>0,"Number of gauss hermite integration points (" << gaussHermitePoints_ << ") must be positive");
				QL_REQUIRE(digitalGap_>0.0 && rateCutLeft_-digitalGap_/2.0 >=0.0 &&
					rateCutLeft_+digitalGap_/2.0<=rateCutRight_ && rateCutRight_+digitalGap_/2.0 <=upperRateBound_,
					"Rate cut left (" << rateCutLeft_ << "), digital gap (" << digitalGap_ << "), rate cut right (" << rateCutRight_ <<
					") and upper rate bound (" << upperRateBound_ << ") are not consistent");
				QL_REQUIRE(arbitrageCheckGridPoints_>0,"Number of grid points for arbitrage check (" << arbitrageCheckGridPoints_ << ") must be positive");
				QL_REQUIRE(digitalInterpolationDegree_>=0 && digitalInterpolationDegree_<=4,"Polynomial degree for digital payoff interpolation (" << digitalInterpolationDegree_ <<") must be between 0 and 4");

			}

			const int xGridPoints_;
			const Real xStdDevs_;
			const int gaussHermitePoints_;
			const Real digitalGap_;
			const int arbitrageCheckGridPoints_;
			const Real rateCutLeft_;
			const Real rateCutRight_;
			const Real upperRateBound_;
			const bool adjustDigitals_;
			const bool forceExactYtsFit_;
			const int digitalInterpolationDegree_;

		};

		struct CalibrationPoint {
			bool isCaplet_;
			//Date expiry_;
			Period tenor_;
			std::vector<Date> paymentDates_;
			std::vector<Real> yearFractions_;
			Real atm_;
			Real annuity_;
			Real afLeftBound_, afRightBound_;
			Real alphaL_,betaL_,alphaR_,betaR_;
			boost::shared_ptr<KahaleSmileSection> smileSection_;
		};

		struct ModelOutputs {
			std::vector<Date> expiries_;
			std::vector<Period> tenors_;
			std::vector<Real> afLeftBounds_;
			std::vector<Real> afRightBounds_;
			std::vector<Real> adjustmentFactors_;
			std::vector<Real> digitalsAdjustmentFactors_;
		};

        MarkovFunctional(const Handle<YieldTermStructure>& termStructure,
						const Real reversion,
						const std::vector<Date>& volstepdates,
						const std::vector<Real>& volatilities,
						const Handle<SwaptionVolatilityStructure>& swaptionVol,
						const std::vector<Date>& swaptionExpiries,
						const std::vector<Period>& swaptionTenors,
						const boost::shared_ptr<SwapIndex>& swapIndexBase,
						const int xGridPoints=512, const Real xStdDevs=7.0, const int gaussHermitePoints=32,
						const Real digitalGap=1E-8, const int arbitrageCheckGridPoints=200, const Real rateCutLeft=0.0001, const Real rateCutRight=1.9000,
						const Real upperRateBound=2.0, const bool adjustDigitals=false, const bool forceExactYtsFit=false, const int digitalInterpolationDegree=2);

		MarkovFunctional(const Handle<YieldTermStructure>& termStructure,
						const Real reversion,
						const std::vector<Date>& volstepdates,
						const std::vector<Real>& volatilities,
						const Handle<OptionletVolatilityStructure>& capletVol,
						const std::vector<Date>& capletExpiries,
						const boost::shared_ptr<IborIndex>& iborIndex,
						const int xGridPoints=512, const Real xStdDevs=7.0, const int gaussHermitePoints=32,
						const Real digitalGap=1E-8, const int arbitrageCheckGridPoints=200, const Real rateCutLeft=0.0001, const Real rateCutRight=1.9000,
						const Real upperRateBound=2.0, const bool adjustDigitals=false, const bool forceExactYtsFit=false, const int digitalInterpolationDegree=2);

		const ModelSettings& modelSettings() const { return modelSettings_; }
		const ModelOutputs& modelOutputs() const { return modelOutputs_; }

		const Date& numeraireDate() const { return numeraireDate_; }
		const Time& numeraireTime() const { return numeraireTime_; }

		const boost::shared_ptr<StochasticProcess1D> stateProcess() const { return stateProcess_; }

		Real numeraire(const Time t, const Real x=0.0) const;
		Real deflatedZerobond(const Time T, const Time t=0.0, const Real x=0.0) const;

		Real zerobond(const Time T, const Time t=0.0, const Real x=0.0) const;
		Real zerobond(const Date& maturity, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;

		Real zerobondOption(const Option::Type& type, const Date& expiry, const Date& maturity, const Rate strike, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;

		Real forwardRate(const Date& fixing, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;
		Real swapRate(const Date& fixing, const Period& tenor, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;
		Real swapAnnuity(const Date& fixing, const Period& tenor, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;

		Real capletPrice(const Option::Type& type, const Date& expiry, const Rate strike, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;
		Real swaptionPrice(const Option::Type& type, const Date& expiry, const Period& tenor, const Rate strike, const Date& referenceDate = Null<Date>(), const Real x=0.0) const;

      protected:

		void generateArguments();

      private:

		void initialize();
	    void updateNumeraireTabulation();

		void makeSwaptionCalibrationPoint(const Date& expiry, const Period& tenor);
		void makeCapletCalibrationPoint(const Date& expiry);

		void arbitrageFreeStrikeRange(const Date& expiry, CalibrationPoint& p) const;
		Real marketSwapRate(const Date& expiry, const CalibrationPoint& p, const Real digitalPrice) const;
		Real marketDigitalPrice(const Date& expiry, const CalibrationPoint& p, const Option::Type& type, const Real strike) const;
		static Real gaussianPolynomialIntegral(const Real a, const Real b, const Real c, const Real d, const Real e, const Real x0, const Real x1);

		ModelSettings modelSettings_;
		ModelOutputs modelOutputs_;

		const bool capletCalibrated_;

		boost::shared_ptr<GaussianQuadrature> gaussHermite_;
		const CumulativeNormalDistribution cnd_;

		boost::shared_ptr<IborIndex> ytsLinkedIborIndex_;
		boost::shared_ptr<StochasticProcess1D> stateProcess_;

		std::vector<boost::shared_ptr<std::vector<Real>>> numeraireDiscretization_;
		std::vector<boost::shared_ptr<Interpolation>> numeraire_;

		Parameter reversion_;
		Parameter& sigma_;

		std::vector<Date> volstepdates_;
		std::vector<Time> volsteptimes_;
		Array volsteptimesArray_;				// FIXME this is redundant (copy of volsteptimes_)
		std::vector<double> volatilities_;

		Date numeraireDate_;
		Time numeraireTime_;

		Handle<SwaptionVolatilityStructure> swaptionVol_;
		Handle<OptionletVolatilityStructure> capletVol_;

		std::vector<Date> swaptionExpiries_, capletExpiries_;
		std::vector<Period> swaptionTenors_;
		boost::shared_ptr<SwapIndex> swapIndexBase_;
		boost::shared_ptr<IborIndex> iborIndex_;

		//std::set<CalibrationPoint> calibrationPoints_;
		std::map<Date,CalibrationPoint> calibrationPoints_;
		std::vector<double> times_;
		std::vector<double> y_;

		mutable boost::timer::cpu_timer cpuTimer_,cpuTimer2_;

    };

	//bool operator<(const MarkovFunctional::CalibrationPoint& a, const MarkovFunctional::CalibrationPoint& b);

}

#endif


#include <map>
#include <queue>
#include <unordered_map>
#include <Eigen/Sparse>
#include <Euclid/Geometry/DEC.h>
#include <Euclid/Topology/HomologyGenerator.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh, typename T>
void build_closedness_matrix(const Mesh& mesh,
                             std::vector<Eigen::Triplet<T>>& triplets)
{
    auto fimap = get(boost::face_index, mesh);
    auto eimap = get(boost::edge_index, mesh);
    auto frange = faces(mesh);
    --frange.second; // the last face is redundant
    for (auto f : frange) {
        auto fidx = get(fimap, f);
        for (auto h : halfedges_around_face(halfedge(f, mesh), mesh)) {
            auto e = edge(h, mesh);
            triplets.emplace_back(
                fidx, get(eimap, e), halfedge_orientation(mesh, h));
        }
    }
}

template<typename Mesh, typename T>
void build_harmonicity_matrix(const Mesh& mesh,
                              std::vector<Eigen::Triplet<T>>& triplets)
{
    auto vimap = get(boost::vertex_index, mesh);
    auto eimap = get(boost::edge_index, mesh);
    auto offset = static_cast<int>(num_faces(mesh)) - 1;
    auto vrange = vertices(mesh);
    --vrange.second; // the last vertex is redundant
    for (auto v : vrange) {
        auto vidx = get(vimap, v);
        for (auto h : halfedges_around_target(v, mesh)) {
            auto e = edge(h, mesh);
            auto w = cotangent_weight(h, mesh);
            triplets.emplace_back(offset + vidx,
                                  get(eimap, e),
                                  halfedge_orientation(mesh, h) * w);
        }
    }
}

template<typename Mesh, typename T>
void build_duality_matrix(const Mesh& mesh,
                          const VertexChains<Mesh>& basis,
                          std::vector<Eigen::Triplet<T>>& triplets)
{
    auto eimap = get(boost::edge_index, mesh);
    auto offset = static_cast<int>(num_faces(mesh) + num_vertices(mesh)) - 2;
    for (size_t i = 0; i < basis.size(); ++i) {
        for (size_t j = 0; j < basis[i].size(); ++j) {
            auto vcurrent = basis[i][j];
            auto vnext = basis[i][(j + 1) % basis[i].size()];
            auto [h, hfound] = halfedge(vcurrent, vnext, mesh);
            auto e = edge(h, mesh);
            triplets.emplace_back(
                offset + i, get(eimap, e), halfedge_orientation(mesh, h));
        }
    }
}

template<typename Mesh,
         typename Derived,
         typename SEM,
         typename SVM,
         typename VertexUVMap,
         typename VertexParameterizedMap>
void integrate_holomorphic_one_forms(
    const Mesh& mesh,
    const Eigen::MatrixBase<Derived>& one_forms,
    CGAL::Seam_mesh<Mesh, SEM, SVM>& seam_mesh,
    VertexUVMap uvmap,
    VertexParameterizedMap vpmap)
{
    using Seam_mesh = CGAL::Seam_mesh<Mesh, SEM, SVM>;
    using Point_2 =
        typename CGAL::Kernel_traits<typename boost::property_traits<
            VertexUVMap>::value_type>::Kernel::Point_2;
    using Vector_2 =
        typename CGAL::Kernel_traits<typename boost::property_traits<
            VertexUVMap>::value_type>::Kernel::Vector_2;
    using vertex_descriptor =
        typename boost::graph_traits<Seam_mesh>::vertex_descriptor;
    auto underlying_eimap = get(boost::edge_index, mesh);

    std::queue<vertex_descriptor> queue;
    auto root = *(vertices(seam_mesh).first);
    queue.push(root);
    put(uvmap, root, Point_2(0, 0));
    put(vpmap, root, true);
    while (!queue.empty()) {
        auto v = queue.front();
        queue.pop();
        for (auto h : halfedges_around_source(v, seam_mesh)) {
            auto vv = target(h, seam_mesh);
            if (!get(vpmap, vv)) {
                auto underlying_e = edge(h.tmhd, mesh);
                auto e_idx = get(underlying_eimap, underlying_e);
                auto uv = get(uvmap, v);
                auto s = halfedge_orientation(mesh, h.tmhd);
                uv += s * Vector_2(one_forms(e_idx, 0), one_forms(e_idx, 1));
                put(uvmap, vv, uv);
                put(vpmap, vv, true);
                queue.push(vv);
            }
        }
    }
}

} // namespace _impl

template<typename Mesh, typename DerivedA, typename DerivedB>
void holomorphic_one_form_basis(const Mesh& mesh,
                                Eigen::MatrixBase<DerivedA>& primal,
                                Eigen::MatrixBase<DerivedB>& conjugate)
{
    auto basis = greedy_homology_generators(mesh);
    holomorphic_one_form_basis(mesh, basis, primal, conjugate);
}

template<typename Mesh, typename DerivedA, typename DerivedB>
void holomorphic_one_form_basis(const Mesh& mesh,
                                const VertexChains<Mesh>& homology_generators,
                                Eigen::MatrixBase<DerivedA>& primal,
                                Eigen::MatrixBase<DerivedB>& conjugate)
{
    using Scalar = typename DerivedA::Scalar;
    using Triplet = Eigen::Triplet<Scalar>;
    using Triplets = std::vector<Triplet>;
    using SpMat = Eigen::SparseMatrix<Scalar>;
    using Vec = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using Mat = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    auto ng = homology_generators.size();
    auto nv = num_vertices(mesh);
    auto ne = num_edges(mesh);
    auto nf = num_faces(mesh);
    primal.derived().resize(ne, ng);

    // Primal one form basis
    Triplets triplets;
    _impl::build_closedness_matrix(mesh, triplets);
    _impl::build_harmonicity_matrix(mesh, triplets);
    _impl::build_duality_matrix(mesh, homology_generators, triplets);
    SpMat lhs(ne, ne);
    lhs.setFromTriplets(triplets.begin(), triplets.end());
    Eigen::SparseLU<SpMat> primal_solver;
    primal_solver.compute(lhs);
    for (size_t i = 0; i < ng; ++i) {
        Vec rhs = Vec::Zero(ne);
        rhs(nv + nf - 2 + i) = 1.0;
        primal.col(i) = primal_solver.solve(rhs);
    }

    // Conjugate one form basis
    Mat W(ng, ng);
    for (size_t i = 0; i < ng; ++i) {
        for (size_t j = 0; j < ng; ++j) {
            Vec wedge;
            wedge1(mesh, primal.col(i), primal.col(j), wedge);
            W(i, j) = wedge.sum();
        }
    }
    Mat B(ng, ng);
    for (size_t i = 0; i < ng; ++i) {
        for (size_t j = 0; j < ng; ++j) {
            Vec wedge;
            star_wedge1(mesh, primal.col(i), primal.col(j), wedge);
            B(i, j) = wedge.sum();
        }
    }
    Eigen::ColPivHouseholderQR<Mat> conjugate_solver;
    conjugate_solver.compute(W);
    Mat L = conjugate_solver.solve(B);
    conjugate = primal * L;
}

template<typename Mesh,
         typename Derived,
         typename SEM,
         typename SVM,
         typename VertexUVMap>
void integrate_holomorphic_one_forms(
    const Mesh& mesh,
    const Eigen::MatrixBase<Derived>& one_forms,
    CGAL::Seam_mesh<Mesh, SEM, SVM>& seam_mesh,
    VertexUVMap uvmap)
{
    using SM = CGAL::Seam_mesh<Mesh, SEM, SVM>;
    using vd = typename boost::graph_traits<SM>::vertex_descriptor;
    using PM = std::map<vd, bool>;
    PM parameterized;
    boost::associative_property_map<PM> vpmap(parameterized);
    _impl::integrate_holomorphic_one_forms(
        mesh, one_forms, seam_mesh, uvmap, vpmap);
}

template<typename Mesh, typename SEM, typename SVM>
Holomorphic_one_forms_parameterizer3<Mesh, SEM, SVM>::
    Holomorphic_one_forms_parameterizer3(const Mesh& mesh)
    : _underlying_mesh(mesh)
{
    holomorphic_one_form_basis(mesh, _primal, _conjugate);
    Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(_primal.cols());
    coeffs(0) = 1.0;
    set_coeffs(coeffs);
}

template<typename Mesh, typename SEM, typename SVM>
Holomorphic_one_forms_parameterizer3<Mesh, SEM, SVM>::
    Holomorphic_one_forms_parameterizer3(
        const Mesh& mesh,
        const VertexChains<Mesh>& homology_generators)
    : _underlying_mesh(mesh)
{
    holomorphic_one_form_basis(mesh, homology_generators, _primal, _conjugate);
    Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(_primal.cols());
    coeffs(0) = 1.0;
    set_coeffs(coeffs);
}

template<typename Mesh, typename SEM, typename SVM>
template<typename Derived>
void Holomorphic_one_forms_parameterizer3<Mesh, SEM, SVM>::set_coeffs(
    const Eigen::MatrixBase<Derived>& coeffs)
{
    _one_forms.resize(_primal.rows(), 2);
    _one_forms.col(0) = _primal * coeffs;
    _one_forms.col(1) = _conjugate * coeffs;
}

template<typename Mesh, typename SEM, typename SVM>
template<typename VertexUVMap,
         typename VertexIndexMap,
         typename VertexParameterizedMap>
CGAL::Surface_mesh_parameterization::Error_code
Holomorphic_one_forms_parameterizer3<Mesh, SEM, SVM>::parameterize(
    TriangleMesh& mesh,
    halfedge_descriptor,
    VertexUVMap uvmap,
    VertexIndexMap,
    VertexParameterizedMap vpmap)
{
    _impl::integrate_holomorphic_one_forms(
        _underlying_mesh, _one_forms, mesh, uvmap, vpmap);
    return CGAL::Surface_mesh_parameterization::OK;
}

} // namespace Euclid


/**======================================================================================================================================================
 * @brief  Approximate inference Marginalization
 *        /Script to find the amalgamation on CIMs.
 *          i.e Marginalization operation takes a CIM Q S|C , a set of variables X from,
 *            and an initial distribution P over the variables of S. It returns a reduced
 *            CIM of the form: Q S'|C = marg PX ( Q S|C ) where S'=S-X.
 *              The resulting approximation is a linear approximation of the marginal.
 *              Q S'|C (s'1 → s'2|c)≈ ∑ Q S|C (s'1⊕y → s'2⊕y|c) P0 (y|s'1,c)
 *
 * @version 1.0
 * @author Intern: Bernardin HOUESSOU for LIP6
 * Created on 6 juillet 2015, 17:37
 * File:   main.cpp
 *=====================================================================================================================================================
*/

//Used and required library(ies)/header(s)/inlcude(s)===============================================================================================================================
#include <cstdlib>                    //
#include <armadillo>                 // Armadillo header::armadillo library
#include <iostream>                 // Console commands ::defines the standard Input/Output stream objects std::cout
#include <string>                  // String header::defines several functions to manipulate C++ strings
#include <fstream>                // File commands::defines the Input/Output stream class to operate on files
#include <vector>                // For vector
#include <cctype>               // For uppercase & lowercase conForersion
#include <cstring>             // For  strcopy() and ....
#include <map>                // Use of Map
#include "TabVar.h"
#include "MatrixCIM.h"       // Calling created header/library
#include <sstream>
#include <functional>   // std::minus
#include <numeric>     // std::accumulate
//Used namespaces====================================================================================================================================================
using namespace std;
using namespace arma;

/********************************************************************************************************************************
 LIST OF USEFUL FUNCTIONS
 **/
/***********************************************************************************************************************************************
Function "countnumberfile()" to count number of times the file "lines1.txt" has been used in the background
-Specially created for the use in the function "speciallogs()
*/
int countnumberfile()
{
	//Needed variables
	ofstream outputFile1;
	int count1=0;

    //checking number of times the file 'lines1.txt' has been used
	ifstream inputFile1("Resources/Temporary/lines1.txt");
	inputFile1 >> count1;
	inputFile1.close();
	count1++ ;

    //Saving count to file 'lines1.txt'
	outputFile1.open ("Resources/Temporary/lines1.txt");
	outputFile1 <<count1 << endl;
	outputFile1.close();

return count1;
}
//End of Function "countsaveddistrotofile()"**************************************************************************************

/****************************************************************************************************************************************************
Function "speciallogs() to save all printed elements by "cout<<" on the screen into a file
*/
void speciallogs()
{
        //redirect output of cout to file "Result/output.txt
	freopen("Resources/Result/resultlogMarg.txt", "app", stdout );
	//Displaying the n_th number before each trial
	std::cout<<"|"<<"N°"<<countnumberfile()<<"|"<<endl;
}
//End of Function "speciallogs()"*********************************************************************************************************************

/********************************************************************************************************************************
Function "createemptymfoismmatrix() " to convert int to string
*/
string int2strconvertor(int n) {
	stringstream ss;
	ss << n;
	return ss.str();
}
//End of function "int2strconvertor() " ******************************************************************************************************

/********************************************************************************************************************************
Function "str2intconvertor() " to convert string to int
*/
int str2intconvertor(std::string str) {

	return atoi( str.c_str() );
}
//End of function "str2intconvertor( ) " ******************************************************************************************************

/********************************************************************************************************************************
Function "convert() " to convert multimaptomap
*/

ms convert(const multimap_tablestring& multimapstring)
{
    ms r;

    for (multimap_tablestring::const_iterator it = multimapstring.begin(); it !=multimapstring.end(); ++it)
    {

        std::vector<std::string>& s(r[it->first]);
        s.push_back(it->second);

    }

    return r;
}
//End of function "convertmultimaptomap() " ******************************************************************************************************

ms2 convert2(const multimap_tablestring& multimapstring2)
{
    ms2 r;

    for (multimap_tablestring::const_iterator it = multimapstring2.begin(); it !=multimapstring2.end(); ++it)
    {

        std::set<std::string>& s(r[it->first]);
        s.insert(it->second);

    }

    return r;
}
//End of function "convertmultimaptomap() " ******************************************************************************************************

/*Vector split function to split matrix values only from text file
 */
StringVector splitvarnbstate(string str, char delimiter) {
  StringVector internal;
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;

  while(getline(ss, tok, delimiter))
  {
        tok=tok2;
        char delimiter2=';';
        while(getline(ss, tok, delimiter2)) {
           if(getline(ss, tok, delimiter2)) {
             // std::cout<<tok;
            internal.push_back(tok);
           }
        }
  }
return internal;
}

/*Vector split function to split matrix values only from text file
 */
StringVector splitmatrixvaluesonly11(string str, char delimiter) {
  StringVector internal;
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
  while(getline(ss, tok, delimiter))
  {
        tok=tok2;
        char delimiter2=';';
        if(getline(ss, tok, delimiter2)) {
                    if(getline(ss, tok, delimiter2)) {
           if(getline(ss, tok, delimiter2)) {
             // std::cout<<tok;
            internal.push_back(tok);
           }
        }
        }
  }
return internal;
}

/*Vector split function to split Variables only from text file
 */
StringVector splitdistrovariable(string strv, char delimiterv) {
  StringVector internalv;
  delimiterv='|';//replace old delimiter by delimiterv
  stringstream ssv(strv); // Turn the string into a stream.
  string tokv;
  string tok2v=tokv;
  while(getline(ssv, tokv, delimiterv))
  {
        tokv=tok2v;
        char delimiter2v='+';
        if(getline(ssv, tokv, delimiter2v)) {
           if(getline(ssv ,tokv, delimiter2v)) {
              //std::cout<<tok;
            internalv.push_back(tokv);
             }
        }
  }
return internalv;
}

/*Vector split function to split distro Variables from text file
 */
StringVector splitdistroparents(string str, char delimiter) {

  StringVector internal;
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
  delimiter='+';//replacing old delimiter=* to this one
  while(getline(ss, tok, delimiter))
  {
        tok=tok2;
        char delimiter2='|';
        if(getline(ss, tok, delimiter2)) {
            delimiter2=';';
           if(getline(ss, tok, delimiter2))
              internal.push_back(tok);
        }
  }
return internal;
}

/*Vector split function to split given distribution P0 values from text file
 */
StringVector splitdistrovalues(string str, char delimiter) {

 StringVector internal;
  //char delimiter1=':';
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
  delimiter='+';
    while(getline(ss, tok, delimiter))
    {
         tok=tok2;
         char delimiter2=';';
         if(getline(ss, tok, delimiter2)) {
             if(getline(ss, tok, delimiter2))

            // std::cout<<tok;
            internal.push_back(tok);

         }
    }
return internal;
}

/*Vector split function to split parents variables only from text file
 */
StringVector splitvarparentsonly1(string str, char delimiter) {
  StringVector internal;
  //char delimiter1=':';
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
    while(getline(ss, tok, delimiter))
    {
         tok=tok2;
         char delimiter2=';';
         if(getline(ss, tok, delimiter2)) {
            // std::cout<<tok;
            internal.push_back(tok);

         }
    }
return internal;
}

/*Vector split function to split Variables only from text file
 */
StringVector splitvariable(string strv, char delimiterv) {
  StringVector internalv;
  delimiterv=',';//replace old delimiter by delimiterv
  stringstream ssv(strv); // Turn the string into a stream.
  string tokv;
  string tok2v=tokv;
  while(getline(ssv, tokv, delimiterv))
  {
        tokv=tok2v;
        char delimiter2v=':';
        if(getline(ssv, tokv, delimiter2v)) {
           if(getline(ssv ,tokv, delimiter2v)) {
              //std::cout<<tok;
            internalv.push_back(tokv);
             }
        }
  }
return internalv;
}

/*********************************************************************************************************************************************************************
 * MAIN FUNCTION
 *********************************************************************************************************************************************************************
 */

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

    //Sending result to logfile
    speciallogs();

    //load the text file and put it into a single string:
    std::ifstream in("Resources/statespacevar.txt");
    std::stringstream buffer;
    buffer << in.rdbuf();
    std::string test = buffer.str();
    //Declaring and initializing my delimiter
    char delimiter1='|';
    //Splitting content of file into vector 'varstat' to get state of var
    StringVector varstate=splitvarnbstate(test, delimiter1);
    size_t varstatesize=varstate.size();//size of the vector 'varstat'
    map_table mapdesvarstate;
    map_table::iterator itstate;
    //
    string KEY;

    //Splitting content of file into vector 'sep'
    StringVector sep= splitmatrixvaluesonly11(test, delimiter1);
    size_t aa=sep.size();//size of the vector 'sep'
    //
    StringVector variableinitial=splitvariable(test, delimiter1);
    size_t bb=variableinitial.size();//size of the vector 'sep'
    //
    StringVector parentsvariable=splitvarparentsonly1(test, delimiter1);
    size_t parentsvarsize=parentsvariable.size();//size of the vector 'sep'
    //
    StringVector matrixvalues=splitmatrixvaluesonly11(test, delimiter1);
    size_t matrixvaluessize=matrixvalues.size();//size of the vector 'sep'
    //
    map2string mapvarparstate;
    map2string::iterator mp3st;
    //
    map_tablelistbig my_map;
    map_tablelistbig::iterator iterr;
    int my_mapsize=my_map.size();

    map_tablelistbig my_map2;
    map_tablelistbig::iterator iterr2;
    int my_mapsize2=my_map2.size();
    //
    Biglist biglist;
    //
    std::vector<std::string> VartableIni;
    biglist.Vartable=VartableIni;
    //
    std::vector<std::string> VarstateIni;
    biglist.Varstate=VarstateIni;
    //
    std::vector<std::string> ParentstableIni;
    biglist.Parentstable=ParentstableIni;
    //
    std::vector<std::string> VarMatrixIni;
    biglist.VarMatrix=VarMatrixIni;
    //
    for(size_t i = 0; i < bb; i++){
        biglist.Vartable.push_back (variableinitial[i]);
        biglist.Varstate.push_back (varstate[i+i]);
        biglist.Parentstable.push_back (parentsvariable[i]);
        biglist.VarMatrix.push_back (matrixvalues[i]);
        std::string Keystring;
        Keystring="  "+biglist.Vartable.at(i)+"        "+"  "+biglist.Varstate.at(i)+" "+"       "+biglist.Parentstable.at(i)+"         "+" "+biglist.VarMatrix.at(i);
        my_map.insert( std::pair<int,std::string > (i,Keystring));
    }
            //Display content of map initial
           // showmate(" ", my_map);
          //Delete element at position i and display content of map after deletion
         // deletemapelement(0, my_map);

        //Splitting content of file into vector 'sep'
        StringVector septest= splitvariable(test, delimiter1);
        size_t aatest=septest.size();//size of the vector 'sep'

        map_tablestring mapdesvarstring;
        map_tablestring::iterator itestring;

        mapdesvarstring[septest[0]]=varstate[0];

    for (itestring=mapdesvarstring.begin(); itestring!=mapdesvarstring.end(); ++itestring) {
        for(size_t i = 1; i <aatest; i++){
            KEY=septest[i];
            std::pair<map_tablestring::iterator,bool> retstring;
            retstring = mapdesvarstring.insert ( std::pair<std::string,std::string >(KEY,varstate[i+i]) );
        }
    }
        //Creation of stringvector to handle variable,state and ...
        StringVector varok1;
        StringVector varok1state;
        StringVector savedoldvarok1;

        for (itestring=mapdesvarstring.begin();itestring!=mapdesvarstring.end(); ++itestring){
            varok1.push_back(itestring->first);
            savedoldvarok1.push_back(itestring->first);
            varok1state.push_back(itestring->second);
            std::reverse(varok1state.begin(),varok1state.end());
        }
        //cout << "Map size " << mapdesvarstring.size() << endl;
        multimap_tablestring multimapstring;
        multimap_tablestring::iterator multiitestring;
        StringVector varandapparitioninmap;

          for(size_t j = 0; j <bb; j++)
              multimapstring.insert(std::pair<const char* const,std::string>(variableinitial[j].c_str(), parentsvariable[j]));

            for(size_t j = 0; j <varok1.size(); j++)  {
               varandapparitioninmap.push_back(int2strconvertor(multimapstring.count(varok1[j].c_str())));
            }

        StringVector reversevarok1;
        StringVector reversevarandapparitioninmap;
        //std::cout << "****************************************"<<endl;
        //std::cout << "Number of elements with key: "<<endl;
        //for(size_t j = 0; j <varandapparitioninmap.size(); j++)
        //cout<<""<<varandapparitioninmap[j]<<endl<<endl;
        std::reverse(varok1.begin(),varok1.end());
        std::reverse(varandapparitioninmap.begin(),varandapparitioninmap.end());

        for (std::vector<std::string>::iterator ite=varok1.begin(); ite!=varok1.end(); ++ite)
              reversevarok1.push_back(*ite);
        for (std::vector<std::string>::iterator it=varandapparitioninmap.begin(); it!=varandapparitioninmap.end(); ++it)
             reversevarandapparitioninmap.push_back(*it);
         /*
                 for(size_t j = 0; j <reversevarok1.size(); j++){
                      cout<<""<<reversevarok1[j]<<" : "<<reversevarandapparitioninmap[j]<<endl<<endl;
                }
         */

        for(size_t j = 0; j <varok1.size(); j++)  {

         varandapparitioninmap.push_back(int2strconvertor(multimapstring.count(varok1[j].c_str())));
        }

        //calling function to convert multimap to map
        ms s(convert(multimapstring));

        //declaration of an int vector to get the offset or "pas" value of a special variable for later use
        intVector offsetorpas;

          for(size_t i = 0; i < mapdesvarstring.size(); i++)
         {
            int offsetovar;
            int wght1var=1;
            if(i==0)
            {
              offsetovar=1;
              //cout<<"--Offset or Weight of Variable: "<<i+1<<" -- "<<varok1.at(i)<<" = "<<offsetovar<<endl;
              offsetorpas.push_back(offsetovar);
            }
            //Determination of the different offset basing on the number of state of each variable
            if(i!=0){
                //convert string to integer
                int stateprevious;
                stateprevious=atoi( varok1state.at(i-1).c_str() );
                //cout<<stateprevious<<endl;
                if(i==1){
                    offsetovar=1;
                    offsetovar=offsetovar * stateprevious;
                    offsetorpas.push_back(offsetovar);
                }
                if(i!=1){
                offsetovar= offsetovar * stateprevious;
                offsetorpas.push_back(offsetovar);
                }
             //cout<<"--Offset or Weight of Variable: "<<i+1<<" -- "<<varok1.at(i)<<" = "<<offsetovar<<endl;
            }
            //cout<<varok1state.at(i)<<endl<<endl;
         }

/*****************************************************************************************************************************************************/
                //STATING CREATING CIM MATRIXES AND USING THEIR VALUES FOR THE JIM MATRIX
                int nbcimmatrix=bb;int matrixnbrows=2;int nbvariable=mapdesvarstring.size();

                //intVector valuediagonal;
                doubleVector matvalueeach;
                std::string indexmat;
                std::string matvalue;
                string fmatrixname;
                mat matrixname;
		for (int i=0;i<nbcimmatrix;i++)
                {   //cout<<matrixvalues[i]<<endl;
                    indexmat=int2strconvertor(i);
                    //string fmatrixname="Q"+indexmat;
                    fmatrixname="Q"+biglist.Vartable.at(i)+"|"+biglist.Parentstable.at(i);
                    string fulltrixfilename="Resources/Matrices/CIMs/"+ fmatrixname +".txt";
                    matrixname.resize(matrixnbrows,matrixnbrows); // change the size of matrixname atomatically(data is not preserved)
                    //matrixname.zeros();//set all elements to Zeros
                    matrixname=matrixvalues[i];
                    matrixname.save(fulltrixfilename,csv_ascii);
                    //matrixname.print(fulltrixfilename);
                     //cout << fmatrixname<<".n_rows: " << matrixname.n_rows << endl;  // .n_rows and .n_cols are read only
                    //cout << fmatrixname<< ".n_cols: " << matrixname.n_cols << endl;
                    //get each matrix value and print them
                     //cout<<fmatrixname<<endl;
                     int nbdelements=matrixname.size();
                     for(int i=0;i<=nbdelements-1;i++){
                        indexmat=int2strconvertor(i);
                        matvalue="val "+indexmat;
                        //cout<< matvalue<<": "<<matrixname.at(i)<<endl;
                        matvalueeach.push_back(matrixname.at(i));
                     }
		}
                   // std::cout <<endl; //JUMP LINE
                    int cimmatrow=pow(nbvariable, 2);
                    mat JIM;string varall;string fjimmatname;string fulljimtrixname;
                    fjimmatname="Q_JIM";
                    fulljimtrixname="Resources/Result/"+ fjimmatname +".txt";
                    JIM.resize(cimmatrow,cimmatrow);
                    int matvalueeachsize=matvalueeach.size();

 /*                    cout<<"Vector containing all the elements of the CIM matrixes:"<<endl;
                     for (int n=0;n<matvalueeachsize;n++){
                     cout<<matvalueeach[n]<<"; ";
                     }*/

                    //Convert String vector to int vector
                    intVector convstr2intstate;
                    for (int i=0;i<mapdesvarstring.size();i++){
                    convstr2intstate.push_back(atoi(varok1state[i].c_str()));
                    }
                    //Produit cartésien des états des variables ---  line=Prod*card(Var) orπ*card(Var)
                    //ostream_iterator< int > output( cout, " " );
                    //cout << "Contenu du vecteur: ";
                    //copy( convstr2intstate.begin(), convstr2intstate.end(), output );
                    //cout<<"Produit Cartésien des variables:π*card(Var): "<< accumulate (convstr2intstate.begin(), convstr2intstate.end(),1,multiplies<int>())<< endl;
                    int lineprodcarte=accumulate (convstr2intstate.begin(), convstr2intstate.end(),1,multiplies<int>());
                    //cout <<lineprodcarte <<endl;

                    int offsetoupas;
                    for (int i=0;i<mapdesvarstring.size();i++)
                    {
                       offsetoupas=offsetorpas[i];
                       //cout<<"--Offset or Weight of Variable: "<<varok1.at(i)<<" is:"<<offsetoupas <<" with state: "<<varok1state[i]<<endl;

                    }
                     cout<<"***************************************************"<<endl;
                    int offsetpassize=offsetorpas.size();
                    for (int init=0;init<offsetpassize;init++)
                    {
                        offsetorpas[init];
                    }

                   ///Initializing some values for the JIM
                    //WORKFORALL MATRIX
                    JIM.at(0,1)=matvalueeach[1];//1 ->1 ->0,1
                    JIM.at(1,0)=matvalueeach[2];//1 ->1 ->1,0

                     int newvaluerow;
                     int newvaluecol;
                     int nullvalue=0;
                     for (int l=0;l<cimmatrow;l++){
                     int newrowcolsize=sqrt(cimmatrow);

                     //Starting distribution of CIM value to JIM
                     for (int at=1;at<newrowcolsize;at++){

                         int normalsize=my_map.size();
                         int nonnormalsize=mapdesvarstring.size();
                         int squaredenormal=sqrt(normalsize);
                         if(normalsize%2!=0){
                            newvaluerow=JIM.at(at+1,0)=matvalueeach[matvalueeachsize-6];//10->8 ->2,0
                            newvaluecol=JIM.at(at+1,l)=matvalueeach[matvalueeachsize-11];//5 ->11 ->2,3
                            newvaluecol=JIM.at(at+2,at)=matvalueeach[matvalueeachsize-2];//14->13->3,1
                            newvaluecol=JIM.at(at+2,at+1)=matvalueeach[matvalueeachsize-10];//6 ->14->3,2
                         }
                         else
                         {
                            newvaluerow=JIM.at(at,0)=matvalueeach[matvalueeachsize-14];//2 ->4 ->1,0
                            newvaluerow=JIM.at(at+1,0)=matvalueeach[matvalueeachsize-6];//10->8 ->2,0
                            newvaluecol=JIM.at(at+1,l)=matvalueeach[matvalueeachsize-11];//5 ->11 ->2,3
                            newvaluecol=JIM.at(at+2,at)=matvalueeach[matvalueeachsize-2];//14->13->3,1
                            newvaluecol=JIM.at(at+2,at+1)=matvalueeach[matvalueeachsize-10];//6 ->14->3,2
                        }
                     }
                        //cout<<"**************************************"<<endl;
                        for (int c=0;c<cimmatrow;c++){
                            for (int at=1;at<newrowcolsize;at++){
                                newvaluerow=JIM.at(0,at)=matvalueeach[at];//1 ->1 ->0,1
                                newvaluerow=JIM.at(0,at+1)=matvalueeach[matvalueeachsize-7];//9  ->2 ->0,2
                                newvaluecol=JIM.at(at,c+1)=matvalueeach[matvalueeachsize-3];//13 ->7 ->1,3
                            }
                        }
                    }
                        //getting anti or opposite of the main diagonal of the JIM matrix
                        vec newoppdiagonalval= diagvec(fliplr(JIM));
                        vec newoppdiagonalval1= newoppdiagonalval.zeros();
                        mat newJIM;
                        mat revnewJIM;
                        newJIM=fliplr(JIM);
                        newJIM.diag().zeros();
                        revnewJIM=fliplr(newJIM);
                        //revnewJIM.print("JIM after oppo diagonal values set to 0");
                        //newoppdiagonalval.print("newoppdiagonalval");
                        JIM=revnewJIM;

                        //calculate -end diagonal value
                        // Write the diagonal of the matrix when i = j
                        for (int i=0;i<cimmatrow;i++)
                        {
                            double sumij=0;
                            //display table
                            for (int j=0;j<cimmatrow;j++)
                                if (i != j){
                                    sumij=sumij+ JIM(i,j);

                            }
                            //set all JIM(rows,rows) equal to sumij
                           JIM(i,i)= -sumij;
                        }

                        JIM.print(fulljimtrixname);
			JIM.save(fulljimtrixname,csv_ascii);
                        std::cout <<endl; //JUMP LINE
                        cout<<"***************************************************"<<endl;

 /*****************************************************************************************************************************************************/
/////STARTING MARGINALIZATION OF JIM
    //Splitting distribution contents for marginalization from file into differents vectors
    char delimiterdistro='*';
    StringVector distrovar=splitdistrovariable(test, delimiterdistro);
    StringVector distrovarparents=splitdistroparents(test, delimiterdistro);
    StringVector distroval=splitdistrovalues(test, delimiterdistro);
    //Getting sise of the different vectors
    size_t distrovarsize=distrovar.size();//size of the vector 'distrovar'
    size_t distrovarparsize=distrovarparents.size();//size of the vector 'distrovarparents'
    size_t distrovalsize=distroval.size();//size of the vector 'distroval'

    //Printing their content

/*  for(auto i :distrovar)      cout<<"VALUE OF distrovariables: "<<i <<endl;
    for(auto j :distrovarparents)      cout<<"VALUE OF distrovarparents: "<<j <<endl;
    for(auto k :distroval)      cout<<"VALUE OF distrovalues: "<<k <<endl;
 */

   //STATING CREATING Given Distribution matrices files AND USING THEIR VALUES TO get another MATRIX and marg...
    //Initialization and creation of the required variables
    int nbdistribution=distrovarsize;int distrorows=1;int distrocols=2;
    doubleVector distrovalueeach;
    std::string indexdistro;
    std::string distrovalue;
    string fdistroname;
    rowvec distro;

    //starting of loopfor the creation
    for (int i=0;i<nbdistribution;i++)
    {
        //cout<<matrixvalues[i]<<endl;
    indexdistro=int2strconvertor(i);
    fdistroname=distrovar.at(i)+"|"+distrovarparents.at(i);
    string fulltrixfilename="Resources/Matrices/Distribution/"+ fdistroname +".txt";
    distro.resize(distrorows,distrocols); // change the size of matrixname atomatically(data is not preserved)
    //matrixname.zeros();//set all elements to Zeros
    distro=distroval[i];
    distro.save(fulltrixfilename,csv_ascii);
    distro.print(fulltrixfilename);
    std::cout <<endl; //JUMP LINE
/*  cout << fdistroname<<".n_rows: " << distro.n_rows << endl;  // .n_rows and .n_cols are read only
    cout << fdistroname<< ".n_cols: " << distro.n_cols << endl;

    //get each matrix value and print them
    cout<<fdistroname<<endl;
 */
    //Getting value of each distribution depending of the varaible
    int nbdelements=distro.size();
    //printing and stroring into vector those distribution
    for(int i=0;i<=nbdelements-1;i++) {
        indexdistro=int2strconvertor(i);
//        distrovalue="val "+indexdistro;
//        cout<< distrovalue<<": "<<distro.at(i)<<endl;
        distrovalueeach.push_back(distro.at(i));
        }
    }

///This part need to be modify in order to have an automatic assigment of the values of the different distro
    ///creating row vectors for the different distrovalues
    rowvec p01,p02,p03;
    //
    p01=distroval.at(0);
    p02=distroval.at(1);
    p03=distroval.at(2);

    //Concatening or joining distributions withparents"
    mat distoprodcombi=join_horiz(p02.t(),p03.t());
    distoprodcombi.t();//transpose of...
    distoprodcombi.print("Concatenate distributions with parents\n");
    distoprodcombi.each_row() %= p01;   // p01 distri without parents
    mat P0avt=distoprodcombi;
    P0avt.print("\nMatrix-before colonns elements sum\n");
    mat p0 = sum(distoprodcombi,1);
    mat recupP0avt=P0avt;
    //recupP0zavt.print("P0zavt recup");
    p0.print("\nMatrix-after colonns elements sum\n");

    //dividing each row element by the appropraite row element of p0
    size_t divr=0;//initializing divr
    while( divr<p0.size()){
         (P0avt.rows(divr,divr) ) /=p0[divr];
         divr++;
    }
    mat res=P0avt ;
    //res.print("\n res\n");
    cout<<"\n**************************************"<<endl;
  ///Get necessary values from CIM of JIM for margin calculation
    //Calcul of differents values to put in each row using operator * to perform element multiplication,
    res.at(0,0) *=matvalueeach[5];//3 //
    res.at(0,1) *=matvalueeach[9];//5
    res.at(1,0) *=matvalueeach[6];//15
    res.at(1,1) *=matvalueeach[10];//4
    //addition each elemt of a row an put it in a matrix of sise 2*1 of name res
    res = sum(res,1);
    //Concatening or joining distributions res to obtain expected result
    res=join_horiz(res,res);

    //
    mat marg=res;
    int P0varrowcol=distro.size();
    string combidistromatname;string fullcombidistroname;
    combidistromatname="Marg P0";
    fullcombidistroname="Resources/Result/"+ combidistromatname +".txt";
    marg.resize(P0varrowcol,P0varrowcol);

    // Write the diagonal of the matrix when i = j
    for (int i=0;i<P0varrowcol;i++)
    {
        double sumij=0;
        double sumji=0;
        //display table
        for (int j=0;j<P0varrowcol;j++)
            if (i != j){
                sumij=sumij+ marg(i,j);
                sumji=sumij+ marg(j,i);
        }
        //set all JIM(rows,rows) equal to sumij
       marg(i,i)= -sumij;

    }
    //DISPLAYING MARGINALIZATION VALUE
    //marg.print(fullcombidistroname);
    cout<<"Result of the Marginalization is: "<<endl<<endl<<combidistromatname<<":"<<endl;
    cout<<marg<<endl;
/*    JIM.save(fullcombidistroname,csv_ascii);
    cout << combidistromatname<<".n_rows: " << marg.n_rows << endl;  // .n_rows and .n_cols are read only
    cout << combidistromatname<< ".n_cols: "<< marg.n_cols << endl;
    cout<< combidistromatname<<" size:"<<marg.size()<<endl;
    std::cout <<endl; //JUMP LINE*/
    cout<<"**************************************"<<endl;

    std::cout <<endl; //JUMP LINE
     return 0;
}
//END OF MAIN===================================================================================================================================================


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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_MATH_FFT_BSLBACKEND_HPP
  #define BOOST_MATH_FFT_BSLBACKEND_HPP

  #include <algorithm>
  #include <cmath>
  #include <type_traits>
  #include <boost/math/fft/multiprecision_complex.hpp>

  #include <boost/math/fft/algorithms.hpp>
  #include <boost/math/fft/real_algorithms.hpp>
  #include <boost/math/fft/dft_api.hpp>

  namespace boost { namespace math {  namespace fft {

  namespace detail {

  /*
    Boost DFT backend:
    It handles RingTypes and it calls the appropriate specialized functions if
    the type is complex.

    A type is considered "complex" if is_boost_complex::value == true. A user-defined
    type can become complex by specializing that trait.

    We have specialized algorithms for complex numbers and general purpose DFT
    that need the specification of a root of unity.
    The general purpose DFT work with complex and non-complex types, but its
    performance and precision could be lower than the specialized complex
    versions.

    This interface selects general purpose DFT for non-complex types,
    and for complex types the default behaviour is to use the specialized
    complex algorithms, unless the user provides a root of unity 'W', in which case
    the interface will execute general purpose DFT using W.
  */
  template<class RingType, class allocator_t = std::allocator<RingType> >
  class bsl_backend
  {
  public:
    using value_type     = RingType;
    using allocator_type = allocator_t;

  private:
    enum plan_type { forward_plan , backward_plan};

    void execute(plan_type plan, const RingType * in, RingType* out)const
    {
      const long N = static_cast<long>(size());
      const int sign = (plan == forward_plan ? 1 : -1);

      // select the implementation according to the DFT size
      switch(N)
      {
        case 0:
          return;
        case 1:
          out[0]=in[0];
          return;
        case 2:
          detail::complex_dft_2(in,out,sign);
          return;
      }

      if( detail::is_power2(N) )
      {
        detail::complex_dft_power2(in,in+N,out,sign);
      }
      else if(detail::is_prime(N))
      {
        // detail::complex_dft_prime_bruteForce(in,in+N,out,sign);
        detail::complex_dft_prime_rader(in,in+N,out,sign,alloc);
      }
      else
      {
        detail::complex_dft_composite(in,in+N,out,sign,alloc);
      }
    }

  public:

    // the provided root of unity is used instead of exp(-i 2 pi/n)
    constexpr bsl_backend(std::size_t n, const allocator_type& in_alloc = allocator_type{}):
        alloc{in_alloc},
        my_size{n}
    {
    }

    ~bsl_backend()
    {
    }

    void resize(std::size_t new_size)
    {
      my_size = new_size;
    }
    RingType inverse_root(RingType root) const
    {
      return detail::power(root,size()-1);
    }

    constexpr std::size_t size() const { return my_size; }

    void forward(const RingType* in, RingType* out) const
    {
      execute(forward_plan,in,out);
    }

    void backward(const RingType* in, RingType* out) const
    {
      execute(backward_plan,in,out);
    }
    void dft(const RingType* in, RingType* out, RingType w) const
    {
      const long N = static_cast<long>(size());
      // select the implementation according to the DFT size
      if( detail::is_power2(N))
      {
        detail::dft_power2(in,in+N,out,w);
      }
      else
      {
        detail::dft_composite(in,in+N,out,w,alloc);
      }
    }

  private:
    allocator_type alloc;
    std::size_t my_size{};
  };

  template<class T, class allocator_t = std::allocator<T> >
  class bsl_rfft_backend
  {
  public:
    using value_type     = T;
    using allocator_type = allocator_t;

    // the provided root of unity is used instead of exp(-i 2 pi/n)
    constexpr bsl_rfft_backend(std::size_t n, const allocator_type& in_alloc = allocator_type{}):
        alloc{in_alloc},
        my_size{n}
    {
    }

    ~bsl_rfft_backend()
    {
    }

    void resize(std::size_t new_size)
    {
      my_size = new_size;
    }

    constexpr std::size_t size() const { return my_size; }
    constexpr std::size_t unique_complex_size() const { return my_size/2 + 1;}

    void real_to_halfcomplex(const value_type* in, value_type* out) const
    {
      const long N = static_cast<long>(size());
      // select the implementation according to the DFT size
      switch(N)
      {
        case 0:
          return;
        case 1:
          out[0]=in[0];
          return;
        case 2:
          detail::real_dft_2(in,out,1);
          return;
      }
      if( detail::is_power2(N))
      {
        detail::real_dft_power2(in,in+N,out,1);
      }else
      {
        detail::real_dft_composite(in,in+N,out,1,alloc);
      }
      //if(detail::is_prime(N))
      //{
      //  detail::real_dft_prime_rader(in,in+N,out,sign,alloc);
      //}
    }
    void halfcomplex_to_real(const value_type* in, value_type* out) const
    {
      const long N = static_cast<long>(size());
      // select the implementation according to the DFT size
      switch(N)
      {
        case 0:
          return;
        case 1:
          out[0]=in[0];
          return;
        case 2:
          detail::real_dft_2(in,out,1);
          return;
      }
      if( detail::is_power2(N))
      {
        detail::real_inverse_dft_power2(in,in+N,out,1);
      }else
      {
        detail::real_inverse_dft_composite(in,in+N,out,1,alloc);
      }
      //if(detail::is_prime(N))
      //{
      //  detail::real_inverse_dft_prime_rader(in,in+N,out,sign,alloc);
      //}
    }

  private:
    allocator_type alloc;
    std::size_t my_size{};
  };

  } // namespace detail

  template<class RingType = std::complex<double>, class Allocator_t = std::allocator<RingType> >
  using bsl_dft = detail::complex_dft<detail::bsl_backend,RingType,Allocator_t>;

  template<class T = double, class Allocator_t = std::allocator<T> >
  using bsl_rdft = detail::real_dft<detail::bsl_rfft_backend,T,Allocator_t>;

  template<class RingType = std::complex<double>, class Allocator_t = std::allocator<RingType> >
  using bsl_algebraic_dft = detail::algebraic_dft<detail::bsl_backend,RingType,Allocator_t>;

  using bsl_transform = transform< bsl_dft<> >;
  using bsl_algebraic_transform = transform< bsl_algebraic_dft<> >;
  using bsl_real_transform = transform< bsl_rdft<> >;

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_FFT_BSLBACKEND_HPP


//Copyright(c) 2016 Shuda Li[lishuda1980@gmail.com]
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files(the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions :
//
//The above copyright notice and this permission notice shall be included in
//all copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
//FOR A PARTICULAR PURPOSE AND NON - INFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR
//COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.
//

#ifndef BTL_Eigen_UTILITY_HEADER
#define BTL_Eigen_UTILITY_HEADER

//eigen-based helpers
#include "OtherUtil.hpp"
#include <Eigen/Dense>
#include <se3.hpp>
#include <opencv2/core.hpp>

namespace btl
{
namespace utility
{

using namespace Eigen;
using namespace Sophus;
using namespace btl::utility;

template< class T >
Eigen::Matrix< T, 4, 4 > inver(const Eigen::Matrix< T, 4, 4 >& F_ctw_){
	using namespace Eigen;
	Matrix< T, 4, 4 > F_wtc; F_wtc.setIdentity();

	Matrix< T, 3, 3 > R_trans = F_ctw_.block(0,0,3,3);
	Matrix< T, 3, 1 > Cw = F_ctw_.block(0,3,3,1);

	F_wtc.block(0,0,3,3) = R_trans.transpose();
	F_wtc.block(0,3,3,1) = -R_trans*Cw;
	return F_wtc;
}

template< class T >
void getCwVwFromPrj_Cam2World(const Eigen::Matrix< T, 4, 4 >&  Prj_ctw_,Eigen::Matrix< T, 3, 1 >* pCw_,Eigen::Matrix< T, 3, 1 >* pVw_){
	*pCw_ = Prj_ctw_.template block<3,1>(0,3); //4th column is camera centre
	*pVw_ = Prj_ctw_.template block<3,1>(0,0); //1st column is viewing direction
}

template< class T >
void getRTCVfromModelViewGL ( const Eigen::Matrix< T, 4, 4 >&  mMat_, Eigen::Matrix< T, 3, 3 >* pmR_, Eigen::Matrix< T, 3, 1 >* pvT_ )
{
    (* pmR_) ( 0, 0 ) =  mMat_ ( 0, 0 );   (* pmR_) ( 0, 1 ) =   mMat_ ( 0, 1 );  (* pmR_) ( 0, 2 ) = mMat_ ( 0, 2 );
    (* pmR_) ( 1, 0 ) = -mMat_ ( 1, 0 );   (* pmR_) ( 1, 1 ) = - mMat_ ( 1, 1 );  (* pmR_) ( 1, 2 ) = -mMat_ ( 1, 2 );
    (* pmR_) ( 2, 0 ) = -mMat_ ( 2, 0 );   (* pmR_) ( 2, 1 ) = - mMat_ ( 2, 1 );  (* pmR_) ( 2, 2 ) = -mMat_ ( 2, 2 );

	(*pvT_) ( 0 ) = mMat_ ( 0, 3 );
    (*pvT_) ( 1 ) = -mMat_ ( 1, 3 );
    (*pvT_) ( 2 ) = -mMat_ ( 2, 3 );

    return;
}

template< class T >
Eigen::Matrix< T, 4, 4 > setModelViewGLfromPrj(const Eigen::Transform<T, 3, Eigen::Affine> & Prj_)
{
	// column first for pGLMat_[16];
	// row first for Matrix3d;
	// pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
	// pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
	// pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
	// pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

	Eigen::Matrix< T , 4, 4 > mMat;
	mMat.row( 0 ) =  Prj_.matrix().row( 0 );
	mMat.row( 1 ) = -Prj_.matrix().row( 1 );
	mMat.row( 2 ) = -Prj_.matrix().row( 2 );
	mMat.row( 3 ) =  Prj_.matrix().row( 3 );

	return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRTCV ( const SO3Group<T>& mR_, const Eigen::Matrix< T, 3, 1 >& vT_ )
{
    // column first for pGLMat_[16];
    // row first for Matrix3d;
    // pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
    // pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
    // pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
    // pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

    Eigen::Matrix< T , 4, 4 > mMat;
    mMat ( 0, 0 ) =  mR_.matrix() ( 0, 0 ); mMat ( 0, 1 ) =  mR_.matrix() ( 0, 1 ); mMat ( 0, 2 ) =  mR_.matrix() ( 0, 2 ); mMat ( 0, 3 ) =  vT_ ( 0 );
    mMat ( 1, 0 ) = -mR_.matrix() ( 1, 0 ); mMat ( 1, 1 ) = -mR_.matrix() ( 1, 1 ); mMat ( 1, 2 ) = -mR_.matrix() ( 1, 2 ); mMat ( 1, 3 ) = -vT_ ( 1 );
    mMat ( 2, 0 ) = -mR_.matrix() ( 2, 0 ); mMat ( 2, 1 ) = -mR_.matrix() ( 2, 1 ); mMat ( 2, 2 ) = -mR_.matrix() ( 2, 2 ); mMat ( 2, 3 ) = -vT_ ( 2 );
    mMat ( 3, 0 ) =  0;            mMat ( 3, 1 ) =  0;            mMat ( 3, 2 ) =  0;            mMat ( 3, 3 ) =  1;

    return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRCCV ( const Eigen::Matrix< T, 3, 3 >& mR_, const Eigen::Matrix< T, 3, 1 >& vC_ )
{
	Eigen::Matrix< T, 3,1> eivT = -mR_.transpose()*vC_;
	return setModelViewGLfromRTCV(mR_,vC_);
}

template< class T1, class T2 >
void unprojectCamera2World ( const int& nX_, const int& nY_, const unsigned short& nD_, const Eigen::Matrix< T1, 3, 3 >& mK_, Eigen::Matrix< T2, 3, 1 >* pVec_ )
{
	//the pixel coordinate is defined w.r.t. opencv camera reference, which is defined as x-right, y-downward and z-forward. It's
	//a right hand system.
	//when rendering the point using opengl's camera reference which is defined as x-right, y-upward and z-backward. the
	//	glVertex3d ( Pt(0), -Pt(1), -Pt(2) );
	if ( nD_ > 400 ) {
		T2 dZ = nD_ / 1000.; //convert to meter
		T2 dX = ( nX_ - mK_ ( 0, 2 ) ) / mK_ ( 0, 0 ) * dZ;
		T2 dY = ( nY_ - mK_ ( 1, 2 ) ) / mK_ ( 1, 1 ) * dZ;
		( *pVec_ ) << dX + 0.0025, dY, dZ + 0.00499814; // the value is esimated using CCalibrateKinectExtrinsics::calibDepth()
		// 0.0025 by experience.
	}
	else {
		( *pVec_ ) << 0, 0, 0;
	}
}

template< class T >
void projectWorld2Camera ( const Eigen::Matrix< T, 3, 1 >& vPt_, const Eigen::Matrix3d& mK_, Eigen::Matrix< short, 2, 1>* pVec_  )
{
	// this is much faster than the function
	// eiv2DPt = mK * vPt; eiv2DPt /= eiv2DPt(2);
	( *pVec_ ) ( 0 ) = short ( mK_ ( 0, 0 ) * vPt_ ( 0 ) / vPt_ ( 2 ) + mK_ ( 0, 2 ) + 0.5 );
	( *pVec_ ) ( 1 ) = short ( mK_ ( 1, 1 ) * vPt_ ( 1 ) / vPt_ ( 2 ) + mK_ ( 1, 2 ) + 0.5 );
}

template< class T >
void convertPrj2Rnt(const Eigen::Transform< T, 3, Eigen::Affine >& Prj_, SO3Group< T >* pR_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_ = SO3Group<T>(Prj_.linear());
	*pT_ = Prj_.translation();
	return;
}
template< class T >
Eigen::Transform< T, 3, Eigen::Affine > convertRnt2Prj(const SO3Group< T >& R_, const Eigen::Matrix< T, 3, 1 >& T_)
{
	Eigen::Transform< T, 3, Eigen::Affine > prj;
	prj.setIdentity();
	prj.linear() = R_.matrix();
	prj.translation() = T_;
	return prj;
}
template< class T >
void convertPrjInv2RpnC( const Eigen::Matrix< T, 4, 4 >& Prj_, Eigen::Matrix< T, 3, 3 >* pR_trans_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_trans_ = Prj_.template block<3,3>(0,0);
	*pT_ = Prj_.template block<3,1>(0,3);
	return;
}
template< class T >
Eigen::Matrix< T, 4, 4 > convertRpnC2PrjInv(  const Eigen::Matrix< T, 3, 3 >& R_trans_, const Eigen::Matrix< T, 3, 1 >& C_ )
{
	Eigen::Matrix< T, 4, 4 > prj;
	prj.setIdentity();
	prj.template block<3,3>(0,0) = R_trans_;
	prj.template block<3,1>(0,3) = C_;
	return prj;
}

template< class T, int ROW, int COL >
T matNormL1 ( const Eigen::Matrix< T, ROW, COL >& eimMat1_, const Eigen::Matrix< T, ROW, COL >& eimMat2_ )
{
	Eigen::Matrix< T, ROW, COL > eimTmp = eimMat1_ - eimMat2_;
	Eigen::Matrix< T, ROW, COL > eimAbs = eimTmp.cwiseAbs();
	return (T) eimAbs.sum();
}

template< class T >
void setSkew( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimMat_){
	*peimMat_ << 0, -z_, y_, z_, 0, -x_, -y_, x_, 0 ;
}

template< class T >
void setRotMatrixUsingExponentialMap( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimR_ ){
	//http://opencv.itseez.com/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=rodrigues#void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian)
	T theta = sqrt( x_*x_ + y_*y_ + z_*z_ );
	if(	theta < std::numeric_limits<T>::epsilon() ){
		*peimR_ = Eigen::Matrix< T, 3,3 >::Identity();
		return;
	}
	T sinTheta = sin(theta);
	T cosTheta = cos(theta);
	Eigen::Matrix< T, 3,3 > eimSkew;
	setSkew< T >(x_/theta,y_/theta,z_/theta,&eimSkew);
	*peimR_ = Eigen::Matrix< T, 3,3 >::Identity() + eimSkew*sinTheta + eimSkew*eimSkew*(1-cosTheta);
}

}//utility
}//btl
#endif


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_HPP
#define ODE_HPP

// ODE

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/make-candidate.hpp>
#include <kv/psa.hpp>
#include <kv/ode-param.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

#ifndef ODE_STEP_COMPONENT
#define ODE_STEP_COMPONENT 0
#endif

#ifndef ODE_RESTART_RATIO
#define ODE_RESTART_RATIO 1
#endif

#ifndef ODE_COEF_MID
#define ODE_CORF_MID 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
ode(F f, ub::vector< interval<T> >& init, const interval<T>& start, interval<T>& end, const ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL) {
	int n = init.size();
	int i, j;

	ub::vector< psa< interval<T> > > x, y;
	psa< interval<T> > torg;
	psa< interval<T> > t;

	ub::vector< psa< interval<T> > > z, w;

	psa< interval<T> > temp;
	T m;
	ub::vector<T> newton_step;

	bool flag, resized;

	interval<T> deltat;
	ub::vector< interval<T> > result;

	T radius, radius_tmp;

	#if ODE_STEP_COMPONENT == 1
	ub::vector<T> tolerance(n);
	#else
	T tolerance;
	#endif

	int n_rad;

	#if ODE_RESTART_RATIO == 1
	T max_ratio;
	#endif

	int ret_val;
	interval<T> end2;
	int restart;

	bool save_mode, save_uh, save_rh;

	#if ODE_STEP_COMPONENT == 1
	for (i=0; i<n; i++) {
		tolerance(i) = std::max(T(1.), norm(init(i))) * p.epsilon;
	}
	#else
	m = 1.;
	for (i=0; i<n; i++) {
		m = std::max(m, norm(init(i)));
	}
	tolerance = m * p.epsilon;
	#endif

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< interval<T> >::mode();
	save_uh = psa< interval<T> >::use_history();
	save_rh = psa< interval<T> >::record_history();
	psa< interval<T> >::mode() = 1;
	psa< interval<T> >::use_history() = false;
	psa< interval<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< interval<T> >::record_history() = true;
	psa< interval<T> >::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< interval<T> >::use_history() = true;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			// set order preparing for constant function
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		// use two non-zero coefficients of higher order term
		#if ODE_STEP_COMPONENT == 1
		radius = std::numeric_limits<T>::infinity();
		for (i=0; i<n; i++) {
			radius_tmp = 0.;
			n_rad = 0;
			for (j = p.order; j>=1; j--) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = abs(mid(x(i).v(j)));
				#else
				m = norm(x(i).v(j));
				#endif
				if (m == 0.) continue;
				radius_tmp = std::max(radius_tmp, (T)std::pow((double)m, 1./j));
				n_rad++;
				if (n_rad == 2) break;
			}
			radius = std::min(radius, std::pow((double)(tolerance(i)), 1./p.order) / radius_tmp);
		}
		#else // ODE_STEP_COMPONENT
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = std::max(m, abs(mid(x(i).v(j))));
				#else
				m = std::max(m, norm(x(i).v(j)));
				#endif
			}
			if (m == 0.) continue;
			radius = std::max(radius, (T)std::pow((double)m, 1./j));
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
		#endif // ODE_STEP_COMPONENT
	}

	psa< interval<T> >::mode() = 2;

	restart = 0;
	resized = false;

	while (true) {
		if (p.autostep) {
			end2 = mid(start + radius);
			if (end2 >= end.lower()) {
				end2 = end;
				radius = mid(end2 - start);
				ret_val = 2;
			} else {
				ret_val = 1;
			}
		} else {
			end2 = end;
			ret_val = 2;
		}
		deltat = end2 - start;

		psa< interval<T> >::domain() = interval<T>(0., deltat.upper());

		z = x;
		t = setorder(torg, p.order);

		try {
			w = f(z, t);
		}
		catch (std::domain_error& e) {
			if (p.autostep && restart < p.restart_max) {
				psa< interval<T> >::use_history() = false;
				if (p.verbose == 1) {
					std::cout << "ode: radius changed: " << radius;
				}
				radius *= 0.5;
				if (p.verbose == 1) {
					std::cout << " -> " << radius << "\n";
				}
				restart++;
				continue;
			} else {
				throw std::domain_error("ode: evaluation error");
			}
		}

		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		newton_step.resize(n);
		for (i=0; i<n; i++) {
			newton_step(i) = norm(w(i).v(p.order) - z(i).v(p.order));
		}
		make_candidate(newton_step);
		for (i=0; i<n; i++) {
			z(i).v(p.order) += newton_step(i) * interval<T>(-1., 1.);
		}

		if (p.autostep && ret_val != 2 && resized == false) {
			resized = true;
			m = (std::numeric_limits<T>::min)();
			#if ODE_STEP_COMPONENT == 1
			for (i=0; i<n; i++) {
				m = std::max(m, (rad(eval(z(i), deltat)) - rad(init(i))) / tolerance(i));
			}
			#else
			for (i=0; i<n; i++) {
				m = std::max(m, rad(eval(z(i), deltat)) - rad(init(i)));
			}
			m = m / tolerance;
			#endif
			radius_tmp = radius / std::pow((double)m, 1. / p.order);
			if (radius_tmp >= radius && restart > 0) {
				// do nothing, not continue
			} else {
				radius = radius_tmp;
				continue;
			}
		}

		w = f(z, t);
		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		flag = true;
		#if ODE_RESTART_RATIO == 1
		max_ratio = 0.;
		#endif
		for (i=0; i<n; i++) {
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(w(i).v(p.order)) / width(z(i).v(p.order)));
			#endif
			flag = flag && subset(w(i).v(p.order), z(i).v(p.order));
		}
		if (flag) break;

		if (!p.autostep || restart >= p.restart_max) {
			ret_val = 0;
			break;
		}
		if (p.verbose == 1) {
			std::cout << "ode: radius changed: " << radius;
		}
		#if ODE_RESTART_RATIO == 1
		radius *= std::max(std::min((T)0.5, (T)0.5 / max_ratio), (T)0.125);
		#else
		radius *= 0.5;
		#endif
		if (p.verbose == 1) {
			std::cout << " -> " << radius << "\n";
		}
		restart++;
	}

	if (ret_val != 0) {
		for (j=0; j<p.iteration; j++) {
			z = w;
			w = f(z, t);
			for (i=0; i<n; i++) {
				temp = integrate(w(i));
				w(i) = setorder(temp, p.order);
			}
			w = init + w;
			for (i=0; i<n; i++) {
				w(i).v(p.order) = intersect(w(i).v(p.order), z(i).v(p.order));
			}
		}

		result.resize(n);
		for (i=0; i<n; i++) {
			result(i) = eval(w(i), deltat);
		}

		init = result;
		if (ret_val == 1) end = end2;
		if (result_psa != NULL) *result_psa = w;
	}

	psa< interval<T> >::mode() = save_mode;
	psa< interval<T> >::use_history() = save_uh;
	psa< interval<T> >::record_history() = save_rh;

	return ret_val;
}

template <class T, class F>
int
odelong(F f, ub::vector< interval<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>()) {

	ub::vector< interval<T> > x;
	interval<T> t, t1;
	int r;
	int ret_val = 0;

	x = init;
	t = start;
	p.set_autostep(true);
	while (1) {
		t1 = end;

		r = ode(f, x, t, t1, p);
		if (r == 0) {
			if (ret_val == 1) {
				init = x;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x << "\n";
		}
		if (r == 2) {
			init = x;
			return 2;
		}
		t = t1;
	}
}

} // namespace kv

#endif // ODE_HPP