//==============================================================================
//         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 <boost/simd/ieee/include/functions/exponentbits.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

NT2_TEST_CASE_TPL ( exponentbits_real,  (float))
{
  using boost::simd::exponentbits;
  using boost::simd::tag::exponentbits_;
  typedef typename boost::dispatch::meta::call<exponentbits_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type  wished_r_t;

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

  // specific values tests
  for(int i=1, k = 0; i < 10; i*= 2, ++k)
  {
    NT2_TEST_EQUAL(1065353216+k*8388608, exponentbits(T(i)));
  }
}

NT2_TEST_CASE_TPL ( exponentbits_reald,  (double))
{
  using boost::simd::exponentbits;
  using boost::simd::tag::exponentbits_;
  typedef typename boost::dispatch::meta::call<exponentbits_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type  wished_r_t;

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

  // specific values tests
  for(int i=1, k = 0; i < 10; i*= 2, ++k)
   {
     NT2_TEST_EQUAL(4607182418800017408ll+k*4503599627370496ll, exponentbits(T(i)));
   }
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

// discard stack allocation as that too bypasses malloc
#define EIGEN_STACK_ALLOCATION_LIMIT 0
#define EIGEN_RUNTIME_NO_MALLOC
#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType, int QRPreconditioner>
void jacobisvd_check_full(const MatrixType& m, const JacobiSVD<MatrixType, QRPreconditioner>& svd)
{
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime> MatrixUType;
  typedef Matrix<Scalar, ColsAtCompileTime, ColsAtCompileTime> MatrixVType;
  typedef Matrix<Scalar, RowsAtCompileTime, 1> ColVectorType;
  typedef Matrix<Scalar, ColsAtCompileTime, 1> InputVectorType;

  MatrixType sigma = MatrixType::Zero(rows,cols);
  sigma.diagonal() = svd.singularValues().template cast<Scalar>();
  MatrixUType u = svd.matrixU();
  MatrixVType v = svd.matrixV();

  VERIFY_IS_APPROX(m, u * sigma * v.adjoint());
  VERIFY_IS_UNITARY(u);
  VERIFY_IS_UNITARY(v);
}

template<typename MatrixType, int QRPreconditioner>
void jacobisvd_compare_to_full(const MatrixType& m,
                               unsigned int computationOptions,
                               const JacobiSVD<MatrixType, QRPreconditioner>& referenceSvd)
{
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();
  Index diagSize = (std::min)(rows, cols);

  JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);

  VERIFY_IS_APPROX(svd.singularValues(), referenceSvd.singularValues());
  if(computationOptions & ComputeFullU)
    VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU());
  if(computationOptions & ComputeThinU)
    VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU().leftCols(diagSize));
  if(computationOptions & ComputeFullV)
    VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV());
  if(computationOptions & ComputeThinV)
    VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV().leftCols(diagSize));
}

template<typename MatrixType, int QRPreconditioner>
void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef Matrix<Scalar, RowsAtCompileTime, Dynamic> RhsType;
  typedef Matrix<Scalar, ColsAtCompileTime, Dynamic> SolutionType;

  RhsType rhs = RhsType::Random(rows, internal::random<Index>(1, cols));
  JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);
  SolutionType x = svd.solve(rhs);
  // evaluate normal equation which works also for least-squares solutions
  VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
}

template<typename MatrixType, int QRPreconditioner>
void jacobisvd_test_all_computation_options(const MatrixType& m)
{
  if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols())
    return;
  JacobiSVD<MatrixType, QRPreconditioner> fullSvd(m, ComputeFullU|ComputeFullV);

  jacobisvd_check_full(m, fullSvd);
  jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV);

  if(QRPreconditioner == FullPivHouseholderQRPreconditioner)
    return;

  jacobisvd_compare_to_full(m, ComputeFullU, fullSvd);
  jacobisvd_compare_to_full(m, ComputeFullV, fullSvd);
  jacobisvd_compare_to_full(m, 0, fullSvd);

  if (MatrixType::ColsAtCompileTime == Dynamic) {
    // thin U/V are only available with dynamic number of columns
    jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd);
    jacobisvd_compare_to_full(m,              ComputeThinV, fullSvd);
    jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd);
    jacobisvd_compare_to_full(m, ComputeThinU             , fullSvd);
    jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd);
    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV);
    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV);
    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV);

    // test reconstruction
    typedef typename MatrixType::Index Index;
    Index diagSize = (std::min)(m.rows(), m.cols());
    JacobiSVD<MatrixType, QRPreconditioner> svd(m, ComputeThinU | ComputeThinV);
    VERIFY_IS_APPROX(m, svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint());
  }
}

template<typename MatrixType>
void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
{
  MatrixType m = pickrandom ? MatrixType::Random(a.rows(), a.cols()) : a;

  jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m);
  jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m);
  jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m);
  jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m);
}

template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef Matrix<Scalar, RowsAtCompileTime, 1> RhsType;

  RhsType rhs(rows);

  JacobiSVD<MatrixType> svd;
  VERIFY_RAISES_ASSERT(svd.matrixU())
  VERIFY_RAISES_ASSERT(svd.singularValues())
  VERIFY_RAISES_ASSERT(svd.matrixV())
  VERIFY_RAISES_ASSERT(svd.solve(rhs))

  MatrixType a = MatrixType::Zero(rows, cols);
  a.setZero();
  svd.compute(a, 0);
  VERIFY_RAISES_ASSERT(svd.matrixU())
  VERIFY_RAISES_ASSERT(svd.matrixV())
  svd.singularValues();
  VERIFY_RAISES_ASSERT(svd.solve(rhs))

  if (ColsAtCompileTime == Dynamic)
  {
    svd.compute(a, ComputeThinU);
    svd.matrixU();
    VERIFY_RAISES_ASSERT(svd.matrixV())
    VERIFY_RAISES_ASSERT(svd.solve(rhs))

    svd.compute(a, ComputeThinV);
    svd.matrixV();
    VERIFY_RAISES_ASSERT(svd.matrixU())
    VERIFY_RAISES_ASSERT(svd.solve(rhs))

    JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner> svd_fullqr;
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeFullU|ComputeThinV))
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeThinV))
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeFullV))
  }
  else
  {
    VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinU))
    VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinV))
  }
}

template<typename MatrixType>
void jacobisvd_method()
{
  enum { Size = MatrixType::RowsAtCompileTime };
  typedef typename MatrixType::RealScalar RealScalar;
  typedef Matrix<RealScalar, Size, 1> RealVecType;
  MatrixType m = MatrixType::Identity();
  VERIFY_IS_APPROX(m.jacobiSvd().singularValues(), RealVecType::Ones());
  VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixU());
  VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixV());
  VERIFY_IS_APPROX(m.jacobiSvd(ComputeFullU|ComputeFullV).solve(m), m);
}

// work around stupid msvc error when constructing at compile time an expression that involves
// a division by zero, even if the numeric type has floating point
template<typename Scalar>
EIGEN_DONT_INLINE Scalar zero() { return Scalar(0); }

// workaround aggressive optimization in ICC
template<typename T> EIGEN_DONT_INLINE  T sub(T a, T b) { return a - b; }

template<typename MatrixType>
void jacobisvd_inf_nan()
{
  // all this function does is verify we don't iterate infinitely on nan/inf values

  JacobiSVD<MatrixType> svd;
  typedef typename MatrixType::Scalar Scalar;
  Scalar some_inf = Scalar(1) / zero<Scalar>();
  VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf));
  svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV);

  Scalar some_nan = zero<Scalar>() / zero<Scalar>();
  VERIFY(some_nan != some_nan);
  svd.compute(MatrixType::Constant(10,10,some_nan), ComputeFullU | ComputeFullV);

  MatrixType m = MatrixType::Zero(10,10);
  m(internal::random<int>(0,9), internal::random<int>(0,9)) = some_inf;
  svd.compute(m, ComputeFullU | ComputeFullV);

  m = MatrixType::Zero(10,10);
  m(internal::random<int>(0,9), internal::random<int>(0,9)) = some_nan;
  svd.compute(m, ComputeFullU | ComputeFullV);
}

// Regression test for bug 286: JacobiSVD loops indefinitely with some
// matrices containing denormal numbers.
void jacobisvd_bug286()
{
#if defined __INTEL_COMPILER
// shut up warning #239: floating point underflow
#pragma warning push
#pragma warning disable 239
#endif
  Matrix2d M;
  M << -7.90884e-313, -4.94e-324,
                 0, 5.60844e-313;
#if defined __INTEL_COMPILER
#pragma warning pop
#endif
  JacobiSVD<Matrix2d> svd;
  svd.compute(M); // just check we don't loop indefinitely
}

void jacobisvd_preallocate()
{
  Vector3f v(3.f, 2.f, 1.f);
  MatrixXf m = v.asDiagonal();

  internal::set_is_malloc_allowed(false);
  VERIFY_RAISES_ASSERT(VectorXf v(10);)
  JacobiSVD<MatrixXf> svd;
  internal::set_is_malloc_allowed(true);
  svd.compute(m);
  VERIFY_IS_APPROX(svd.singularValues(), v);

  JacobiSVD<MatrixXf> svd2(3,3);
  internal::set_is_malloc_allowed(false);
  svd2.compute(m);
  internal::set_is_malloc_allowed(true);
  VERIFY_IS_APPROX(svd2.singularValues(), v);
  VERIFY_RAISES_ASSERT(svd2.matrixU());
  VERIFY_RAISES_ASSERT(svd2.matrixV());
  svd2.compute(m, ComputeFullU | ComputeFullV);
  VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
  VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
  internal::set_is_malloc_allowed(false);
  svd2.compute(m);
  internal::set_is_malloc_allowed(true);

  JacobiSVD<MatrixXf> svd3(3,3,ComputeFullU|ComputeFullV);
  internal::set_is_malloc_allowed(false);
  svd2.compute(m);
  internal::set_is_malloc_allowed(true);
  VERIFY_IS_APPROX(svd2.singularValues(), v);
  VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
  VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
  internal::set_is_malloc_allowed(false);
  svd2.compute(m, ComputeFullU|ComputeFullV);
  internal::set_is_malloc_allowed(true);
}

void test_jacobisvd()
{
  CALL_SUBTEST_3(( jacobisvd_verify_assert(Matrix3f()) ));
  CALL_SUBTEST_4(( jacobisvd_verify_assert(Matrix4d()) ));
  CALL_SUBTEST_7(( jacobisvd_verify_assert(MatrixXf(10,12)) ));
  CALL_SUBTEST_8(( jacobisvd_verify_assert(MatrixXcd(7,5)) ));

  for(int i = 0; i < g_repeat; i++) {
    Matrix2cd m;
    m << 0, 1,
         0, 1;
    CALL_SUBTEST_1(( jacobisvd(m, false) ));
    m << 1, 0,
         1, 0;
    CALL_SUBTEST_1(( jacobisvd(m, false) ));

    Matrix2d n;
    n << 0, 0,
         0, 0;
    CALL_SUBTEST_2(( jacobisvd(n, false) ));
    n << 0, 0,
         0, 1;
    CALL_SUBTEST_2(( jacobisvd(n, false) ));

    CALL_SUBTEST_3(( jacobisvd<Matrix3f>() ));
    CALL_SUBTEST_4(( jacobisvd<Matrix4d>() ));
    CALL_SUBTEST_5(( jacobisvd<Matrix<float,3,5> >() ));
    CALL_SUBTEST_6(( jacobisvd<Matrix<double,Dynamic,2> >(Matrix<double,Dynamic,2>(10,2)) ));

    int r = internal::random<int>(1, 30),
        c = internal::random<int>(1, 30);
    CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(r,c)) ));
    CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(r,c)) ));
    (void) r;
    (void) c;

    // Test on inf/nan matrix
    CALL_SUBTEST_7( jacobisvd_inf_nan<MatrixXf>() );
  }

  CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
  CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3))) ));

  // test matrixbase method
  CALL_SUBTEST_1(( jacobisvd_method<Matrix2cd>() ));
  CALL_SUBTEST_3(( jacobisvd_method<Matrix3f>() ));

  // Test problem size constructors
  CALL_SUBTEST_7( JacobiSVD<MatrixXf>(10,10) );

  // Check that preallocation avoids subsequent mallocs
  CALL_SUBTEST_9( jacobisvd_preallocate() );

  // Regression check for bug 286
  CALL_SUBTEST_2( jacobisvd_bug286() );
}


//***********************************************************
// 26/01/2021	1.0.0	Rémi Saint-Amant   Creation
//***********************************************************
#include "ALeucopodaModel.h"
//#include "ALeucopodaEquations.h"
#include "ModelBase/EntryPoint.h"
#include "Basic\DegreeDays.h"
//#include "Leucopoda.h"
//#include <boost/math/distributions/weibull.hpp>
//#include <boost/math/distributions/beta.hpp>
//#include <boost/math/distributions/Rayleigh.hpp>
#include <boost/math/distributions/logistic.hpp>
//#include <boost/math/distributions/exponential.hpp>
//#include <boost/math/distributions/lognormal.hpp>
#include "ModelBase/SimulatedAnnealingVector.h"

using namespace WBSF::HOURLY_DATA;
using namespace std;
using namespace WBSF::TZZ;

//static const bool BEGIN_JULY = true;
//static const size_t FIRST_Y = BEGIN_JULY ? 1 : 0;

namespace WBSF
{

	static const size_t NB_GENERATIONS_MAX = 6;

	//static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::MODIFIED_ALLEN_WAVE;
	static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::ALLEN_WAVE;
	//enum { O_CDD, O_GENERATION, O_DIAPAUSED, O_EGG, O_LARVA, O_PUPA, O_ADULT, O_DEAD_ADULT, O_BROOD, NB_OUTPUTS };
	enum { O_EGG, O_LARVA, O_PREPUPA, O_PUPA, O_ADULT, O_DEAD_ADULT, O_BROOD, O_DEAD_ATTRITION, NB_OUTPUT_ONE_G, O_IN_DIAPAUSE = (NB_OUTPUT_ONE_G * NB_GENERATIONS_MAX - O_PUPA), O_D_DAY_LENGTH, NB_DAILY_OUTPUTS };

	//this line link this model with the EntryPoint of the DLL
	static const bool bRegistred =
		CModelFactory::RegisterModel(CAprocerosLeucopodaModel::CreateObject);

	CAprocerosLeucopodaModel::CAprocerosLeucopodaModel()
	{
		//NB_INPUT_PARAMETER is used to determine if the dll
		//uses the same number of parameters than the model interface
		NB_INPUT_PARAMETER = -1;
		VERSION = "1.0.0 (2021)";

		m_generationSurvival = 1;
//		m_bCumul = false;
//		m_stage = 0;
	//	m_T = 0;

		//m_EWD.fill(0);
		//m_EAS.fill(0);
		//Set parameters to equation
		//ASSERT(stand.m_equations.m_EWD.size() == m_EWD.size());
		for (size_t p = 0; p < m_EWD.size(); p++)
			m_EWD[p] = CAprocerosLeucopodaEquations::EWD[p];

		//ASSERT(stand.m_equations.m_EAS.size() == m_EAS.size());
		for (size_t p = 0; p < m_EAS.size(); p++)
			m_EAS[p] = CAprocerosLeucopodaEquations::EAS[p];

		//m_P.fill(0);
		/*m_P[Τᴴ²] = 19.1;
		m_P[delta] = 45;
		m_P[μ1] = 239.6;
		m_P[ѕ1] = 41.4;
		m_P[μ2] = 876.0;
		m_P[ѕ2] = 55.6;
		m_P[μ3] = 625.2;
		m_P[ѕ3] = 38.3;
			*/
	}

	CAprocerosLeucopodaModel::~CAprocerosLeucopodaModel()
	{
	}

	//this method is call to load your parameter in your variable
	ERMsg CAprocerosLeucopodaModel::ProcessParameters(const CParameterVector& parameters)
	{
		ERMsg msg;

		size_t c = 0;

		//		m_stage = parameters[c++].GetInt() + 1;
			//	m_T = parameters[c++].GetInt()+1;

		//m_bCumul = parameters[c++].GetBool();
		if (parameters.size() == m_EWD.size() + m_EAS.size() + 1)
		{
			//entering winter diapause  parameters
			for (size_t p = 0; p < m_EWD.size(); p++)
			{
				m_EWD[p] = parameters[c++].GetFloat();
			}

			//Emerging Adult from Soil (spring) parameters
			for (size_t p = 0; p < m_EAS.size(); p++)
			{
				m_EAS[p] = parameters[c++].GetFloat();
			}
		}

		return msg;
	}

	ERMsg CAprocerosLeucopodaModel::OnExecuteDaily()
	{
		ERMsg msg;

		/*	if (m_weather.GetNbYears() < 2)
			{
				msg.ajoute("Laricobius nigrinus model need at least 2 years of data");
				return msg;
			}*/

		if (!m_weather.IsHourly())
			m_weather.ComputeHourlyVariables();

		//This is where the model is actually executed
		m_output.Init(m_weather.GetEntireTPeriod(CTM(CTM::DAILY)), NB_DAILY_OUTPUTS, 0);

		//we simulate 2 years at a time.
		//we also manager the possibility to have only one year
		for (size_t y = 0; y < m_weather.size(); y++)
		{
			//one output by generation
			vector<CModelStatVector> outputs;
			ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, outputs);

			//merge generations vector into one output vector (max of 5 generations)
			size_t maxG = min(NB_GENERATIONS_MAX, outputs.size());

			CTPeriod p = m_weather[y].GetEntireTPeriod(CTM(CTM::DAILY));
			for (CTRef TRef = p.Begin(); TRef <= p.End(); TRef++)
			{
				CStatistic diapause;
				for (size_t g = 0, ss = 0; g < maxG; g++)
				{
					size_t s_i = (g==0)?PUPA:EGG;
					for (size_t s = s_i; s < NB_OUTPUT_ONE_G; s++, ss++)
					{
						m_output[TRef][ss] = outputs[g][TRef][s];
						diapause += outputs[g][TRef][S_DIAPAUSE];
					}
				}

				m_output[TRef][O_IN_DIAPAUSE] = diapause[SUM];
				m_output[TRef][O_D_DAY_LENGTH] = m_weather.GetDayLength(TRef) / 3600.;
			}
		}

		return msg;
	}

	void CAprocerosLeucopodaModel::ExecuteDaily(int year, const CWeatherYears& weather, vector<CModelStatVector>& output)
	{
		//Create stand
		CTZZStand stand(this);
		stand.m_bApplyAttrition = true;
		stand.m_generationSurvival = m_generationSurvival;

		//Set parameters to equation

		ASSERT(stand.m_equations.m_EWD.size() == m_EWD.size());
		for (size_t p = 0; p < m_EWD.size(); p++)
			stand.m_equations.m_EWD[p] = m_EWD[p];

		ASSERT(stand.m_equations.m_EAS.size() == m_EAS.size());
		for (size_t p = 0; p < m_EAS.size(); p++)
			stand.m_equations.m_EAS[p] = m_EAS[p];

		stand.init(year, weather);

		//compute 30 days avg
		//stand.ComputeTavg30(year, weather);

		//Create host
		CTZZHostPtr pHost(new CTZZHost(&stand));

		pHost->m_nbMinObjects = 10;
		pHost->m_nbMaxObjects = 1000;

		pHost->Initialize<CAprocerosLeucopoda>(CInitialPopulation(CTRef(year, JANUARY, DAY_01), 0, 400, 100, TZZ::PUPA, WBSF::FEMALE, true, 0));

		//add host to stand
		stand.m_host.push_front(pHost);

		CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		for (CTRef d = p.Begin(); d <= p.End(); d++)
		{

			stand.Live(weather.GetDay(d));
			//if (output.IsInside(d))
				//stand.GetStat(d, output[d]);

			size_t nbGenerations = stand.GetFirstHost()->GetNbGeneration();
			if (nbGenerations > output.size())
				output.push_back(CModelStatVector(p, NB_STATS, 0));

			for (size_t g = 0; g < nbGenerations; g++)
				stand.GetStat(d, output[g][d], g);

			stand.AdjustPopulation();
			HxGridTestConnection();
		}

		//if (m_bCumul)
		//{
		//	for (size_t g = 0; g < output.size(); g++)
		//	{
		//		//cumulative result
		//		for (size_t s = S_EGG; s < S_ADULT; s++)
		//		{
		//			CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		//			CStatistic stat = output[g].GetStat(s, p);
		//			if (stat.IsInit() && stat[SUM] > 0)
		//			{
		//				output[g][0][s] = output[g][0][s] * 100 / stat[SUM];//when first day is not 0
		//				for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
		//				{
		//					output[g][d][s] = output[g][d - 1][s] + output[g][d][s] * 100 / stat[SUM];
		//					_ASSERTE(!_isnan(output[g][d][s]));
		//				}
		//			}
		//		}
		//	}
		//}
	}

	//void CAprocerosLeucopodaModel::GetCDD(int year, const CWeatherYears& weather, CModelStatVector& CDD)
	//{
	//	CDegreeDays DDmodel(DD_METHOD, m_P[Τᴴ¹], m_P[Τᴴ²]);
	//	CModelStatVector DD;
	//	DDmodel.Execute(weather[year], DD);

	//	CDD.Init(DD.GetTPeriod(), 1, 0.0);

	//	//for (CTRef TRef = DD.GetFirstTRef(); TRef <= DD.GetLastTRef(); TRef++)
	//	CDD[0][0] = DD[0][CDegreeDays::S_DD];
	//	for (size_t i = 1; i < DD.size(); i++)
	//		CDD[i][0] = CDD[i - 1][0] + DD[i][CDegreeDays::S_DD];
	//}

	void CAprocerosLeucopodaModel::GetCDD(const CWeatherYears& weather, CModelStatVector& CDD)
	{
		//CTZZStand* pStand = GetStand();
		CDegreeDays DDmodel(DD_METHOD, m_EAS[Τᴴ¹], m_EAS[Τᴴ²]);
		CModelStatVector DD;
		DDmodel.Execute(weather, DD);

		CDD.Init(DD.GetTPeriod(), 1, -999);

		for (size_t y = 0; y < weather.GetNbYears(); y++)
		{
			CTPeriod p = weather[y].GetEntireTPeriod();
			//p.Begin() = p.Begin() + int(m_P[delta]);
			CDD[p.Begin()][0] = DD[p.Begin()][CDegreeDays::S_DD];

			for (CTRef TRef = p.Begin() + 1; TRef <= p.End(); TRef++)
				CDD[TRef][0] = CDD[TRef - 1][0] + DD[TRef][CDegreeDays::S_DD];
		}

	}

	//enum TSpecies { S_LA_G1, S_LA_G2, S_LP, S_LN };
	enum TInput { I_KEYID, I_DATE, I_STAGE, I_GENERATION, I_N, I_CUMUL, NB_INPUTS };
	void CAprocerosLeucopodaModel::AddDailyResult(const StringVector& header, const StringVector& data)
	{
		//KeyID	Date	Stage	Generation	N	Cumul
		ASSERT(data.size() == NB_INPUTS);
		//SYC	site	Year	collection	col_date	emerge_date	daily_count	species	n_days P Time
		//if (stoi(data[I_VARIABLE]) == m_stage && stoi(data[I_T]) == m_T)
		//{
		CSAResult obs;
		obs.m_ref.FromFormatedString(data[I_DATE]);
		obs.m_obs.resize(NB_INPUTS);
		for (size_t i = 2; i < NB_INPUTS; i++)
		{
			if (i == I_STAGE)
				obs.m_obs[i] = 0;
			else
				obs.m_obs[i] = stod(data[i]);
		}

		//CSAResult obs;

		//CStatistic egg_creation_date;

		//obs.m_obs.resize(3);
		//obs.m_obs[0] = 0;
		//obs.m_obs[1] = stod(data[I_GENERATION]);
		//obs.m_obs[2] = stod(data[I_CUMUL]);
		////obs.m_obs[I_S] = ;
		////obs.m_obs[I_P] = stod(data[10]);
		////obs.m_obs[I_CDD] = stod(data[11]);
		////obs.m_obs[I_P] = stod(data[12]);

		//if (data[7] == "LA" && data[8] == "1")
		//	obs.m_obs[I_S] = S_LA_G1;
		//else if (data[7] == "LA" && data[8] == "2")
		//	obs.m_obs[I_S] = S_LA_G2;
		//else if (data[7] == "LP")
		//	obs.m_obs[I_S] = S_LP;
		//else if (data[7] == "LN")
		//	obs.m_obs[I_S] = S_LN;

		m_SAResult.push_back(obs);
	}

	//ASSERT(data.size() == NB_INPUTS);
	////SYC	site	Year	collection	col_date	emerge_date	daily_count	species	n_days P Time
	//if (stoi(data[I_VARIABLE]) == m_stage && stoi(data[I_T]) == m_T)
	//{
	//	CSAResult obs;
	//	obs.m_obs.resize(NB_INPUTS);
	//	for (size_t i = 0; i < NB_INPUTS; i++)
	//		obs.m_obs[i] = stod(data[i]);
	//	//CSAResult obs;

	//	//CStatistic egg_creation_date;

	//	//obs.m_ref.FromFormatedString(data[5]);
	//	//obs.m_obs.resize(NB_INPUTS);
	//	//obs.m_obs[I_N] = stod(data[6]);
	//	////obs.m_obs[I_S] = ;
	//	////obs.m_obs[I_P] = stod(data[10]);
	//	////obs.m_obs[I_CDD] = stod(data[11]);
	//	////obs.m_obs[I_P] = stod(data[12]);

	//	//if (data[7] == "LA" && data[8] == "1")
	//	//	obs.m_obs[I_S] = S_LA_G1;
	//	//else if (data[7] == "LA" && data[8] == "2")
	//	//	obs.m_obs[I_S] = S_LA_G2;
	//	//else if (data[7] == "LP")
	//	//	obs.m_obs[I_S] = S_LP;
	//	//else if (data[7] == "LN")
	//	//	obs.m_obs[I_S] = S_LN;

	//	m_SAResult.push_back(obs);
	//}
//	}

	//double GetSimX(size_t s, CTRef TRefO, double obs, const CModelStatVector& output)
	//{
	//	double x = -999;

	//	if (obs > -999)
	//	{
	//		//if (obs > 0.01 && obs < 99.99)
	//		if (obs >= 100)
	//			obs = 99.99;//to avoid some problem of truncation

	//		long index = output.GetFirstIndex(s, ">=", obs, 1, CTPeriod(TRefO.GetYear(), FIRST_MONTH, FIRST_DAY, TRefO.GetYear(), LAST_MONTH, LAST_DAY));
	//		if (index >= 1)
	//		{
	//			double obsX1 = output.GetFirstTRef().GetJDay() + index;
	//			double obsX2 = output.GetFirstTRef().GetJDay() + index + 1;

	//			double obsY1 = output[index][s];
	//			double obsY2 = output[index + 1][s];
	//			if (obsY2 != obsY1)
	//			{
	//				double slope = (obsX2 - obsX1) / (obsY2 - obsY1);
	//				double obsX = obsX1 + (obs - obsY1)*slope;
	//				ASSERT(!_isnan(obsX) && _finite(obsX));

	//				x = obsX;
	//			}
	//		}
	//	}

	//	return x;
	//}

	bool CAprocerosLeucopodaModel::IsParamValid()const
	{
		bool bValid = true;

		return bValid;
	}

	//static const int ROUND_VAL = 4;
	//CTRef CAprocerosLeucopodaModel::GetEmergence(const CWeatherYear& weather)
	//{
	//	CTPeriod p = weather.GetEntireTPeriod(CTM(CTM::DAILY));

	//	double sumDD = 0;
	//	for (CTRef TRef = p.Begin()+172; TRef <= p.End()&& TRef<= p.Begin() + int(m_ADE[ʎ0]); TRef++)
	//	{
	//		//size_t ii = TRef - p.Begin();
	//		const CWeatherDay& wday = m_weather.GetDay(TRef);
	//		double T = wday[H_TNTX][MEAN];
	//		T = Round(max(m_ADE[ʎa], T), ROUND_VAL);

	//		double DD = min(0.0, T - m_ADE[ʎb]);//DD is negative

	//		//if (ii < m_ADE[ʎ0])
	//			sumDD += DD;
	//	}

	//	boost::math::logistic_distribution<double> begin_dist(m_ADE[ʎ2], m_ADE[ʎ3]);
	//	int begin = (int)Round(m_ADE[ʎ0] + m_ADE[ʎ1] * cdf(begin_dist, sumDD), 0);
	//	return  p.Begin() + begin;
	//}
	//enum TPout {P_CDD, P_CE, LA_G1= P_CE, P_LA_G2, P_LP, P_LN, NB_P};//CE = cumulative emergence
	//void CAprocerosLeucopodaModel::GetPobs(CModelStatVector& P)
	//{
	//	string ID = GetInfo().m_loc.m_ID;
	//	string SY = ID.substr(0, ID.length() - 2);

	//	//compute CDD for all temperature rprofile
	//	array< double, 4> total = { 0 };
	//	vector<tuple<double, CTRef, double, bool, size_t>> d;
	//	const CSimulatedAnnealingVector& SA = GetSimulatedAnnealingVector();

	//	for (size_t i = 0; i < SA.size(); i++)
	//	{
	//		string IDi = SA[i]->GetInfo().m_loc.m_ID;
	//		string SYi = IDi.substr(0, IDi.length() - 2);
	//		if (SYi == SY)
	//		{
	//			CModelStatVector CDD;
	//			GetCDD(SA[i]->m_weather, CDD);
	//			const CSAResultVector& v = SA[i]->GetSAResult();
	//			for (size_t ii = 0; ii < v.size(); ii++)
	//			{
	//				d.push_back(make_tuple(CDD[v[ii].m_ref][0], v[ii].m_ref, v[ii].m_obs[I_N], IDi == ID, v[ii].m_obs[I_S]));
	//				total[v[ii].m_obs[I_S]] += v[ii].m_obs[I_N];
	//			}
	//		}
	//	}

	//	sort(d.begin(), d.end());

	//	P.Init(m_weather.GetEntireTPeriod(CTM::DAILY), NB_P, 0);
	//	array< double, 4> sum = { 0 };
	//	for (size_t i = 0; i < d.size(); i++)
	//	{
	//		size_t s = std::get<4>(d[i]);
	//		sum[s] += std::get<2>(d[i]);
	//		if (std::get<3>(d[i]))
	//		{
	//			CTRef Tref = std::get<1>(d[i]);
	//			/*double obsP = -999;
	//			for (size_t k = 0; k < m_SAResult.size(); k++)
	//				if (m_SAResult[k].m_ref == Tref)
	//					obsP = m_SAResult[k].m_obs[I_P];*/

	//			double CDD = std::get<0>(d[i]);
	//			double p = Round(100 * sum[s] / total[s], 1);
	//
	//			P[Tref][P_CDD] = CDD;
	//			P[Tref][P_CE + s] = p;
	//		}
	//	}
	//}

	//void CAprocerosLeucopodaModel::CalibrateEmergence(CStatisticXY& stat)
	//{
	//	if (m_SAResult.empty())
	//		return;

	//	//boost::math::lognormal_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);

	//	//boost::math::weibull_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
	//	//boost::math::beta_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
	//	//boost::math::exponential_distribution<double> emerge_dist(m_P[ѕ]);
	//	//boost::math::rayleigh_distribution<double> emerge_dist(m_P[ѕ]);

	//	//CModelStatVector CDD;
	//	//GetCDD(m_weather, CDD);

	//	double n = 0;

	//	for (size_t i = 0; i < m_SAResult.size(); i++)
	//		n += m_SAResult[i].m_obs[I_N];

	//	CModelStatVector P;
	//	GetPobs(P);

	//	//array<boost::math::logistic_distribution<double>,4> emerge_dist(mu, S);
	//	//		array<boost::math::logistic_distribution<double>, 4> emerge_dist = { {m_P[μ1], m_P[ѕ1], m_P[μ1], m_P[ѕ1]} };

	//	for (size_t i = 0; i < m_SAResult.size(); i++)
	//	{
	//		size_t s = m_SAResult[i].m_obs[I_S];
	//		double mu = m_P[μ1 + 2 * s];
	//		double S = m_P[ѕ1 + 2 * s];

	//		boost::math::logistic_distribution<double> emerge_dist(mu, S);

	//		double CDD = P[m_SAResult[i].m_ref][P_CDD];
	//		double obs = P[m_SAResult[i].m_ref][P_CE+s];
	//		ASSERT(obs >= 0 && obs <= 100);

	//		double sim = Round(100 * cdf(emerge_dist, max(0.0, CDD)), 1);
	//		for (size_t ii = 0; ii < log(n); ii++)
	//			stat.Add(obs, sim);

	//	}//for all results

	//	return;

	//}

	//static double ei(size_t n) { return pow(1.0 + 1.0 / n, n); }
	//static double cv_2_sigma(double cv, size_t n)
	//{
	//	static const double e = exp(1);
	//	static const double p[3] = { 0.528196, 2.373248, 3.493202 };//with 10 000 replication
	//	return e * cv*(1 - p[0] * sqrt(e - ei(n))) / (p[1] + cv * (1 - p[2] * sqrt(e - ei(n))));
	//}

	void CAprocerosLeucopodaModel::GetFValueDaily(CStatisticXY& stat)
	{

		ASSERT(!m_SAResult.empty() );

		//CTZZStand* pStand = GetStand();
		//for (size_t p = 0; p < m_EAS.size(); p++)
			//pStand->m_equations.m_EAS[p] = m_EAS[p];

		if (!IsParamValid())
			return;

		//boost::math::lognormal_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);

		//boost::math::weibull_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::beta_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::exponential_distribution<double> emerge_dist(m_P[ѕ]);
		//boost::math::rayleigh_distribution<double> emerge_dist(m_P[ѕ]);

		//CModelStatVector CDD;
		//GetCDD(m_weather, CDD);

		double n = 0;

		for (size_t i = 0; i < m_SAResult.size(); i++)
			n += m_SAResult[i].m_obs[I_N];

		CModelStatVector CDD;
		GetCDD(m_weather, CDD);
		//GetPobs(P);

		boost::math::logistic_distribution<double> emerge_dist(m_EAS[μ], m_EAS[ѕ]);

		for (size_t i = 0; i < m_SAResult.size(); i++)
		{
			size_t s = m_SAResult[i].m_obs[I_STAGE];

			double GDD = CDD[m_SAResult[i].m_ref][CDegreeDays::S_DD];
			double obs = m_SAResult[i].m_obs[I_CUMUL];
			//ASSERT(obs >= 0 && obs <= 100);

			double sim = Round(100 * cdf(emerge_dist, max(0.0, GDD)), 1);
			//for (size_t ii = 0; ii < log(n); ii++)
			stat.Add(obs, sim);

		}//for all results

		return;

		//bool bZero = false;
		//CStatistic x;
		//for (size_t i = 0; i < m_SAResult.size(); i++)
		//{
		//	//ASSERT(m_SAResult[i].m_obs[I_VARIABLE] == m_stage);
		//	//ASSERT(m_SAResult[i].m_obs[I_T] == m_T);

		//	size_t n = size_t(ceil(m_P[i]));
		//	bZero |= n == 0;

		//	for (size_t b = 0; b < n; b++)
		//		x += m_SAResult[i].m_obs[I_TIME];

		//}//for all results

		////if (!bZero  && int(x[NB_VALUE]) == int(m_SAResult[0].m_obs[I_N]) )
		//{
		//	double obs = m_SAResult[0].m_obs[I_MEAN_TIME];
		//	double sim = x[MEAN];
		//	stat.Add(obs, sim);

		//	obs = m_SAResult[0].m_obs[I_TIME_SD] * 10;
		//	sim = x[STD_DEV] * 10;
		//	stat.Add(obs, sim);

		//	obs = size_t(m_SAResult[0].m_obs[I_N]);
		//	sim = size_t(x[NB_VALUE]);
		//	stat.Add(obs, sim);

		//	CStatistic stat_ws;
		//	CStatistic stat_n;

		//	//compute sigma

		//	double mean = x[MEAN];
		//	double sd = x[STD_DEV];
		//	double n = x[NB_VALUE];
		//	if (n > 0)
		//	{
		//		double cv = sd / mean;
		//		double sigma = cv_2_sigma(cv, n);

		//		stat_ws += n * sigma;
		//		stat_n += n;
		//	}

		//	static const double SIGMA_OBS[4] = { 0.102, 0.157, 0.203, 0.118 };

		//	obs = SIGMA_OBS[m_stage];
		//	sim = stat_ws[SUM] / stat_n[SUM];

		//}

	}

	//void CAprocerosLeucopodaModel::FinalizeStat(CStatisticXY& stat)
	//{
	//
	//}
}


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

#ifndef BOOST_MATH_CCMATH_FDIM_HPP
#define BOOST_MATH_CCMATH_FDIM_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T fdim_impl(const T x, const T y) noexcept
{
    if (x <= y)
    {
        return 0;
    }
    else if ((y < 0) && (x > (std::numeric_limits<T>::max)() + y))
    {
        return std::numeric_limits<T>::infinity();
    }
    else
    {
        return x - y;
    }
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real fdim(Real x, Real y) noexcept
{
    if (BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return boost::math::ccmath::isnan(x) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::isnan(y) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::detail::fdim_impl(x, y);
    }
    else
    {
        using std::fdim;
        return fdim(x, y);
    }
}

template <typename T1, typename T2>
inline constexpr auto fdim(T1 x, T2 y) noexcept
{
    if (BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        // If the type is an integer (e.g. epsilon == 0) then set the epsilon value to 1 so that type is at a minimum
        // cast to double
        constexpr auto T1p = std::numeric_limits<T1>::epsilon() > 0 ? std::numeric_limits<T1>::epsilon() : 1;
        constexpr auto T2p = std::numeric_limits<T2>::epsilon() > 0 ? std::numeric_limits<T2>::epsilon() : 1;

        using promoted_type =
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              std::conditional_t<T1p <= LDBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= LDBL_EPSILON && T2p <= T1p, T2,
                              #endif
                              std::conditional_t<T1p <= DBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= DBL_EPSILON && T2p <= T1p, T2, double
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              >>>>;
                              #else
                              >>;
                              #endif

        return boost::math::ccmath::fdim(promoted_type(x), promoted_type(y));
    }
    else
    {
        using std::fdim;
        return fdim(x, y);
    }
}

inline constexpr float fdimf(float x, float y) noexcept
{
    return boost::math::ccmath::fdim(x, y);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double fdiml(long double x, long double y) noexcept
{
    return boost::math::ccmath::fdim(x, y);
}
#endif

} // Namespace boost::math::ccmath

#endif // BOOST_MATH_CCMATH_FDIM_HPP


//     Copyright (c) 2013 Jeff Heckey jheckey(at)ece(dot)ucsb(dot)edu
//     This file uses SQCT, Copyright (c) 2012 Vadym Kliuchnikov, Dmitri Maslov, Michele Mosca;
//     SQCT is distributed under LGPL v3
//

#include "skdecomposer.h"
#include "output.h"

#include <fstream>
#include <sstream>
#include <chrono>
#include <cmath>
#include <boost/timer/timer.hpp>

#ifndef SKD_H
#define SKD_H

namespace btm = boost::timer;

SKDecompose::SKDecompose()
{
	r.num = 1.0;
}

/// \brief Summarize gate statistics for Clifford + T library
void SKDecompose::updateForCliffordT()
{
	typedef gateLibrary gl;
	total = c.cost();
	auto counts = gl::toCliffordT( c.count() );
	hc = counts[ gl::H ];
	tc = counts[ gl::T ];
	pc = counts[ gl::P ];
	plc =  counts[ gl::Z ] + counts[ gl::X ] + counts[ gl::Y ];
}

/// \brief Finds circuit and all supplementary information for unitary
void SKDecompose::calculate( const matrix2x2hpr& matrix, int recursion_level )
{
	// boost::timer::cpu_times ct;
	// boost::timer::cpu_times ct2;

	sk::Me res;
    skd.decompose(matrix,res,recursion_level);

	int gde_before = res.min_gde_abs2();
	res.reduce();
	int gde_after = res.min_gde_abs2();
	denom_reduction = gde_before - gde_after;
	denom = res.max_sde_abs2();
	exact_uni = res;

    ed.decompose(res,c);

	sk::Ma conv(res);

	dst = trace_dist(matrix,conv);
	// tappr = (double) ct.wall * 1e-9;
	// tdecomp = (double) ct2.wall * 1e-9;
	nr = recursion_level;
	updateForCliffordT();
}

void SKDecompose::rotation( double angle, int iter, bool frac )
{
	static const double TwoPi = 2. * hprHelpers::toMachine( hprHelpers::pi() );
	if ( frac ) {
		r.num = 1.0;
		r.den = angle;
	} else {
		r.num = angle;
		r.den = TwoPi;
	}
	calculate( r.matrix() , iter );
}

/// \brief generate a X rotation
circuit SKDecompose::rotX( double angle, int iter, bool frac )
{
	r.nx = 1;
	r.ny = 0;
	r.nz = 0;
	rotation( angle, iter, frac );
	return c;
}

/// \brief generate a Y rotation
circuit SKDecompose::rotY( double angle, int iter, bool frac )
{
	r.nx = 0;
	r.ny = 1;
	r.nz = 0;
	rotation( angle, iter, frac );
	return c;
}

/// \brief generate a Z rotation
circuit SKDecompose::rotZ( double angle, int iter, bool frac )
{
	r.nx = 0;
	r.ny = 0;
	r.nz = 1;
	rotation( angle, iter, frac );
	return c;
}

#endif // SKD_H


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

   The MIT License

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

   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 <Core/Datatypes/Tests/MatrixTestCases.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/MatrixIO.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Algorithms/DataIO/ReadMatrix.h>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Utils/StringUtil.h>
#include <boost/filesystem.hpp>

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

TEST(ReadMatrixTests, CanReadFromStream)
{
  const std::string matrixString = "0 1 2\n3 4 5\n6 7 8\n";

  std::istringstream istr(matrixString);

  DenseMatrix in;

  istr >> in;

  EXPECT_EQ(matrix1(), in);
}

TEST(ReadMatrixTests, RoundTripViaString)
{
  DenseMatrix m = matrix1();

  std::ostringstream ostr;
  ostr << m;
  std::string str = ostr.str();
  std::istringstream istr(str);
  DenseMatrix m2;
  istr >> m2;

  EXPECT_EQ(m, m2);
}

TEST(ReadMatrixAlgorithmTest, TestFromRealTextFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "matrix1.txt";
  if (boost::filesystem::exists(filename))
  {
    DenseMatrixConstHandle matrix = castMatrix::toDense(algo.run(filename.string()));
    ASSERT_TRUE(matrix.get() != nullptr);
    EXPECT_EQ(matrix1(), *matrix);
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, ThrowsForFileNotFound)
{
  ReadMatrixAlgorithm algo;
  const std::string filename = "ZZZ:\\no way this file exists............\\\\////\\\\//";
  EXPECT_THROW(algo.run(filename), Core::Algorithms::AlgorithmInputException);
}

TEST(ReadMatrixAlgorithmTest, TestSparseFromRealASCIIMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "sp2.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::sparse(matrix));

    auto sp = castMatrix::toSparse(matrix);

    DenseMatrix a(3, 4);
    a << 1, 0, 0, -1,
      0, 2, 0, 0,
      0, 0, 3, 0;

    EXPECT_EQ(a, *convertMatrix::toDense(matrix));
#if !DEBUG
    EXPECT_EQ(to_string(a), to_string(sp->castForPrinting()));
#endif
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestDenseFromRealASCIIMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "xScirun.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::dense(matrix));

    auto dense = castMatrix::toDense(matrix);

    EXPECT_EQ(4, dense->cols());
    EXPECT_EQ(3, dense->rows());
    EXPECT_NEAR(-1, dense->minCoeff(), 1e-12);
    EXPECT_NEAR(3, dense->maxCoeff(), 1e-12);
    EXPECT_DOUBLE_EQ(1, (*dense)(0,0));
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestColumnFromRealASCIIMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "xScirunColumn.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::column(matrix));

    auto col = castMatrix::toColumn(matrix);
    EXPECT_EQ(1, col->cols());
    EXPECT_EQ(5, col->rows());
    EXPECT_NEAR(-4, col->minCoeff(), 1e-12);
    EXPECT_NEAR(3, col->maxCoeff(), 1e-12);
    EXPECT_DOUBLE_EQ(1, (*col)[0]);
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, DISABLED_TestLargeSparseFromRealASCIIMatFile)
{
  ReadMatrixAlgorithm algo;
  auto AFile = TestResources::rootDir() / "Matrices" / "CGDarrell" / "A_txt.mat";
  if (boost::filesystem::exists(AFile))
  {
    auto matrix = algo.run(AFile.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::sparse(matrix));

    auto sp = castMatrix::toSparse(matrix);

    EXPECT_EQ(428931, sp->rows());
    EXPECT_EQ(428931, sp->cols());
    EXPECT_EQ(5540569, sp->nonZeros());
  }
  else
    FAIL() << "file " << AFile.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, UnknownFileFormatThrows)
{
  ReadMatrixAlgorithm algo;
  auto notAMatrixFile = TestResources::rootDir() / "Other" / "scirun5demo.srn";
  if (boost::filesystem::exists(notAMatrixFile))
  {
    EXPECT_THROW(algo.run(notAMatrixFile.string()), Core::Algorithms::AlgorithmInputException);
  }
  else
    FAIL() << "file " << notAMatrixFile.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, DISABLED_ThrowsForMatlabFilesICantThemReadYet)
{
  /// @todo: will hook up Matlab readers soon.
  ReadMatrixAlgorithm algo;
  auto matlabFile = TestResources::rootDir() / "Matrices" / "CGDarrell" / "RHS.mat";
  if (boost::filesystem::exists(matlabFile))
  {
    EXPECT_THROW(algo.run(matlabFile.string()), Core::Algorithms::AlgorithmInputException);
  }
  else
    FAIL() << "file " << matlabFile.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, DISABLED_NonMatrixTextFile)
{
  /// @todo: will hook-up Importers soon.
  ReadMatrixAlgorithm algo;
  const boost::filesystem::path txt("E:/seg3d/trunk/src/Install.txt");
  if (boost::filesystem::exists(txt))
  {
    EXPECT_THROW(algo.run(txt.string()), Core::Algorithms::AlgorithmInputException);
  }
  else
    FAIL() << "file " << txt.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestSparseFromRealBinaryMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "eye3x3sparse_bin.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::sparse(matrix));

    auto sp = castMatrix::toSparse(matrix);

    DenseMatrix a = DenseMatrix::Identity(3,3);

    EXPECT_EQ(a, *convertMatrix::toDense(matrix));
#if !DEBUG
    EXPECT_EQ(to_string(a), to_string(sp->castForPrinting()));
#endif
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestDenseFromRealBinaryMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "eye3x3dense_bin.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::dense(matrix));

    auto dense = castMatrix::toDense(matrix);

    DenseMatrix a = DenseMatrix::Identity(3,3);

    EXPECT_EQ(to_string(a), to_string(*dense));
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestColumnFromRealBinaryMatFile)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "columnBinMat.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::dense(matrix)); // artifact of v4 IO

    auto col = convertMatrix::toColumn(matrix);
    DenseColumnMatrix expected(3);
    expected << 1, 2, 3;
    EXPECT_EQ(to_string(*col), to_string(expected));
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, TestColumnFromRealBinaryMatFile2)
{
  ReadMatrixAlgorithm algo;
  auto filename = TestResources::rootDir() / "Matrices" / "columnBin2.mat";
  if (boost::filesystem::exists(filename))
  {
    auto matrix = algo.run(filename.string());
    ASSERT_TRUE(matrix.get() != nullptr);
    ASSERT_TRUE(matrixIs::column(matrix));

    auto col = castMatrix::toColumn(matrix);
    DenseColumnMatrix expected(5);
    expected << 0, 1, 2, 3, 4;
    EXPECT_EQ(to_string(*col), to_string(expected));
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

void CallLegacyPio(const boost::filesystem::path& filename, const DenseMatrix& expected = DenseMatrix::Identity(3,3))
{
  if (boost::filesystem::exists(filename))
  {
    PiostreamPtr stream = auto_istream(filename.string());
    if (!stream)
    {
      FAIL() << "Error reading file '" << filename << "'.";
    }

    MatrixHandle matrix;
    Pio(*stream, matrix);

    ASSERT_TRUE(matrix.get() != nullptr);
    EXPECT_EQ(expected.nrows(), matrix->nrows());
    EXPECT_EQ(expected.ncols(), matrix->ncols());

    std::cout << *matrix << std::endl;

    EXPECT_EQ(expected, *convertMatrix::toDense(matrix));
#if !DEBUG
    EXPECT_EQ(to_string(expected), to_string(*matrix));
#endif
  }
  else
    FAIL() << "file " << filename.string() << " does not exist, skipping test." << std::endl;
}

TEST(ReadMatrixAlgorithmTest, CallLegacyPioDense)
{
  CallLegacyPio(TestResources::rootDir() / "Matrices" / "eye3x3dense_bin.mat");
}

TEST(ReadMatrixAlgorithmTest, CallLegacyPioSparse)
{
  CallLegacyPio(TestResources::rootDir() / "Matrices" / "eye3x3sparse_bin.mat");
}


// Copyright (c) Facebook, Inc. and its affiliates.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.

#include <IsometricPatternMatcher/LocalParamSe3.h>

#include <Eigen/Core>
#include <sophus/se3.hpp>

namespace surreal_opensource {

bool LocalParamSe3::Plus(const double* T_raw, const double* delta_raw,
                         double* T_plus_delta_raw) const {
  const Eigen::Map<const Sophus::SE3d> T(T_raw);
  const Eigen::Map<const Sophus::SE3d::Tangent> delta(delta_raw);
  Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
  T_plus_delta = T * Sophus::SE3d::exp(delta);
  return true;
}

bool LocalParamSe3::ComputeJacobian(const double* T_raw,
                                    double* jacobian_raw) const {
  using JacobianMatrix = Eigen::Matrix<double, Sophus::SE3d::num_parameters,
                                       Sophus::SE3d::DoF, Eigen::RowMajor>;

  Eigen::Map<const Sophus::SE3d> T(T_raw);
  Eigen::Map<JacobianMatrix> jacobian(jacobian_raw);
  jacobian = T.Dx_this_mul_exp_x_at_0();
  return true;
}

int LocalParamSe3::GlobalSize() const { return Sophus::SE3d::num_parameters; }

int LocalParamSe3::LocalSize() const { return Sophus::SE3d::DoF; }

}  // namespace surreal_opensource


/**
 * @file recurrent_network_test.cpp
 * @author Marcus Edel
 *
 * Tests the recurrent network.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/identity_function.hpp>
#include <mlpack/methods/ann/activation_functions/softsign_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>
#include <mlpack/methods/ann/activation_functions/rectifier_function.hpp>

#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/init_rules/nguyen_widrow_init.hpp>

#include <mlpack/methods/ann/layer/neuron_layer.hpp>
#include <mlpack/methods/ann/layer/lstm_layer.hpp>
#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>
#include <mlpack/methods/ann/layer/multiclass_classification_layer.hpp>

#include <mlpack/methods/ann/connections/full_connection.hpp>
#include <mlpack/methods/ann/connections/self_connection.hpp>
#include <mlpack/methods/ann/connections/fullself_connection.hpp>
#include <mlpack/methods/ann/connections/connection_traits.hpp>

#include <mlpack/methods/ann/trainer/trainer.hpp>

#include <mlpack/methods/ann/ffnn.hpp>
#include <mlpack/methods/ann/rnn.hpp>

#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/methods/ann/optimizer/rmsprop.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(RecurrentNetworkTest);

// Be careful!  When writing new tests, always get the boolean value and store
// it in a temporary, because the Boost unit test macros do weird things and
// will cause bizarre problems.

/**
 * Construct a 2-class dataset out of noisy sines.
 *
 * @param data Input data used to store the noisy sines.
 * @param labels Labels used to store the target class of the noisy sines.
 * @param points Number of points/features in a single sequence.
 * @param sequences Number of sequences for each class.
 * @param noise The noise factor that influences the sines.
 */
void GenerateNoisySines(arma::mat& data,
                        arma::mat& labels,
                        const size_t points,
                        const size_t sequences,
                        const double noise = 0.3)
{
  arma::colvec x =  arma::linspace<arma::Col<double> >(0,
      points - 1, points) / points * 20.0;
  arma::colvec y1 = arma::sin(x + arma::as_scalar(arma::randu(1)) * 3.0);
  arma::colvec y2 = arma::sin(x / 2.0 + arma::as_scalar(arma::randu(1)) * 3.0);

  data = arma::zeros(points, sequences * 2);
  labels = arma::zeros(2, sequences * 2);

  for (size_t seq = 0; seq < sequences; seq++)
  {
    data.col(seq) = arma::randu(points) * noise + y1 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    labels(0, seq) = 1;

    data.col(sequences + seq) = arma::randu(points) * noise + y2 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    labels(1, sequences + seq) = 1;
  }
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(SequenceClassificationTest)
{
  // Generate 12 (2 * 6) noisy sines. A single sine contains 10 points/features.
  arma::mat input, labels;
  GenerateNoisySines(input, labels, 10, 6);

  /*
   * Construct a network with 1 input unit, 4 hidden units and 2 output units.
   * The hidden layer is connected to itself. The network structure looks like:
   *
   *  Input         Hidden        Output
   * Layer(1)      Layer(4)      Layer(2)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  NeuronLayer<LogisticFunction> inputLayer(1);
  NeuronLayer<LogisticFunction> hiddenLayer0(4);
  NeuronLayer<LogisticFunction> recurrentLayer0(hiddenLayer0.InputSize());
  NeuronLayer<LogisticFunction> hiddenLayer1(2);
  BinaryClassificationLayer outputLayer;

  RandomInitialization randInit(-0.5, 0.5);

  FullConnection<
      decltype(inputLayer),
      decltype(hiddenLayer0),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon0(inputLayer, hiddenLayer0, randInit);

  SelfConnection<
    decltype(recurrentLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(randInit)>
    layerCon2(recurrentLayer0, hiddenLayer0, randInit);

  FullConnection<
      decltype(hiddenLayer0),
      decltype(hiddenLayer1),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon4(hiddenLayer0, hiddenLayer1, randInit);

  auto module0 = std::tie(layerCon0, layerCon2);
  auto module1 = std::tie(layerCon4);
  auto modules = std::tie(module0, module1);

  RNN<decltype(modules),
      decltype(outputLayer),
      MeanSquaredErrorFunction> net(modules, outputLayer);

  // Train the network for 1000 epochs.
  Trainer<decltype(net)> trainer(net, 1000);
  trainer.Train(input, labels, input, labels);

  // Ask the network to classify the trained input data.
  arma::colvec output;
  for (size_t i = 0; i < input.n_cols; i++)
  {
    net.Predict(input.unsafe_col(i), output);

    bool b = arma::all((output == labels.unsafe_col(i)) == 1);
    BOOST_REQUIRE_EQUAL(b, 1);
  }
}

/**
 * Train and evaluate a vanilla feed forward network and a recurrent network
 * with the specified structure and compare the two networks output and overall
 * error.
 */
template<
    typename WeightInitRule,
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void CompareVanillaNetworks(MatType& trainData,
                            MatType& trainLabels,
                            MatType& testData,
                            MatType& testLabels,
                            const size_t hiddenLayerSize,
                            const size_t maxEpochs,
                            WeightInitRule weightInitRule = WeightInitRule())
{
  BiasLayer<> biasLayer0(1);

  NeuronLayer<PerformanceFunction> inputLayer(trainData.n_rows);
  NeuronLayer<PerformanceFunction> hiddenLayer0(hiddenLayerSize);
  NeuronLayer<PerformanceFunction> hiddenLayer1(trainLabels.n_rows);

  OutputLayerType outputLayer;

  FullConnection<
    decltype(inputLayer),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(weightInitRule)>
    ffnLayerCon0(inputLayer, hiddenLayer0, weightInitRule);

  FullConnection<
    decltype(inputLayer),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(weightInitRule)>
    rnnLayerCon0(inputLayer, hiddenLayer0, weightInitRule);

  FullConnection<
    decltype(biasLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(weightInitRule)>
    ffnLayerCon1(biasLayer0, hiddenLayer0, weightInitRule);

  FullConnection<
    decltype(biasLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(weightInitRule)>
    rnnLayerCon1(biasLayer0, hiddenLayer0, weightInitRule);

  FullConnection<
      decltype(hiddenLayer0),
      decltype(hiddenLayer1),
      mlpack::ann::RMSPROP,
      decltype(weightInitRule)>
      ffnLayerCon2(hiddenLayer0, hiddenLayer1, weightInitRule);

  FullConnection<
      decltype(hiddenLayer0),
      decltype(hiddenLayer1),
      mlpack::ann::RMSPROP,
      decltype(weightInitRule)>
      rnnLayerCon2(hiddenLayer0, hiddenLayer1, weightInitRule);

  auto ffnModule0 = std::tie(ffnLayerCon0, ffnLayerCon1);
  auto ffnModule1 = std::tie(ffnLayerCon2);
  auto ffnModules = std::tie(ffnModule0, ffnModule1);

  auto rnnModule0 = std::tie(rnnLayerCon0, rnnLayerCon1);
  auto rnnModule1 = std::tie(rnnLayerCon2);
  auto rnnModules = std::tie(rnnModule0, rnnModule1);

  /*
   * Construct a feed forward network with trainData.n_rows input units,
   * hiddenLayerSize hidden units and trainLabels.n_rows output units. The
   * network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |       |     |       |     |
   * +-----+       +--+--+       +-----+
   */
  FFNN<decltype(ffnModules), decltype(outputLayer), PerformanceFunctionType>
      ffn(ffnModules, outputLayer);

  /*
   * Construct a recurrent network with trainData.n_rows input units,
   * hiddenLayerSize hidden units and trainLabels.n_rows output units. The
   * hidden layer is connected to itself. The network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  RNN<decltype(rnnModules), decltype(outputLayer), PerformanceFunctionType>
      rnn(rnnModules, outputLayer);

  // Train the network for maxEpochs epochs or until we reach a validation error
  // of less then 0.001.
  Trainer<decltype(ffn)> ffnTrainer(ffn, maxEpochs, 1, 0.001, false);
  Trainer<decltype(rnn)> rnnTrainer(rnn, maxEpochs, 1, 0.001, false);

  for (size_t i = 0; i < 5; i++)
  {
    rnnTrainer.Train(trainData, trainLabels, testData, testLabels);
    ffnTrainer.Train(trainData, trainLabels, testData, testLabels);

    if (!arma::is_finite(ffnTrainer.ValidationError()))
      continue;

    BOOST_REQUIRE_CLOSE(ffnTrainer.ValidationError(),
        rnnTrainer.ValidationError(), 1e-3);
  }
}

/**
 * Train a vanilla feed forward and recurrent network on a sequence with len
 * one. Ideally the recurrent network should produce the same output as the
 * recurrent network. The self connection shouldn't affect the output when using
 * a sequence with a length of one.
 */
BOOST_AUTO_TEST_CASE(FeedForwardRecurrentNetworkTest)
{
  arma::mat input;
  arma::mat labels;

  RandomInitialization randInit(1, 1);

  // Test on a non-linearly separable dataset (XOR).
  input << 0 << 1 << 1 << 0 << arma::endr
        << 1 << 0 << 1 << 0 << arma::endr;
  labels << 0 << 0 << 1 << 1;

  // Vanilla neural net with logistic activation function.
  CompareVanillaNetworks<RandomInitialization,
                      LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (input, labels, input, labels, 10, 10, randInit);

  // Vanilla neural net with identity activation function.
  CompareVanillaNetworks<RandomInitialization,
                      IdentityFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (input, labels, input, labels, 1, 1, randInit);

  // Vanilla neural net with rectifier activation function.
  CompareVanillaNetworks<RandomInitialization,
                    RectifierFunction,
                    BinaryClassificationLayer,
                    MeanSquaredErrorFunction>
    (input, labels, input, labels, 10, 10, randInit);

  // Vanilla neural net with softsign activation function.
  CompareVanillaNetworks<RandomInitialization,
                    SoftsignFunction,
                    BinaryClassificationLayer,
                    MeanSquaredErrorFunction>
    (input, labels, input, labels, 10, 10, randInit);

  // Vanilla neural net with tanh activation function.
  CompareVanillaNetworks<RandomInitialization,
                    TanhFunction,
                    BinaryClassificationLayer,
                    MeanSquaredErrorFunction>
    (input, labels, input, labels, 10, 10, randInit);
}

/**
 * Generate a random Reber grammar.
 *
 * For more information, see the following thesis.
 *
 * @code
 * @misc{Gers2001,
 *   author = {Felix Gers},
 *   title = {Long Short-Term Memory in Recurrent Neural Networks},
 *   year = {2001}
 * }
 * @endcode
 *
 * @param transitions Reber grammar transition matrix.
 * @param reber The generated Reber grammar string.
 */
void GenerateReber(const arma::Mat<char>& transitions, std::string& reber)
{
  size_t idx = 0;
  reber = "B";

  do
  {
    const int grammerIdx = rand() % 2;
    reber += arma::as_scalar(transitions.submat(idx, grammerIdx, idx,
        grammerIdx));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  } while (idx != 0);

  reber =  "BPTVVE";
}

/**
 * Generate a random embedded Reber grammar.
 *
 * @param transitions Embedded Reber grammar transition matrix.
 * @param reber The generated embedded Reber grammar string.
 */
void GenerateEmbeddedReber(const arma::Mat<char>& transitions,
                           std::string& reber)
{
  GenerateReber(transitions, reber);
  const char c = (rand() % 2) == 1 ? 'P' : 'T';
  reber = c + reber + c;
  reber = "B" + reber + "E";
}

/**
 * Convert a Reber symbol to a unit vector.
 *
 * @param symbol Reber symbol to be converted.
 * @param translation The converted symbol stored as unit vector.
 */
void ReberTranslation(const char symbol, arma::colvec& translation)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(symbols == symbol, 1, "first"));

  translation = arma::zeros<arma::colvec>(7);
  translation(idx) = 1;
}

/**
 * Convert a unit vector to a Reber symbol.
 *
 * @param translation The unit vector to be converted.
 * @param symbol The converted unit vector stored as Reber symbol.
 */
void ReberReverseTranslation(const arma::colvec& translation, char& symbol)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(translation == 1, 1, "first"));

  symbol = symbols(idx);
}

/**
 * Given a Reber string, return a Reber string with all reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextReber(const arma::Mat<char>& transitions,
                       const std::string& reber, std::string& nextReber)
{
  size_t idx = 0;

  for (size_t grammer = 1; grammer < reber.length(); grammer++)
  {
    const int grammerIdx = arma::as_scalar(arma::find(
        transitions.row(idx) == reber[grammer], 1, "first"));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  }

  nextReber = arma::as_scalar(transitions.submat(idx, 0, idx, 0));
  nextReber += arma::as_scalar(transitions.submat(idx, 1, idx, 1));
}

/**
 * Given a embedded Reber string, return a embedded Reber string with all
 * reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextEmbeddedReber(const arma::Mat<char>& transitions,
                               const std::string& reber, std::string& nextReber)
{
  if (reber.length() <= 2)
  {
    nextReber = reber.length() == 1 ? "TP" : "B";
  }
  else
  {
    size_t pos = reber.find('E');
    if (pos != std::string::npos)
    {
      nextReber = pos == reber.length() - 1 ? std::string(1, reber[1]) : "E";
    }
    else
    {
      GenerateNextReber(transitions, reber.substr(2), nextReber);
    }
  }
}

/**
 * Train the specified network and the construct a Reber grammar dataset.
 */
template<typename HiddenLayerType>
void ReberGrammarTestNetwork(HiddenLayerType& hiddenLayer0,
                             bool embedded = false)
{
  // Reber state transition matrix. (The last two columns are the indices to the
  // next path).
  arma::Mat<char> transitions;
  transitions << 'T' << 'P' << '1' << '2' << arma::endr
              << 'X' << 'S' << '3' << '1' << arma::endr
              << 'V' << 'T' << '4' << '2' << arma::endr
              << 'X' << 'S' << '2' << '5' << arma::endr
              << 'P' << 'V' << '3' << '5' << arma::endr
              << 'E' << 'E' << '0' << '0' << arma::endr;

  const size_t trainReberGrammarCount = 1000;
  const size_t testReberGrammarCount = 1000;

  std::string trainReber, testReber;
  arma::field<arma::mat> trainInput(1, trainReberGrammarCount);
  arma::field<arma::mat> trainLabels(1, trainReberGrammarCount);
  arma::field<arma::mat> testInput(1, testReberGrammarCount);
  arma::colvec translation;

  // Generate the training data.
  for (size_t i = 0; i < trainReberGrammarCount; i++)
  {
    if (embedded)
      GenerateEmbeddedReber(transitions, trainReber);
    else
      GenerateReber(transitions, trainReber);

    for (size_t j = 0; j < trainReber.length() - 1; j++)
    {
      ReberTranslation(trainReber[j], translation);
      trainInput(0, i) = arma::join_cols(trainInput(0, i), translation);

      ReberTranslation(trainReber[j + 1], translation);
      trainLabels(0, i) = arma::join_cols(trainLabels(0, i), translation);
    }
  }

  // Generate the test data.
  for (size_t i = 0; i < testReberGrammarCount; i++)
  {
    if (embedded)
      GenerateEmbeddedReber(transitions, testReber);
    else
      GenerateReber(transitions, testReber);

    for (size_t j = 0; j < testReber.length() - 1; j++)
    {
      ReberTranslation(testReber[j], translation);
      testInput(0, i) = arma::join_cols(testInput(0, i), translation);
    }
  }

  /*
   * Construct a network with 7 input units, layerSize hidden units and 7 output
   * units. The hidden layer is connected to itself. The network structure looks
   * like:
   *
   *  Input         Hidden        Output
   * Layer(7)  Layer(layerSize)   Layer(7)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  NeuronLayer<LogisticFunction> inputLayer(7);
  NeuronLayer<IdentityFunction> recurrentLayer0(hiddenLayer0.OutputSize());
  NeuronLayer<LogisticFunction> hiddenLayer1(7);
  BinaryClassificationLayer outputLayer;

  NguyenWidrowInitialization randInit;

  FullConnection<
      decltype(inputLayer),
      decltype(hiddenLayer0),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon0(inputLayer, hiddenLayer0, randInit);

  FullselfConnection<
    decltype(recurrentLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(randInit)>
    layerTypeLSTM(recurrentLayer0, hiddenLayer0, randInit);

  SelfConnection<
    decltype(recurrentLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(randInit)>
    layerTypeBasis(recurrentLayer0, hiddenLayer0, randInit);

  typename std::conditional<LayerTraits<HiddenLayerType>::IsLSTMLayer,
      typename std::remove_reference<decltype(layerTypeLSTM)>::type,
      typename std::remove_reference<decltype(layerTypeBasis)>::type>::type
      layerCon2(recurrentLayer0, hiddenLayer0, randInit);

  FullConnection<
      decltype(hiddenLayer0),
      decltype(hiddenLayer1),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon4(hiddenLayer0, hiddenLayer1, randInit);

  auto module0 = std::tie(layerCon0, layerCon2);
  auto module1 = std::tie(layerCon4);
  auto modules = std::tie(module0, module1);

  RNN<decltype(modules),
      decltype(outputLayer),
      MeanSquaredErrorFunction> net(modules, outputLayer);

  // Train the network for (500 * trainReberGrammarCount) epochs.
  Trainer<decltype(net)> trainer(net, 1, 1, 0, false);

  arma::mat inputTemp, labelsTemp;
  for (size_t i = 0; i < 100; i++)
  {
    for (size_t j = 0; j < trainReberGrammarCount; j++)
    {
      inputTemp = trainInput.at(0, j);
      labelsTemp = trainLabels.at(0, j);
      trainer.Train(inputTemp, labelsTemp, inputTemp, labelsTemp);
    }
  }

  double error = 0;

  // Ask the network to predict the next Reber grammar in the given sequence.
  for (size_t i = 0; i < testReberGrammarCount; i++)
  {
    arma::colvec output;
    arma::colvec input = testInput.at(0, i);

    net.Predict(input, output);

    const size_t reberGrammerSize = 7;
    std::string inputReber = "";

    size_t reberError = 0;
    for (size_t j = 0; j < (output.n_elem / reberGrammerSize); j++)
    {
      if (arma::sum(output.subvec(j * reberGrammerSize, (j + 1) *
          reberGrammerSize - 1)) != 1) break;

      char predictedSymbol, inputSymbol;
      std::string reberChoices;

      ReberReverseTranslation(output.subvec(j * reberGrammerSize, (j + 1) *
          reberGrammerSize - 1), predictedSymbol);
      ReberReverseTranslation(input.subvec(j * reberGrammerSize, (j + 1) *
          reberGrammerSize - 1), inputSymbol);
      inputReber += inputSymbol;

      if (embedded)
        GenerateNextEmbeddedReber(transitions, inputReber, reberChoices);
      else
        GenerateNextReber(transitions, inputReber, reberChoices);

      if (reberChoices.find(predictedSymbol) != std::string::npos)
        reberError++;
    }

    if (reberError != (output.n_elem / reberGrammerSize))
      error += 1;
  }

  error /= testReberGrammarCount;

  BOOST_REQUIRE_LE(error, 0.2);
}

/**
 * Train the specified networks on a Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(ReberGrammarTest)
{
  LSTMLayer<> hiddenLayerLSTM(10);
  ReberGrammarTestNetwork(hiddenLayerLSTM);

  NeuronLayer<LogisticFunction> hiddenLayerLogistic(5);
  ReberGrammarTestNetwork(hiddenLayerLogistic);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(EmbeddedReberGrammarTest)
{
  LSTMLayer<> hiddenLayerLSTM(10);
  ReberGrammarTestNetwork(hiddenLayerLSTM, true);

  LSTMLayer<> hiddenLayerLSTMPeephole(10, 1, true);
  ReberGrammarTestNetwork(hiddenLayerLSTMPeephole, true);
}

/*
 * This sample is a simplified version of Derek D. Monner's Distracted Sequence
 * Recall task, which involves 10 symbols:
 *
 * Targets: must be recognized and remembered by the network.
 * Distractors: never need to be remembered.
 * Prompts: direct the network to give an answer.
 *
 * A single trial consists of a temporal sequence of 10 input symbols. The first
 * 8 consist of 2 randomly chosen target symbols and 6 randomly chosen
 * distractor symbols in an random order. The remaining two symbols are two
 * prompts, which direct the network to produce the first and second target in
 * the sequence, in order.
 *
 * For more information, see the following paper.
 *
 * @code
 * @misc{Monner2012,
 *   author = {Monner, Derek and Reggia, James A},
 *   title = {A generalized LSTM-like training algorithm for second-order
 *   recurrent neural networks},
 *   year = {2012}
 * }
 * @endcode
 *
 * @param input The generated input sequence.
 * @param input The generated output sequence.
 */
void GenerateDistractedSequence(arma::mat& input, arma::mat& output)
{
  input = arma::zeros<arma::mat>(10, 10);
  output = arma::zeros<arma::mat>(3, 10);

  arma::Col<size_t> index = arma::shuffle(arma::linspace<arma::Col<size_t> >(
      0, 7, 8));

  // Set the target in the input sequence and the corresponding targets in the
  // output sequence by following the correct order.
  for (size_t i = 0; i < 2; i++)
  {
    size_t idx = rand() % 2;
    input(idx, index(i)) = 1;
    output(idx, index(i) > index(i == 0) ? 9 : 8) = 1;
  }

  for (size_t i = 2; i < 8; i++)
    input(2 + rand() % 6, index(i)) = 1;

  // Set the prompts which direct the network to give an answer.
  input(8, 8) = 1;
  input(9, 9) = 1;

  input.reshape(input.n_elem, 1);
  output.reshape(output.n_elem, 1);
}

/**
 * Train the specified network and the construct distracted sequence recall
 * dataset.
 */
template<typename HiddenLayerType>
void DistractedSequenceRecallTestNetwork(HiddenLayerType& hiddenLayer0)
{
  const size_t trainDistractedSequenceCount = 1000;
  const size_t testDistractedSequenceCount = 1000;

  arma::field<arma::mat> trainInput(1, trainDistractedSequenceCount);
  arma::field<arma::mat> trainLabels(1, trainDistractedSequenceCount);
  arma::field<arma::mat> testInput(1, testDistractedSequenceCount);
  arma::field<arma::mat> testLabels(1, testDistractedSequenceCount);

  // Generate the training data.
  for (size_t i = 0; i < trainDistractedSequenceCount; i++)
    GenerateDistractedSequence(trainInput(0, i), trainLabels(0, i));

  // Generate the test data.
  for (size_t i = 0; i < testDistractedSequenceCount; i++)
    GenerateDistractedSequence(testInput(0, i), testLabels(0, i));

  /*
   * Construct a network with 7 input units, layerSize hidden units and 7 output
   * units. The hidden layer is connected to itself. The network structure looks
   * like:
   *
   *  Input         Hidden        Output
   * Layer(7)  Layer(layerSize)   Layer(7)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  NeuronLayer<LogisticFunction> inputLayer(10);
  NeuronLayer<IdentityFunction> recurrentLayer0(hiddenLayer0.OutputSize());
  NeuronLayer<LogisticFunction> hiddenLayer1(3);
  BinaryClassificationLayer outputLayer;

  NguyenWidrowInitialization randInit;

  FullConnection<
      decltype(inputLayer),
      decltype(hiddenLayer0),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon0(inputLayer, hiddenLayer0, randInit);

  FullselfConnection<
    decltype(recurrentLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(randInit)>
    layerTypeLSTM(recurrentLayer0, hiddenLayer0, randInit);

  SelfConnection<
    decltype(recurrentLayer0),
    decltype(hiddenLayer0),
    mlpack::ann::RMSPROP,
    decltype(randInit)>
    layerTypeBasis(recurrentLayer0, hiddenLayer0, randInit);

  typename std::conditional<LayerTraits<HiddenLayerType>::IsLSTMLayer,
      typename std::remove_reference<decltype(layerTypeLSTM)>::type,
      typename std::remove_reference<decltype(layerTypeBasis)>::type>::type
      layerCon2(recurrentLayer0, hiddenLayer0, randInit);

  FullConnection<
      decltype(hiddenLayer0),
      decltype(hiddenLayer1),
      mlpack::ann::RMSPROP,
      decltype(randInit)>
      layerCon4(hiddenLayer0, hiddenLayer1, randInit);

  auto module0 = std::tie(layerCon0, layerCon2);
  auto module1 = std::tie(layerCon4);
  auto modules = std::tie(module0, module1);

  RNN<decltype(modules),
      decltype(outputLayer),
      MeanSquaredErrorFunction> net(modules, outputLayer);

  // Train the network for (500 * trainDistractedSequenceCount) epochs.
  Trainer<decltype(net)> trainer(net, 1, 1, 0, false);

  arma::mat inputTemp, labelsTemp;
  for (size_t i = 0; i < 100; i++)
  {
    for (size_t j = 0; j < trainDistractedSequenceCount; j++)
    {
      inputTemp = trainInput.at(0, j);
      labelsTemp = trainLabels.at(0, j);

      trainer.Train(inputTemp, labelsTemp, inputTemp, labelsTemp);
    }
  }

  double error = 0;

  // Ask the network to predict the targets in the given sequence at the
  // prompts.
  for (size_t i = 0; i < testDistractedSequenceCount; i++)
  {
    arma::colvec output;
    arma::colvec input = testInput.at(0, i);

    net.Predict(input, output);

    if (arma::sum(arma::abs(testLabels.at(0, i) - output)) != 0)
      error += 1;
  }

  error /= testDistractedSequenceCount;

  // Can we reproduce the results from the paper. They provide an 95% accuracy
  // on a test set of 1000 randomly selected sequences.
  // Ensure that this is within tolerance, which is at least as good as the
  // paper's results (plus a little bit for noise).
  BOOST_REQUIRE_LE(error, 0.1);
}

/**
 * Train the specified networks on the Derek D. Monner's distracted sequence
 * recall task.
 */
BOOST_AUTO_TEST_CASE(DistractedSequenceRecallTest)
{
  LSTMLayer<> hiddenLayerLSTM(10, 10);
  DistractedSequenceRecallTestNetwork(hiddenLayerLSTM);

  LSTMLayer<> hiddenLayerLSTMPeephole(10, 1, true);
  DistractedSequenceRecallTestNetwork(hiddenLayerLSTM);
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/polygon/voronoi_builder.hpp>


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2015 Adam Wulkiewicz, Lodz, Poland.

// 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)

//[polygon
//` Declaration and use of the Boost.Geometry model::polygon, modelling the Polygon Concept

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace bg = boost::geometry;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::polygon<point_t> polygon_t; /*< Default parameters, clockwise, closed polygon. >*/

    polygon_t poly1; /*< Default-construct a polygon. >*/

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) \
 && !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)

    polygon_t polygon2{{{0.0, 0.0}, {0.0, 5.0}, {5.0, 5.0}, {5.0, 0.0}, {0.0, 0.0}},
                       {{1.0, 1.0}, {4.0, 1.0}, {4.0, 4.0}, {1.0, 4.0}, {1.0, 1.0}}}; /*< Construct a polygon containing an exterior and interior ring, using C++11 unified initialization syntax. >*/

#endif

    bg::append(poly1.outer(), point_t(0.0, 0.0)); /*< Append point to the exterior ring. >*/
    bg::append(poly1.outer(), point_t(0.0, 5.0));
    bg::append(poly1.outer(), point_t(5.0, 5.0));
    bg::append(poly1.outer(), point_t(5.0, 0.0));
    bg::append(poly1.outer(), point_t(0.0, 0.0));

    poly1.inners().resize(1); /*< Resize a container of interior rings. >*/
    bg::append(poly1.inners()[0], point_t(1.0, 1.0)); /*< Append point to the interior ring. >*/
    bg::append(poly1.inners()[0], point_t(4.0, 1.0));
    bg::append(poly1.inners()[0], point_t(4.0, 4.0));
    bg::append(poly1.inners()[0], point_t(1.0, 4.0));
    bg::append(poly1.inners()[0], point_t(1.0, 1.0));

    double a = bg::area(poly1);

    std::cout << a << std::endl;

    return 0;
}

//]

//[polygon_output
/*`
Output:
[pre
16
]
*/
//]


#include <armadillo>
using namespace arma;

vec dataFormer(vec x, double dt)
{
  vec dx(size(x));

  vec p1 = diff(x)/dt;
  dx(0) = p1(0);
  for (int i = 1; i< x.n_elem; i++)
  {
    dx(i) = p1(i-1);
  }
  // std::cout << "p1 = " <<p1<< '\n';
  // std::cout << "dx = " <<dx<< '\n';
  return dx;
}

mat dataFormer(mat x, double dt)
{
  mat dx(size(x));

  mat p1 = diff(x)/dt;
  dx(0) = p1(0);
  for (int i = 1; i< x.n_elem; i++)
  {
    dx(i) = p1(i-1);
  }
  // std::cout << "p1 = " <<p1<< '\n';
  // std::cout << "dx = " <<dx<< '\n';
  return dx;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REMAINDER_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/remainder.hpp>
#include <boost/simd/include/functions/simd/idivround.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::remainder_, tag::cpu_,
                              (A0)(X),
                              ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0-idivround(a0, a1)*a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::remainder_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return if_nan_else(logical_or(is_invalid(a0),is_eqz(a1)),
                      a0-divround(a0, a1)*a1);
    }
  };
} } }

#endif


//State Vector- variable acceleration model
	// X(0) = quad_position.point.x;
    // X(1) = m_velocity.point.x;
    // X(2) = m_acc_bias.point.x;
    // X(3) = quad_position.point.y;
    // X(4) = m_velocity.point.y;
    // X(5) = m_acc_bias.point.y;
    // X(6) = quad_position.point.z;
    // X(7) = m_velocity.point.z;
    // X(8) = m_acc_bias.point.z;
	// X(9)  = leader.point.x;
    // X(10) = leader.point.y;
    // X(11) = leader.point.z;
	// X(12) = leader.point.x;
	// X(13) = leader.point.y;
	// X(14) = leader.point.z;
//Input Vector
	// U(0) = quad.imu.acc.x
	// U(1) = 0
	// U(2) = 0
	// U(3) = quad.imu.acc.y
	// U(4) = 0
	// U(5) = 0
	// U(6) = quad.imu.acc.z
	// U(7) = 0
	// U(8) = 0
	// U(9)  = leader_quad.point.x
	// U(10) = leader_quad.point.y
	// U(11) = leader_quad.point.z
	// U(12) = neighbor_quad.point.x
	// U(13) = neighbor_quad.point.y
	// U(14) = neighbor_quad.point.z

#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <bits/stdc++.h>
#include <ros/ros.h>
#include <math.h>
#include <stdio.h>
#include <dwm1001/anchor.h>
#include <sensor_msgs/Imu.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"

// Declarations
int count =0 ;
double r_pred_l,r_meas_l;
Eigen::MatrixXd H_l(1,15);
double R_l;
double m_R_scale_l,sigma_r_l;
Eigen::VectorXd K_l(15);
Eigen::VectorXd X_p_l(15);
Eigen::MatrixXd P_p_l(15,15);
double error_l;
double precisionRangeErrEst_l;
//
double r_pred_n,r_meas_n;
Eigen::MatrixXd H_n(1,15);
double R_n;
double m_R_scale_n,sigma_r_n;
Eigen::VectorXd K_n(15);
Eigen::VectorXd X_p_n(15);
Eigen::MatrixXd P_p_n(15,15);
double error_n;
double precisionRangeErrEst_n;
//
geometry_msgs::Vector3Stamped pose;
geometry_msgs::Vector3Stamped vel;
geometry_msgs::Vector3Stamped acc_;
float ax,ay,az;
Eigen::Quaternionf q;
Eigen::MatrixXf R_mat(3,3);
Eigen::MatrixXf imuacc(3,1);
Eigen::MatrixXf acc(3,1);
//
Eigen::VectorXd X(15);
Eigen::VectorXd X_e(15);
Eigen::MatrixXd F(15,15);
Eigen::MatrixXd block_F(3,3);
Eigen::MatrixXd Q(15,15);
Eigen::VectorXd u(15);
Eigen::MatrixXd B(15,15);
Eigen::MatrixXd P(15,15);
Eigen::MatrixXd M(15,15);
Eigen::MatrixXd block_B(3,3);
Eigen::MatrixXd block_Q(3,3);
Eigen::MatrixXd B_I(6,6);
Eigen::MatrixXd Q_lead(3,3);
Eigen::MatrixXd Q_neigh(3,3);
double tao_acc;
double tao_bias;
double sigma_a,r_meas;
double T;
double m_last_range_time;
std_msgs::Float32MultiArray output;
double error_threshold,precisionRangeMm;
double m_kalman_sigma_a,T_sq,m_tao_acc_sqrt,m_tao_bias_sqrt,T_cub,m_z_damping_factor,m_Q_scale;
double m_snr_threshold,error;
double x,y,z,x_l,y_l,z_l,x_n,y_n,z_n;
double q_l,q_n;

void setState(Eigen::VectorXd Y)
{
    X = Y;
}
void setCovariance(Eigen::MatrixXd S)
{
    P = S;
}

void convert_NED(sensor_msgs::Imu imu)
{
	//conerts the acceleration from body frame to earth frame(NED)
    q = Eigen::Quaternionf(imu.orientation.w, imu.orientation.x, imu.orientation.y, imu.orientation.z);
    R_mat= q.toRotationMatrix();

    ax=imu.linear_acceleration.x;
    ay=imu.linear_acceleration.y;
    az=imu.linear_acceleration.z;
    imuacc << ax,ay,az;
    acc= R_mat*imuacc;

}

void prediction_step(const sensor_msgs::Imu::ConstPtr& msg)
{
	convert_NED(*msg);
  	ax=acc(0,0);
  	ay=-acc(1,0);
  	az=-1*(acc(2,0)-9.8);

    //ROS_WARN("Acceleration:## %f, ## %f, ## %f", ax,ay,az);
    T = msg->header.stamp.toSec() - m_last_range_time;
    if(T>1){
        T = 1;
    } else if(T<0){
        T = 0.01;
    }

    sigma_a = m_kalman_sigma_a;
   // r_meas = double(precisionRangeMm) / 1000.0;

    T_sq = std::pow(T,2);
    T_cub = std::pow(T,3);

    // F is a 9x9 State Transition Matrix
    F = Eigen::MatrixXd::Zero(15,15);

    block_F << 1, T, -T_sq/2.0,
               0, 1, -T,
               0, 0, 1;
    F.block<3,3>(0,0) = block_F;
    F.block<3,3>(3,3) = block_F;
    F.block<3,3>(6,6) = block_F;

    u << ax, 0, 0,
         ay, 0, 0,
         az, 0, 0,
         x_l,y_l,z_l,
         x_n,y_n,z_n;
    B = Eigen::MatrixXd::Zero(15,15);
    B_I = Eigen::MatrixXd::Identity(6,6);
    block_B << T_sq/2.0,  0,  0,
               T      ,  0,  0,
               0      ,  0,  0;
    B.block<3,3>(0,0) = block_B;
    B.block<3,3>(3,3) = block_B;
    B.block<3,3>(6,6) = block_B;
    B.block<6,6>(9,9) = B_I;

    // X is the predicted state vector and the predicted covariance matrix
    X_e = F*X + B * u;

    // Q is the acceleration model
    tao_acc = m_tao_acc_sqrt * m_tao_acc_sqrt;
    tao_bias = m_tao_bias_sqrt * m_tao_bias_sqrt;
    Q = Eigen::MatrixXd::Zero(15,15);

    block_Q << (T_cub*tao_acc/3.0)+(T_cub*T_sq)*tao_bias/20.0, (T_sq*tao_acc/2)+(T_sq*T_sq)*tao_bias/8.0  ,-T_cub*tao_bias/6,
        	   (T_sq*tao_acc/2.0)+(T_sq*T_sq)*tao_bias/8.0 ,   T*tao_acc+(T_cub*tao_bias/3) 			  ,-T_sq*tao_bias/2,
         	   -T_cub*tao_bias/6.0,						    -T_sq*tao_bias/2 						  ,T*tao_bias         ;

    Q_lead <<  q_l,  0,  0,
    			0, q_l, 0,
    			0,  0,  q_l ;
    Q_neigh << q_n,  0,  0,
    			0, q_n, 0,
    			0,  0,  q_n ;

    Q.block<3,3>(0,0) = block_Q;
    Q.block<3,3>(3,3) = block_Q;
    Q.block<3,3>(6,6) = block_Q * m_z_damping_factor;
    Q *= m_Q_scale;
    Q.block<3,3>(9,9) = Q_lead;
    Q.block<3,3>(12,12) = Q_neigh;
    //ROS_WARN("covariance:## %f, ## %f, ## %f",(T_cub*tao_acc/3.0)+(T_cub*T_sq)*tao_bias/20.0 , T*tao_acc+(T_cub*tao_bias/3), T*tao_bias);

    // if(count%250 == 0)
    // std::cout << P << '\n'<<'\n';
    // M is the predicted covariance matrix
	M = F*P*F.transpose() + Q;

    // time update
    m_last_range_time = msg->header.stamp.toSec();
    //count++;
    error = X.squaredNorm()-X_e.squaredNorm();
    if(error < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_e);
        setCovariance(M);
        return ;
    } else {

        ROS_WARN("\n Estimate too large: %f", error);
        return ;
    }

}

void correction_step_leader(const dwm1001::anchor::ConstPtr& msg)
{
	r_pred_l = std::sqrt( std::pow(X(0) -  X(9), 2) +
                          std::pow(X(3) - X(10), 2) +
                          std::pow(X(6) - X(11), 2) ) + 1e-5;

	r_meas_l = msg->range;
    // H is the linearized measurement matrix
    H_l << (X(0) -  X(9))/r_pred_l, 0, 0,
           (X(3) - X(10))/r_pred_l, 0, 0,
           (X(6) - X(11))/r_pred_l, 0, 0,
           (X(9) - X(0))/r_pred_l, (X(10) - X(3))/r_pred_l, (X(11) - X(6))/r_pred_l,
         	0,	0,	0;

    // K is the Kalman Gain
    sigma_r_l = double(precisionRangeErrEst_l) / 1000.0;
    R_l = std::pow(sigma_r_l,2) * m_R_scale_l;
    //ROS_WARN("R_matrix:## %f",R);
    K_l = P*H_l.transpose() / ( (H_l*P*H_l.transpose())(0,0) + R_l );
    // Update P for the a posteriori covariance matrix
    P_p_l = ( Eigen::MatrixXd::Identity(15,15) - K_l*H_l ) * P;
    // Return the measurement innovation
    error_l = std::fabs(r_meas_l - r_pred_l);
    // Update the state
    X_p_l = X + K_l * (r_meas_l - r_pred_l);
    // decide to take the range info or not.
    if(error_l < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_p_l);
        setCovariance(P_p_l);

        return ;
    } else {

        ROS_WARN("Anchor id , Update too large: %f --", error_l);
        std::cout << msg->device_id << "\n";
        return ;
    }

}

void correction_step_neigh(const dwm1001::anchor::ConstPtr& msg)
{
	r_pred_n = std::sqrt( std::pow(X(0) - X(12), 2) +
                          std::pow(X(3) - X(13), 2) +
                          std::pow(X(6) - X(14), 2) ) + 1e-5;

	r_meas_n = msg->range;
    // H is the linearized measurement matrix
    H_n << (X(0) - X(12))/r_pred_n, 0, 0,
           (X(3) - X(13))/r_pred_n, 0, 0,
           (X(6) - X(14))/r_pred_n, 0, 0,
         	0,	0,	0,
           (X(12) - X(0))/r_pred_n, (X(13) - X(3))/r_pred_n, (X(14) - X(6))/r_pred_n ;

    // K is the Kalman Gain
    sigma_r_n = double(precisionRangeErrEst_n) / 1000.0;
    R_n = std::pow(sigma_r_n,2) * m_R_scale_n;
    //ROS_WARN("R_matrix:## %f",R);
    K_n = P*H_n.transpose() / ( (H_n*P*H_n.transpose())(0,0) + R_n );
    // Update P for the a posteriori covariance matrix
    P_p_n = ( Eigen::MatrixXd::Identity(15,15) - K_n*H_n ) * P;
    // Return the measurement innovation
    error_n = std::fabs(r_meas_n - r_pred_n);
    // Update the state
    X_p_n = X + K_n * (r_meas_n - r_pred_n);
    // decide to take the range info or not.
    if(error < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_p_n);
        setCovariance(P_p_n);

        return ;
    } else {

        ROS_WARN("Anchor id , Update too large: %f --", error_n);
        std::cout << msg->device_id << "\n";
        return ;
    }

}

// void anchor_lead_cb(const dwm1001::anchor::ConstPtr& msg)
// {
// 	correction_step_leader(msg);
// }

// void anchor_neigh_cb(const dwm1001::anchor::ConstPtr& msg)
// {
// 	correction_step_neigh(msg);
// }

void position_lead_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	x_l =  msg->pose.position.x;
    y_l =  msg->pose.position.y;
    z_l =  msg->pose.position.z;
}

void position_neigh_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	x_n =  msg->pose.position.x;
    y_n =  msg->pose.position.y;
    z_n =  msg->pose.position.z;
}

void param(ros::NodeHandle& nh)
{
    nh.getParam("KalmanFilter_swarm/start_x", x);
    nh.getParam("KalmanFilter_swarm/start_y", y);
    nh.getParam("KalmanFilter_swarm/start_z", z);
    nh.getParam("KalmanFilter_swarm/lead_x", x_l);
    nh.getParam("KalmanFilter_swarm/lead_y", y_l);
    nh.getParam("KalmanFilter_swarm/lead_z", z_l);
    nh.getParam("KalmanFilter_swarm/neighbor_x", x_n);
    nh.getParam("KalmanFilter_swarm/neighbor_y", y_n);
    nh.getParam("KalmanFilter_swarm/neighbor_z", z_n);
    nh.getParam("KalmanFilter_swarm/error_threshold", error_threshold);
    nh.getParam("KalmanFilter_swarm/m_R_scale_leader", m_R_scale_l);
    nh.getParam("KalmanFilter_swarm/precisionRangeErrEst_leader", precisionRangeErrEst_l);
    nh.getParam("KalmanFilter_swarm/m_R_scale_neighbor", m_R_scale_n);
    nh.getParam("KalmanFilter_swarm/precisionRangeErrEst_neighbor", precisionRangeErrEst_n);
    nh.getParam("KalmanFilter_swarm/m_kalman_sigma_a", m_kalman_sigma_a);
    nh.getParam("KalmanFilter_swarm/precisionRangeMm", precisionRangeMm);
    nh.getParam("KalmanFilter_swarm/leader_covariance", q_l);
    nh.getParam("KalmanFilter_swarm/neighbor_covariance", q_n);
    nh.getParam("KalmanFilter_swarm/m_tao_acc_sqrt", m_tao_acc_sqrt);
    nh.getParam("KalmanFilter_swarm/m_tao_bias_sqrt", m_tao_bias_sqrt );
    nh.getParam("KalmanFilter_swarm/m_z_damping_factor", m_z_damping_factor);
    nh.getParam("KalmanFilter_swarm/m_Q_scale", m_Q_scale);
}

void Initialize(ros::NodeHandle& nh)
{

    param(nh);
    // ROS_WARN("m_Q_scale %f", m_Q_scale);
    // ROS_WARN("x %f", x);

    m_kalman_sigma_a = 0.125;
    m_snr_threshold = -100;

    Eigen::MatrixXd nine_cov = Eigen::MatrixXd::Identity(15,15);
    nine_cov(0,0) = 0.001;
    nine_cov(3,3) = 0.001;
    nine_cov(6,6) = 0.001;
    // set cov of vel
    nine_cov(1,1) = 0.01;
    nine_cov(4,4) = 0.01;
    nine_cov(7,7) = 0.01;
    // set cov of acc_bias
    nine_cov(2,2) = 0.01;
    nine_cov(5,5) = 0.01;
    nine_cov(8,8) = 0.01;

    nine_cov(9,9)   = 0.001;
    nine_cov(10,10) = 0.001;
    nine_cov(11,11) = 0.001;

    nine_cov(12,12) = 0.001;
    nine_cov(13,13) = 0.001;
    nine_cov(14,14) = 0.001;

    setCovariance(nine_cov);
    X<<x,0,0,y,0,0,z,0,0,x_l,y_l,z_l,x_n,y_n,z_n;
    m_last_range_time = ros::Time::now().toSec();

}

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

    ros::init(argc,argv,"KalmanFilter_swarm");
    ros::NodeHandle nh;
    //Initializinging the parameters
    Initialize(nh);

    //Subscriber and Publisher for the data. remapped in the launch file to the topic required
    ros::Subscriber Imu=nh.subscribe("imu",100,prediction_step);
    ros::Subscriber anchor_1 = nh.subscribe("anchor_lead", 100,correction_step_leader);
    ros::Subscriber anchor_2 = nh.subscribe("anchor_neigh",100,correction_step_neigh);
    ros::Subscriber anchor_3 = nh.subscribe("position_leader",100,position_lead_cb);
    ros::Subscriber anchor_4 = nh.subscribe("position_neighour",100,position_neigh_cb);

    //ros::Publisher fused = nh.advertise<std_msgs::Float32MultiArray>("Filtered_data", 100);
    ros::Publisher fused_pose = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_pose", 10);
    ros::Publisher fused_vel = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_velocity", 10);
    ros::Publisher fused_acc = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_acc", 10);

    ros::Rate loop_rate(50);
    while(ros::ok()){
        param(nh);
    //	output.data.clear();
	//	for (int i = 0; i < 15; i++)
	//		output.data.push_back(X(i));

        pose.header.stamp = ros::Time::now();
        pose.vector.x=X(0);
        pose.vector.y=X(3);
        pose.vector.z=X(6);

        vel.header.stamp = ros::Time::now();
        vel.vector.x=X(1);
        vel.vector.y=X(4);
        vel.vector.z=X(7);

        acc_.header.stamp = ros::Time::now();
        acc_.vector.x=ax;
        acc_.vector.y=ay;
        acc_.vector.z=az;
    //    fused.publish(output);
        fused_pose.publish(pose);
        fused_vel.publish(vel);
        fused_acc.publish(acc_);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}