// This file is distributed under the MIT license.
// See the LICENSE file for details.

#include <cstddef>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include <visionaray/math/serialization.h>

#include <gtest/gtest.h>

using namespace visionaray;

TEST(Serialization, AABB)
{
    // use as input
    basic_aabb<int>    dsti;
    basic_aabb<float>  dstf;
    basic_aabb<double> dstd;

    // test some invalid boxes
    basic_aabb<int>    srci;
    basic_aabb<float>  srcf;
    basic_aabb<double> srcd;

    srci.invalidate();
    srcf.invalidate();
    srcd.invalidate();

    std::stringstream sstream;
    boost::archive::text_oarchive oa(sstream);

    oa << srci;
    oa << srcf;
    oa << srcd;

    boost::archive::text_iarchive ia(sstream);

    ia >> dsti;
    ia >> dstf;
    ia >> dstd;

    ASSERT_EQ(dsti.min, srci.min);
    ASSERT_EQ(dsti.max, srci.max);
    EXPECT_FLOAT_EQ(dstf.min.x, srcf.min.x);
    EXPECT_FLOAT_EQ(dstf.min.y, srcf.min.y);
    EXPECT_FLOAT_EQ(dstf.min.z, srcf.min.z);
    EXPECT_FLOAT_EQ(dstf.max.x, srcf.max.x);
    EXPECT_FLOAT_EQ(dstf.max.y, srcf.max.y);
    EXPECT_FLOAT_EQ(dstf.max.z, srcf.max.z);
    EXPECT_DOUBLE_EQ(dstd.min.x, srcd.min.x);
    EXPECT_DOUBLE_EQ(dstd.min.y, srcd.min.y);
    EXPECT_DOUBLE_EQ(dstd.min.z, srcd.min.z);
    EXPECT_DOUBLE_EQ(dstd.max.x, srcd.max.x);
    EXPECT_DOUBLE_EQ(dstd.max.y, srcd.max.y);
    EXPECT_DOUBLE_EQ(dstd.max.z, srcd.max.z);
}

TEST(Serialization, Matrix)
{
    matrix<3, 3, float>  dst3f;
    matrix<3, 3, double> dst3d;
    matrix<4, 4, float>  dst4f;
    matrix<4, 4, double> dst4d;

    matrix<3, 3, float>  src3f(
            0.0f, 1.0f, 2.0f,
            3.0f, 4.0f, 5.0f,
            6.0f, 7.0f, 8.0f
            );
    matrix<3, 3, double> src3d(
            0.0, 1.0, 2.0,
            3.0, 4.0, 5.0,
            6.0, 7.0, 8.0
            );
    matrix<4, 4, float> src4f(
             0.0f,  1.0f,  2.0f,  3.0f,
             4.0f,  5.0f,  6.0f,  7.0f,
             8.0f,  9.0f, 10.0f, 11.0f,
            12.0f, 13.0f, 14.0f, 15.0f
            );
    matrix<4, 4, double> src4d(
             0.0,  1.0,  2.0,  3.0,
             4.0,  5.0,  6.0,  7.0,
             8.0,  9.0, 10.0, 11.0,
            12.0, 13.0, 14.0, 15.0
            );

    std::stringstream sstream;
    boost::archive::text_oarchive oa(sstream);

    oa << src3f;
    oa << src3d;
    oa << src4f;
    oa << src4d;

    boost::archive::text_iarchive ia(sstream);

    ia >> dst3f;
    ia >> dst3d;
    ia >> dst4f;
    ia >> dst4d;

    for (size_t i = 0; i < 4; ++i)
    {
        for (size_t j = 0; j < 4; ++j)
        {
            if (i < 3 && j < 3)
            {
                EXPECT_FLOAT_EQ(dst3f(i, j), src3f(i, j));
                EXPECT_DOUBLE_EQ(dst3d(i, j), src3d(i, j));
            }
            EXPECT_FLOAT_EQ(dst4f(i, j), src4f(i, j));
            EXPECT_DOUBLE_EQ(dst4d(i, j), src4d(i, j));
        }
    }
}

TEST(Serialization, RectangleXYWH)
{
    rectangle<xywh_layout<int>, int>       dsti;
    rectangle<xywh_layout<float>, float>   dstf;
    rectangle<xywh_layout<double>, double> dstd;

    rectangle<xywh_layout<int>, int>       srci(0, 0, 1024, 768);
    rectangle<xywh_layout<float>, float>   srcf(-3.14f, -314.15f, 6.28f, 628.31f);
    rectangle<xywh_layout<double>, double> srcd(700.0, 700.0, 0.0, 0.0);

    std::stringstream sstream;
    boost::archive::text_oarchive oa(sstream);

    oa << srci;
    oa << srcf;
    oa << srcd;

    boost::archive::text_iarchive ia(sstream);

    ia >> dsti;
    ia >> dstf;
    ia >> dstd;

    ASSERT_EQ(dsti.x, srci.x);
    ASSERT_EQ(dsti.y, srci.y);
    ASSERT_EQ(dsti.w, srci.w);
    ASSERT_EQ(dsti.h, srci.h);

    EXPECT_FLOAT_EQ(dstf.x, srcf.x);
    EXPECT_FLOAT_EQ(dstf.y, srcf.y);
    EXPECT_FLOAT_EQ(dstf.w, srcf.w);
    EXPECT_FLOAT_EQ(dstf.h, srcf.h);

    EXPECT_DOUBLE_EQ(dstd.x, srcd.x);
    EXPECT_DOUBLE_EQ(dstd.y, srcd.y);
    EXPECT_DOUBLE_EQ(dstd.w, srcd.w);
    EXPECT_DOUBLE_EQ(dstd.h, srcd.h);
}

TEST(Serialization, Vector)
{
    float f1[] = { 3.14f };
    float f2[] = { 3.14f, 3.15f };
    float f3[] = { 3.14f, 3.15f, 3.16f };
    float f4[] = { 3.14f, 3.15f, 3.16f, 3.17f };
    float f5[] = { 3.14f, 3.15f, 3.16f, 3.17f, 3.18f };
    float f6[] = { 3.14f, 3.15f, 3.16f, 3.17f, 3.18f, 3.19f };
    float f7[] = { 3.14f, 3.15f, 3.16f, 3.17f, 3.18f, 3.19f, 3.20f };

    vector<1, float> src1f(f1);
    vector<2, float> src2f(f2);
    vector<3, float> src3f(f3);
    vector<4, float> src4f(f4);
    vector<5, float> src5f(f5);
    vector<6, float> src6f(f6);
    vector<7, float> src7f(f7);

    vector<1, float> dst1f;
    vector<2, float> dst2f;
    vector<3, float> dst3f;
    vector<4, float> dst4f;
    vector<5, float> dst5f;
    vector<6, float> dst6f;
    vector<7, float> dst7f;

    std::stringstream sstream;
    boost::archive::text_oarchive oa(sstream);

    oa << src1f;
    oa << src2f;
    oa << src3f;
    oa << src4f;
    oa << src5f;
    oa << src6f;
    oa << src7f;

    boost::archive::text_iarchive ia(sstream);

    ia >> dst1f;
    ia >> dst2f;
    ia >> dst3f;
    ia >> dst4f;
    ia >> dst5f;
    ia >> dst6f;
    ia >> dst7f;

    EXPECT_FLOAT_EQ(dst1f[0], src1f[0]);

    EXPECT_FLOAT_EQ(dst2f[0], src2f[0]);
    EXPECT_FLOAT_EQ(dst2f[1], src2f[1]);

    EXPECT_FLOAT_EQ(dst3f[0], src3f[0]);
    EXPECT_FLOAT_EQ(dst3f[1], src3f[1]);
    EXPECT_FLOAT_EQ(dst3f[2], src3f[2]);

    EXPECT_FLOAT_EQ(dst4f[0], src4f[0]);
    EXPECT_FLOAT_EQ(dst4f[1], src4f[1]);
    EXPECT_FLOAT_EQ(dst4f[2], src4f[2]);
    EXPECT_FLOAT_EQ(dst4f[3], src4f[3]);

    EXPECT_FLOAT_EQ(dst5f[0], src5f[0]);
    EXPECT_FLOAT_EQ(dst5f[1], src5f[1]);
    EXPECT_FLOAT_EQ(dst5f[2], src5f[2]);
    EXPECT_FLOAT_EQ(dst5f[3], src5f[3]);
    EXPECT_FLOAT_EQ(dst5f[4], src5f[4]);

    EXPECT_FLOAT_EQ(dst6f[0], src6f[0]);
    EXPECT_FLOAT_EQ(dst6f[1], src6f[1]);
    EXPECT_FLOAT_EQ(dst6f[2], src6f[2]);
    EXPECT_FLOAT_EQ(dst6f[3], src6f[3]);
    EXPECT_FLOAT_EQ(dst6f[4], src6f[4]);
    EXPECT_FLOAT_EQ(dst6f[5], src6f[5]);

    EXPECT_FLOAT_EQ(dst7f[0], src7f[0]);
    EXPECT_FLOAT_EQ(dst7f[1], src7f[1]);
    EXPECT_FLOAT_EQ(dst7f[2], src7f[2]);
    EXPECT_FLOAT_EQ(dst7f[3], src7f[3]);
    EXPECT_FLOAT_EQ(dst7f[4], src7f[4]);
    EXPECT_FLOAT_EQ(dst7f[5], src7f[5]);
    EXPECT_FLOAT_EQ(dst7f[6], src7f[6]);
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_k1.hpp>
#include <eve/function/diff/cyl_bessel_k1.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_k1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_k1(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_k1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_k1 on wide"
        , eve::test::simd::ieee_reals
         , eve::test::generate( eve::test::randoms(0.0, 0.5)
                              , eve::test::randoms(0.5, 1.5)
                              , eve::test::randoms(1.5, 500.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_k1 =  [](auto x) { return eve::cyl_bessel_k1(x); };
#if defined(__cpp_lib_math_special_functions)
  auto std__cyl_bessel_k1 =  [](auto x)->v_t { return std::cyl_bessel_k(v_t(1), x); };
#else
  auto std__cyl_bessel_k1 =  [](auto x)->v_t { return boost::math::cyl_bessel_k(v_t(1), x); };
#endif
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::zero(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::inf(eve::as<T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k1(eve::zero(eve::as<T>())),  eve::inf(eve::as< T>()), 0);
  }
  TTS_IEEE_EQUAL(eve__cyl_bessel_k1(v_t(-1)), eve::nan(eve::as<v_t>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(500)), std__cyl_bessel_k1(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(10)), std__cyl_bessel_k1(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(5)),  std__cyl_bessel_k1(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(2)),  std__cyl_bessel_k1(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(1.5)),std__cyl_bessel_k1(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(0.5)),std__cyl_bessel_k1(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(0.05)),std__cyl_bessel_k1(v_t(0.05)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(1)),  std__cyl_bessel_k1(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(v_t(0)),  eve::inf(eve::as<v_t>()), 0.0);

  TTS_IEEE_EQUAL(eve__cyl_bessel_k1(T(-1)), eve::nan(eve::as<T>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(500)),  T(std__cyl_bessel_k1(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(10)) ,  T(std__cyl_bessel_k1( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(5))  ,  T(std__cyl_bessel_k1( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(2))  ,  T(std__cyl_bessel_k1( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(1.5)),  T(std__cyl_bessel_k1( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(0.5)),  T(std__cyl_bessel_k1( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(1))  ,  T(std__cyl_bessel_k1( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1( T(0))  , eve::inf(eve::as<T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_k1(a0), map(std__cyl_bessel_k1, a0), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k1(a1), map(std__cyl_bessel_k1, a1), 10.0);
  TTS_RELATIVE_EQUAL(eve__cyl_bessel_k1(a2), map(std__cyl_bessel_k1, a2), 0.001);
};

EVE_TEST( "Check behavior of cyl_bessel_k1 on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_k1 =  [](auto x) { return eve::diff(eve::cyl_bessel_k1)(x); };
  auto std__diff_bessel_k1 =  [](auto x)->v_t { return boost::math::cyl_bessel_k_prime(1, x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_k1(a0),   map(std__diff_bessel_k1, a0)   , 1.0e-3);

};


/*
 * dijkstra.cpp
 *
 * 	\brief     My dijkstra implementation for the fifth exercise
 *  \details   This class computes the longest shortest path for starting point 1.
 *  \author    Julia Baumbach
 *  \date      17.06.2017
 */

#include "dijkstra.h"
#include <climits>
#include <iostream>
#include <boost/heap/fibonacci_heap.hpp>
#include <exception>

using VisitedMap = std::vector<bool>;

/**
 * \struct compare_function
 * \brief defines how 2 dijkstra pairs should be compared at fibonacci-heap
 * \param edge1 first edge to compare
 * \param edge2 second edge to compare
 * \return true if the weight of edge2 is smaller than edge1's weight
 */
struct compare_function{
	bool operator()(const DijkstraPair edge1, const DijkstraPair edge2) const{
		return edge1.second > edge2.second;
	}
};

/**
 * \fn the constructor
 * \brief initialize a new dijkstra instance while sorting the given edges
 * \param weights Vector of the weights for the given graph
 * \param edges Vector of the edges for the given graph
 * \return the new dijkstra instance
 */
dijkstra::dijkstra(WeightMap weights, Edges edges, unsigned int numberOfVertices):
	numberOfVertices(numberOfVertices) {
	//sort the edges in a more efficient data structure
	sortedEdges.resize(numberOfVertices);
	for (unsigned int i = 0; i < edges.size(); i++){
		sortedEdges.at(edges.at(i).first).push_back(std::make_pair(edges.at(i).second, weights.at(i)));
		sortedEdges.at(edges.at(i).second).push_back(std::make_pair(edges.at(i).first, weights.at(i)));
	}
}

/**
 * \fn vector<int> dijkstra::computeShortestPath
 * \brief computes all shortest paths from given start vertex for the initialized dijkstra instance
 * \param unsigned int numberOfVertices Number of Vertices for the graph
 * \return vector of the weights for all shortest paths
 */
WeightMap dijkstra::computeShortestPath(int startNode){
	if(startNode > numberOfVertices){
		std::cerr << "Index of StartVertex must be less or equal to number of vertices" << std::endl;
		throw std::exception();
	}
	WeightMap weightsToVertices(numberOfVertices); //should be returned
	std::vector<int> predecessorMap(numberOfVertices);
	VisitedMap alreadyVisited(numberOfVertices);

	for (unsigned int i = 0; i < numberOfVertices; i++) {
		weightsToVertices[i] = INT_MAX;
		predecessorMap[i] = -1;
	}
	//Start in point startNode
	weightsToVertices[startNode] = 0;
	predecessorMap[startNode] = startNode;
	int currentVertex = startNode;
	int currentDist = 0;

	boost::heap::fibonacci_heap<DijkstraPair, boost::heap::compare<compare_function>> heap;
	//Put the start vertex in the verticesToVisit list
	heap.push(std::make_pair(startNode, 0));

	while(!heap.empty()){
		if (!alreadyVisited.at(heap.top().first)){
			currentVertex = heap.top().first;
			currentDist = heap.top().second;

			heap.pop();

			std::vector<DijkstraPair> currentEdges = sortedEdges.at(currentVertex);

			for (const DijkstraPair pair : currentEdges){
				//Find out the current neighbor vertex and the weight of the current edge
				int neighborVertex = pair.first;
				int currentWeight = pair.second;
				//Search for the right position in verticesToVisit-list. If Vertex is already visited, don't add this edge
				if (!alreadyVisited.at(neighborVertex)){
					heap.push(std::make_pair(neighborVertex, currentDist + currentWeight));
				}

				int currentDistance = currentDist + currentWeight;

				//Update the predecessor and weightsToVerices maps
				if (currentDistance < weightsToVertices[neighborVertex]){
					weightsToVertices[neighborVertex] = currentDistance;
					predecessorMap[neighborVertex] = currentVertex;
				}
			}
			//Current vertex is visited now
			alreadyVisited.at(currentVertex) = true;
		} else {
			heap.pop();
		}
	}
	return weightsToVertices;
}


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

#ifndef BOOST_MATH_SPECIAL_BETA_HPP
#define BOOST_MATH_SPECIAL_BETA_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/static_assert.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost{ namespace math{

namespace detail{

//
// Implementation of Beta(a,b) using the Lanczos approximation:
//
template <class T, class Lanczos, class Policy>
T beta_imp(T a, T b, const Lanczos&, const Policy& pol)
{
   BOOST_MATH_STD_USING  // for ADL of std names

   if(a <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got b=%1%).", b, pol);

   T result;

   T prefix = 1;
   T c = a + b;

   // Special cases:
   if((c == a) && (b < tools::epsilon<T>()))
      return boost::math::tgamma(b, pol);
   else if((c == b) && (a < tools::epsilon<T>()))
      return boost::math::tgamma(a, pol);
   if(b == 1)
      return 1/a;
   else if(a == 1)
      return 1/b;

   /*
   //
   // This code appears to be no longer necessary: it was
   // used to offset errors introduced from the Lanczos
   // approximation, but the current Lanczos approximations
   // are sufficiently accurate for all z that we can ditch
   // this.  It remains in the file for future reference...
   //
   // If a or b are less than 1, shift to greater than 1:
   if(a < 1)
   {
      prefix *= c / a;
      c += 1;
      a += 1;
   }
   if(b < 1)
   {
      prefix *= c / b;
      c += 1;
      b += 1;
   }
   */

   if(a < b)
      std::swap(a, b);

   // Lanczos calculation:
   T agh = a + Lanczos::g() - T(0.5);
   T bgh = b + Lanczos::g() - T(0.5);
   T cgh = c + Lanczos::g() - T(0.5);
   result = Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b) / Lanczos::lanczos_sum_expG_scaled(c);
   T ambh = a - T(0.5) - b;
   if((fabs(b * ambh) < (cgh * 100)) && (a > 100))
   {
      // Special case where the base of the power term is close to 1
      // compute (1+x)^y instead:
      result *= exp(ambh * boost::math::log1p(-b / cgh, pol));
   }
   else
   {
      result *= pow(agh / cgh, a - T(0.5) - b);
   }
   if(cgh > 1e10f)
      // this avoids possible overflow, but appears to be marginally less accurate:
      result *= pow((agh / cgh) * (bgh / cgh), b);
   else
      result *= pow((agh * bgh) / (cgh * cgh), b);
   result *= sqrt(boost::math::constants::e<T>() / bgh);

   // If a and b were originally less than 1 we need to scale the result:
   result *= prefix;

   return result;
} // template <class T, class Lanczos> beta_imp(T a, T b, const Lanczos&)

//
// Generic implementation of Beta(a,b) without Lanczos approximation support
// (Caution this is slow!!!):
//
template <class T, class Policy>
T beta_imp(T a, T b, const lanczos::undefined_lanczos& /* l */, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(a <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got b=%1%).", b, pol);

   T result;

   T prefix = 1;
   T c = a + b;

   // special cases:
   if((c == a) && (b < tools::epsilon<T>()))
      return boost::math::tgamma(b, pol);
   else if((c == b) && (a < tools::epsilon<T>()))
      return boost::math::tgamma(a, pol);
   if(b == 1)
      return 1/a;
   else if(a == 1)
      return 1/b;

   // shift to a and b > 1 if required:
   if(a < 1)
   {
      prefix *= c / a;
      c += 1;
      a += 1;
   }
   if(b < 1)
   {
      prefix *= c / b;
      c += 1;
      b += 1;
   }
   if(a < b)
      std::swap(a, b);

   // set integration limits:
   T la = (std::max)(T(10), a);
   T lb = (std::max)(T(10), b);
   T lc = (std::max)(T(10), T(a+b));

   // calculate the fraction parts:
   T sa = detail::lower_gamma_series(a, la, pol) / a;
   sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
   T sb = detail::lower_gamma_series(b, lb, pol) / b;
   sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
   T sc = detail::lower_gamma_series(c, lc, pol) / c;
   sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());

   // and the exponent part:
   result = exp(lc - la - lb) * pow(la/lc, a) * pow(lb/lc, b);

   // and combine:
   result *= sa * sb / sc;

   // if a and b were originally less than 1 we need to scale the result:
   result *= prefix;

   return result;
} // template <class T>T beta_imp(T a, T b, const lanczos::undefined_lanczos& l)

//
// Compute the leading power terms in the incomplete Beta:
//
// (x^a)(y^b)/Beta(a,b) when normalised, and
// (x^a)(y^b) otherwise.
//
// Almost all of the error in the incomplete beta comes from this
// function: particularly when a and b are large. Computing large
// powers are *hard* though, and using logarithms just leads to
// horrendous cancellation errors.
//
template <class T, class Lanczos, class Policy>
T ibeta_power_terms(T a,
                        T b,
                        T x,
                        T y,
                        const Lanczos&,
                        bool normalised,
                        const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(!normalised)
   {
      // can we do better here?
      return pow(x, a) * pow(y, b);
   }

   T result;

   T prefix = 1;
   T c = a + b;

   // combine power terms with Lanczos approximation:
   T agh = a + Lanczos::g() - T(0.5);
   T bgh = b + Lanczos::g() - T(0.5);
   T cgh = c + Lanczos::g() - T(0.5);
   result = Lanczos::lanczos_sum_expG_scaled(c) / (Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b));

   // l1 and l2 are the base of the exponents minus one:
   T l1 = (x * b - y * agh) / agh;
   T l2 = (y * a - x * bgh) / bgh;
   if(((std::min)(fabs(l1), fabs(l2)) < 0.2))
   {
      // when the base of the exponent is very near 1 we get really
      // gross errors unless extra care is taken:
      if((l1 * l2 > 0) || ((std::min)(a, b) < 1))
      {
         //
         // This first branch handles the simple cases where either:
         //
         // * The two power terms both go in the same direction
         // (towards zero or towards infinity).  In this case if either
         // term overflows or underflows, then the product of the two must
         // do so also.
         // *Alternatively if one exponent is less than one, then we
         // can't productively use it to eliminate overflow or underflow
         // from the other term.  Problems with spurious overflow/underflow
         // can't be ruled out in this case, but it is *very* unlikely
         // since one of the power terms will evaluate to a number close to 1.
         //
         if(fabs(l1) < 0.1)
         {
            result *= exp(a * boost::math::log1p(l1, pol));
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            result *= pow((x * cgh) / agh, a);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         if(fabs(l2) < 0.1)
         {
            result *= exp(b * boost::math::log1p(l2, pol));
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            result *= pow((y * cgh) / bgh, b);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
      else if((std::max)(fabs(l1), fabs(l2)) < 0.5)
      {
         //
         // Both exponents are near one and both the exponents are
         // greater than one and further these two
         // power terms tend in opposite directions (one towards zero,
         // the other towards infinity), so we have to combine the terms
         // to avoid any risk of overflow or underflow.
         //
         // We do this by moving one power term inside the other, we have:
         //
         //    (1 + l1)^a * (1 + l2)^b
         //  = ((1 + l1)*(1 + l2)^(b/a))^a
         //  = (1 + l1 + l3 + l1*l3)^a   ;  l3 = (1 + l2)^(b/a) - 1
         //                                    = exp((b/a) * log(1 + l2)) - 1
         //
         // The tricky bit is deciding which term to move inside :-)
         // By preference we move the larger term inside, so that the
         // size of the largest exponent is reduced.  However, that can
         // only be done as long as l3 (see above) is also small.
         //
         bool small_a = a < b;
         T ratio = b / a;
         if((small_a && (ratio * l2 < 0.1)) || (!small_a && (l1 / ratio > 0.1)))
         {
            T l3 = boost::math::expm1(ratio * boost::math::log1p(l2, pol), pol);
            l3 = l1 + l3 + l3 * l1;
            l3 = a * boost::math::log1p(l3, pol);
            result *= exp(l3);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            T l3 = boost::math::expm1(boost::math::log1p(l1, pol) / ratio, pol);
            l3 = l2 + l3 + l3 * l2;
            l3 = b * boost::math::log1p(l3, pol);
            result *= exp(l3);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
      else if(fabs(l1) < fabs(l2))
      {
         // First base near 1 only:
         T l = a * boost::math::log1p(l1, pol)
            + b * log((y * cgh) / bgh);
         result *= exp(l);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // Second base near 1 only:
         T l = b * boost::math::log1p(l2, pol)
            + a * log((x * cgh) / agh);
         result *= exp(l);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
   }
   else
   {
      // general case:
      T b1 = (x * cgh) / agh;
      T b2 = (y * cgh) / bgh;
      l1 = a * log(b1);
      l2 = b * log(b2);
      BOOST_MATH_INSTRUMENT_VARIABLE(b1);
      BOOST_MATH_INSTRUMENT_VARIABLE(b2);
      BOOST_MATH_INSTRUMENT_VARIABLE(l1);
      BOOST_MATH_INSTRUMENT_VARIABLE(l2);
      if((l1 >= tools::log_max_value<T>())
         || (l1 <= tools::log_min_value<T>())
         || (l2 >= tools::log_max_value<T>())
         || (l2 <= tools::log_min_value<T>())
         )
      {
         // Oops, overflow, sidestep:
         if(a < b)
            result *= pow(pow(b2, b/a) * b1, a);
         else
            result *= pow(pow(b1, a/b) * b2, b);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // finally the normal case:
         result *= pow(b1, a) * pow(b2, b);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
   }
   // combine with the leftover terms from the Lanczos approximation:
   result *= sqrt(bgh / boost::math::constants::e<T>());
   result *= sqrt(agh / cgh);
   result *= prefix;

   BOOST_MATH_INSTRUMENT_VARIABLE(result);

   return result;
}
//
// Compute the leading power terms in the incomplete Beta:
//
// (x^a)(y^b)/Beta(a,b) when normalised, and
// (x^a)(y^b) otherwise.
//
// Almost all of the error in the incomplete beta comes from this
// function: particularly when a and b are large. Computing large
// powers are *hard* though, and using logarithms just leads to
// horrendous cancellation errors.
//
// This version is generic, slow, and does not use the Lanczos approximation.
//
template <class T, class Policy>
T ibeta_power_terms(T a,
                        T b,
                        T x,
                        T y,
                        const boost::math::lanczos::undefined_lanczos&,
                        bool normalised,
                        const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(!normalised)
   {
      return pow(x, a) * pow(y, b);
   }

   T result= 0; // assignment here silences warnings later

   T c = a + b;

   // integration limits for the gamma functions:
   //T la = (std::max)(T(10), a);
   //T lb = (std::max)(T(10), b);
   //T lc = (std::max)(T(10), a+b);
   T la = a + 5;
   T lb = b + 5;
   T lc = a + b + 5;
   // gamma function partials:
   T sa = detail::lower_gamma_series(a, la, pol) / a;
   sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
   T sb = detail::lower_gamma_series(b, lb, pol) / b;
   sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
   T sc = detail::lower_gamma_series(c, lc, pol) / c;
   sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());
   // gamma function powers combined with incomplete beta powers:

   T b1 = (x * lc) / la;
   T b2 = (y * lc) / lb;
   T e1 = lc - la - lb;
   T lb1 = a * log(b1);
   T lb2 = b * log(b2);

   if((lb1 >= tools::log_max_value<T>())
      || (lb1 <= tools::log_min_value<T>())
      || (lb2 >= tools::log_max_value<T>())
      || (lb2 <= tools::log_min_value<T>())
      || (e1 >= tools::log_max_value<T>())
      || (e1 <= tools::log_min_value<T>())
      )
   {
      result = exp(lb1 + lb2 - e1);
   }
   else
   {
      T p1, p2;
      if((fabs(b1 - 1) * a < 10) && (a > 1))
         p1 = exp(a * boost::math::log1p((x * b - y * la) / la, pol));
      else
         p1 = pow(b1, a);
      if((fabs(b2 - 1) * b < 10) && (b > 1))
         p2 = exp(b * boost::math::log1p((y * a - x * lb) / lb, pol));
      else
         p2 = pow(b2, b);
      T p3 = exp(e1);
      result = p1 * p2 / p3;
   }
   // and combine with the remaining gamma function components:
   result /= sa * sb / sc;

   return result;
}
//
// Series approximation to the incomplete beta:
//
template <class T>
struct ibeta_series_t
{
   typedef T result_type;
   ibeta_series_t(T a_, T b_, T x_, T mult) : result(mult), x(x_), apn(a_), poch(1-b_), n(1) {}
   T operator()()
   {
      T r = result / apn;
      apn += 1;
      result *= poch * x / n;
      ++n;
      poch += 1;
      return r;
   }
private:
   T result, x, apn, poch;
   int n;
};

template <class T, class Lanczos, class Policy>
T ibeta_series(T a, T b, T x, T s0, const Lanczos&, bool normalised, T* p_derivative, T y, const Policy& pol)
{
   BOOST_MATH_STD_USING

   T result;

   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(normalised)
   {
      T c = a + b;

      // incomplete beta power term, combined with the Lanczos approximation:
      T agh = a + Lanczos::g() - T(0.5);
      T bgh = b + Lanczos::g() - T(0.5);
      T cgh = c + Lanczos::g() - T(0.5);
      result = Lanczos::lanczos_sum_expG_scaled(c) / (Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b));
      if(a * b < bgh * 10)
         result *= exp((b - 0.5f) * boost::math::log1p(a / bgh, pol));
      else
         result *= pow(cgh / bgh, b - 0.5f);
      result *= pow(x * cgh / agh, a);
      result *= sqrt(agh / boost::math::constants::e<T>());

      if(p_derivative)
      {
         *p_derivative = result * pow(y, b);
         BOOST_ASSERT(*p_derivative >= 0);
      }
   }
   else
   {
      // Non-normalised, just compute the power:
      result = pow(x, a);
   }
   if(result < tools::min_value<T>())
      return s0; // Safeguard: series can't cope with denorms.
   ibeta_series_t<T> s(a, b, x, result);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
   result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, s0);
   policies::check_series_iterations<T>("boost::math::ibeta<%1%>(%1%, %1%, %1%) in ibeta_series (with lanczos)", max_iter, pol);
   return result;
}
//
// Incomplete Beta series again, this time without Lanczos support:
//
template <class T, class Policy>
T ibeta_series(T a, T b, T x, T s0, const boost::math::lanczos::undefined_lanczos&, bool normalised, T* p_derivative, T y, const Policy& pol)
{
   BOOST_MATH_STD_USING

   T result;
   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(normalised)
   {
      T c = a + b;

      // figure out integration limits for the gamma function:
      //T la = (std::max)(T(10), a);
      //T lb = (std::max)(T(10), b);
      //T lc = (std::max)(T(10), a+b);
      T la = a + 5;
      T lb = b + 5;
      T lc = a + b + 5;

      // calculate the gamma parts:
      T sa = detail::lower_gamma_series(a, la, pol) / a;
      sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
      T sb = detail::lower_gamma_series(b, lb, pol) / b;
      sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
      T sc = detail::lower_gamma_series(c, lc, pol) / c;
      sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());

      // and their combined power-terms:
      T b1 = (x * lc) / la;
      T b2 = lc/lb;
      T e1 = lc - la - lb;
      T lb1 = a * log(b1);
      T lb2 = b * log(b2);

      if((lb1 >= tools::log_max_value<T>())
         || (lb1 <= tools::log_min_value<T>())
         || (lb2 >= tools::log_max_value<T>())
         || (lb2 <= tools::log_min_value<T>())
         || (e1 >= tools::log_max_value<T>())
         || (e1 <= tools::log_min_value<T>()) )
      {
         T p = lb1 + lb2 - e1;
         result = exp(p);
      }
      else
      {
         result = pow(b1, a);
         if(a * b < lb * 10)
            result *= exp(b * boost::math::log1p(a / lb, pol));
         else
            result *= pow(b2, b);
         result /= exp(e1);
      }
      // and combine the results:
      result /= sa * sb / sc;

      if(p_derivative)
      {
         *p_derivative = result * pow(y, b);
         BOOST_ASSERT(*p_derivative >= 0);
      }
   }
   else
   {
      // Non-normalised, just compute the power:
      result = pow(x, a);
   }
   if(result < tools::min_value<T>())
      return s0; // Safeguard: series can't cope with denorms.
   ibeta_series_t<T> s(a, b, x, result);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
   result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, s0);
   policies::check_series_iterations<T>("boost::math::ibeta<%1%>(%1%, %1%, %1%) in ibeta_series (without lanczos)", max_iter, pol);
   return result;
}

//
// Continued fraction for the incomplete beta:
//
template <class T>
struct ibeta_fraction2_t
{
   typedef std::pair<T, T> result_type;

   ibeta_fraction2_t(T a_, T b_, T x_, T y_) : a(a_), b(b_), x(x_), y(y_), m(0) {}

   result_type operator()()
   {
      T aN = (a + m - 1) * (a + b + m - 1) * m * (b - m) * x * x;
      T denom = (a + 2 * m - 1);
      aN /= denom * denom;

      T bN = m;
      bN += (m * (b - m) * x) / (a + 2*m - 1);
      bN += ((a + m) * (a * y - b * x + 1 + m *(2 - x))) / (a + 2*m + 1);

      ++m;

      return std::make_pair(aN, bN);
   }

private:
   T a, b, x, y;
   int m;
};
//
// Evaluate the incomplete beta via the continued fraction representation:
//
template <class T, class Policy>
inline T ibeta_fraction2(T a, T b, T x, T y, const Policy& pol, bool normalised, T* p_derivative)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING
   T result = ibeta_power_terms(a, b, x, y, lanczos_type(), normalised, pol);
   if(p_derivative)
   {
      *p_derivative = result;
      BOOST_ASSERT(*p_derivative >= 0);
   }
   if(result == 0)
      return result;

   ibeta_fraction2_t<T> f(a, b, x, y);
   T fract = boost::math::tools::continued_fraction_b(f, boost::math::policies::get_epsilon<T, Policy>());
   BOOST_MATH_INSTRUMENT_VARIABLE(fract);
   BOOST_MATH_INSTRUMENT_VARIABLE(result);
   return result / fract;
}
//
// Computes the difference between ibeta(a,b,x) and ibeta(a+k,b,x):
//
template <class T, class Policy>
T ibeta_a_step(T a, T b, T x, T y, int k, const Policy& pol, bool normalised, T* p_derivative)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;

   BOOST_MATH_INSTRUMENT_VARIABLE(k);

   T prefix = ibeta_power_terms(a, b, x, y, lanczos_type(), normalised, pol);
   if(p_derivative)
   {
      *p_derivative = prefix;
      BOOST_ASSERT(*p_derivative >= 0);
   }
   prefix /= a;
   if(prefix == 0)
      return prefix;
   T sum = 1;
   T term = 1;
   // series summation from 0 to k-1:
   for(int i = 0; i < k-1; ++i)
   {
      term *= (a+b+i) * x / (a+i+1);
      sum += term;
   }
   prefix *= sum;

   return prefix;
}
//
// This function is only needed for the non-regular incomplete beta,
// it computes the delta in:
// beta(a,b,x) = prefix + delta * beta(a+k,b,x)
// it is currently only called for small k.
//
template <class T>
inline T rising_factorial_ratio(T a, T b, int k)
{
   // calculate:
   // (a)(a+1)(a+2)...(a+k-1)
   // _______________________
   // (b)(b+1)(b+2)...(b+k-1)

   // This is only called with small k, for large k
   // it is grossly inefficient, do not use outside it's
   // intended purpose!!!
   BOOST_MATH_INSTRUMENT_VARIABLE(k);
   if(k == 0)
      return 1;
   T result = 1;
   for(int i = 0; i < k; ++i)
      result *= (a+i) / (b+i);
   return result;
}
//
// Routine for a > 15, b < 1
//
// Begin by figuring out how large our table of Pn's should be,
// quoted accuracies are "guestimates" based on empiracal observation.
// Note that the table size should never exceed the size of our
// tables of factorials.
//
template <class T>
struct Pn_size
{
   // This is likely to be enough for ~35-50 digit accuracy
   // but it's hard to quantify exactly:
   BOOST_STATIC_CONSTANT(unsigned, value = 50);
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<T>::value >= 100);
};
template <>
struct Pn_size<float>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 15); // ~8-15 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<float>::value >= 30);
};
template <>
struct Pn_size<double>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 30); // 16-20 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<double>::value >= 60);
};
template <>
struct Pn_size<long double>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 50); // ~35-50 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<long double>::value >= 100);
};

template <class T, class Policy>
T beta_small_b_large_a_series(T a, T b, T x, T y, T s0, T mult, const Policy& pol, bool normalised)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING
   //
   // This is DiDonato and Morris's BGRAT routine, see Eq's 9 through 9.6.
   //
   // Some values we'll need later, these are Eq 9.1:
   //
   T bm1 = b - 1;
   T t = a + bm1 / 2;
   T lx, u;
   if(y < 0.35)
      lx = boost::math::log1p(-y, pol);
   else
      lx = log(x);
   u = -t * lx;
   // and from from 9.2:
   T prefix;
   T h = regularised_gamma_prefix(b, u, pol, lanczos_type());
   if(h <= tools::min_value<T>())
      return s0;
   if(normalised)
   {
      prefix = h / boost::math::tgamma_delta_ratio(a, b, pol);
      prefix /= pow(t, b);
   }
   else
   {
      prefix = full_igamma_prefix(b, u, pol) / pow(t, b);
   }
   prefix *= mult;
   //
   // now we need the quantity Pn, unfortunatately this is computed
   // recursively, and requires a full history of all the previous values
   // so no choice but to declare a big table and hope it's big enough...
   //
   T p[ ::boost::math::detail::Pn_size<T>::value ] = { 1 };  // see 9.3.
   //
   // Now an initial value for J, see 9.6:
   //
   T j = boost::math::gamma_q(b, u, pol) / h;
   //
   // Now we can start to pull things together and evaluate the sum in Eq 9:
   //
   T sum = s0 + prefix * j;  // Value at N = 0
   // some variables we'll need:
   unsigned tnp1 = 1; // 2*N+1
   T lx2 = lx / 2;
   lx2 *= lx2;
   T lxp = 1;
   T t4 = 4 * t * t;
   T b2n = b;

   for(unsigned n = 1; n < sizeof(p)/sizeof(p[0]); ++n)
   {
      /*
      // debugging code, enable this if you want to determine whether
      // the table of Pn's is large enough...
      //
      static int max_count = 2;
      if(n > max_count)
      {
         max_count = n;
         std::cerr << "Max iterations in BGRAT was " << n << std::endl;
      }
      */
      //
      // begin by evaluating the next Pn from Eq 9.4:
      //
      tnp1 += 2;
      p[n] = 0;
      T mbn = b - n;
      unsigned tmp1 = 3;
      for(unsigned m = 1; m < n; ++m)
      {
         mbn = m * b - n;
         p[n] += mbn * p[n-m] / boost::math::unchecked_factorial<T>(tmp1);
         tmp1 += 2;
      }
      p[n] /= n;
      p[n] += bm1 / boost::math::unchecked_factorial<T>(tnp1);
      //
      // Now we want Jn from Jn-1 using Eq 9.6:
      //
      j = (b2n * (b2n + 1) * j + (u + b2n + 1) * lxp) / t4;
      lxp *= lx2;
      b2n += 2;
      //
      // pull it together with Eq 9:
      //
      T r = prefix * p[n] * j;
      sum += r;
      if(r > 1)
      {
         if(fabs(r) < fabs(tools::epsilon<T>() * sum))
            break;
      }
      else
      {
         if(fabs(r / tools::epsilon<T>()) < fabs(sum))
            break;
      }
   }
   return sum;
} // template <class T, class Lanczos>T beta_small_b_large_a_series(T a, T b, T x, T y, T s0, T mult, const Lanczos& l, bool normalised)

//
// For integer arguments we can relate the incomplete beta to the
// complement of the binomial distribution cdf and use this finite sum.
//
template <class T>
inline T binomial_ccdf(T n, T k, T x, T y)
{
   BOOST_MATH_STD_USING // ADL of std names
   T result = pow(x, n);
   T term = result;
   for(unsigned i = itrunc(T(n - 1)); i > k; --i)
   {
      term *= ((i + 1) * y) / ((n - i) * x) ;
      result += term;
   }

   return result;
}

//
// The incomplete beta function implementation:
// This is just a big bunch of spagetti code to divide up the
// input range and select the right implementation method for
// each domain:
//
template <class T, class Policy>
T ibeta_imp(T a, T b, T x, const Policy& pol, bool inv, bool normalised, T* p_derivative)
{
   static const char* function = "boost::math::ibeta<%1%>(%1%, %1%, %1%)";
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING // for ADL of std math functions.

   BOOST_MATH_INSTRUMENT_VARIABLE(a);
   BOOST_MATH_INSTRUMENT_VARIABLE(b);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);
   BOOST_MATH_INSTRUMENT_VARIABLE(inv);
   BOOST_MATH_INSTRUMENT_VARIABLE(normalised);

   bool invert = inv;
   T fract;
   T y = 1 - x;

   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(p_derivative)
      *p_derivative = -1; // value not set.

   if((x < 0) || (x > 1))
      policies::raise_domain_error<T>(function, "Parameter x outside the range [0,1] in the incomplete beta function (got x=%1%).", x, pol);

   if(normalised)
   {
      if(a < 0)
         policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be >= zero (got a=%1%).", a, pol);
      if(b < 0)
         policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be >= zero (got b=%1%).", b, pol);
      // extend to a few very special cases:
      if(a == 0)
      {
         if(b == 0)
            policies::raise_domain_error<T>(function, "The arguments a and b to the incomplete beta function cannot both be zero, with x=%1%.", x, pol);
         if(b > 0)
            return inv ? 0 : 1;
      }
      else if(b == 0)
      {
         if(a > 0)
            return inv ? 1 : 0;
      }
   }
   else
   {
      if(a <= 0)
         policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be greater than zero (got a=%1%).", a, pol);
      if(b <= 0)
         policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be greater than zero (got b=%1%).", b, pol);
   }

   if(x == 0)
   {
      if(p_derivative)
      {
         *p_derivative = (a == 1) ? (T)1 : (a < 1) ? T(tools::max_value<T>() / 2) : T(tools::min_value<T>() * 2);
      }
      return (invert ? (normalised ? T(1) : boost::math::beta(a, b, pol)) : T(0));
   }
   if(x == 1)
   {
      if(p_derivative)
      {
         *p_derivative = (b == 1) ? T(1) : (b < 1) ? T(tools::max_value<T>() / 2) : T(tools::min_value<T>() * 2);
      }
      return (invert == 0 ? (normalised ? 1 : boost::math::beta(a, b, pol)) : 0);
   }

   if((std::min)(a, b) <= 1)
   {
      if(x > 0.5)
      {
         std::swap(a, b);
         std::swap(x, y);
         invert = !invert;
         BOOST_MATH_INSTRUMENT_VARIABLE(invert);
      }
      if((std::max)(a, b) <= 1)
      {
         // Both a,b < 1:
         if((a >= (std::min)(T(0.2), b)) || (pow(x, a) <= 0.9))
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else
         {
            std::swap(a, b);
            std::swap(x, y);
            invert = !invert;
            if(y >= 0.3)
            {
               if(!invert)
               {
                  fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else
            {
               // Sidestep on a, and then use the series representation:
               T prefix;
               if(!normalised)
               {
                  prefix = rising_factorial_ratio(T(a+b), a, 20);
               }
               else
               {
                  prefix = 1;
               }
               fract = ibeta_a_step(a, b, x, y, 20, pol, normalised, p_derivative);
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
         }
      }
      else
      {
         // One of a, b < 1 only:
         if((b <= 1) || ((x < 0.1) && (pow(b * x, a) <= 0.7)))
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else
         {
            std::swap(a, b);
            std::swap(x, y);
            invert = !invert;

            if(y >= 0.3)
            {
               if(!invert)
               {
                  fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else if(a >= 15)
            {
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(a, b, x, y, T(0), T(1), pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(a, b, x, y, fract, T(1), pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else
            {
               // Sidestep to improve errors:
               T prefix;
               if(!normalised)
               {
                  prefix = rising_factorial_ratio(T(a+b), a, 20);
               }
               else
               {
                  prefix = 1;
               }
               fract = ibeta_a_step(a, b, x, y, 20, pol, normalised, p_derivative);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
         }
      }
   }
   else
   {
      // Both a,b >= 1:
      T lambda;
      if(a < b)
      {
         lambda = a - (a + b) * x;
      }
      else
      {
         lambda = (a + b) * y - b;
      }
      if(lambda < 0)
      {
         std::swap(a, b);
         std::swap(x, y);
         invert = !invert;
         BOOST_MATH_INSTRUMENT_VARIABLE(invert);
      }

      if(b < 40)
      {
         if((floor(a) == a) && (floor(b) == b) && (a < (std::numeric_limits<int>::max)() - 100))
         {
            // relate to the binomial distribution and use a finite sum:
            T k = a - 1;
            T n = b + k;
            fract = binomial_ccdf(n, k, x, y);
            if(!normalised)
               fract *= boost::math::beta(a, b, pol);
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else if(b * x <= 0.7)
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else if(a > 15)
         {
            // sidestep so we can use the series representation:
            int n = itrunc(T(floor(b)), pol);
            if(n == b)
               --n;
            T bbar = b - n;
            T prefix;
            if(!normalised)
            {
               prefix = rising_factorial_ratio(T(a+bbar), bbar, n);
            }
            else
            {
               prefix = 1;
            }
            fract = ibeta_a_step(bbar, a, y, x, n, pol, normalised, static_cast<T*>(0));
            fract = beta_small_b_large_a_series(a,  bbar, x, y, fract, T(1), pol, normalised);
            fract /= prefix;
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else if(normalised)
         {
            // the formula here for the non-normalised case is tricky to figure
            // out (for me!!), and requires two pochhammer calculations rather
            // than one, so leave it for now....
            int n = itrunc(T(floor(b)), pol);
            T bbar = b - n;
            if(bbar <= 0)
            {
               --n;
               bbar += 1;
            }
            fract = ibeta_a_step(bbar, a, y, x, n, pol, normalised, static_cast<T*>(0));
            fract += ibeta_a_step(a, bbar, x, y, 20, pol, normalised, static_cast<T*>(0));
            if(invert)
               fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
            //fract = ibeta_series(a+20, bbar, x, fract, l, normalised, p_derivative, y);
            fract = beta_small_b_large_a_series(T(a+20),  bbar, x, y, fract, T(1), pol, normalised);
            if(invert)
            {
               fract = -fract;
               invert = false;
            }
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else
         {
            fract = ibeta_fraction2(a, b, x, y, pol, normalised, p_derivative);
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
      }
      else
      {
         fract = ibeta_fraction2(a, b, x, y, pol, normalised, p_derivative);
         BOOST_MATH_INSTRUMENT_VARIABLE(fract);
      }
   }
   if(p_derivative)
   {
      if(*p_derivative < 0)
      {
         *p_derivative = ibeta_power_terms(a, b, x, y, lanczos_type(), true, pol);
      }
      T div = y * x;

      if(*p_derivative != 0)
      {
         if((tools::max_value<T>() * div < *p_derivative))
         {
            // overflow, return an arbitarily large value:
            *p_derivative = tools::max_value<T>() / 2;
         }
         else
         {
            *p_derivative /= div;
         }
      }
   }
   return invert ? (normalised ? 1 : boost::math::beta(a, b, pol)) - fract : fract;
} // template <class T, class Lanczos>T ibeta_imp(T a, T b, T x, const Lanczos& l, bool inv, bool normalised)

template <class T, class Policy>
inline T ibeta_imp(T a, T b, T x, const Policy& pol, bool inv, bool normalised)
{
   return ibeta_imp(a, b, x, pol, inv, normalised, static_cast<T*>(0));
}

template <class T, class Policy>
T ibeta_derivative_imp(T a, T b, T x, const Policy& pol)
{
   static const char* function = "ibeta_derivative<%1%>(%1%,%1%,%1%)";
   //
   // start with the usual error checks:
   //
   if(a <= 0)
      policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be greater than zero (got b=%1%).", b, pol);
   if((x < 0) || (x > 1))
      policies::raise_domain_error<T>(function, "Parameter x outside the range [0,1] in the incomplete beta function (got x=%1%).", x, pol);
   //
   // Now the corner cases:
   //
   if(x == 0)
   {
      return (a > 1) ? 0 :
         (a == 1) ? 1 / boost::math::beta(a, b, pol) : policies::raise_overflow_error<T>(function, 0, pol);
   }
   else if(x == 1)
   {
      return (b > 1) ? 0 :
         (b == 1) ? 1 / boost::math::beta(a, b, pol) : policies::raise_overflow_error<T>(function, 0, pol);
   }
   //
   // Now the regular cases:
   //
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   T f1 = ibeta_power_terms<T>(a, b, x, 1 - x, lanczos_type(), true, pol);
   T y = (1 - x) * x;

   if(f1 == 0)
      return 0;

   if((tools::max_value<T>() * y < f1))
   {
      // overflow:
      return policies::raise_overflow_error<T>(function, 0, pol);
   }

   f1 /= y;

   return f1;
}
//
// Some forwarding functions that dis-ambiguate the third argument type:
//
template <class RT1, class RT2, class Policy>
inline typename tools::promote_args<RT1, RT2>::type
   beta(RT1 a, RT2 b, const Policy&, const mpl::true_*)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::beta_imp(static_cast<value_type>(a), static_cast<value_type>(b), evaluation_type(), forwarding_policy()), "boost::math::beta<%1%>(%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   beta(RT1 a, RT2 b, RT3 x, const mpl::false_*)
{
   return boost::math::beta(a, b, x, policies::policy<>());
}
} // namespace detail

//
// The actual function entry-points now follow, these just figure out
// which Lanczos approximation to use
// and forward to the implementation functions:
//
template <class RT1, class RT2, class A>
inline typename tools::promote_args<RT1, RT2, A>::type
   beta(RT1 a, RT2 b, A arg)
{
   typedef typename policies::is_policy<A>::type tag;
   return boost::math::detail::beta(a, b, arg, static_cast<tag*>(0));
}

template <class RT1, class RT2>
inline typename tools::promote_args<RT1, RT2>::type
   beta(RT1 a, RT2 b)
{
   return boost::math::beta(a, b, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   beta(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), false, false), "boost::math::beta<%1%>(%1%,%1%,%1%)");
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   betac(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), true, false), "boost::math::betac<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   betac(RT1 a, RT2 b, RT3 x)
{
   return boost::math::betac(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), false, true), "boost::math::ibeta<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibeta(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), true, true), "boost::math::ibetac<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibetac(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta_derivative(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_derivative_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy()), "boost::math::ibeta_derivative<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta_derivative(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibeta_derivative(a, b, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#include <boost/math/special_functions/detail/ibeta_inverse.hpp>
#include <boost/math/special_functions/detail/ibeta_inv_ab.hpp>

#endif // BOOST_MATH_SPECIAL_BETA_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Suites
#include <boost/test/unit_test.hpp>
#include"basis.hpp"

using namespace boost::unit_test;
using boost::unit_test_framework::test_suite;
using namespace Many_Body;
BOOST_AUTO_TEST_SUITE(basistesting)
BOOST_AUTO_TEST_CASE(phonondimension)
{
  const size_t L1=3;
  const size_t L2=4;
  const size_t L3=5;
  const size_t L4=6;

  BosonState b1( 4, 5);

  std::vector<size_t> state {1, 4, 1, 0};
  BosonState b2(state, 5);

      BOOST_CHECK(b1.GetId()==0);
      BOOST_CHECK(b2[1]==4);
      PhononBasis g1{ 4, 1};

    for(size_t i=2; i<3; i++)
      {
	PhononBasis g1{ 4, i};
        PhononBasis g2{ 3, i};

        BOOST_CHECK(g1.dim==static_cast<size_t>(std::pow((i+1), 4)));
        BOOST_CHECK(g2.dim==static_cast<size_t>(std::pow((i+1), 3)));

   }
}
BOOST_AUTO_TEST_CASE(electrondimension)
{
  ElectronState df;
  std::vector<size_t> state {1, 1,      1, 0};
  ElectronState constru1(state);

     BOOST_CHECK(constru1.GetId()==14);
      constru1.flip(1);

      BOOST_CHECK(constru1.GetId()==10);
        ElectronState constru2(3, 4);

//        BOOST_CHECK(constru2.GetId()==3);
 ElectronBasis g1( 3);

 const ElectronBasis g2(5, 2);
       ElectronBasis g3( 3, 1);

       BOOST_CHECK(g1.dim==std::pow(2, 3));
     BOOST_CHECK(g2.dim==Factorial(5)/(Factorial(5-2)*Factorial(2)));
     BOOST_CHECK(g3.dim== 3);

 }

// BOOST_AUTO_TEST_CASE(bosondimension)
// {

   // const size_t L2=4;
   // const size_t L3=5;
   // std::array<size_t, L2> stateArray{0, 1, 2};

   //   BosonState<L2> a{stateArray};

   // const BosonBasis<L2> g2{2};
   // BosonBasis<L3> g3{1};

   // //BOOST_CHECK(g1.dim==Factorial(L2)/(Factorial(L2-2)*Factorial(2)));
   // BOOST_CHECK(g2.dim==Factorial(L2+2-1)/(Factorial(L2-1)*Factorial(2)));
   // BOOST_CHECK(g3.dim==Factorial(L3+1-1)/(Factorial(L3-1)*Factorial(1)));

// }

 BOOST_AUTO_TEST_CASE(tensorproductdimension)
 {
//   // const size_t L1=3;
//  //  const size_t L2=4;
//  //  const size_t L3=5;
//  // //  const size_t L4=6;

   PhononBasis g{ 4, 1};

 ElectronBasis e{ 4, 1};

 TensorProduct<  ElectronBasis,  PhononBasis> TP(e, g);

     for(size_t i=1; i<2; i++)
         {
	   PhononBasis g1{3, i};
	   PhononBasis g2{ 4, i};
	   PhononBasis g3{ 5, i};

	   ElectronBasis e1{ 3, 1};
	   ElectronBasis e2{ 4, 2};
	   ElectronBasis e3{ 5, 3};

 TensorProduct<  ElectronBasis,  PhononBasis> TP1(e1, g1);
  	 TensorProduct<  ElectronBasis,  PhononBasis> TP2(e2, g2);
          TensorProduct<  ElectronBasis,  PhononBasis> TP3(e3, g3);

  	 BOOST_CHECK(g1.dim*e1.dim==TP1.dim);
 	 BOOST_CHECK(g2.dim*e2.dim==TP2.dim);
  	 BOOST_CHECK(g3.dim*e3.dim==TP3.dim);

   }
}
BOOST_AUTO_TEST_SUITE_END()


//  Copyright (c) 2006 Xiaogang Zhang
//  Copyright (c) 2006 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to ensure
//  that the code continues to work no matter how many digits
//  type T has.

#ifndef BOOST_MATH_ELLINT_1_HPP
#define BOOST_MATH_ELLINT_1_HPP

#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>

// Elliptic integrals (complete and incomplete) of the first kind
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math {

template <class T1, class T2, class Policy>
typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const Policy& pol);

namespace detail{

template <typename T, typename Policy>
T ellint_k_imp(T k, const Policy& pol);

// Elliptic integral (Legendre form) of the first kind
template <typename T, typename Policy>
T ellint_f_imp(T phi, T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    static const char* function = "boost::math::ellint_f<%1%>(%1%,%1%)";
    BOOST_MATH_INSTRUMENT_VARIABLE(phi);
    BOOST_MATH_INSTRUMENT_VARIABLE(k);
    BOOST_MATH_INSTRUMENT_VARIABLE(function);

    if (abs(k) > 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }

    bool invert = false;
    if(phi < 0)
    {
       BOOST_MATH_INSTRUMENT_VARIABLE(phi);
       phi = fabs(phi);
       invert = true;
    }

    T result;

    if(phi >= tools::max_value<T>())
    {
       // Need to handle infinity as a special case:
       result = policies::raise_overflow_error<T>(function, 0, pol);
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
    }
    else if(phi > 1 / tools::epsilon<T>())
    {
       // Phi is so large that phi%pi is necessarily zero (or garbage),
       // just return the second part of the duplication formula:
       result = 2 * phi * ellint_k_imp(k, pol) / constants::pi<T>();
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
    }
    else
    {
       // Carlson's algorithm works only for |phi| <= pi/2,
       // use the integrand's periodicity to normalize phi
       //
       // Xiaogang's original code used a cast to long long here
       // but that fails if T has more digits than a long long,
       // so rewritten to use fmod instead:
       //
       BOOST_MATH_INSTRUMENT_CODE("pi/2 = " << constants::pi<T>() / 2);
       T rphi = boost::math::tools::fmod_workaround(phi, constants::pi<T>() / 2);
       BOOST_MATH_INSTRUMENT_VARIABLE(rphi);
       T m = 2 * (phi - rphi) / constants::pi<T>();
       BOOST_MATH_INSTRUMENT_VARIABLE(m);
       int s = 1;
       if(boost::math::tools::fmod_workaround(m, T(2)) > 0.5)
       {
          m += 1;
          s = -1;
          rphi = constants::pi<T>() / 2 - rphi;
          BOOST_MATH_INSTRUMENT_VARIABLE(rphi);
       }
       T sinp = sin(rphi);
       T cosp = cos(rphi);
       BOOST_MATH_INSTRUMENT_VARIABLE(sinp);
       BOOST_MATH_INSTRUMENT_VARIABLE(cosp);
       result = s * sinp * ellint_rf_imp(cosp * cosp, 1 - k * k * sinp * sinp, T(1), pol);
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
       if(m != 0)
       {
          result += m * ellint_k_imp(k, pol);
          BOOST_MATH_INSTRUMENT_VARIABLE(result);
       }
    }
    return invert ? -result : result;
}

// Complete elliptic integral (Legendre form) of the first kind
template <typename T, typename Policy>
T ellint_k_imp(T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_k<%1%>(%1%)";

    if (abs(k) > 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }
    if (abs(k) == 1)
    {
       return policies::raise_overflow_error<T>(function, 0, pol);
    }

    T x = 0;
    T y = 1 - k * k;
    T z = 1;
    T value = ellint_rf_imp(x, y, z, pol);

    return value;
}

template <typename T, typename Policy>
inline typename tools::promote_args<T>::type ellint_1(T k, const Policy& pol, const mpl::true_&)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::ellint_k_imp(static_cast<value_type>(k), pol), "boost::math::ellint_1<%1%>(%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const mpl::false_&)
{
   return boost::math::ellint_1(k, phi, policies::policy<>());
}

}

// Complete elliptic integral (Legendre form) of the first kind
template <typename T>
inline typename tools::promote_args<T>::type ellint_1(T k)
{
   return ellint_1(k, policies::policy<>());
}

// Elliptic integral (Legendre form) of the first kind
template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::ellint_f_imp(static_cast<value_type>(phi), static_cast<value_type>(k), pol), "boost::math::ellint_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi)
{
   typedef typename policies::is_policy<T2>::type tag_type;
   return detail::ellint_1(k, phi, tag_type());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_1_HPP


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sinhcosh.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/cosh.hpp>
#include <boost/simd/function/sinh.hpp>

STF_CASE_TPL("sinhcosh", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sinhcosh;

  T a[] = {bs::Zero<T>(), bs::One<T>(), T(5), T(-5)};
  size_t N =  sizeof(a)/sizeof(T);
  STF_EXPR_IS( (sinhcosh(T()))
                  , (std::pair<T,T>)
                  );

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = sinhcosh(a[i]);
      STF_ULP_EQUAL(p.first,  bs::sinh(a[i]), 1);
      STF_ULP_EQUAL(p.second, bs::cosh(a[i]), 1);
    }
  }

  T b[] = {bs::Inf<T>(), bs::Minf<T>(), bs::Nan<T>()};
  N =  sizeof(b)/sizeof(T);
#ifndef BOOST_SIMD_NO_INVALIDS

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = sinhcosh(b[i]);
      STF_ULP_EQUAL(p.first,  bs::sinh(b[i]), 1);
      STF_ULP_EQUAL(p.second, bs::cosh(b[i]), 1);
    }
  }
#endif
}


/*
 * PolygonTest.cpp
 *
 *  Created on: Mar 24, 2015
 *      Author: Martin Wermelinger, Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_core/Polygon.hpp"

// gtest
#include <gtest/gtest.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace grid_map;

TEST(Polygon, getCentroidTriangle)
{
  Polygon triangle;
  triangle.addVertex(Vector2d(0.0, 0.0));
  triangle.addVertex(Vector2d(1.0, 0.0));
  triangle.addVertex(Vector2d(0.5, 1.0));

  Position expectedCentroid;
  expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5);
  expectedCentroid.y() = 1.0 / 3.0;
  Position centroid = triangle.getCentroid();
  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}

TEST(Polygon, getCentroidRectangle)
{
  Polygon rectangle;
  rectangle.addVertex(Vector2d(-2.0, -1.0));
  rectangle.addVertex(Vector2d(-2.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, -1.0));

  Position expectedCentroid(-0.5, 0.5);
  Position centroid = rectangle.getCentroid();
  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}

TEST(Polygon, getBoundingBox)
{
  Polygon triangle;
  triangle.addVertex(Vector2d(0.0, 0.0));
  triangle.addVertex(Vector2d(0.5, -1.2));
  triangle.addVertex(Vector2d(1.0, 0.0));

  Position expectedCenter(0.5, -0.6);
  Length expectedLength(1.0, 1.2);
  Position center;
  Length length;
  triangle.getBoundingBox(center, length);

  EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x());
  EXPECT_DOUBLE_EQ(expectedCenter.y(), center.y());
  EXPECT_DOUBLE_EQ(expectedLength.x(), length.x());
  EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
}

TEST(Polygon, convexHullPolygon)
{
  Polygon polygon1;
  polygon1.addVertex(Vector2d(0.0, 0.0));
  polygon1.addVertex(Vector2d(1.0, 1.0));
  polygon1.addVertex(Vector2d(0.0, 1.0));
  polygon1.addVertex(Vector2d(1.0, 0.0));

  Polygon polygon2;
  polygon2.addVertex(Vector2d(0.5, 0.5));
  polygon2.addVertex(Vector2d(0.5, 1.5));
  polygon2.addVertex(Vector2d(1.5, 0.5));
  polygon2.addVertex(Vector2d(1.5, 1.5));

  Polygon hull = Polygon::convexHull(polygon1, polygon2);

  EXPECT_EQ(6, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49)));
}

TEST(Polygon, convexHullCircles)
{
  Position center1(0.0, 0.0);
  Position center2(1.0, 0.0);
  double radius = 0.5;
  const int nVertices = 15;

  Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius);
  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));

  hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices);
  EXPECT_EQ(nVertices + 1, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));

  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius);
  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));

  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices);
  EXPECT_EQ(nVertices, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
}

TEST(Polygon, convexHullCircle)
{
  Position center(0.0, 0.0);
  double radius = 0.5;
  const int nVertices = 15;

  Polygon hull = Polygon::fromCircle(center, radius);

  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));

  hull = Polygon::fromCircle(center, radius, nVertices);
  EXPECT_EQ(nVertices, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
}

TEST(convertToInequalityConstraints, triangle1)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)});
  MatrixXd A;
  VectorXd b;
  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
  EXPECT_NEAR(-1.3636, A(0, 0), 1e-4);
  EXPECT_NEAR( 1.3636, A(0, 1), 1e-4);
  EXPECT_NEAR(-1.5000, A(1, 0), 1e-4);
  EXPECT_NEAR(-1.5000, A(1, 1), 1e-4);
  EXPECT_NEAR( 2.8636, A(2, 0), 1e-4);
  EXPECT_NEAR( 0.1364, A(2, 1), 1e-4);
  EXPECT_NEAR( 0.0000, b(0), 1e-4);
  EXPECT_NEAR( 0.0000, b(1), 1e-4);
  EXPECT_NEAR( 3.0000, b(2), 1e-4);
}

TEST(convertToInequalityConstraints, triangle2)
{
  Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)});
  MatrixXd A;
  VectorXd b;
  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
  EXPECT_NEAR(-1.5000, A(0, 0), 1e-4);
  EXPECT_NEAR( 0.0000, A(0, 1), 1e-4);
  EXPECT_NEAR( 0.0000, A(1, 0), 1e-4);
  EXPECT_NEAR(-3.0000, A(1, 1), 1e-4);
  EXPECT_NEAR( 1.5000, A(2, 0), 1e-4);
  EXPECT_NEAR( 3.0000, A(2, 1), 1e-4);
  EXPECT_NEAR( 1.5000, b(0), 1e-4);
  EXPECT_NEAR( 1.5000, b(1), 1e-4);
  EXPECT_NEAR( 0.0000, b(2), 1e-4);
}

TEST(offsetInward, triangle)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
  polygon.offsetInward(0.1);
  EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4);
  EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4);
  EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4);
  EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4);
  EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4);
  EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4);
}

TEST(triangulation, triangle)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
  std::vector<Polygon> polygons;
  polygons = polygon.triangulate();
  ASSERT_EQ(1, polygons.size());
  EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x());
  EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y());
  EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x());
  EXPECT_EQ(polygon.getVertex(1).y(), polygons[0].getVertex(1).y());
  EXPECT_EQ(polygon.getVertex(2).x(), polygons[0].getVertex(2).x());
  EXPECT_EQ(polygon.getVertex(2).y(), polygons[0].getVertex(2).y());
}

TEST(triangulation, rectangle)
{
  Polygon rectangle;
  rectangle.addVertex(Vector2d(-2.0, -1.0));
  rectangle.addVertex(Vector2d(-2.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, -1.0));
  std::vector<Polygon> polygons;
  polygons = rectangle.triangulate();
  ASSERT_EQ(2, polygons.size());
  // TODO Extend.
}


// Copyright (C) 2021 Icey Chiu
// Using to translate the camera reference system to the arbitray one.

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>

#include <dirent.h>
#include <sys/types.h>

#include <opencv2/opencv.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

class Calibration
{
public:
    Calibration ( const std::string& setting_dir ) {
        /* load parameters */
        cv::FileStorage fs ( setting_dir, cv::FileStorage::READ );

        // camera params
        fx = fs["camera.fx"];
        fy = fs["camera.fy"];
        cx = fs["camera.cx"];
        cy = fs["camera.cy"];

        k1 = fs["camera.k1"];
        k2 = fs["camera.k2"];
        p1 = fs["camera.p1"];
        p2 = fs["camera.p2"];
        k3 = fs["camera.k3"];

        K = ( cv::Mat_<float> ( 3, 3 ) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0 );
        dist = ( cv::Mat_<float> ( 1, 5 ) << k1, k2, p1, p2, k3 );
        eK << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;

        // target params
        target_cols = fs["target.cols"];
        target_rows = fs["target.rows"];
        target_col_grid_size = fs["target.col_grid_size"];
        target_row_grid_size = fs["target.row_grid_size"];
        target_offset = fs["target.offset"];
    }

    bool calibrate ( cv::Mat& image ) {
        /* process the img with one time calibration */

        // extract corners
        std::vector<cv::Point2f> corners, undist_corners;
        bool found = cv::findChessboardCorners ( image, cv::Size ( 3, 7 ), corners );

        if ( found ) {

            // refine corners
            cv::cornerSubPix ( image, corners, cv::Size ( 11, 8 ), cv::Size ( 6 , 3 ),
                               cv::TermCriteria ( cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1 ) );

            // show corners
            cv::drawChessboardCorners ( image, cv::Size ( target_cols, target_rows ), corners, found );
            cv::resize ( image, image, cv::Size ( image.cols / 4,  image.rows/4 ) );
            cv::imshow ( "detect corners", image );
            cv::waitKey ( 1 );

            // undistort
            cv::undistortPoints ( corners, undist_corners, K, dist, cv::Mat(), K );
        }// if found target
        else {
            return false;
        }

        // get Tct, translation between camera and the target
        // get 3d mappoints refer to the target coordinate
        std::vector<cv::Point3f> pts;
        for ( int i = 0; i < target_rows; ++i )
            for ( int j = 0; j < target_cols; ++j ) {
// 				int yj = ( target_cols - j - 1);
// 				pts.push_back ( cv::Point3f ( yj*target_col_grid_size, i*target_row_grid_size, 0 ) );
				pts.push_back ( cv::Point3f ( i*target_row_grid_size, j*target_col_grid_size, 0 ) );
            }

        cv::Vec3d rvec, tvec;
        bool solve_ok =  cv::solvePnP ( pts, undist_corners, K, cv::Mat(), rvec, tvec );
        if ( !solve_ok ) {
            return false;
        }

        // convert to homo T
        Eigen::Matrix3d R;
        cv::Mat cvR;
        cv::Rodrigues ( rvec, cvR );
        R <<  cvR.at<double> ( 0, 0 ), cvR.at<double> ( 0, 1 ), cvR.at<double> ( 0, 2 ),
          cvR.at<double> ( 1, 0 ), cvR.at<double> ( 1, 1 ), cvR.at<double> ( 1, 2 ),
          cvR.at<double> ( 2, 0 ), cvR.at<double> ( 2, 1 ), cvR.at<double> ( 2, 2 );

        Eigen::Vector3d t;
        t << tvec[0], tvec[1], tvec[2];

        Eigen::Vector3d r1 = R.block ( 0, 0, 3, 1 );
        Eigen::Vector3d r2 = R.block ( 0, 1, 3, 1 );

        Eigen::Vector3d tb;
        tb << 0.0, 0.0, target_offset;

        Eigen::Matrix3d RT;
        RT.block ( 0,0, 3, 1 ) = r1;
        RT.block ( 0,1, 3, 1 ) = r2;
        RT.block ( 0,2, 3, 1 ) = R * tb + t;

        H = eK * RT; // homography
        std::cout << std::endl << H << std::endl << std::endl << std::endl;
        // Save result.
        return true;
    }

    void saveResult ( const std::string& result_dir ) {
        cv::FileStorage fs ( result_dir, cv::FileStorage::WRITE );

        // Save homography Matrix
        cv::Mat cvH = ( cv::Mat_<double> ( 3, 3 ) << H ( 0,0 ), H ( 0,1 ), H ( 0,2 ), H ( 1,0 ), H ( 1,1 ), H ( 1,2 ), H ( 2,0 ), H ( 2,1 ), H ( 2,2 ) );
        fs << "homograph_matrix" << cvH;
        fs.release();
    }

private:

    float fx;
    float fy;
    float cx;
    float cy;

    float k1;
    float k2;
    float p1;
    float p2;
    float k3;
    cv::Mat K;
    cv::Mat dist;
    Eigen::Matrix3d eK;

    int target_cols;
    int target_rows;
    float target_col_grid_size;
    float target_row_grid_size;
    float target_offset;

    Eigen::Matrix3d H;
};// class Calbration

int main ( int argc, char **argv )
{
    //if ( argc != 2 ) {
    //    std::cout << "Please input: setting file, homograph_matrix.yaml\n";
    //    return -1;
    //}

    /* load parameters */
    const std::string cfg_dir = "/home/icey/workspace/Aruco/intrinsic_calibrationfile.yml";
    const std::string calibration_dir = "/home/icey/workspace/Aruco/extrinsic_calibrationfile.yml";
    cv::Mat img = cv::imread("/home/icey/图片/calibration/666.jpg", 2 | 4);
    // Init Calbration class.
    Calibration calibration ( cfg_dir );

    // Calibration.
    calibration.calibrate ( img );
    calibration.saveResult ( calibration_dir );

    cv::waitKey( 0 );

    std::cout << "Complete" << std::endl;
    return 0;
}

/*
cv::Mat getRTMatrix ( const cv::Mat &R_,const cv::Mat &T_ ,int forceType ) {
   cv::Mat M;
   cv::Mat R,T;
   R_.copyTo ( R );
   T_.copyTo ( T );
   if ( R.type() ==CV_64F ) {
       assert ( T.type() ==CV_64F );
       cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_64FC1 );
       cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
       if ( R.total() ==3 ) {
           cv::Rodrigues ( R,R33 );
       } else if ( R.total() ==9 ) {
           cv::Mat R64;
           R.convertTo ( R64,CV_64F );
           R.copyTo ( R33 );
       }
       for ( int i=0; i<3; i++ )
           Matrix.at<double> ( i,3 ) =T.ptr<double> ( 0 ) [i];
       M=Matrix;
   } else if ( R.depth() ==CV_32F ) {
       cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_32FC1 );
       cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
       if ( R.total() ==3 ) {
           cv::Rodrigues ( R,R33 );
       } else if ( R.total() ==9 ) {
           cv::Mat R32;
           R.convertTo ( R32,CV_32F );
           R.copyTo ( R33 );
       }
       for ( int i=0; i<3; i++ )
           Matrix.at<float> ( i,3 ) =T.ptr<float> ( 0 ) [i];
       M=Matrix;
   }
   if ( forceType==-1 ) return M;
   else {
       cv::Mat MTyped;
       M.convertTo ( MTyped,forceType );
       return MTyped;
   }
}
*/


#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Eigen/SparseCholesky>
#include <Eigen/Dense>
#include <chrono>

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd;

#include "mesh_reader_eigen.hpp"
#include "constants_sparse.hpp"
#include "newton_raphson_sparse.hpp"

// Compile command:
// g++ -Wall -std=c++14 eigen_sparse.cpp -o sparse.o -I/usr/local/include/eigen3
// Run command:
// ./sparse.o filename T(in C) eta_u(in %) eta_v(in %) 0/1 (1 for quasi-newton, 0 for newton)

// u = Cu = concentration O2, v = Cv = concentration CO2

SpMat block(SpMat A,SpMat B,SpMat C, SpMat D)
{
  SpMat R(2*A.rows(),2*A.rows());
  int n = A.rows();
  for (int k=0; k<A.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
    {
      R.insert(it.row(),it.col())  = it.value();
    }
  }
  for (int k=0; k<B.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(B,k); it; ++it)
    {
      R.insert(it.row(),n+it.col())  = it.value();
    }
  }
  for (int k=0; k<C.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(C,k); it; ++it)
    {
      R.insert(n+it.row(),it.col())  = it.value();
    }
  }
  for (int k=0; k<D.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(D,k); it; ++it)
    {
      R.insert(n+it.row(),n+it.col())  = it.value();
    }
  }
  return R;
}

class F
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    SpMat& Au_;
    SpMat& Av_;
    SpMat& B_;
    SpMat& C_;
    SpMat& D_;

  public:
    F(SpMat& Au, SpMat& Av,SpMat& B,SpMat& C,SpMat& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    VectorXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n).sparseView();
      VectorXd v = x.tail(n).sparseView();
      VectorXd func(2*n);
      VectorXd f1 = Au_*u + B_*Ru(u,v) + hu*(C_*u - D_*uamb);
      VectorXd f2 = Av_*v - B_*Rv(u,v) + hv*(C_*v - D_*vamb);
      func << f1,f2;
      return func;
    }
};

class J
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    SpMat& Au_;
    SpMat& Av_;
    SpMat& B_;
    SpMat& C_;
    SpMat& D_;
  public:
    J(SpMat& Au, SpMat& Av,SpMat& B,SpMat& C,SpMat& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    SpMat operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      SpMat func(2*n,2*n);
      func = block(Au_ + B_*dRudu(u,v)+hu*C_,B_*dRudv(u,v),-B_*dRvdu(u,v),Av_ - (B_*dRvdv(u,v)) + hv*C_);
      return func;
    }
};

int main(int argc, char *argv[])
{
  std::cout << std::endl;
  std::cout << "Setting constants ..." << std::endl;
  int quasi = atoi(argv[5]);
  setConstants(atof(argv[2])+273.15,atof(argv[3])/100,atof(argv[4])/100);

  auto t1 = std::chrono::high_resolution_clock::now();
  std::string file_name = argv[1];
  std::string location = "../mesh/"+ file_name +".1";
  MatrixXd vertices = mesh::read_vertices(location+".node");
  MatrixXd triangles = mesh::read_triangles(vertices,location+".ele");
  MatrixXi boundaries = mesh::read_boundaries(vertices,location+".poly");
  auto t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << "Number of vertices :" << vertices.rows() << std::endl;
  std::cout << "Number of triangles:" << triangles.rows() <<std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  /* Calculate coefficient matrix B related to the respiration kinetics,
  part of right hand side in system of nonlinear equations */

  SpMat B_init(vertices.rows(), vertices.rows());
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    B_init.coeffRef(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(std::min(a,b),std::max(a,b)) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B_init.coeffRef(std::min(c, a),std::max(c, a)) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(std::min(b, c),std::max(b, c)) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
  }
  B_init *= (1./60.);
  SpMat B = B_init.selfadjointView<Eigen::Upper>();
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the first part of the stiffness matrix in the lefthand
  side of the the nonlinear system, A */

  t1 = std::chrono::high_resolution_clock::now();
  SpMat A_U_init(vertices.rows(), vertices.rows());
  SpMat A_V_init(vertices.rows(), vertices.rows());
  Eigen::Matrix<double,3,2> G;
  Eigen::Matrix<double,3,3> GGT_U;
  Eigen::Matrix<double,3,3> GGT_V;
  Eigen::Matrix<double,2,2> I_U;
  Eigen::Matrix<double,2,2> I_V;
  I_U(0,0) = DU_R;
  I_U(1,0) = 0;
  I_U(0,1) = 0;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  I_V(1,0) = 0;
  I_V(0,1) = 0;
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_U*G.transpose());
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_V*G.transpose());
    #ifdef DEBUG
    std::cout<<"GGT_U" << std::endl << GGT_U << std::endl;
    std::cout<<"GGT_V" << std::endl << GGT_V << std::endl;
    #endif
    A_U_init.coeffRef(a, a) += GGT_U(0, 0);
    A_U_init.coeffRef(std::min(b, a),std::max(b, a)) += GGT_U(1, 0);
    A_U_init.coeffRef(std::min(c, a),std::max(c, a)) += GGT_U(2, 0);
    A_U_init.coeffRef(b, b) += GGT_U(1, 1);
    A_U_init.coeffRef(std::min(b, c),std::max(b,c)) += GGT_U(1, 2);
    A_U_init.coeffRef(c, c) += GGT_U(2, 2);
    A_V_init.coeffRef(a, a) += GGT_V(0, 0);
    A_V_init.coeffRef(std::min(b, a),std::max(b, a)) += GGT_V(1, 0);
    A_V_init.coeffRef(std::min(c, a),std::max(c, a)) += GGT_V(2, 0);
    A_V_init.coeffRef(b, b) += GGT_V(1, 1);
    A_V_init.coeffRef(std::min(b, c),std::max(b,c)) += GGT_V(1, 2);
    A_V_init.coeffRef(c, c) += GGT_V(2, 2);
  }
  SpMat A_U = A_U_init.selfadjointView<Eigen::Upper>();
  SpMat A_V = A_V_init.selfadjointView<Eigen::Upper>();
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the second part of the stiffness matrix (C) and the
  second part of the righthand side (D) */

  SpMat C(vertices.rows(), vertices.rows());
  SpMat D(vertices.rows(),1);
  for (unsigned b = 0; b < boundaries.rows(); ++b)
  {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C.coeffRef(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 1), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    D.coeffRef(boundaries(b,0),0) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    D.coeffRef(boundaries(b,1),0) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Solve linearized problem with ambient concentrations as input_field
  to obtain starting concentration values, then solve the nonlinear
  system with Newton-Raphson */

  t1 = std::chrono::high_resolution_clock::now();

  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  SparseLU<SpMat> solver;
  VectorXd guess(vertices.rows()*2);
  SpMat T1 = A_U+(Vmu/Kmu)*B+hu*C;
  SpMat T2 = A_V+hv*C;

  solver.analyzePattern(T1);
  solver.factorize(T1);
  VectorXd u_0 = solver.solve(hu*D*uamb);
  solver.analyzePattern(T2);
  solver.factorize(T2);
  VectorXd v_0 = solver.solve(rq*(Vmu/Kmu)*B*u_0+hv*vamb*D);

  guess << u_0,v_0;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  #ifdef DEBUG
  std::cout << "A_U =" << std::endl;
  std::cout << MatrixXd(A_U) << std::endl;
  std::cout << "A_V =" << std::endl;
  std::cout << MatrixXd(A_V) << std::endl;
  std::cout << "B =" << std::endl;
  std::cout << MatrixXd(B) << std::endl;
  std::cout << "C =" << std::endl;
  std::cout << MatrixXd(C) << std::endl;
  std::cout << "D =" << std::endl;
  std::cout << MatrixXd(D) << std::endl;
  std::cout << "u_0" << std::endl;
  std::cout << u_0 << std::endl;
  std::cout << "v_0" << std::endl;
  std::cout << v_0 <<std::endl;
  std::cout << "Initial function value = " << std::endl;
  std::cout << F_funct(guess) <<std::endl;
  std::cout << "Initial Jacobian = " << std::endl;
  std::cout << J_funct(guess);
  #endif

  std::cout << std::endl;
  std::cout<< "Calculating nonlinear system solution ..." << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  if (quasi==0)
  {
    newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  }
  else
  {
    quasi_newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  }
  t2 = std::chrono::high_resolution_clock::now();

  std::cout<< "Numerical solution nonlinear system calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Write out the result for python matplotlib code */

  t1 = std::chrono::high_resolution_clock::now();
  int n = vertices.rows();
  VectorXd u = guess.head(n);
  VectorXd v = guess.tail(n);
  mesh::write_result(u,v);
  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout<< "Results for u and v written out" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << std::endl;

  return 0;
}