//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/elliptic/include/functions/am.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/half.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( am_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::am;
  using nt2::tag::am_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<am_(vT,vT, char)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  T in1[] = {nt2::One<T>(),  nt2::Pio_2<T>(), nt2::Zero<T>()};
  T in1i[] =  {nt2::Inf<T>(), nt2::Minf<T>(), nt2::Nan<T>()};
  T in2[] = {nt2::One<T>(),  nt2::Half<T>(), nt2::Zero<T>() };
  char in3[] = {'a', 'k', 'm'};

 for(int i=0; i < 3 ; i++)
 {
   for(int j=0; j < 3 ; j++)
   {
     for(int k=0; k < 3 ; k++)
     {
       NT2_TEST_ULP_EQUAL(am(nt2::splat<vT>(in1[i]), nt2::splat<vT>(in2[j]), in3[k]),
                          nt2::splat<vT>(am( in1[i], in2[j], in3[k])), 1
                         );
       #ifndef BOOST_SIMD_NO_INVALIDS
       NT2_TEST_ULP_EQUAL(am(nt2::splat<vT>(in1i[i]), nt2::splat<vT>(in2[j]), in3[k]),
                          nt2::splat<vT>(am( in1i[i], in2[j], in3[k])), 1
                         );

       #endif
     }
   }
 }

}


/*
For more information, please see: http://software.sci.utah.edu

The MIT License

Copyright (c) 2015 Scientific Computing and Imaging Institute,
University of Utah.

License for the specific language governing rights and limitations under
Permission is hereby granted, free of charge, to any person obtaining a
copy of this software and associated documentation files (the "Software"),
to deal in the Software without restriction, including without limitation
the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included
in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
*/

#include <Testing/Utils/SCIRunUnitTests.h>

#include <fstream>
#include <boost/filesystem.hpp>
#include <Core/Algorithms/Math/LinearSystem/SolveLinearSystemAlgo.h>
#include <Core/Algorithms/DataIO/ReadMatrix.h>
#include <Core/Algorithms/DataIO/WriteMatrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixIO.h>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Testing/Utils/MatrixTestUtilities.h>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::DataIO;
using namespace SCIRun::TestUtils;
using namespace SCIRun;
using namespace ::testing;

#if GTEST_HAS_COMBINE

using ::testing::Values;
using ::testing::Combine;
using ::testing::Range;

class SolveLinearSystemTestsAlgoParameterized : public ::testing::TestWithParam < std::tuple<const char*, const char*, int> >
{
public:
	ReadMatrixAlgorithm reader;
	SparseRowMatrixHandle A;
	DenseMatrixHandle rhs;
	DenseColumnMatrixHandle x0;
	SolveLinearSystemAlgo algo;
	DenseColumnMatrixHandle solution;

	~SolveLinearSystemTestsAlgoParameterized() {}
protected:
	virtual void SetUp()
	{
		auto Afile = TestResources::rootDir() / "Matrices" /  "moritz_A.mat";

		auto rhsFile = TestResources::rootDir() / "Matrices" / "moritz_b.mat";
		if (!boost::filesystem::exists(Afile) || !boost::filesystem::exists(rhsFile))
		{
			FAIL() << "TODO: Issue #142 will standardize these file locations other than being on Dan's hard drive." << std::endl
				<< "Once that issue is done however, this will be a user setup error." << std::endl;
			return;
		}
		{
			ScopedTimer t("reading sparse matrix");
			A = castMatrix::toSparse(reader.run(Afile.string()));
		}
		{
			ScopedTimer t("reading rhs");
			rhs = castMatrix::toDense(reader.run(rhsFile.string()));
		}
		 // algo object will initialize x0 to the zero vector

		algo.set(Variables::Preconditioner, std::string(std::get<1>(GetParam())));
		algo.set(Variables::MaxIterations, 670);
		algo.set(Variables::TargetError, std::get<2>(GetParam()));

		algo.set(Variables::MaxIterations, 667);
		algo.set(Variables::TargetError, 1e-4);

		algo.setOption(Variables::Method, std::string(std::get<0>(GetParam())));
		algo.setUpdaterFunc([](double x) {});

		/*
		{
			ScopedTimer t("Running solver");
		}
        const std::string& c = ::std::tr1::get<0>(GetParam());

		auto solutionFile = TestResources::rootDir() / ("dan_sol_" + c + ".mat");

		auto scirun4solution = reader.run(solutionFile.string());
		ASSERT_TRUE(scirun4solution.get() != nullptr);
		DenseColumnMatrixHandle expected = convertMatrix::toColumn(scirun4solution);

		EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*expected, *solution, ::std::tr1::get<1>(GetParam()));

		WriteMatrixAlgorithm writer;
		auto portedSolutionFile = TestResources::rootDir() / ("portedSolution_" +  c + ".txt");
		writer.run(solution, portedSolutionFile.string());

		auto diff = *expected - *solution;
		auto maxDiff = diff.maxCoeff();
		std::cout << "max diff is: " << maxDiff << std::endl;
		*/
		}
	virtual void TearDown(){}
};

TEST_P(SolveLinearSystemTestsAlgoParameterized, CanSolveDarrellParameterized)
{
	//EXPECT_NO_FATAL_FAILURE(SolveLinearSystemTestsAlgoParameterized);
		ASSERT_TRUE(A.get() != nullptr);
		EXPECT_EQ(10149, A->nrows());
		EXPECT_EQ(10149, A->ncols());

		ASSERT_TRUE(rhs.get() != nullptr);
		EXPECT_EQ(10149, rhs->nrows());
		EXPECT_EQ(1, rhs->ncols());

		ASSERT_FALSE(x0);

		ASSERT_TRUE(algo.run(A, convertMatrix::toColumn(rhs), x0, solution));

		ASSERT_TRUE(solution.get() != nullptr);
		EXPECT_EQ(10149, solution->nrows());
		EXPECT_EQ(1, solution->ncols());
}

INSTANTIATE_TEST_CASE_P(
	CanSolveDarrellParameterized,
	SolveLinearSystemTestsAlgoParameterized,
	Combine(Values(
		"cg",
		"bicg",
		"jacobi",
		"minres"
		),
		Values("jacobi","none"),
	Values(1e-1,1e-3,1e-4,1e-5))
	);
#else
TEST(DummyTest, CombineIsNotSupportedOnThisPlatform(){}
#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 euler toolbox - erfc/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of euler components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 22/02/2011
///
#include <nt2/euler/include/functions/erfc.hpp>
#include <boost/simd/sdk/simd/native.hpp>

extern "C" {long double cephes_erfcl(long double);}

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>

NT2_TEST_CASE_TPL ( erfc_real__1_0,  NT2_SIMD_REAL_TYPES)
{

  using nt2::erfc;
  using nt2::tag::erfc_;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<erfc_(vT)>::type r_t;
  typedef typename nt2::meta::call<erfc_(T)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(erfc(nt2::Inf<vT>())[0], nt2::Zero<sr_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Mzero<vT>())[0], nt2::One<sr_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Nan<vT>()[0]), nt2::Nan<sr_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::One<vT>())[0], T(0.157299207050285), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Half<vT>())[0], T(0.479500122186953), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Two<vT>())[0], T(0.004677734981047), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Ten<vT>())[0],  nt2::Zero <sr_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Zero<vT>())[0], nt2::One<sr_t>(), 200);
} // end of test for floating_


//  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_STATS_GAMMA_HPP
#define BOOST_STATS_GAMMA_HPP

// http://www.itl.nist.gov/div898/handbook/eda/section3/eda366b.htm
// http://mathworld.wolfram.com/GammaDistribution.html
// http://en.wikipedia.org/wiki/Gamma_distribution

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>

#include <utility>

namespace boost{ namespace math
{
namespace detail
{

template <class RealType, class Policy>
inline bool check_gamma_shape(
      const char* function,
      RealType shape,
      RealType* result, const Policy& pol)
{
   if((shape <= 0) || !(boost::math::isfinite)(shape))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Shape parameter is %1%, but must be > 0 !", shape, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool check_gamma_x(
      const char* function,
      RealType const& x,
      RealType* result, const Policy& pol)
{
   if((x < 0) || !(boost::math::isfinite)(x))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Random variate is %1% but must be >= 0 !", x, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool check_gamma(
      const char* function,
      RealType scale,
      RealType shape,
      RealType* result, const Policy& pol)
{
   return check_scale(function, scale, result, pol) && check_gamma_shape(function, shape, result, pol);
}

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class gamma_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   gamma_distribution(RealType shape, RealType scale = 1)
      : m_shape(shape), m_scale(scale)
   {
      RealType result;
      detail::check_gamma("boost::math::gamma_distribution<%1%>::gamma_distribution", scale, shape, &result, Policy());
   }

   RealType shape()const
   {
      return m_shape;
   }

   RealType scale()const
   {
      return m_scale;
   }
private:
   //
   // Data members:
   //
   RealType m_shape;     // distribution shape
   RealType m_scale;     // distribution scale
};

// NO typedef because of clash with name of gamma function.

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const gamma_distribution<RealType, Policy>& /* dist */)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const gamma_distribution<RealType, Policy>& /* dist */)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   using boost::math::tools::min_value;
   return std::pair<RealType, RealType>(min_value<RealType>(),  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const gamma_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::pdf(const gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_gamma_x(function, x, &result, Policy()))
      return result;

   if(x == 0)
   {
      return 0;
   }
   result = gamma_p_derivative(shape, x / scale, Policy()) / scale;
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const gamma_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::cdf(const gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_gamma_x(function, x, &result, Policy()))
      return result;

   result = boost::math::gamma_p(shape, x / scale, Policy());
   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const gamma_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 1)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = gamma_p_inv(shape, p, Policy()) * scale;

   return result;
}

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<gamma_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const gamma_distribution<%1%>&, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_gamma_x(function, c.param, &result, Policy()))
      return result;

   result = gamma_q(shape, c.param / scale, Policy());

   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<gamma_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const gamma_distribution<%1%>&, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();
   RealType q = c.param;

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 0)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = gamma_q_inv(shape, q, Policy()) * scale;

   return result;
}

template <class RealType, class Policy>
inline RealType mean(const gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

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

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;

   result = shape * scale;
   return result;
}

template <class RealType, class Policy>
inline RealType variance(const gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

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

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;

   result = shape * scale * scale;
   return result;
}

template <class RealType, class Policy>
inline RealType mode(const gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

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

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;

   if(shape < 1)
      return policies::raise_domain_error<RealType>(
         function,
         "The mode of the gamma distribution is only defined for values of the shape parameter >= 1, but got %1%.",
         shape, Policy());

   result = (shape - 1) * scale;
   return result;
}

//template <class RealType, class Policy>
//inline RealType median(const gamma_distribution<RealType, Policy>& dist)
//{  // Rely on default definition in derived accessors.
//}

template <class RealType, class Policy>
inline RealType skewness(const gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

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

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;

   result = 2 / sqrt(shape);
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

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

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result = 0;
   if(false == detail::check_gamma(function, scale, shape, &result, Policy()))
      return result;

   result = 6 / shape;
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis(const gamma_distribution<RealType, Policy>& dist)
{
   return kurtosis_excess(dist) + 3;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_GAMMA_HPP


/**
 * \file libs/numeric/ublasx/test/transform.cpp
 *
 * \brief Test suite for the \c transform operation.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/operation/transform.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <cmath>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1.0e-5;

template <typename AT, typename RT>
static RT my_function(AT const& x)
{
	return ::std::abs(x);
}

template <typename AT, typename RT>
struct my_functor: public std::unary_function<AT, RT>
{
	RT operator()(AT const& x)
	{
		return ::std::abs(x);
	}
};

BOOST_UBLASX_TEST_DEF( test_vector_function )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Vector - Unary Function");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_traits<vector_type>::size_type size_type;

	const std::size_t n(4);

	vector_type v(n);
	v(0) = -1;
	v(1) =  2;
	v(2) =  3;
	v(3) = -4;

	vector_type res;
	vector_type expect_res(n);

	res = ublasx::transform(v, boost::function<value_type (value_type)>(my_function<value_type,value_type>));

	BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );

	for (size_type i = 0; i < n; ++i)
	{
		expect_res(i) = my_function<value_type,value_type>(v(i));
	}

	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE(res, expect_res, n, tol);
}

BOOST_UBLASX_TEST_DEF( test_vector_functor )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Vector - Unary Functor");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_traits<vector_type>::size_type size_type;

	const std::size_t n(4);

	vector_type v(n);
	v(0) = -1;
	v(1) =  2;
	v(2) =  3;
	v(3) = -4;

	vector_type res;
	vector_type expect_res(n);

	res = ublasx::transform(v, my_functor<value_type,value_type>());

	BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );

	for (size_type i = 0; i < n; ++i)
	{
		expect_res(i) = my_functor<value_type,value_type>()(v(i));
	}

	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE(res, expect_res, n, tol);
}

BOOST_UBLASX_TEST_DEF( test_matrix_function )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Matrix - Unary Function");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	const std::size_t nr(3);
	const std::size_t nc(2);

	matrix_type A(nr,nc);
	A(0,0) = -1; A(0,1) =  2;
	A(1,0) =  3; A(1,1) = -4;
	A(2,0) = -5; A(2,1) = -6;

	matrix_type R;
	matrix_type expect_R(nr,nc);

	R = ublasx::transform(A, ::boost::function<value_type (value_type)>(my_function<value_type,value_type>));

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "R = " << R );

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			expect_R(r,c) = my_function<value_type,value_type>(A(r,c));
		}
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE(R, expect_R, nr, nc, tol);
}

BOOST_UBLASX_TEST_DEF( test_matrix_functor )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Matrix - Unary Functor");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	const std::size_t nr(3);
	const std::size_t nc(2);

	matrix_type A(nr,nc);
	A(0,0) = -1; A(0,1) =  2;
	A(1,0) =  3; A(1,1) = -4;
	A(2,0) = -5; A(2,1) = -6;

	matrix_type R;
	matrix_type expect_R(nr,nc);

	R = ublasx::transform(A, my_functor<value_type,value_type>());

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "R = " << R );

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			expect_R(r,c) = my_functor<value_type,value_type>()(A(r,c));
		}
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE(R, expect_R, nr, nc, tol);
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'transform' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( test_vector_function );
	BOOST_UBLASX_TEST_DO( test_vector_functor );
	BOOST_UBLASX_TEST_DO( test_matrix_function );
	BOOST_UBLASX_TEST_DO( test_matrix_functor );

	BOOST_UBLASX_TEST_END();
}


//
// Created by abakfja on 3/29/21.
//

#ifndef LA_VECTOR_ENGINE_TRAITS_HPP
#define LA_VECTOR_ENGINE_TRAITS_HPP

#include <boost/numeric/ublas/matrix/engine_traits.hpp>

namespace boost::numeric::ublas::experimental {

template<typename T, std::size_t N, class Layout>
struct static_vector_engine;

template<typename T, class layout_tag>
struct dynamic_vector_engine;

} // namespace boost::numeric::ublas::experimental

namespace boost::numeric::ublas::experimental::detail {
template<typename T, std::size_t N, class Layout>
struct is_static<static_vector_engine<T, N, Layout>> : std::true_type {
};

template<typename T, class Layout>
struct is_dynamic<dynamic_vector_engine<T, Layout>> : std::true_type {
};

}

#endif //LA_ENGINE_TRAITS_HPP


/*
Brian Staber (brian.staber@gmail.com)
*/

#ifndef DIRICHLETSTRIPELONGATION_STOCHASTICPOLYCONVEXHGO_HPP
#define DIRICHLETSTRIPELONGATION_STOCHASTICPOLYCONVEXHGO_HPP

#include "tensor_calculus.hpp"
#include "laplacepp.hpp"
#include "nearlyIncompressibleHyperelasticity.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

class dirichletStripElongation_StochasticPolyconvexHGO : public nearlyIncompressibleHyperelasticity
{
public:

    double w1, w2, w3, w4;
    double mean_c1, c1, deltaC1;
    double mean_c2, c2, deltaC2;
    double mean_u1, u1, deltaU1;
    double mean_mu4, mu4, deltaG4;
    double mean_mu1, mu1;
    double mean_mu2, mu2;
    double mean_mu3, mu3;
    double alpha1, alpha2;
    double alpha3, alpha4;
    double tau1, tau2;
    double alpha5, alpha6;
    double beta3, beta4;
    double theta;
    double epsilon = 1e-6;

    Epetra_IntSerialDenseVector cells_nodes_p1_med;
    Epetra_SerialDenseVector w1_gmrf, w2_gmrf, w3_gmrf, w4_gmrf;
    Epetra_SerialDenseVector a, b;
    Epetra_SerialDenseVector E1,E2,E3;

    dirichletStripElongation_StochasticPolyconvexHGO(Epetra_Comm & comm, Teuchos::ParameterList & Parameters){

        std::string mesh_file = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"), "mesh_file");
        //std::string boundary_file = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"), "boundary_file");
        unsigned int number_physical_groups = Teuchos::getParameter<unsigned int>(Parameters.sublist("Mesh"), "nb_phys_groups");
        std::string select_model = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"), "model");

        mean_mu1   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu1");
        mean_mu2   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu2");
        mean_mu3   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu3");
        mean_mu4   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu4");
        beta3      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "beta3");
        beta4      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "beta4");
        theta      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "theta");
        deltaC1    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaC1");
        deltaC2    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaC2");
        deltaU1    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaU1");
        deltaG4    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaG4");

        mean_c1 = 2.0*mean_mu3*beta3*beta3;
        mean_c2 = 2.0*mean_mu1 + std::sqrt(3.0)*3.0*mean_mu2;
        mean_u1 = 2.0*mean_mu1/mean_c2;

        double gamma = 2.0*mean_mu1/(std::sqrt(3.0)*3.0*mean_mu2);
        tau2 = (1.0 - deltaU1*deltaU1)/(deltaU1*deltaU1*gamma*(gamma+1.0));
        tau1 = (2.0*mean_mu1/(std::sqrt(3.0)*3.0*mean_mu2))*tau2;

        alpha1 = 1.0/(deltaC1*deltaC1);
        alpha2 = mean_c1*deltaC1*deltaC1;
        alpha3 = 1.0/(deltaC2*deltaC2);
        alpha4 = mean_c2*deltaC2*deltaC2;
        alpha5 = 1.0/(deltaG4*deltaG4);
        alpha6 = mean_mu4*deltaG4*deltaG4;

        Mesh = new mesh(comm, mesh_file, 1000.0);
        //Mesh->read_boundary_file(boundary_file,number_physical_groups);
        Comm = Mesh->Comm;

        StandardMap = new Epetra_Map(-1,3*Mesh->n_local_nodes_without_ghosts,&Mesh->local_dof_without_ghosts[0],0,*Comm);
        OverlapMap = new Epetra_Map(-1,3*Mesh->n_local_nodes,&Mesh->local_dof[0],0,*Comm);
        ImportToOverlapMap = new Epetra_Import(*OverlapMap,*StandardMap);
        create_FECrsGraph();

        a.Resize(3); b.Resize(3);
        E1.Resize(3); E2.Resize(3); E3.Resize(3);
        E1(0) = 1.0; E1(1) = 0.0; E1(2) = 0.0;
        E2(0) = 0.0; E2(1) = 1.0; E2(2) = 0.0;
        E3(0) = 0.0; E3(1) = 0.0; E3(2) = 1.0;
        for (int i=0; i<3; ++i){
            a(i) = std::cos(theta)*E1(i) + std::sin(theta)*E2(i);
            b(i) = std::cos(theta)*E1(i) - std::sin(theta)*E2(i);
        }

        setup_dirichlet_conditions();
    }

    ~dirichletStripElongation_StochasticPolyconvexHGO(){
    }

    void get_media(unsigned int & n_cells, unsigned int & n_nodes, std::string & path){

        std::ifstream connectivity_file_med;
        connectivity_file_med.open(path);

        w1_gmrf.Resize(n_nodes);
        w2_gmrf.Resize(n_nodes);
        w3_gmrf.Resize(n_nodes);
        w4_gmrf.Resize(n_nodes);
        if (connectivity_file_med.is_open()){
            cells_nodes_p1_med.Resize(4*n_cells);
            for (unsigned int e=0; e<4*n_cells; ++e){
                connectivity_file_med >> cells_nodes_p1_med[e];
                cells_nodes_p1_med[e] = cells_nodes_p1_med[e]-1;
            }
            connectivity_file_med.close();
        }
        else{
            std::cout << "Couldn't open the connectivity file for the media.\n";
        }

    }

    void get_matrix_and_rhs(Epetra_Vector & x, Epetra_FECrsMatrix & K, Epetra_FEVector & F){
        assemblePureDirichlet_homogeneousForcing(x,K,F);
    }

    void setup_dirichlet_conditions(){
        n_bc_dof = 0;
        int dof = 1;
        double coord;
        unsigned int node;
        for (unsigned int i=0; i<Mesh->n_local_nodes_without_ghosts; ++i){
            node = Mesh->local_nodes[i];
            coord = Mesh->nodes_coord[3*node+dof];
            if(coord==0.0){
                n_bc_dof+=3;
            }
            if(coord==10.0/1000.0){
                n_bc_dof+=3;
            }
        }

        int indbc = 0;
        dof_on_boundary = new int [n_bc_dof];
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            node = Mesh->local_nodes[inode];
            coord = Mesh->nodes_coord[3*node+dof];
            if (coord==0.0){
                dof_on_boundary[indbc+0] = 3*inode+0;
                dof_on_boundary[indbc+1] = 3*inode+1;
                dof_on_boundary[indbc+2] = 3*inode+2;
                indbc+=3;
            }
            if (coord==10.0/1000.0){
                dof_on_boundary[indbc+0] = 3*inode+0;
                dof_on_boundary[indbc+1] = 3*inode+1;
                dof_on_boundary[indbc+2] = 3*inode+2;
                indbc+=3;
            }
        }
    }

    void apply_dirichlet_conditions(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        Epetra_MultiVector v(*StandardMap,true);
        v.PutScalar(0.0);

        int node;
        int dof = 1;
        double coord;
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            node = Mesh->local_nodes[inode];
            coord = Mesh->nodes_coord[3*node+dof];
            if (coord==10.0/1000.0){
                v[0][StandardMap->LID(3*node+dof)] = displacement;
            }
        }

        Epetra_MultiVector rhs_dir(*StandardMap,true);
        K.Apply(v,rhs_dir);
        F.Update(-1.0,rhs_dir,1.0);

        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            node = Mesh->local_nodes[inode];
            coord = Mesh->nodes_coord[3*node+dof];
            if (coord==0.0){
                F[0][StandardMap->LID(3*node+0)] = 0.0;
                F[0][StandardMap->LID(3*node+1)] = 0.0;
                F[0][StandardMap->LID(3*node+2)] = 0.0;
            }
            if (coord==10.0/1000.0){
                F[0][StandardMap->LID(3*node+0)]   = 0.0;
                F[0][StandardMap->LID(3*node+dof)] = displacement;
                F[0][StandardMap->LID(3*node+2)]   = 0.0;
            }
        }
        ML_Epetra::Apply_OAZToMatrix(dof_on_boundary,n_bc_dof,K);
    }

    void get_material_parameters(unsigned int & e_lid, unsigned int & gp){

        int n_gauss_points = Mesh->n_gauss_cells;
        int e_gid = Mesh->local_cells[e_lid];
        int node;
        double xi = Mesh->xi_cells[gp]; double eta = Mesh->eta_cells[gp]; double zeta = Mesh->zeta_cells[gp];
        Epetra_SerialDenseVector N(4);
        tetra4::shape_functions(N,xi,eta,zeta);

        w1 = 0.0; w2 = 0.0; w3 = 0.0; w4 = 0.0;
        for (unsigned int j=0; j<4; ++j){
            node = cells_nodes_p1_med(4*e_gid+j);
            w1 += N(j)*w1_gmrf(node);
            w2 += N(j)*w2_gmrf(node);
            w3 += N(j)*w3_gmrf(node);
            w4 += N(j)*w4_gmrf(node);
        }

        c1  = icdf_gamma(w1,alpha1,alpha2);
        c2  = icdf_gamma(w2,alpha3,alpha4);
        u1  = icdf_beta (w3,tau1,tau2);
        mu4 = icdf_gamma(w4,alpha5,alpha6);

        mu1 = ( epsilon*mean_mu1 + (1.0/2.0)*c2*u1 )/( 1.0+epsilon );
        mu2 = ( epsilon*mean_mu2 + (1.0/(std::sqrt(3.0)*3.0))*c2*(1.0-u1) )/( 1.0+epsilon );
        mu3 = ( epsilon*mean_mu3 + c1/(2.0*beta3*beta3) )/( 1.0+epsilon );
        mu4 = ( epsilon*mean_mu4 + mu4 )/( 1.0+epsilon );

    }

    double icdf_gamma(double & w, double & alpha, double & beta){
        double erfx = boost::math::erf<double>(w/std::sqrt(2.0));
        double y = (1.0/2.0)*(1.0 + erfx);
        double yinv = boost::math::gamma_p_inv<double,double>(alpha,y);
        double z = yinv*beta;
        return z;
    }

    double icdf_beta(double & w, double & tau1, double & tau2){
        double erfx = boost::math::erf<double>(w/std::sqrt(2.0));
        double y = (1.0/2.0)*(1.0 + erfx);
        double z = boost::math::ibeta_inv<double,double,double>(tau1,tau2,y);
        return z;
    }

    void get_constitutive_tensors_static_condensation(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & inverse_cauchy, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){
        model_C(deformation_gradient, det, inverse_cauchy, piola_isc, piola_vol, tangent_piola_isc, tangent_piola_vol);
    }

    void get_internal_pressure(double & theta, double & pressure, double & dpressure){
        double ptheta = std::pow(theta,beta3);
        pressure  = beta3*( (ptheta/theta) - (1.0/(ptheta*theta)) );
        dpressure = beta3*( (beta3-1.0)*(ptheta/(theta*theta)) + (beta3+1.0)/(ptheta*theta*theta) );
    }

    void get_material_parameters_for_recover(unsigned int & e_lid){

        int e_gid = Mesh->local_cells[e_lid];
        double xi = 1.0/3.0; double eta = 1.0/3.0; double zeta = 1.0/3.0;
        Epetra_SerialDenseVector N(4);
        tetra4::shape_functions(N,xi,eta,zeta);

        w1 = 0.0; w2 = 0.0; w3 = 0.0; w4 = 0.0;
        for (unsigned int j=0; j<4; ++j){
            int node = cells_nodes_p1_med(4*e_gid+j);
            w1 += N(j)*w1_gmrf(node);
            w2 += N(j)*w2_gmrf(node);
            w3 += N(j)*w3_gmrf(node);
            w4 += N(j)*w4_gmrf(node);
        }

        c1  = icdf_gamma(w1,alpha1,alpha2);
        c2  = icdf_gamma(w2,alpha3,alpha4);
        u1  = icdf_beta (w3,tau1,tau2);
        mu4 = icdf_gamma(w4,alpha5,alpha6);

        mu1 = ( epsilon*mean_mu1 + (1.0/2.0)*c2*u1 )/( 1.0+epsilon );
        mu2 = ( epsilon*mean_mu2 + (1.0/(std::sqrt(3.0)*3.0))*c2*(1.0-u1) )/( 1.0+epsilon );
        mu3 = ( epsilon*mean_mu3 + c1/(2.0*beta3*beta3) )/( 1.0+epsilon );
        mu4 = ( epsilon*mean_mu4 + mu4 )/( 1.0+epsilon );

    }

    void get_stress_for_recover(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseMatrix & piola_stress){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)-deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)-deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)+deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)+deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)-deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);
        double beta = 1.0/(det*det);
        Epetra_SerialDenseMatrix eye(3,3);
        Epetra_SerialDenseMatrix M1(3,3), M2(3,3);
        Epetra_SerialDenseMatrix C(3,3), L(3,3);
        Epetra_SerialDenseMatrix piola_ani1(3,3), piola_ani2(3,3);

        eye(0,0) = 1.0; eye(0,1) = 0.0; eye(0,2) = 0.0;
        eye(1,0) = 0.0; eye(1,1) = 1.0; eye(1,2) = 0.0;
        eye(2,0) = 0.0; eye(2,1) = 0.0; eye(2,2) = 1.0;

        M1.Multiply('N','T',1.0,a,a,0.0);
        M2.Multiply('N','T',1.0,b,b,0.0);

        C.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        L(0,0) = (1.0/(det*det))*(C(1,1)*C(2,2)-C(1,2)*C(2,1));
        L(1,1) = (1.0/(det*det))*(C(0,0)*C(2,2)-C(0,2)*C(2,0));
        L(2,2) = (1.0/(det*det))*(C(0,0)*C(1,1)-C(0,1)*C(1,0));
        L(1,2) = (1.0/(det*det))*(C(0,2)*C(1,0)-C(0,0)*C(1,2));
        L(0,2) = (1.0/(det*det))*(C(0,1)*C(1,2)-C(0,2)*C(1,1));
        L(0,1) = (1.0/(det*det))*(C(0,2)*C(2,1)-C(0,1)*C(2,2));
        L(2,1) = L(1,2); L(2,0) = L(0,2); L(1,0) = L(0,1);

        double I1   = C(0,0) + C(1,1) + C(2,2);
        double II1  = C(0,0)*C(0,0) + C(1,1)*C(1,1) + C(2,2)*C(2,2) + 2.0*C(1,2)*C(1,2) + 2.0*C(0,2)*C(0,2) + 2.0*C(0,1)*C(0,1);
        double I2   = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0,0)*M1(0,0) + C(1,1)*M1(1,1) + C(2,2)*M1(2,2) + 2.0*C(0,1)*M1(0,1) + 2.0*C(0,2)*M1(0,2) + 2.0*C(1,2)*M1(1,2);
        double I4_2 = C(0,0)*M2(0,0) + C(1,1)*M2(1,1) + C(2,2)*M2(2,2) + 2.0*C(0,1)*M2(0,1) + 2.0*C(0,2)*M2(0,2) + 2.0*C(1,2)*M2(1,2);
        double pI2  = std::sqrt(I2);

        double S4_1 = (I4_1-1.0)*(I4_1-1.0);
        double S4_2 = (I4_2-1.0)*(I4_2-1.0);

        double ptheta = std::pow(det,beta3);
        double pressure = mu3*beta3*( (ptheta/det) - (1.0/(ptheta*det)) );

        for (unsigned int i=0; i<3; ++i){
            for (unsigned int j=0; j<3; ++j){
                piola_stress(i,j) = 2.0*mu1*alpha*(eye(i,j)-(1.0/3.0)*L(i,j))
                + mu2*beta*( 3.0*pI2*(I1*eye(i,j)-C(i,j)) - 2.0*I2*pI2*L(i,j) )
                + det*pressure*L(i,j);
                piola_ani1(i,j) = 4.0*mu4*(I4_1-1.0)*exp(beta4*S4_1)*M1(i,j);
                piola_ani2(i,j) = 4.0*mu4*(I4_2-1.0)*exp(beta4*S4_2)*M2(i,j);
            }
        }

        if (I4_1>1.0){
            piola_stress += piola_ani1;
        }
        if (I4_2>1.0){
            piola_stress += piola_ani2;
        }

    }

    void model_C(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & L, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)-deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)-deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)+deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)+deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)-deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);

        Epetra_SerialDenseVector eye(6);
        Epetra_SerialDenseMatrix rightCauchy(3,3);
        Epetra_SerialDenseVector M1(6), M2(6);
        Epetra_SerialDenseVector C(6), D(6);
        Epetra_SerialDenseVector piola_nh(6), piola_ani1(6), piola_ani2(6);

        M1(0) = a(0)*a(0); M2(0) = b(0)*b(0);
        M1(1) = a(1)*a(1); M2(1) = b(1)*b(1);
        M1(2) = a(2)*a(2); M2(2) = b(2)*b(2);
        M1(3) = a(1)*a(2); M2(3) = b(1)*b(2);
        M1(4) = a(0)*a(2); M2(4) = b(0)*b(2);
        M1(5) = a(0)*a(1); M2(5) = b(0)*b(1);

        rightCauchy.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        eye(0) = 1.0; eye(1) = 1.0; eye(2) = 1.0; eye(3) = 0.0; eye(4) = 0.0; eye(5) = 0.0;

        C(0) = rightCauchy(0,0); C(1) = rightCauchy(1,1); C(2) = rightCauchy(2,2);
        C(3) = rightCauchy(1,2); C(4) = rightCauchy(0,2); C(5) = rightCauchy(0,1);

        L(0) = (1.0/(det*det))*(rightCauchy(1,1)*rightCauchy(2,2)-rightCauchy(1,2)*rightCauchy(2,1));
        L(1) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(2,2)-rightCauchy(0,2)*rightCauchy(2,0));
        L(2) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(1,1)-rightCauchy(0,1)*rightCauchy(1,0));
        L(3) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(1,0)-rightCauchy(0,0)*rightCauchy(1,2));
        L(4) = (1.0/(det*det))*(rightCauchy(0,1)*rightCauchy(1,2)-rightCauchy(0,2)*rightCauchy(1,1));
        L(5) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(2,1)-rightCauchy(0,1)*rightCauchy(2,2));

        double I1 = C(0) + C(1) + C(2);
        double II1 = C(0)*C(0) + C(1)*C(1) + C(2)*C(2) + 2.0*C(3)*C(3) + 2.0*C(4)*C(4) + 2.0*C(5)*C(5);
        double I2 = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0)*M1(0) + C(1)*M1(1) + C(2)*M1(2) + 2.0*C(5)*M1(5) + 2.0*C(4)*M1(4) + 2.0*C(3)*M1(3);
        double I4_2 = C(0)*M2(0) + C(1)*M2(1) + C(2)*M2(2) + 2.0*C(5)*M2(5) + 2.0*C(4)*M2(4) + 2.0*C(3)*M2(3);
        double pI2 = std::sqrt(I2);

        double S4_1 = (I4_1-1.0)*(I4_1-1.0);
        double S4_2 = (I4_2-1.0)*(I4_2-1.0);

        for (unsigned int i=0; i<6; ++i){
            D(i) = I1*eye(i) - C(i);
            piola_nh(i) = 2.0*alpha*mu1*(eye(i)-(1.0/3.0)*I1*L(i));
            piola_isc(i) = piola_nh(i) + (mu2/(det*det))*( 3.0*pI2*D(i) - 2.0*pI2*I2*L(i) );

            piola_ani1(i) = 4.0*mu4*(I4_1-1.0)*exp(beta4*S4_1)*M1(i);
            piola_ani2(i) = 4.0*mu4*(I4_2-1.0)*exp(beta4*S4_2)*M2(i);

            piola_vol(i) = mu3*det*L(i);
        }

        double scalarAB;

        scalarAB = mu3*det;
        tensor_product(mu3*det,L,L,tangent_piola_vol,0.0);
        scalarAB = -2.0*mu3*det;
        sym_tensor_product(scalarAB,L,L,tangent_piola_vol,1.0);

        scalarAB = -2.0/3.0;
        tensor_product(scalarAB,piola_nh,L,tangent_piola_isc,0.0);
        tensor_product(scalarAB,L,piola_nh,tangent_piola_isc,1.0);

        scalarAB = -6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,D,eye,tangent_piola_isc,1.0);
        tensor_product(scalarAB,eye,D,tangent_piola_isc,1.0);

        scalarAB = (-4.0/9.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = (4.0/3.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        sym_tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = 3.0*mu2/(det*det*pI2);
        tensor_product(scalarAB,D,D,tangent_piola_isc,1.0);

        scalarAB = 6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        scalarAB = -scalarAB;
        sym_tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        if (I4_1>1.0){
            piola_isc += piola_ani1;
            scalarAB = (8.0*mu4 + 16.0*mu4*beta4*S4_1)*exp(beta4*S4_1);
            tensor_product(scalarAB,M1,M1,tangent_piola_isc,1.0);
        }
        if (I4_2>1.0){
            piola_isc += piola_ani2;
            scalarAB = (8.0*mu4 + 16.0*mu4*beta4*S4_2)*exp(beta4*S4_2);
            tensor_product(scalarAB,M2,M2,tangent_piola_isc,1.0);
        }
    }

};

#endif


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_BLOCK_CYPHERS_DETAIL_SHACAL_FUNCTIONS_HPP
#define BOOST_HASH_BLOCK_CYPHERS_DETAIL_SHACAL_FUNCTIONS_HPP

#include <boost/hash/block_cyphers/detail/basic_functions.hpp>

namespace boost {
namespace hashes {
namespace block_cyphers {
namespace detail {

//
// Implemented directly from the standard as found at
// http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
//

// Specifically, subsection 4.1

template <unsigned word_bits_>
struct basic_shacal_functions : basic_functions<word_bits_> {
    typedef typename basic_functions<word_bits_>::word_type word_type;

    static word_type Ch(word_type x, word_type y, word_type z) {
        return (x & y) ^ (~x & z);
    }
    static word_type Maj(word_type x, word_type y, word_type z) {
        return (x & y) ^ (x & z) ^ (y & z);
    }
};

struct shacal_functions : public basic_shacal_functions<32> {
    static word_type Parity(word_type x, word_type y, word_type z) {
        return x ^ y ^ z;
    }
    static word_type f(unsigned t, word_type x, word_type y, word_type z) {
        if (t < 40) {
            if (t < 20) return Ch(x, y, z);
        } else {
            if (t < 60) return Maj(x, y, z);
        }
        return Parity(x, y, z);
    }
};

typedef shacal_functions shacal0_functions;
typedef shacal_functions shacal1_functions;

template <unsigned word_bits_>
struct shacal2_functions;
template <>
struct shacal2_functions<32> : public basic_shacal_functions<32> {
    static word_type Sigma_0(word_type x) {
        return ROTR< 2>(x) ^ ROTR<13>(x) ^ ROTR<22>(x);
    }
    static word_type Sigma_1(word_type x) {
        return ROTR< 6>(x) ^ ROTR<11>(x) ^ ROTR<25>(x);
    }

    static word_type sigma_0(word_type x) {
        return ROTR< 7>(x) ^ ROTR<18>(x) ^ SHR< 3>(x);
    }
    static word_type sigma_1(word_type x) {
        return ROTR<17>(x) ^ ROTR<19>(x) ^ SHR<10>(x);
    }
};
template <>
struct shacal2_functions<64> : public basic_shacal_functions<64> {
    static word_type Sigma_0(word_type x) {
        return ROTR<28>(x) ^ ROTR<34>(x) ^ ROTR<39>(x);
    }
    static word_type Sigma_1(word_type x) {
        return ROTR<14>(x) ^ ROTR<18>(x) ^ ROTR<41>(x);
    }

    static word_type sigma_0(word_type x) {
        return ROTR< 1>(x) ^ ROTR< 8>(x) ^ SHR< 7>(x);
    }
    static word_type sigma_1(word_type x) {
        return ROTR<19>(x) ^ ROTR<61>(x) ^ SHR< 6>(x);
    }
};

} // namespace detail
} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_DETAIL_SHACAL_FUNCTIONS_HPP


#include "gr/io/io.h"
#include "gr/utils/geometry.h"
#include "gr/utils/sampling.h"
#include "gr/algorithms/match4pcsBase.h"
#include "gr/algorithms/Functor4pcs.h"
#include "gr/algorithms/FunctorSuper4pcs.h"
#include "gr/algorithms/FunctorBrute4pcs.h"
#include <gr/algorithms/PointPairFilter.h>

#include <Eigen/Dense>

#include <fstream>
#include <iostream>
#include <string>

#include "../demo-utils.h"

#include "ext/point_conversion.h"
#include "ext/point_extlib1.hpp"
#include "ext/point_extlib2.hpp"
#include "ext/pointadapter_extlib1.hpp"
#include "ext/pointadapter_extlib2.hpp"

#define sqr(x) ((x) * (x))

using namespace std;
using namespace gr;
using namespace gr::Demo;

// data IO
IOManager ioManager;

static inline void printExtPBindingParameterList(){ // TODO: Change name
    fprintf(stderr, "\t[ -p point_type (%d)]\n", point_type);
    fprintf(stderr, "\t[ -r result_file_name (%s) ]\n", output.c_str());
    fprintf(stderr, "\t[ -m output matrix file (%s) ]\n", outputMat.c_str());
    fprintf(stderr, "\t[ --sampled1 (output sampled cloud 1 -- debug only) ]\n");
    fprintf(stderr, "\t[ --sampled2 (output sampled cloud 2 -- debug only) ]\n");
}
struct TransformVisitor {
    template <typename Derived>
    inline void operator()(
            float fraction,
            float best_LCP,
            const Eigen::MatrixBase<Derived>& /*transformation*/) const {
      if (fraction >= 0)
        {
          printf("done: %d%c best: %f                  \r",
               static_cast<int>(fraction * 100), '%', best_LCP);
          fflush(stdout);
        }
    }
    constexpr bool needsGlobalTransformation() const { return false; }
};

/**
  * If desired (by setting outputSampled1 and outputSampled2), the clouds could be
  * exported. The function takes the matcher as parameter, and exports the sampled
  * clouds of the matcher.
  */
template<typename Matcher>
void exportSampledClouds(const Matcher& matcher, const Utils::Logger& logger)
{
  if(! outputSampled1.empty() ){
    logger.Log<Utils::Verbose>( "Exporting Sampled cloud 1 to ",
                                outputSampled1.c_str(),
                                " ..." );
    ioManager.WriteObject((char *)outputSampled1.c_str(),
                           matcher.getFirstSampled(),
                           vector<Eigen::Matrix2f>(),
                           vector<typename Point3D<float>::VectorType>(), // dummy
                           vector<tripple>(),
                           vector<string>());
    logger.Log<Utils::Verbose>( "Export DONE" );
  }
  if(! outputSampled2.empty() ){
    logger.Log<Utils::Verbose>( "Exporting Sampled cloud 2 to ",
                                outputSampled2.c_str(),
                                " ..." );
    ioManager.WriteObject((char *)outputSampled2.c_str(),
                           matcher.getSecondSampled(),
                           vector<Eigen::Matrix2f>(),
                           vector<typename Point3D<float>::VectorType>(), // dummy
                           vector<tripple>(),
                           vector<string>());
    logger.Log<Utils::Verbose>( "Export DONE" );
  }
}

template <
    typename Matcher,
    typename PointType,
    typename Options,
    typename Range,
    template<typename> typename Sampler,
    typename TransformVisitor>
typename PointType::Scalar computeAlignment (
    const Options& options,
    const Utils::Logger& logger,
    const Range& P,
    const Range& Q,
    Eigen::Ref<Eigen::Matrix<typename PointType::Scalar, 4, 4>> mat,
    const Sampler<PointType>& sampler,
    TransformVisitor& visitor
    ) {
  Matcher matcher (options, logger);
  logger.Log<Utils::Verbose>( "Starting registration" );
  typename PointType::Scalar score = matcher.ComputeTransformation(P, Q, mat, sampler, visitor);

  logger.Log<Utils::Verbose>( "Score: ", score );
  logger.Log<Utils::Verbose>( "(Homogeneous) Transformation from ",
                              input2.c_str(),
                              " to ",
                              input1.c_str(),
                              ": \n",
                              mat);

  if(! outputSampled1.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Sampled cloud 1 to ",
                                  outputSampled1.c_str(),
                                  " ..." );
      ioManager.WriteObject((char *)outputSampled1.c_str(),
                             matcher.getFirstSampled(),
                             vector<Eigen::Matrix2f>(),
                             vector<typename Point3D<float>::VectorType>(), // dummy
                             vector<tripple>(),
                             vector<string>());
      logger.Log<Utils::Verbose>( "Export DONE" );
  }
  if(! outputSampled2.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Sampled cloud 2 to ",
                                  outputSampled2.c_str(),
                                  " ..." );
      ioManager.WriteObject((char *)outputSampled2.c_str(),
                             matcher.getSecondSampled(),
                             vector<Eigen::Matrix2f>(),
                             vector<typename Point3D<float>::VectorType>(), // dummy
                             vector<tripple>(),
                             vector<string>());
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  return score;
}

int main(int argc, char **argv) {
  using namespace gr;
  using Scalar = float;

  /**
   * Point clouds are read as gr::Point3D, then converted to other types to emulate
   * PointAdapter usage. This way, we can make use of IOManager to read point cloud
   * files, and lightweight point converter functions to convert gr points to example
   * external point type
   */
  vector<Point3D<Scalar> > set1, set2;
  vector<Eigen::Matrix2f> tex_coords1, tex_coords2;
  vector<typename Point3D<Scalar>::VectorType> normals1, normals2;
  vector<tripple> tris1, tris2;
  vector<std::string> mtls1, mtls2;

  // Match and return the score (estimated overlap or the LCP).
  typename Point3D<Scalar>::Scalar score = 0;

  constexpr Utils::LogLevel loglvl = Utils::Verbose;

  using TrVisitorType = typename std::conditional <loglvl==Utils::NoLog,
                            DummyTransformVisitor,
                            TransformVisitor>::type;
  using PairFilter = gr::AdaptivePointFilter;

  TrVisitorType visitor;
  Utils::Logger logger(loglvl);

  /// TODO Add proper error codes
  if(argc < 4){
    Demo::printUsage(argc, argv);
    printExtPBindingParameterList();
    exit(-2);
  }
  if(int c = Demo::getArgs(argc, argv) != 0)
  {
    Demo::printUsage(argc, argv);
    printExtPBindingParameterList();
    exit(std::max(c,0));
  }

  using MatrixType = Eigen::Matrix<Scalar, 4, 4>;
  MatrixType mat (MatrixType::Identity());

  // Read the inputs.
  if (!ioManager.ReadObject((char *)input1.c_str(), set1, tex_coords1, normals1, tris1,
                  mtls1)) {
    logger.Log<Utils::ErrorReport>("Can't read input set1");
    exit(-1);
  }

  if (!ioManager.ReadObject((char *)input2.c_str(), set2, tex_coords2, normals2, tris2,
                  mtls2)) {
    logger.Log<Utils::ErrorReport>("Can't read input set2");
    exit(-1);
  }

  // clean only when we have pset to avoid wrong face to point indexation
  if (tris1.size() == 0)
    Utils::CleanInvalidNormals(set1, normals1);
  if (tris2.size() == 0)
    Utils::CleanInvalidNormals(set2, normals2);

  try {
    if(point_type == 0) // gr::Point3D, directly pass the read sets
    {
      logger.Log<Utils::Verbose>("Registration on std::vector of gr::Point3D<float>"
                                 " instances - a point adapter is not required."    );

      using PointType    = gr::Point3D<Scalar>;
      /**
       * Here, PointAdapter does not really serve as an adapter since the point type
       * that we have as the type of the instances of points already comply with
       * the PointConcept. Therefore, we just pass the point type as the adapter type.
       * */
      using PointAdapter = gr::Point3D<Scalar>;
      using MatcherType  = gr::Match4pcsBase<gr::FunctorSuper4PCS, PointAdapter,
                                             TrVisitorType, gr::AdaptivePointFilter,
                                             gr::AdaptivePointFilter::Options>;
      using OptionType   = typename MatcherType::OptionsType;

      UniformDistSampler<PointAdapter> sampler;

      OptionType options;
      if(! Demo::setOptionsFromArgs(options, logger))
        exit(-2); /// \FIXME use status codes for error reporting

      // Create a matcher instance
      MatcherType matcher (options, logger);

      logger.Log<Utils::Verbose>( "Starting registration" );

      // Compute transformation: put transformation to mat, return registration score
      Scalar score = matcher.ComputeTransformation(set1, set2, mat, sampler, visitor);

      logger.Log<Utils::Verbose>( "Score: ", score );
      logger.Log<Utils::Verbose>( "(Homogeneous) Transformation from ",
                              input2.c_str(),
                              " to ",
                              input1.c_str(),
                              ": \n",
                              mat);

      // Export sampled point clouds - if paths were set
      exportSampledClouds(matcher, logger);
    }
    else if(point_type == 1) /* extlib1::PointType1, convert read sets to vector
                              * of PointType1 to emulate PointAdapter usage */
    {
      logger.Log<Utils::Verbose>("Registration on std::vector of extlib1::PointType1"
                                 "instances using extlib1::PointAdapter"             );

      /**
       * Convert the vectors of gr::Point3D<float> instances to some containers of
       * extlib1::PointType1 instances, so that, we can assume that we have some
       * containers of extlib1::PointType1 instances in hand to use as input to
       * gr registration methods.
       */
//! [Creating a vector of extlib1::PointType1]
      std::vector<extlib1::PointType1> set1_point1 = getExtlib1Points(set1);
      std::vector<extlib1::PointType1> set2_point1 = getExtlib1Points(set2);
//! [Creating a vector of extlib1::PointType1]
      using PointType    = extlib1::PointType1;
      /**
       * Here, extlib1::PointType1 is not a model of gr::PointConcept. Hence, an adapter
       * type is needed to have registration on this external point type. extlib1::PointAdapter
       * serves as the adapter. Therefore, whenever gr requires a point type to be passed,
       * the adapter type will be passed, which it will use to wrap the extlib1::PointType1
       * instances on-the-fly to reach the information.
       */
//! [Point adapter and matcher type]
      using PointAdapter = extlib1::PointAdapter;
      using MatcherType = gr::Match4pcsBase<gr::FunctorSuper4PCS,
                                            PointAdapter, // here is the adapter!
                                            TrVisitorType,
                                            gr::AdaptivePointFilter,
                                            gr::AdaptivePointFilter::Options>;
//! [Point adapter and matcher type]
      using OptionType  = typename MatcherType::OptionsType;
//! [Sampler type]
      UniformDistSampler<PointAdapter> sampler;
//! [Sampler type]
      OptionType options;
      if(! Demo::setOptionsFromArgs(options, logger))
        exit(-2); /// \FIXME use status codes for error reporting

      logger.Log<Utils::Verbose>( "Starting registration" );
//! [Matcher instantiation and computation]
      // Create a matcher instance
      MatcherType matcher (options, logger);
      // Compute transformation: put transformation to mat, return registration score
      Scalar score = matcher.ComputeTransformation(set1_point1, set2_point1, mat, sampler, visitor);
//! [Matcher instantiation and computation]
      logger.Log<Utils::Verbose>( "Score: ", score );
      logger.Log<Utils::Verbose>( "(Homogeneous) Transformation from ",
                              input2.c_str(),
                              " to ",
                              input1.c_str(),
                              ": \n",
                              mat);

      // Export sampled point clouds - if paths were set
      exportSampledClouds(matcher, logger);
    }
    else if(point_type == 2) /* extlib2::PointType2, convert read sets to lists
                              * of PointType2 to emulate PointAdapter usage */
    {
      logger.Log<Utils::Verbose>("Registration on std::list of extlib2::PointType2"
                                 " instances using extlib2::PointAdapter");

      /**
       * Convert the vectors of gr::Point3D<float> instances to some containers of
       * extlib2::PointType2 instances, so that, we can assume that we have some
       * containers of extlib2::PointType2 instances in hand to use as input to
       * gr registration methods.
       */
      std::list<extlib2::PointType2> set1_point2 = getExtlib2Points(set1);
      std::list<extlib2::PointType2> set2_point2 = getExtlib2Points(set2);

      using PointType    = extlib2::PointType2;
      /**
       * Here, extlib2::PointType2 is not a model of gr::PointConcept. Hence, an adapter
       * type is needed to have registration on this external point type. extlib2::PointAdapter
       * serves as the adapter. Therefore, whenever gr requires a point type to be passed,
       * the adapter type will be passed, which it will use to wrap the extlib2::PointType2
       * instances on-the-fly to reach the information.
       */
      using PointAdapter = extlib2::PointAdapter;
      using MatcherType = gr::Match4pcsBase<gr::FunctorSuper4PCS, PointAdapter, TrVisitorType, gr::AdaptivePointFilter, gr::AdaptivePointFilter::Options>;
      using OptionType  = typename MatcherType::OptionsType;

      UniformDistSampler<PointAdapter> sampler;

      OptionType options;
      if(! Demo::setOptionsFromArgs(options, logger))
        exit(-2); /// \FIXME use status codes for error reporting

      // Create a matcher instance
      MatcherType matcher (options, logger);

      logger.Log<Utils::Verbose>( "Starting registration" );

      // Compute transformation: put transformation to mat, return registration score
      Scalar score = matcher.ComputeTransformation(set1_point2, set2_point2, mat, sampler, visitor);

      logger.Log<Utils::Verbose>( "Score: ", score );
      logger.Log<Utils::Verbose>( "(Homogeneous) Transformation from ",
                              input2.c_str(),
                              " to ",
                              input1.c_str(),
                              ": \n",
                              mat);

      // Export sampled point clouds - if paths were set
      exportSampledClouds(matcher, logger);

      // deallocate memory for points2: needed due to internals of extlib2::PointType2
      if(!set1_point2.empty()) {
        delete[] set1_point2.front().posBuffer;
        delete[] set1_point2.front().nBuffer;
        delete[] set1_point2.front().colorBuffer;
        set1_point2.clear();
      }

      if(!set2_point2.empty()) {
        delete[] set2_point2.front().posBuffer;
        delete[] set2_point2.front().nBuffer;
        delete[] set2_point2.front().colorBuffer;
        set2_point2.clear();
      }
    }
    else
    {
      std::cerr << "Invalid point type" << std::endl;
      exit(-1);
    }
  }
  catch (const std::exception& e) {
      logger.Log<Utils::ErrorReport>( "[Error]: " , e.what() );
      logger.Log<Utils::ErrorReport>( "Aborting with code -3 ..." );
      return -3;
  }
  catch (...) {
      logger.Log<Utils::ErrorReport>( "[Unknown Error]: Aborting with code -4 ..." );
      return -4;
  }

  if(! outputMat.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Matrix to ",
                                  outputMat.c_str(),
                                  "..." );
      ioManager.WriteMatrix(outputMat, mat.cast<double>(), IOManager::POLYWORKS);
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  if (! output.empty() ){

      logger.Log<Utils::Verbose>( "Exporting Registered geometry to ",
                                  output.c_str(),
                                  "..." );
      Utils::TransformPointCloud(set2, mat);
      ioManager.WriteObject((char *)output.c_str(),
                             set2,
                             tex_coords2,
                             normals2,
                             tris2,
                             mtls2);
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  return 0;
}


//
// Created by AIYOJ on 2019-03-04.
//

#include "gtest/gtest.h"
#include <iostream>
#include "calibration/hybrid.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace algocomp_test {

    class TestHybrid : public ::testing::Test {
    protected:
        std::unique_ptr<algocomp::Hybrid> calibration;

        virtual void SetUp() {
            calibration.reset(new algocomp::Hybrid());
        }

        virtual void TearDown() {

        }
    };

    TEST_F(TestHybrid, hybrid) {
        calibration->update(1);
        std::vector<float> scores;
        scores.push_back(0.8);
        scores.push_back(0.9);
        scores.push_back(0.4);

        std::vector<std::pair<float, float>> bins;
        bins.push_back(std::make_pair(0.2, 0.3));
        bins.push_back(std::make_pair(0.4, 0.6));
        bins.push_back(std::make_pair(0.7, 0.8));
        bins.push_back(std::make_pair(0.9, 0.9));
        calibration->update(bins);
        std::vector<float> calibrated_scores = calibration->calibrate(scores);
        for (int i = 0; i < scores.size(); ++i) {
            EXPECT_EQ(calibrated_scores[i], calibration->get(scores[i]));
        }

        calibration->update(0.5);
        scores.clear();
        scores.push_back(0.5);
        scores.push_back(0.7);
        scores.push_back(0.8);
        calibrated_scores = calibration->calibrate(scores);
        for (int i = 0; i < scores.size(); ++i) {
            float score = scores[i] / (scores[i] + (1 - scores[i]) * 2);
            EXPECT_EQ(calibrated_scores[i], calibration->get(score));
        }
    }

    TEST_F(TestHybrid, load_hybrid) {
        calibration = algocomp::Hybrid::load("test/test-data/1-calibrations/1-prior-correction.txt",
                                             "test/test-data/1-calibrations/1-isotonic-regression.txt");

        std::vector<float> scores;
        scores.push_back(0.8);
        scores.push_back(0.9);
        scores.push_back(0.4);

        std::vector<float> correct_calibrated_scores;
        std::for_each(scores.begin(), scores.end(), [this, &correct_calibrated_scores](const float &v) {
            float score = v / (v + (1 - v) * 2);
            correct_calibrated_scores.push_back(calibration->get(score));
        });

        std::vector<float> calibrated_scores = calibration->calibrate(scores);
        for (int i = 0; i < scores.size(); ++i) {
            EXPECT_EQ(calibrated_scores[i], correct_calibrated_scores[i]);
        }
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/algorithms/detail/assign_values.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

// Temporary / will be Strategy as template parameter
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/strategies/side_info.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Segment, size_t Dimension>
struct segment_arrange
{
    template <typename T>
    static inline void apply(Segment const& s, T& s_1, T& s_2, bool& swapped)
    {
        s_1 = get<0, Dimension>(s);
        s_2 = get<1, Dimension>(s);
        if (s_1 > s_2)
        {
            std::swap(s_1, s_2);
            swapped = true;
        }
    }
};

template <std::size_t Index, typename Segment>
inline typename geometry::point_type<Segment>::type get_from_index(
            Segment const& segment)
{
    typedef typename geometry::point_type<Segment>::type point_type;
    point_type point;
    geometry::detail::assign::assign_point_from_index
        <
            Segment, point_type, Index, 0, dimension<Segment>::type::value
        >::apply(segment, point);
    return point;
}

}
#endif

/***
template <typename T>
inline std::string rdebug(T const& value)
{
    if (math::equals(value, 0)) return "'0'";
    if (math::equals(value, 1)) return "'1'";
    if (value < 0) return "<0";
    if (value > 1) return ">1";
    return "<0..1>";
}
***/

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template <typename Policy, typename CalculationType = void>
struct relate_cartesian_segments
{
    typedef typename Policy::return_type return_type;
    typedef typename Policy::segment_type1 segment_type1;
    typedef typename Policy::segment_type2 segment_type2;

    //typedef typename point_type<segment_type1>::type point_type;
    //BOOST_CONCEPT_ASSERT( (concept::Point<point_type>) );

    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type1>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type2>) );

    typedef typename select_calculation_type
        <segment_type1, segment_type2, CalculationType>::type coordinate_type;

    /// Relate segments a and b
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b)
    {
        coordinate_type const dx_a = get<1, 0>(a) - get<0, 0>(a); // distance in x-dir
        coordinate_type const dx_b = get<1, 0>(b) - get<0, 0>(b);
        coordinate_type const dy_a = get<1, 1>(a) - get<0, 1>(a); // distance in y-dir
        coordinate_type const dy_b = get<1, 1>(b) - get<0, 1>(b);
        return apply(a, b, dx_a, dy_a, dx_b, dy_b);
    }

    // Relate segments a and b using precalculated differences.
    // This can save two or four subtractions in many cases
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b,
            coordinate_type const& dx_a, coordinate_type const& dy_a,
            coordinate_type const& dx_b, coordinate_type const& dy_b)
    {
        // 1) Handle "disjoint", probably common case.
        // per dimension, 2 cases: a_1----------a_2    b_1-------b_2 or B left of A
        coordinate_type ax_1, ax_2, bx_1, bx_2;
        bool ax_swapped = false, bx_swapped = false;
        detail::segment_arrange<segment_type1, 0>::apply(a, ax_1, ax_2, ax_swapped);
        detail::segment_arrange<segment_type2, 0>::apply(b, bx_1, bx_2, bx_swapped);
        if (ax_2 < bx_1 || ax_1 > bx_2)
        {
            return Policy::disjoint();
        }

        // 1b) In Y-dimension
        coordinate_type ay_1, ay_2, by_1, by_2;
        bool ay_swapped = false, by_swapped = false;
        detail::segment_arrange<segment_type1, 1>::apply(a, ay_1, ay_2, ay_swapped);
        detail::segment_arrange<segment_type2, 1>::apply(b, by_1, by_2, by_swapped);
        if (ay_2 < ay_1 || ay_1 > by_2)
        {
            return Policy::disjoint();
        }

        typedef side::side_by_triangle<coordinate_type> side;
        side_info sides;

        // 2) Calculate sides
        // Note: Do NOT yet calculate the determinant here, but use the SIDE strategy.
        // Determinant calculation is not robust; side (orient) can be made robust
        // (and is much robuster even without measures)
        sides.set<1>
            (
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<0>(b)),
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<1>(b))
            );

        if (sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        // 2b) For other segment
        sides.set<0>
            (
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<0>(a)),
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<1>(a))
            );

        if (sides.same<0>())
        {
            return Policy::disjoint();
        }

        // Degenerate cases: segments of single point, lying on other segment, non disjoint
        coordinate_type const zero = 0;
        if (math::equals(dx_a, zero) && math::equals(dy_a, zero))
        {
            return Policy::degenerate(a, true);
        }
        if (math::equals(dx_b, zero) && math::equals(dy_b, zero))
        {
            return Policy::degenerate(b, false);
        }

        bool collinear = sides.collinear();

        // Get the same type, but at least a double (also used for divisions)
        typedef typename select_most_precise
            <
                coordinate_type, double
            >::type promoted_type;

        promoted_type const d = (dy_b * dx_a) - (dx_b * dy_a);
        // Determinant d should be nonzero.
        // If it is zero, we have an robustness issue here,
        // (and besides that we cannot divide by it)
        if(math::equals(d, zero) && ! collinear)
        //if(! collinear && sides.as_collinear())
        {
#ifdef BOOST_GEOMETRY_DEBUG_INTERSECTION
            std::cout << "Determinant zero? Type : "
                << typeid(coordinate_type).name()
                << std::endl;

            std::cout << " dx_a : " << dx_a << std::endl;
            std::cout << " dy_a : " << dy_a << std::endl;
            std::cout << " dx_b : " << dx_b << std::endl;
            std::cout << " dy_b : " << dy_b << std::endl;

            std::cout << " side a <-> b.first : " << sides.get<0,0>() << std::endl;
            std::cout << " side a <-> b.second: " << sides.get<0,1>() << std::endl;
            std::cout << " side b <-> a.first : " << sides.get<1,0>() << std::endl;
            std::cout << " side b <-> a.second: " << sides.get<1,1>() << std::endl;
#endif

            if (sides.as_collinear())
            {
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;
            }
            else
            {
                return Policy::error("Determinant zero!");
            }
        }

        if(collinear)
        {
            // Segments are collinear. We'll find out how.
            if (math::equals(dx_b, zero))
            {
                // Vertical -> Check y-direction
                return relate_collinear(a, b,
                        ay_1, ay_2, by_1, by_2,
                        ay_swapped, by_swapped);
            }
            else
            {
                // Check x-direction
                return relate_collinear(a, b,
                        ax_1, ax_2, bx_1, bx_2,
                        ax_swapped, bx_swapped);
            }
        }

        return Policy::segments_intersect(sides,
            dx_a, dy_a, dx_b, dy_b,
            a, b);
    }

private :

    /// Relate segments known collinear
    static inline return_type relate_collinear(segment_type1 const& a
            , segment_type2 const& b
            , coordinate_type a_1, coordinate_type a_2
            , coordinate_type b_1, coordinate_type b_2
            , bool a_swapped, bool b_swapped)
    {
        // All ca. 150 lines are about collinear rays
        // The intersections, if any, are always boundary points of the segments. No need to calculate anything.
        // However we want to find out HOW they intersect, there are many cases.
        // Most sources only provide the intersection (above) or that there is a collinearity (but not the points)
        // or some spare sources give the intersection points (calculated) but not how they align.
        // This source tries to give everything and still be efficient.
        // It is therefore (and because of the extensive clarification comments) rather long...

        // \see http://mpa.itc.it/radim/g50history/CMP/4.2.1-CERL-beta-libes/file475.txt
        // \see http://docs.codehaus.org/display/GEOTDOC/Point+Set+Theory+and+the+DE-9IM+Matrix
        // \see http://mathworld.wolfram.com/Line-LineIntersection.html

        // Because of collinearity the case is now one-dimensional and can be checked using intervals
        // This function is called either horizontally or vertically
        // We get then two intervals:
        // a_1-------------a_2 where a_1 < a_2
        // b_1-------------b_2 where b_1 < b_2
        // In all figures below a_1/a_2 denotes arranged intervals, a1-a2 or a2-a1 are still unarranged

        // Handle "equal", in polygon neighbourhood comparisons a common case

        // Check if segments are equal...
        bool const a1_eq_b1 = math::equals(get<0, 0>(a), get<0, 0>(b))
                    && math::equals(get<0, 1>(a), get<0, 1>(b));
        bool const a2_eq_b2 = math::equals(get<1, 0>(a), get<1, 0>(b))
                    && math::equals(get<1, 1>(a), get<1, 1>(b));
        if (a1_eq_b1 && a2_eq_b2)
        {
            return Policy::segment_equal(a, false);
        }

        // ... or opposite equal
        bool const a1_eq_b2 = math::equals(get<0, 0>(a), get<1, 0>(b))
                    && math::equals(get<0, 1>(a), get<1, 1>(b));
        bool const a2_eq_b1 = math::equals(get<1, 0>(a), get<0, 0>(b))
                    && math::equals(get<1, 1>(a), get<0, 1>(b));
        if (a1_eq_b2 && a2_eq_b1)
        {
            return Policy::segment_equal(a, true);
        }

        // The rest below will return one or two intersections.
        // The delegated class can decide which is the intersection point, or two, build the Intersection Matrix (IM)
        // For IM it is important to know which relates to which. So this information is given,
        // without performance penalties to intersection calculation

        bool const has_common_points = a1_eq_b1 || a1_eq_b2 || a2_eq_b1 || a2_eq_b2;

        // "Touch" -> one intersection point -> one but not two common points
        // -------->             A (or B)
        //         <----------   B (or A)
        //        a_2==b_1         (b_2==a_1 or a_2==b1)

        // The check a_2/b_1 is necessary because it excludes cases like
        // ------->
        //     --->
        // ... which are handled lateron

        // Corresponds to 4 cases, of which the equal points are determined above
        // #1: a1---->a2 b1--->b2   (a arrives at b's border)
        // #2: a2<----a1 b2<---b1   (b arrives at a's border)
        // #3: a1---->a2 b2<---b1   (both arrive at each others border)
        // #4: a2<----a1 b1--->b2   (no arrival at all)
        // Where the arranged forms have two forms:
        //    a_1-----a_2/b_1-------b_2 or reverse (B left of A)
        if (has_common_points && (math::equals(a_2, b_1) || math::equals(b_2, a_1)))
        {
            if (a2_eq_b1) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, -1);
            if (a1_eq_b2) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, 0);
            if (a2_eq_b2) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, 0);
            if (a1_eq_b1) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, -1);
        }

        // "Touch/within" -> there are common points and also an intersection of interiors:
        // Corresponds to many cases:
        // #1a: a1------->a2  #1b:        a1-->a2
        //          b1--->b2         b1------->b2
        // #2a: a2<-------a1  #2b:        a2<--a1
        //          b1--->b2         b1------->b2
        // #3a: a1------->a2  #3b:        a1-->a2
        //          b2<---b1         b2<-------b1
        // #4a: a2<-------a1  #4b:        a2<--a1
        //          b2<---b1         b2<-------b1

        // Note: next cases are similar and handled by the code
        // #4c: a1--->a2
        //      b1-------->b2
        // #4d: a1-------->a2
        //      b1-->b2

        // For case 1-4: a_1 < (b_1 or b_2) < a_2, two intersections are equal to segment B
        // For case 5-8: b_1 < (a_1 or a_2) < b_2, two intersections are equal to segment A
        if (has_common_points)
        {
            // Either A is in B, or B is in A, or (in case of robustness/equals)
            // both are true, see below
            bool a_in_b = (b_1 < a_1 && a_1 < b_2) || (b_1 < a_2 && a_2 < b_2);
            bool b_in_a = (a_1 < b_1 && b_1 < a_2) || (a_1 < b_2 && b_2 < a_2);

            if (a_in_b && b_in_a)
            {
                // testcase "ggl_list_20110306_javier"
                // In robustness it can occur that a point of A is inside B AND a point of B is inside A,
                // still while has_common_points is true (so one point equals the other).
                // If that is the case we select on length.
                coordinate_type const length_a = geometry::math::abs(a_1 - a_2);
                coordinate_type const length_b = geometry::math::abs(b_1 - b_2);
                if (length_a > length_b)
                {
                    a_in_b = false;
                }
                else
                {
                    b_in_a = false;
                }
            }

            int const arrival_a = a_in_b ? 1 : -1;
            if (a2_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, 0, false);
            if (a1_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, 0, true);
            if (a2_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, -arrival_a, true);
            if (a1_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, -arrival_a, false);
        }

        bool const opposite = a_swapped ^ b_swapped;

        // "Inside", a completely within b or b completely within a
        // 2 cases:
        // case 1:
        //        a_1---a_2        -> take A's points as intersection points
        //   b_1------------b_2
        // case 2:
        //   a_1------------a_2
        //       b_1---b_2         -> take B's points
        if (a_1 > b_1 && a_2 < b_2)
        {
            // A within B
            return Policy::collinear_a_in_b(a, opposite);
        }
        if (b_1 > a_1 && b_2 < a_2)
        {
            // B within A
            return Policy::collinear_b_in_a(b, opposite);
        }

        /*

        Now that all cases with equal,touch,inside,disjoint,
        degenerate are handled the only thing left is an overlap

        Either a1 is between b1,b2
        or a2 is between b1,b2 (a2 arrives)

        Next table gives an overview.
        The IP's are ordered following the line A1->A2

             |                                 |
             |          a_2 in between         |       a_1 in between
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
             |          b1----->b2             |   b1----->b2
             |   (b1,a2), a arrives            |   (a1,b2), b arrives
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|   a2<---------a1*               |       a2<---------a1*
             |           b1----->b2            |   b1----->b2
             |   (a1,b1), no arrival           |   (b2,a2), a and b arrive
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
        b sw.|           b2<-----b1            |   b2<-----b1
             |   (b2,a2), a and b arrive       |   (a1,b1), no arrival
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|    a2<---------a1*              |       a2<---------a1*
        b sw.|            b2<-----b1           |   b2<-----b1
             |   (a1,b2), b arrives            |   (b1,a2), a arrives
             |                                 |
        -----+---------------------------------+--------------------------
        * Note that a_1 < a_2, and a1 <> a_1; if a is swapped,
          the picture might seem wrong but it (supposed to be) is right.
        */

        bool const both_swapped = a_swapped && b_swapped;
        if (b_1 < a_2 && a_2 < b_2)
        {
            // Left column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                ;
        }
        if (b_1 < a_1 && a_1 < b_2)
        {
            // Right column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                ;
        }
        // Nothing should goes through. If any we have made an error
        // Robustness: it can occur here...
        return Policy::error("Robustness issue, non-logical behaviour");
    }
};

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP


#include <drawing.h>
#include <GL/glut.h>
#include <GL/freeglut.h>
#include <iostream>
#include <sstream>
#include <math.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

//MENU
#define STOP (100)
#define FORCE_BASED (101)
#define FUTURE_TIME (102)
#define CLUSTER_BASED (103)
#define CLUSTER_FUTURE (110)

#define CLUSTER_COLOR (120)

#define SG (130)
#define SGBC (131)
#define SGAA (132)
#define FUTURE_SG (133)

#define SEARCH (116)

using namespace boost;

SocialGraph mgraph;

float zoom = 4.0;
float xaxis = 0.0;
float yaxis = 0.0;
int width = 800, height = 600;
int flagSpring = 0;
int node=-1;
//void *font = GLUT_BITMAP_TIMES_ROMAN_24;
void *font = GLUT_BITMAP_HELVETICA_18;
int flagNew = 0;
float kinetic = 0;
Graph tempg;

void cvtAxis(int x1, int y1, double &x2, double &y2){
    x2 = ((double)x1/(double)width) * (2 * (double)width/zoom) + xaxis + ((double)-width/zoom);
	y2 = ((double)(height - y1)/(double)height) * (2 * (double)height/zoom) + yaxis + ((double)-height/zoom);
}

void drawVertices(Nodes nodes, std::vector<double> vbc){
	for(int i=0; i<nodes.size(); i++){
        glPointSize(vbc[i]);
		//glPointSize(5);
		glColor3f(nodes[i].r, nodes[i].g, nodes[i].b);
		glBegin(GL_POINTS);
		glVertex2f(nodes[i].x, nodes[i].y);
		glEnd();
	}
}

void drawNames(Nodes nodes, facebook::Name name){
	glColor3f(0,0,0);
	for(int i=0; i<nodes.size(); i++){
		glRasterPos2f(nodes[i].x, nodes[i].y);
		glutBitmapString(font, reinterpret_cast<const unsigned char*>(name[i].c_str()));
	}
}

void drawKinetic(){
	glColor3f(1.0, 0.0, 0.0);
	std::ostringstream s;
	s << "energy=" << kinetic;
	double x,y;
	cvtAxis(width-150,height-10,x,y);
	glRasterPos2f(x,y);
	glutBitmapString(font, reinterpret_cast<const unsigned char*>(s.str().c_str()));
}

void drawEdges(Nodes nodes, Graph g, MatrixScore ebc){
	EdgeIterator e, e_end;

	glColor3d(0.0, 0.0, 0.0);
	for(tie(e, e_end) = edges(g); e != e_end; e++){
		int s = source(*e, g);
		int t = target(*e, g);
		glLineWidth(ebc[s][t]);
		//glLineWidth(1);
		glBegin(GL_LINES);
		glVertex2d(nodes[s].x, nodes[s].y);
		glVertex2d(nodes[t].x, nodes[t].y);
		glEnd();
	}
}

void drawSGEdges(Nodes nodes, Edges edges, facebook::Name name){
	MatrixScore pr = mgraph.getProbability();

	for(int i=0; i<edges.size(); i++){
		int s = edges[i].first;
		int t = edges[i].second;

		glColor3f(1.0, 0.0, 0.0);
		glLineWidth(5.0);
		glBegin(GL_LINES);
		glVertex2d(nodes[s].x, nodes[s].y);
		glVertex2d(nodes[t].x, nodes[t].y);
		glEnd();

		glColor3f(0,0,0);
		glRasterPos2f(nodes[t].x, nodes[t].y);
		glutBitmapString(font, reinterpret_cast<const unsigned char*>(name[t].c_str()));

	}
}

void drawPREdges(Nodes nodes, facebook::Name name){
	MatrixScore pr;
	if(flagSpring == SGBC)
	pr = mgraph.getProbability();
	else if(flagSpring == SGAA)
	pr = mgraph.getProbabilityAA();

	if(node != -1){
		for(int i=0; i<pr.size(); i++){
			if(pr[node][i] >= 1.0){
				glLineWidth(5.0);
				glColor3f(1.0, 0.0, 0.0);
				glBegin(GL_LINES);
				glVertex2d(nodes[node].x, nodes[node].y);
				glVertex2d(nodes[i].x, nodes[i].y);
				glEnd();

				glColor3f(0,0,0);
				glRasterPos2f(nodes[i].x, nodes[i].y);
				glutBitmapString(font, reinterpret_cast<const unsigned char*>(name[i].c_str()));
			}
		}
	}
}

void drawText(double x, double y, std::string text, void *ft){
	glColor3f(0,0,0.0);
	cvtAxis(10,50,x,y);
	glRasterPos2f(x,y);
	//glutBitmapString(font, reinterpret_cast<const unsigned char*>(text.c_str()));
    glutBitmapString(font, (const unsigned char*)text.c_str());
}

//display graph
void drawgraph(){
	glClear(GL_COLOR_BUFFER_BIT);

	if(flagSpring == 4)
		drawEdges(mgraph.getNodes(), mgraph.getGraph(), mgraph.getEBC());
	else if(flagSpring == 5)
		drawEdges(mgraph.getNodes(), mgraph.getGraph2(), mgraph.getEBC2());
	else
		drawEdges(mgraph.getNodes(), tempg, mgraph.getEBC());

	if(flagSpring == SGBC || flagSpring == SGAA)
	drawPREdges(mgraph.getNodes(), mgraph.getName());
	if(flagSpring == FUTURE_SG)
	drawSGEdges(mgraph.getNodes(), mgraph.getSGEdges(), mgraph.getName());

	if(flagSpring == 5)
		drawVertices(mgraph.getNodes(), mgraph.getVBC2());
	else
		drawVertices(mgraph.getNodes(), mgraph.getVBC());

	if(node != -1){
		facebook::Name name = mgraph.getName();
		Nodes nodes = mgraph.getNodes();
		drawText(nodes[node].x, nodes[node].y, name[node], font);
	}

	drawKinetic();
}

//display function
void display(void){
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(xaxis + (-width / zoom), xaxis + (width / zoom), yaxis + (-height / zoom), yaxis + (height / zoom), -1.0, 1.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	drawgraph();

	glFlush();

	glutSwapBuffers();
}

void windowReshapeFunc(GLint newWidth, GLint newHeight) {
  glViewport( 0, 0, newWidth, newHeight );
  glMatrixMode( GL_PROJECTION );
  glLoadIdentity();
  glOrtho(xaxis + (-width / zoom), xaxis + (width / zoom), yaxis + (-height / zoom), yaxis + (height / zoom), -1.0, 1.0);

  width = newWidth;
  height = newHeight;
  glutPostRedisplay();
}

//display when idle
void idle(void){
	if(flagSpring > 0){
		if(flagSpring == SG)
			kinetic = mgraph.SpringSG(node,0);
		else if(flagSpring == SGBC)
			kinetic = mgraph.SpringSG(node,1);
		else if(flagSpring == SGAA)
			kinetic = mgraph.SpringSG(node,2);
		else if(flagSpring == FUTURE_SG)
			kinetic = mgraph.SpringSG(node,3);
		else
			kinetic = mgraph.springModel(tempg);
		glutPostRedisplay();
	}
}

int findNode(int x, int y){
	Nodes nodes = mgraph.getNodes();
	for(int i=0; i<nodes.size(); i++){
		if(abs(nodes[i].x - x) <= 1 && abs(nodes[i].y - y) <= 1)
			return i;
	}
	return -1;
}

void mouse(int button, int state, int x, int y){
	switch(button){
		case GLUT_LEFT_BUTTON:
			if(state == GLUT_DOWN){
				double x1, y1;
				cvtAxis(x, y, x1, y1);
				node = findNode(x1, y1);

				if(node != -1){
					std::cout << mgraph.getName()[node] << std::endl;
				}
			}
			break;
		default:
			break;
	}
	glutPostRedisplay();
}

void motion(int x, int y){
	if(node != -1){
		double x1, y1;
		cvtAxis(x, y, x1, y1);
		mgraph.setNode(node, x1, y1);
	}
	glutPostRedisplay();
}

void normalkeys(unsigned char key, int x, int y){
	switch(key){
		case 'i':
			zoom *= 1.1;
			break;
		case 'o':
			zoom /= 1.1;
			break;
		case 'q':
			exit(0);
			break;
		case '\033':
			exit(0);
			break;
		case 'c':
			mgraph.showDetected();
			break;
		default:
			break;
	}
	glutPostRedisplay();
}

void specialkeys(int key, int x, int y){
	switch(key){
		case GLUT_KEY_UP:
			yaxis += 20/zoom;
			break;
		case GLUT_KEY_DOWN:
			yaxis -= 20/zoom;
			break;
		case GLUT_KEY_LEFT:
			xaxis -= 20/zoom;
			break;
		case GLUT_KEY_RIGHT:
			xaxis += 20/zoom;
			break;
		default:
			break;
	}
	glutPostRedisplay();
}

void item(int mode){
	switch(mode){
		case FORCE_BASED:
			flagSpring = 1;
			std::cout << "start spring model" << std::endl;
			tempg = mgraph.getGraph();
			glutIdleFunc(idle);
			break;
		case FUTURE_TIME:
			flagSpring = 1;
			std::cout << "start spring model of future time" << std::endl;
			tempg = mgraph.getGraph2();
			glutIdleFunc(idle);
			break;
		case CLUSTER_COLOR:
			std::cout << "colored clusters" << std::endl;
			mgraph.clusterColor(mgraph.getClusterGraph());
			break;
		case CLUSTER_BASED:
			flagSpring = 4;
			std::cout << "start cluster model" << std::endl;
			tempg = mgraph.getClusterGraph();
			glutIdleFunc(idle);
			break;
		case CLUSTER_FUTURE:
			flagSpring = 5;
			std::cout << "start cluster model (future)" << std::endl;
			tempg = mgraph.getClusterGraph2();
			glutIdleFunc(idle);
			break;
		case SG:
			if(node != -1){
				flagSpring = SG;
				std::cout << "drawing graph centered " << node << std::endl;
				mgraph.selectSG(node);
				tempg = mgraph.getSG();
				glutIdleFunc(idle);
			}
			else{
				std::cout << "select node" << std::endl;
			}
			break;
		case SGBC:
			if(node != -1){
			flagSpring = SGBC;
			std::cout << "drawing graph centered on " << node << " (BC)" << std::endl;
			}
			break;
		case SGAA:
			if(node != -1){
			flagSpring = SGAA;
			std::cout << "drawing graph centered on " << node << " (AA)" << std::endl;
			}
			break;
		case FUTURE_SG:
			if(node != -1){
			flagSpring = FUTURE_SG;
			std::cout << "drawing future graph centered on " << node << std::endl;
			mgraph.selectFutureSG(node);
			tempg = mgraph.getSG2();
			glutIdleFunc(idle);
			}
			break;
		case STOP:
			flagSpring = 0;
			std::cout << "stopped" << std::endl;
			glutIdleFunc(NULL);
			break;
		case SEARCH:
			std::cout << "\nwrite name: ";
			std::string temp_name;
			std::cin >> temp_name;
			facebook::Name name = mgraph.getName();
			int i;
			for(i=0; i<name.size(); i++){
				if(temp_name == name[i]){
					node = i;
					flagSpring = 10;
					mgraph.selectSG(node);
					tempg = mgraph.getSG();
					break;
				}
			}
			if(i == name.size()){
				std::cout << "did not find name" << std::endl;
				node = -1;
				flagSpring = 0;
			}
			break;
	}
	glutPostRedisplay();
}

void init(void){
	glClearColor(1.0, 1.0, 1.0, 1.0);

	glViewport(0,0,width,height);

	glEnable(GL_POINT_SMOOTH);
	glEnable(GL_LINE_SMOOTH);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
}

//main function
int mainDrawing(int argc, char *argv[]){
	//srand((unsigned int)time(NULL));
	mgraph.initialize(argc, argv);
	mgraph.test();

	//initialize glut
	glutInit(&argc, argv);

	glutInitWindowPosition(300,200);
	glutInitWindowSize(width, height);

	//display using rgba
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);

	//create window
	glutCreateWindow(argv[1]);

	init();

	//display function
	glutDisplayFunc(display);
    glutReshapeFunc(windowReshapeFunc);
	//glutIdleFunc(idle);

	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutKeyboardFunc(normalkeys);
	glutSpecialFunc(specialkeys);

	glutCreateMenu(item);
	glutAddMenuEntry("Force-based", FORCE_BASED);
	glutAddMenuEntry("Future Time", FUTURE_TIME);
	glutAddMenuEntry("Cluster", CLUSTER_BASED);
	glutAddMenuEntry("Cluster Future", CLUSTER_FUTURE);
	glutAddMenuEntry("Color Clusters", CLUSTER_COLOR);
	glutAddMenuEntry("Draw subgraph", SG);
	glutAddMenuEntry("Draw subgraph (BC)", SGBC);
	glutAddMenuEntry("Draw subgraph (AA)", SGAA);
	glutAddMenuEntry("Draw future subgraph", FUTURE_SG);
	glutAddMenuEntry("Stop", STOP);
	glutAddMenuEntry("Search", SEARCH);
	glutAttachMenu(GLUT_RIGHT_BUTTON);

	//glutPostRedisplay();

	glutMainLoop();

	return 1;
}