//
// Copyright (c) 2016-2020 CNRS INRIA
//

#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <boost/test/floating_point_comparison.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_kinetic_energy)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  const VectorXd qmax = VectorXd::Ones(model.nq);
  VectorXd q = randomConfiguration(model,-qmax,qmax);
  VectorXd v = VectorXd::Ones(model.nv);

  data.M.fill(0);  crba(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  double kinetic_energy_ref = 0.5 * v.transpose() * data.M * v;
  double kinetic_energy = computeKineticEnergy(model, data, q, v);

  BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
}

BOOST_AUTO_TEST_CASE(test_potential_energy)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  const VectorXd qmax = VectorXd::Ones(model.nq);
  VectorXd q = randomConfiguration(model,-qmax,qmax);

  double potential_energy = computePotentialEnergy(model, data, q);
  centerOfMass(model,data_ref,q);

  double potential_energy_ref = -data_ref.mass[0] * (data_ref.com[0].dot(model.gravity.linear()));

  BOOST_CHECK_SMALL(potential_energy_ref - potential_energy, 1e-12);
}

BOOST_AUTO_TEST_SUITE_END()


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkNumericTwoCompartmentExchangeModel.h"
#include "mitkAIFParametrizerHelper.h"
#include "mitkTimeGridHelper.h"
#include "mitkTwoCompartmentExchangeModelDifferentialEquations.h"
#include <vnl/algo/vnl_fft_1d.h>
#include <boost/numeric/odeint.hpp>
#include <fstream>

const std::string mitk::NumericTwoCompartmentExchangeModel::MODEL_DISPLAY_NAME =
  "Numeric Two Compartment Exchange Model";

const std::string mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_F = "F";
const std::string mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_PS = "PS";
const std::string mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_ve = "ve";
const std::string mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_vp = "vp";

const std::string mitk::NumericTwoCompartmentExchangeModel::UNIT_PARAMETER_F = "ml/min/100ml";
const std::string mitk::NumericTwoCompartmentExchangeModel::UNIT_PARAMETER_PS = "ml/min/100ml";
const std::string mitk::NumericTwoCompartmentExchangeModel::UNIT_PARAMETER_ve = "ml/ml";
const std::string mitk::NumericTwoCompartmentExchangeModel::UNIT_PARAMETER_vp = "ml/ml";

const unsigned int mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_F = 0;
const unsigned int mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_PS = 1;
const unsigned int mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_ve = 2;
const unsigned int mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_vp = 3;

const unsigned int mitk::NumericTwoCompartmentExchangeModel::NUMBER_OF_PARAMETERS = 4;

const std::string mitk::NumericTwoCompartmentExchangeModel::NAME_STATIC_PARAMETER_ODEINTStepSize = "ODEIntStepSize";

std::string mitk::NumericTwoCompartmentExchangeModel::GetModelDisplayName() const
{
  return MODEL_DISPLAY_NAME;
};

std::string mitk::NumericTwoCompartmentExchangeModel::GetModelType() const
{
  return "Perfusion.MR";
};

mitk::NumericTwoCompartmentExchangeModel::NumericTwoCompartmentExchangeModel()
{

}

mitk::NumericTwoCompartmentExchangeModel::~NumericTwoCompartmentExchangeModel()
{

}

mitk::NumericTwoCompartmentExchangeModel::ParameterNamesType mitk::NumericTwoCompartmentExchangeModel::GetStaticParameterNames() const
{
  ParameterNamesType result;

  result.push_back(NAME_STATIC_PARAMETER_AIF);
  result.push_back(NAME_STATIC_PARAMETER_AIFTimeGrid);
  result.push_back(NAME_STATIC_PARAMETER_ODEINTStepSize);

  return result;
}

mitk::NumericTwoCompartmentExchangeModel::ParametersSizeType  mitk::NumericTwoCompartmentExchangeModel::GetNumberOfStaticParameters()
const
{
  return 3;
}

void mitk::NumericTwoCompartmentExchangeModel::SetStaticParameter(const ParameterNameType& name,
    const StaticParameterValuesType& values)
{
  if (name == NAME_STATIC_PARAMETER_AIF)
  {
    AterialInputFunctionType aif = mitk::convertParameterToArray(values);

    SetAterialInputFunctionValues(aif);
  }

  if (name == NAME_STATIC_PARAMETER_AIFTimeGrid)
  {
    TimeGridType timegrid = mitk::convertParameterToArray(values);

    SetAterialInputFunctionTimeGrid(timegrid);
  }

  if (name == NAME_STATIC_PARAMETER_ODEINTStepSize)
  {
      SetODEINTStepSize(values[0]);
  }
};

mitk::NumericTwoCompartmentExchangeModel::StaticParameterValuesType mitk::NumericTwoCompartmentExchangeModel::GetStaticParameterValue(
  const ParameterNameType& name) const
{
  StaticParameterValuesType result;

  if (name == NAME_STATIC_PARAMETER_AIF)
  {
    result = mitk::convertArrayToParameter(this->m_AterialInputFunctionValues);
  }

  if (name == NAME_STATIC_PARAMETER_AIFTimeGrid)
  {
    result = mitk::convertArrayToParameter(this->m_AterialInputFunctionTimeGrid);
  }
  if (name == NAME_STATIC_PARAMETER_ODEINTStepSize)
  {
    result.push_back(GetODEINTStepSize());
  }

  return result;
};

mitk::NumericTwoCompartmentExchangeModel::ParameterNamesType
mitk::NumericTwoCompartmentExchangeModel::GetParameterNames() const
{
  ParameterNamesType result;

  result.push_back(NAME_PARAMETER_F);
  result.push_back(NAME_PARAMETER_PS);
  result.push_back(NAME_PARAMETER_ve);
  result.push_back(NAME_PARAMETER_vp);

  return result;
}

mitk::NumericTwoCompartmentExchangeModel::ParametersSizeType
mitk::NumericTwoCompartmentExchangeModel::GetNumberOfParameters() const
{
  return NUMBER_OF_PARAMETERS;
}

mitk::NumericTwoCompartmentExchangeModel::ParamterUnitMapType
mitk::NumericTwoCompartmentExchangeModel::GetParameterUnits() const
{
  ParamterUnitMapType result;

  result.insert(std::make_pair(NAME_PARAMETER_F, UNIT_PARAMETER_F));
  result.insert(std::make_pair(NAME_PARAMETER_PS, UNIT_PARAMETER_PS));
  result.insert(std::make_pair(NAME_PARAMETER_vp, UNIT_PARAMETER_vp));
  result.insert(std::make_pair(NAME_PARAMETER_ve, UNIT_PARAMETER_ve));

  return result;
};

mitk::NumericTwoCompartmentExchangeModel::ModelResultType
mitk::NumericTwoCompartmentExchangeModel::ComputeModelfunction(const ParametersType& parameters)
const
{
  typedef itk::Array<double> ConcentrationCurveType;
  typedef std::vector<double> ConcentrationVectorType;

  if (this->m_TimeGrid.GetSize() == 0)
  {
    itkExceptionMacro("No Time Grid Set! Cannot Calculate Signal");
  }

  AterialInputFunctionType aterialInputFunction;
  aterialInputFunction = GetAterialInputFunction(this->m_TimeGrid);

  unsigned int timeSteps = this->m_TimeGrid.GetSize();

  /** @brief Boost::numeric::odeint works with type std::vector<double> thus, aif and grid are converted to ModelParameters( of type std::vector)
   */
  mitk::TwoCompartmentExchangeModelDifferentialEquations::AIFType aif = mitk::convertArrayToParameter(
        aterialInputFunction);
  mitk::TwoCompartmentExchangeModelDifferentialEquations::AIFType grid =
    mitk::convertArrayToParameter(m_TimeGrid);

  mitk::TwoCompartmentExchangeModelDifferentialEquations::AIFType aifODE = aif;
  aifODE.push_back(aif[timeSteps - 1]);
  mitk::TwoCompartmentExchangeModelDifferentialEquations::AIFType gridODE = grid;
  gridODE.push_back(grid[timeSteps - 1] + (grid[timeSteps - 1] - grid[timeSteps - 2]));

  //Model Parameters
  double F = (double) parameters[POSITION_PARAMETER_F] / 6000.0;
  double PS  = (double) parameters[POSITION_PARAMETER_PS] / 6000.0;
  double ve = (double) parameters[POSITION_PARAMETER_ve];
  double vp = (double) parameters[POSITION_PARAMETER_vp];

  /** @brief Initialize class TwoCompartmentExchangeModelDifferentialEquations defining the differential equations. AIF and Grid must be set so that at step t the aterial Concentration Ca(t) can be interpolated from AIF*/
  mitk::TwoCompartmentExchangeModelDifferentialEquations ode;
  ode.initialize(F, PS, ve, vp);
  ode.setAIF(aifODE);
  ode.setAIFTimeGrid(gridODE);

  state_type x(2);
  x[0] = 0.0;
  x[1] = 0.0;
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<state_type> error_stepper_type;
  //typedef boost::numeric::odeint::runge_kutta4< state_type > stepper_type;

  /** @brief Results of odeeint x[0] and x[1]*/
  ConcentrationVectorType Cp;
  ConcentrationVectorType Ce;
  ConcentrationVectorType odeTimeGrid;

  error_stepper_type stepper;

  /** @brief Stepsize. Should be adapted by stepper (runge_kutta_cash_karp54) */
//  const double dt = 0.05;
   const double dt = this->m_ODEINTStepSize;

  /** @brief perform Step t -> t+dt to calculate approximate value x(t+dt)*/
  for (double t = 0.0; t < this->m_TimeGrid(this->m_TimeGrid.GetSize() - 1) - 2*dt; t += dt)
  {
    stepper.do_step(ode, x, t, dt);
    Cp.push_back(x[0]);
    Ce.push_back(x[1]);
    odeTimeGrid.push_back(t);
  }

  /** @brief transfom result of Differential equations back to itk::Array and interpolate to m_TimeGrid (they are calculated on a different grid defined by stepsize of odeint)*/
  ConcentrationCurveType plasmaConcentration = mitk::convertParameterToArray(Cp);
  ConcentrationCurveType EESConcentration = mitk::convertParameterToArray(Ce);
  ConcentrationCurveType rungeKuttaTimeGrid = mitk::convertParameterToArray(odeTimeGrid);

  mitk::ModelBase::ModelResultType C_Plasma = mitk::InterpolateSignalToNewTimeGrid(plasmaConcentration, rungeKuttaTimeGrid, m_TimeGrid);
  mitk::ModelBase::ModelResultType C_EES = mitk::InterpolateSignalToNewTimeGrid(EESConcentration, rungeKuttaTimeGrid, m_TimeGrid);

  //Signal that will be returned by ComputeModelFunction
  mitk::ModelBase::ModelResultType signal(timeSteps);
  signal.fill(0.0);

  mitk::ModelBase::ModelResultType::iterator signalPos = signal.begin();
  mitk::ModelBase::ModelResultType::const_iterator CePos = C_EES.begin();

  mitk::ModelBase::ModelResultType::const_iterator t = this->m_TimeGrid.begin();
  mitk::ModelBase::ModelResultType::const_iterator Cin = aterialInputFunction.begin();

  for (mitk::ModelBase::ModelResultType::const_iterator CpPos = C_Plasma.begin();
       CpPos != C_Plasma.end(); ++CpPos, ++CePos, ++signalPos, ++t, ++Cin)
  {
    *signalPos = vp * (*CpPos) + ve * (*CePos);

  }

  return signal;

}

itk::LightObject::Pointer mitk::NumericTwoCompartmentExchangeModel::InternalClone() const
{
  NumericTwoCompartmentExchangeModel::Pointer newClone = NumericTwoCompartmentExchangeModel::New();

  newClone->SetTimeGrid(this->m_TimeGrid);

  return newClone.GetPointer();
}

void mitk::NumericTwoCompartmentExchangeModel::PrintSelf(std::ostream& os,
    ::itk::Indent indent) const
{
  Superclass::PrintSelf(os, indent);

}


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/hypot.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdHypot,FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<var> x(3.0,1.3);

  fvar<var> z(6.0,1.0);
  fvar<var> a = hypot(x,z);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ((1.3 * 3.0 + 6.0 * 1.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0),g[0]);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0),g[1]);
}
TEST(AgradFwdHypot,FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<var> x(3.0,1.3);
  double z(6.0);
  fvar<var> a = hypot(x,z);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ((1.3 * 3.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0),g[0]);
}
TEST(AgradFwdHypot,Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  double x(3.0);
  fvar<var> z(6.0,1.0);
  fvar<var> a = hypot(x,z);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ((6.0 * 1.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0),g[0]);
}
TEST(AgradFwdHypot,FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<var> x(3.0,1.3);
  fvar<var> z(6.0,1.0);
  fvar<var> a = hypot(x,z);

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ((1.3 * 6.0 * 6.0 - 6.0 * 3.0)
                  / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
  EXPECT_FLOAT_EQ((1.0 * 3.0 * 3.0 - 1.3 * 6.0 * 3.0)
                  / hypot(3.0,6.0) / (9.0 + 36.0),g[1]);
}
TEST(AgradFwdHypot,FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<var> x(3.0,1.3);
  double z(6.0);
  fvar<var> a = hypot(x,z);

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(1.3 * 6.0 * 6.0 / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
}
TEST(AgradFwdHypot,Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  double x(3.0);
  fvar<var> z(6.0,1.0);
  fvar<var> a = hypot(x,z);

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(1.0 * 3.0 * 3.0 / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
}

TEST(AgradFwdHypot,FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.059628479, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0), g[0]);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), g[1]);
}
TEST(AgradFwdHypot,FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x,y);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0), g[0]);
}

TEST(AgradFwdHypot,Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), g[0]);
}
TEST(AgradFwdHypot,FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);

  EXPECT_FLOAT_EQ(36.0 / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
  EXPECT_FLOAT_EQ(-2.0/15.0/std::sqrt(5.0), g[1]);
}
TEST(AgradFwdHypot,FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(-2.0/15.0/std::sqrt(5.0), g[0]);
  EXPECT_FLOAT_EQ((3.0 * 3.0) / hypot(3.0,6.0) / (9.0 + 36.0),g[1]);
}
TEST(AgradFwdHypot,FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);

  EXPECT_FLOAT_EQ(6.0 * 6.0 / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
}

TEST(AgradFwdHypot,Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ((3.0 * 3.0) / hypot(3.0,6.0) / (9.0 + 36.0),g[0]);
}
TEST(AgradFwdHypot,FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.0079504643, g[0]);
  EXPECT_FLOAT_EQ(0.013913312,g[1]);
}
TEST(AgradFwdHypot,FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);

  EXPECT_FLOAT_EQ(-0.02385139175999775676169785246647,g[0]);
}

TEST(AgradFwdHypot,Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::hypot;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = hypot(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.0119256958799988783808489262332,g[0]);
}

struct hypot_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return hypot(arg1,arg2);
  }
};

TEST(AgradFwdHypot, nan) {
  hypot_fun hypot_;
  test_nan_mix(hypot_,3.0,5.0,false);
}


#define EIGEN_RUNTIME_NO_MALLOC
#include <Eigen/Core>
#include <algorithm>
#include <catch2/catch.hpp>
#include <vector>
#include "ear/dsp/gain_interpolator.hpp"
#include "ear/dsp/ptr_adapter.hpp"
#include "eigen_utils.hpp"

using namespace ear;
using namespace ear::dsp;

// adapters to make apply_interp, apply_constant and process work with Eigen
// types
template <typename Interp = LinearInterpSingle, typename In, typename Out,
          typename Point>
void apply_interp(In &&in, Out &&out, SampleIndex block_start,
                  SampleIndex start, SampleIndex end, const Point &start_point,
                  const Point &end_point) {
  PtrAdapter in_p(in.cols());
  in_p.set_eigen(in);
  PtrAdapter out_p(out.cols());
  out_p.set_eigen(out);

  Interp::apply_interp(in_p.ptrs(), out_p.ptrs(), 0, in.rows(), block_start,
                       start, end, start_point, end_point);
}

template <typename Interp = LinearInterpSingle, typename In, typename Out,
          typename Point>
void apply_constant(In &&in, Out &&out, const Point &point) {
  PtrAdapter in_p(in.cols());
  in_p.set_eigen(in);
  PtrAdapter out_p(out.cols());
  out_p.set_eigen(out);

  Interp::apply_constant(in_p.ptrs(), out_p.ptrs(), 0, in.rows(), point);
}

template <typename Interp>
void process(Interp &interp, SampleIndex block_start,
             const Eigen::Ref<const Eigen::MatrixXf> &in,
             Eigen::Ref<Eigen::MatrixXf> out) {
  PtrAdapterConst in_p(in.cols());
  in_p.set_eigen(in);
  PtrAdapter out_p(out.cols());
  out_p.set_eigen(out);

  interp.process(block_start, in.rows(), in_p.ptrs(), out_p.ptrs());
}

// ensure that LinearInterpSingle is correct so that we can use it to generate
// expected results in tests of GainInterpolator; otherwise we would just end up
// reimplementing it here

TEST_CASE("LinearInterpSingle::apply_interp") {
  Eigen::VectorXf input = Eigen::VectorXf::Random(100);
  Eigen::VectorXf output = Eigen::VectorXf::Zero(100);

  apply_interp<LinearInterpSingle>(input, output, 100, 50, 250, 0.2f, 0.8f);

  Eigen::VectorXf p = Eigen::VectorXf::LinSpaced(200, 0, 199) / 200.0;
  Eigen::VectorXf gain_ramp = 0.8f * p.array() + (1.0f - p.array()) * 0.2f;
  Eigen::VectorXf expected =
      gain_ramp(Eigen::seqN(50, 100)).cwiseProduct(input);

  CHECK_THAT(output, IsApprox(expected));
}

TEST_CASE("LinearInterpSingle::apply_constant") {
  Eigen::VectorXf input = Eigen::VectorXf::Random(100);
  Eigen::VectorXf output = Eigen::VectorXf::Zero(100);

  apply_constant<LinearInterpSingle>(input, output, 0.3f);

  Eigen::VectorXf expected = 0.3f * input;

  CHECK_THAT(output, IsApprox(expected));
}

void run_test(GainInterpolator<LinearInterpSingle> &interp,
              Eigen::VectorXf &input, Eigen::VectorXf &expected_output,
              const std::vector<Eigen::Index> &block_sizes) {
  for (auto block_size : block_sizes) {
    Eigen::VectorXf output = Eigen::VectorXf::Zero(input.size());

    Eigen::internal::set_is_malloc_allowed(false);
    for (Eigen::Index offset = 0; offset < input.size(); offset += block_size) {
      auto block =
          Eigen::seq(offset, std::min(offset + block_size, input.size()) - 1);

      process(interp, offset, input(block), output(block));
    }
    Eigen::internal::set_is_malloc_allowed(true);

    CHECK_THAT(output, IsApprox(expected_output));
  }
}

// check that GainInterpolator makes the right calls to the templated InterpType

TEST_CASE("basic") {
  GainInterpolator<LinearInterpSingle> interp;

  interp.interp_points.emplace_back(100, 0.2f);
  interp.interp_points.emplace_back(200, 0.8f);
  interp.interp_points.emplace_back(300, 0.8f);
  interp.interp_points.emplace_back(400, 0.3f);

  Eigen::VectorXf input = Eigen::VectorXf::Random(500);
  Eigen::VectorXf expected_output = Eigen::VectorXf::Zero(500);

  auto block = Eigen::seqN(0, 100);
  apply_constant(input(block), expected_output(block), 0.2f);

  block = Eigen::seqN(100, 100);
  apply_interp(input(block), expected_output(block), 100, 100, 200, 0.2f, 0.8f);

  block = Eigen::seqN(200, 100);
  apply_constant(input(block), expected_output(block), 0.8f);

  block = Eigen::seqN(300, 100);
  apply_interp(input(block), expected_output(block), 300, 300, 400, 0.8f, 0.3f);

  block = Eigen::seqN(400, 100);
  apply_constant(input(block), expected_output(block), 0.3f);

  run_test(interp, input, expected_output, {50, 75, 100, 500});
}

TEST_CASE("step") {
  GainInterpolator<LinearInterpSingle> interp;

  interp.interp_points.emplace_back(100, 0.2f);
  interp.interp_points.emplace_back(200, 0.2f);
  interp.interp_points.emplace_back(200, 0.8f);
  interp.interp_points.emplace_back(300, 0.8f);

  Eigen::VectorXf input = Eigen::VectorXf::Random(400);
  Eigen::VectorXf expected_output = Eigen::VectorXf::Zero(400);

  auto block = Eigen::seqN(0, 200);
  apply_constant(input(block), expected_output(block), 0.2f);

  block = Eigen::seqN(200, 200);
  apply_constant(input(block), expected_output(block), 0.8f);

  run_test(interp, input, expected_output, {50, 75, 100, 400});
}

TEST_CASE("only_step") {
  GainInterpolator<LinearInterpSingle> interp;

  interp.interp_points.emplace_back(100, 0.2f);
  interp.interp_points.emplace_back(100, 0.8f);

  Eigen::VectorXf input = Eigen::VectorXf::Random(200);
  Eigen::VectorXf expected_output = Eigen::VectorXf::Zero(200);

  auto block = Eigen::seqN(0, 100);
  apply_constant(input(block), expected_output(block), 0.2f);

  block = Eigen::seqN(100, 100);
  apply_constant(input(block), expected_output(block), 0.8f);

  run_test(interp, input, expected_output, {50, 75, 100, 200});
}

TEST_CASE("one_point") {
  GainInterpolator<LinearInterpSingle> interp;

  interp.interp_points.emplace_back(100, 0.2);

  Eigen::VectorXf input = Eigen::VectorXf::Random(200);
  Eigen::VectorXf expected_output = Eigen::VectorXf::Zero(200);

  auto block = Eigen::seqN(0, 200);
  apply_constant(input(block), expected_output(block), 0.2f);

  run_test(interp, input, expected_output, {50, 75, 100, 200});
}

// tests for the other InterpTypes

TEST_CASE("vector") {
  GainInterpolator<LinearInterpVector> interp;

  std::vector<float> a{0.0f, 1.0f};
  std::vector<float> b{1.0f, 0.0f};

  interp.interp_points.emplace_back(100, a);
  interp.interp_points.emplace_back(200, b);

  Eigen::VectorXf input = Eigen::VectorXf::Random(300);
  Eigen::MatrixXf output = Eigen::MatrixXf::Zero(300, 2);

  Eigen::internal::set_is_malloc_allowed(false);
  process(interp, 0, input, output);
  Eigen::internal::set_is_malloc_allowed(true);

  Eigen::MatrixXf expected_output = Eigen::MatrixXf::Zero(300, 2);
  auto run_channel = [&](Eigen::Index out) {
    Eigen::MatrixXf tmp = Eigen::MatrixXf::Zero(300, 1);

    GainInterpolator<LinearInterpSingle> interp_test;
    interp_test.interp_points.emplace_back(100, a[out]);
    interp_test.interp_points.emplace_back(200, b[out]);

    process(interp_test, 0, input, tmp);

    expected_output(Eigen::all, out) += tmp;
  };
  run_channel(0);
  run_channel(1);

  CHECK_THAT(output, IsApprox(expected_output));
}

TEST_CASE("matrix") {
  GainInterpolator<LinearInterpMatrix> interp;

  std::vector<std::vector<float>> a{{0.0f, 0.3f}, {0.5f, 0.0f}};
  std::vector<std::vector<float>> b{{0.6f, 0.0f}, {0.0f, 0.7f}};

  interp.interp_points.emplace_back(100, a);
  interp.interp_points.emplace_back(200, b);

  Eigen::MatrixXf input = Eigen::MatrixXf::Random(300, 2);
  Eigen::MatrixXf output = Eigen::MatrixXf::Zero(300, 2);

  Eigen::internal::set_is_malloc_allowed(false);
  process(interp, 0, input, output);
  Eigen::internal::set_is_malloc_allowed(true);

  Eigen::MatrixXf expected_output = Eigen::MatrixXf::Zero(300, 2);
  auto run_channel = [&](Eigen::Index in, Eigen::Index out) {
    Eigen::MatrixXf tmp = Eigen::MatrixXf::Zero(300, 1);

    GainInterpolator<LinearInterpSingle> interp_test;
    interp_test.interp_points.emplace_back(100, a[in][out]);
    interp_test.interp_points.emplace_back(200, b[in][out]);

    process(interp_test, 0, input(Eigen::all, in), tmp);

    expected_output(Eigen::all, out) += tmp;
  };
  run_channel(0, 0);
  run_channel(0, 1);
  run_channel(1, 0);
  run_channel(1, 1);

  CHECK_THAT(output, IsApprox(expected_output));
}


/**
 * \file RemezBasedFilter.cpp
 */

#include <vector>

#include <boost/math/constants/constants.hpp>

#include <Eigen/Dense>

#include <ATK/Core/Utilities.h>
#include <ATK/EQ/RemezBasedFilter.h>
#include <ATK/EQ/FIRFilter.h>

#include <ATK/Utility/FFT.h>

namespace
{
  template<class DataType>
  class RemezBuilder
  {
  public:
    using AlignedScalarVector = typename ATK::TypedBaseFilter<DataType>::AlignedScalarVector;
  private:
    const static gsl::index grid_size = 1024; // grid size, power of two better for FFT
    constexpr static DataType SN = 1e-8;

    gsl::index M;
    std::vector<DataType> grid;
    std::vector<std::pair<std::pair<DataType, DataType>, std::pair<DataType, DataType>> > target;

    /// Computed coefficients
    AlignedScalarVector coeffs;
    /// Selected indices
    std::vector<gsl::index> indices;
    /// Weight function on the grid
    std::vector<DataType> weights;
    /// Objective function on the grid
    std::vector<DataType> objective;
    /// Alternate signs
    std::vector<int> s;

    ATK::FFT<DataType> fft_processor;

  public:
    RemezBuilder(gsl::index order, const std::vector<std::pair<std::pair<DataType, DataType>, std::pair<DataType, DataType>> >& target)
    :M(order / 2), target(target)
    {
      grid.resize(grid_size);
      for(gsl::index i = 0; i < grid_size; ++i)
      {
        grid[i] = i * boost::math::constants::pi<DataType>() / grid_size;
      }
      fft_processor.set_size(2 * grid_size);
    }

    void init()
    {
      coeffs.assign(M * 2 + 1, 0);
      indices.assign(M + 2, -1);
      s.assign(M+2, 0);

      weights.assign(grid_size, 0);
      objective.assign(grid_size, 0);

      int current_template = 0;
      for(gsl::index i = 0; i < grid_size; ++i)
      {
        auto reduced_freq = grid[i] / boost::math::constants::pi<DataType>();
        if(reduced_freq > target[current_template].first.second && current_template + 1 < target.size())
        {
          ++current_template;
        }
        if (reduced_freq < target[current_template].first.first || reduced_freq > target[current_template].first.second)
        {
          weights[i] = 0;
          objective[i] = 0;
        }
        else
        {
          weights[i] = target[current_template].second.second;
          objective[i] = target[current_template].second.first;
        }
      }
      int flag = -1;
      for (gsl::index i = 0; i < M + 2; ++i)
      {
        s[i] = flag;
        flag = -flag;
      }
      indices = set_starting_conditions();
    }

    std::vector<gsl::index> set_starting_conditions() const
    {
      std::vector<gsl::index> indices;

      std::vector<gsl::index> valid_indices;
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        if (weights[i] != 0)
        {
          valid_indices.push_back(i);
        }
      }

      for (gsl::index i = 0; i < M + 2; ++i)
      {
        indices.push_back(valid_indices[std::lround(valid_indices.size() / (M + 4.) * (i + 1))]);
      }

      return indices;
    }

    AlignedScalarVector build()
    {
      if(target.empty())
      {
        coeffs.clear();
        return coeffs;
      }
      init();
      while(true)
      {
        Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> A(M + 2, M + 2);
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          for (gsl::index j = 0; j < M + 1; ++j)
          {
            A(i, j) = std::cos(grid[indices[i]] * j);
          }
        }
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          A(i, M + 1) = s[i] / weights[indices[i]];
        }

        Eigen::Matrix<DataType, Eigen::Dynamic, 1> b(M + 2, 1);
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          b(i) = objective[indices[i]];
        }
        Eigen::Matrix<DataType, Eigen::Dynamic, 1> x = A.colPivHouseholderQr().solve(b);

        for (gsl::index i = 0; i < M; ++i)
        {
          coeffs[i] = coeffs[2 * M - i] = x[M - i] / 2;
        }
        coeffs[M] = x[0];
        auto delta = std::abs(x[M + 1]); // maximum cost

        auto newerr = compute_new_error();
        auto new_indices = locmax(newerr);

        filter_SN(delta, new_indices, newerr);
        filter_monotony(new_indices, newerr);

        DataType max = 0;
        for (auto indice : new_indices)
        {
          max = std::max(max, std::abs(newerr[indice]));
        }

        if ((max - delta) / delta < SN)
        {
          break;
        }

        indices = std::move(new_indices);
      }

      return coeffs;
    }

  private:
    /// Creates a spectral response for a given set of coeffs through an FFT
    std::vector<DataType> firamp() const
    {
      std::vector<std::complex<DataType>> output(grid_size * 2);
      fft_processor.process_forward(coeffs.data(), output.data(), coeffs.size());
      std::vector<DataType> amp(grid_size);
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        amp[i] = (std::complex<DataType>(std::cos(M * grid[i]), std::sin(M * grid[i])) * output[i]).real() * grid_size * 2;
      }
      return amp;
    }

    std::vector<DataType> compute_new_error() const
    {
      auto fir_result = firamp();
      std::vector<DataType> newerr(grid_size);
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        newerr[i] = (fir_result[i] - objective[i]) * weights[i];
      }

      return newerr;
    }

    // Finds min and max
    std::vector<gsl::index> locmax(const std::vector<DataType>& data) const
    {
      std::vector<gsl::index> v;

      std::vector<DataType> temp1;
      std::vector<DataType> temp2;
      temp1.push_back(data[0] - 1);
      for (gsl::index i = 0; i < data.size() - 1; ++i)
      {
        temp1.push_back(data[i]);
        temp2.push_back(data[i + 1]);
      }
      temp2.push_back(data.back() - 1);

      for (gsl::index i = 0; i < data.size(); ++i)
      {
        if ((data[i] > temp1[i]) && (data[i] > temp2[i]))
        {
          v.push_back(i);
        }
      }

      temp1.clear();
      temp2.clear();
      temp1.push_back(-data[0] - 1);
      for (gsl::index i = 0; i < data.size() - 1; ++i)
      {
        temp1.push_back(-data[i]);
        temp2.push_back(-data[i + 1]);
      }
      temp2.push_back(-data.back() - 1);

      for (gsl::index i = 0; i < data.size(); ++i)
      {
        if ((-data[i] > temp1[i]) && (-data[i] > temp2[i]))
        {
          v.push_back(i);
        }
      }

      std::sort(v.begin(), v.end());

      return v;
    }

    void filter_SN(DataType delta, std::vector<gsl::index>& indices, const std::vector<DataType>& err) const
    {
      std::vector<gsl::index> new_indices;

      for (auto indice : indices)
      {
        if (std::abs(err[indice]) > (delta - SN))
        {
          new_indices.push_back(indice);
        }
      }

      indices = std::move(new_indices);
    }

    std::vector<gsl::index> etap(const std::vector<DataType>& data) const
    {
      std::vector<gsl::index> v;
      auto xe = data[0];
      gsl::index xv = 0;
      for (gsl::index i = 1; i < data.size(); ++i)
      {
        if (std::signbit(data[i]) == std::signbit(xe))
        {
          if (std::abs(data[i]) > std::abs(xe))
          {
            xe = data[i];
            xv = i;
          }
        }
        else
        {
          v.push_back(xv);
          xe = data[i];
          xv = i;
        }
      }
      v.push_back(xv);
      return v;
    }

    void filter_monotony(std::vector<gsl::index>& indices, const std::vector<DataType>& err) const
    {
      std::vector<DataType> filtered_err;
      for (auto indice : indices)
      {
        filtered_err.push_back(err[indice]);
      }

      auto selected_indices = etap(filtered_err);
      std::vector<gsl::index> new_indices;
      for (gsl::index i = 0; i < M + 2; ++i)
      {
        new_indices.push_back(indices[selected_indices[i]]);
      }
      indices = std::move(new_indices);
    }
  };
}

namespace ATK
{
  template<class DataType>
  RemezBasedCoefficients<DataType>::RemezBasedCoefficients(gsl::index nb_channels)
    :Parent(nb_channels, nb_channels)
  {
  }

  template<class DataType>
  RemezBasedCoefficients<DataType>::RemezBasedCoefficients(RemezBasedCoefficients&& other)
    :Parent(std::move(other)), target(std::move(other.target)), in_order(std::move(other.in_order)), coefficients_in(std::move(other.coefficients_in))
  {
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::set_template(const std::vector<std::pair<std::pair<CoeffDataType, CoeffDataType>, std::pair<CoeffDataType, CoeffDataType> > >& target)
  {
    this->target = target;
    setup();
  }

  template<class DataType>
  const std::vector<std::pair<std::pair<typename RemezBasedCoefficients<DataType>::CoeffDataType, typename RemezBasedCoefficients<DataType>::CoeffDataType>, std::pair<typename RemezBasedCoefficients<DataType>::CoeffDataType, typename RemezBasedCoefficients<DataType>::CoeffDataType> > >& RemezBasedCoefficients<DataType>::get_template() const
  {
    return target;
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::set_order(gsl::index order)
  {
    if(order % 2 == 1)
    {
      throw ATK::RuntimeError("Need an even filter order (considering order 0 has 1 coefficients)");
    }
    in_order = order;
    setup();
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::setup()
  {
    Parent::setup();

    std::sort(target.begin(), target.end());
    for(gsl::index i = 0; i + 1 < target.size(); ++i)
    {
      if(target[i].first.second > target[i + 1].first.first)
      {
        target.clear();
        throw ATK::RuntimeError("Bad template");
      }
    }

    if (in_order > 0)
    {
      RemezBuilder<CoeffDataType> builder(in_order, target);
      coefficients_in = builder.build();
    }
  }

  template class ATK_EQ_EXPORT RemezBasedCoefficients<double>;
#if ATK_ENABLE_INSTANTIATION
  template class ATK_EQ_EXPORT RemezBasedCoefficients<std::complex<double> >;
#endif
}


#ifndef VECTORQUANTIZER_HPP
#define VECTORQUANTIZER_HPP

#include <Eigen/Dense>
#include "../iterator/iterator.hpp"
/**
 * @brief plain old vector quantizer
 */
template<typename T, uint D, uint C>
class vectorquantizer {

    typedef Eigen::Matrix < T, D, 1 > vec_t;

    static_assert( ((C % 2 == 0) || ( C == 1)), "vectorquantizer: cells = 0 mod 2 failed or C==1");

public:

    // ================================================================================
    // Methods
    // ================================================================================

    vectorquantizer(){
        _raw_centroids = new T[C * D];
        _centroids = Eigen::Map<Eigen::Matrix<T, C, D>>(_raw_centroids, C, D);

    }

    ~vectorquantizer() {
        delete[] _raw_centroids;

    }

    void getAssignment(iterator<T, D> &iter) {
        // for each vector ...
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            // find minimum
            uint bestIdx = 0;
            T bestDist = HUGE_VAL;
            vec_t vec = iter[n];
            // for each cluster
            for (uint c = 0, c_e = _step; c < c_e; ++c) {
                vec_t cec = _centroids.row(c);
                T curDist = (vec - cec).squaredNorm();
                if ( curDist  < bestDist ) {
                    bestDist = curDist;
                    bestIdx = c;
                }
            }
            _mapping[n] = bestIdx;
            _distances[n] = bestDist;
        }
    }

    void updateCentroids(iterator<T, D> &iter) {
        _centroids = Eigen::Matrix<T, C, D>::Zero(C, D);
        T centerCounter[C] = {0};
        // find mean
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            const uint c = _mapping[n];
            _centroids.row(c) += iter[n];
            ++centerCounter[c];
        }
        for (uint c = 0; c < C; ++c) {
            if (centerCounter[c] != 0)
                _centroids.row(c).array() /= centerCounter[c];

        }
    }

    void augmentCentroids() {
        for (uint i = 0; i < _step; ++i) {
            _centroids.row(i + _step) = _centroids.row(i).array() + 0.001;
            _centroids.row(i) = _centroids.row(i).array() - 0.001;
        }
        _step *= 2;
    }

    T loss(iterator<T, D> &iter) {
        Eigen::MatrixXf l = Eigen::Map<  Eigen::MatrixXf >(_distances, iter.num(), 1);
        return l.sum();
    }

    uint id(const vec_t &vec) {
        // compute all l1 distances (row=part)
        for (uint c = 0; c < C; ++c) {
            const Eigen::Matrix< T, D, 1> cec = _centroids.row(c);
            const Eigen::Matrix< T, D, 1> diff = vec - cec;
            _L1distances[c] = diff.squaredNorm();
            _L1order[c] = c;

        }

        // now sort them according their distances (sort each row)
        auto comparator = [&](const uint8_t &lhs, const uint8_t &rhs) -> bool {
            return _L1distances[lhs] < _L1distances[rhs];
        };

        std::sort(_L1order, _L1order + C, comparator);

        return _L1order[0];

    }

    void dist(const vec_t &vec) {
        // compute all l1 distances (row=part)
        for (uint c = 0; c < C; ++c) {
            const Eigen::Matrix< T, D, 1> cec = _centroids.row(c);
            const Eigen::Matrix< T, D, 1> diff = vec - cec;
            _L1distances[c] = diff.squaredNorm();
            _L1order[c] = c;

        }

    }

    void generate(iterator<T, D> &iter) {
        _step = 1;

        _mapping = new uint8_t[iter.num()]();
        _distances = new T[iter.num()]();

        _centroids.row(0) = iter.center();

        T currentLoss = 0.0;
        T lastLoss    = 0.0;

        if (C == 1) {
            getAssignment(iter);    // E step
            updateCentroids(iter);  // M step
            return;                  // do nothing
        }

        do {
            augmentCentroids();
            do {
                lastLoss = currentLoss;
                getAssignment(iter);    // E step
                updateCentroids(iter);  // M step
                currentLoss = loss(iter);
            } while ((abs(lastLoss - currentLoss) > 0.005));
        } while (_step < C );

        delete[] _mapping;
        delete[] _distances;
    }

    // ================================================================================
    // Variables
    // ================================================================================

    Eigen::Matrix<T, C, D> _centroids;
    T *_raw_centroids;
    uint _step;
    uint8_t *_mapping;
    T *_distances;

    T _L1distances[C];
    uint _L1order[C];

};

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_log_search"

#include <boost/test/unit_test.hpp>
#include "nanocv/scalar.h"
#include "nanocv/thread/pool.h"
#include "nanocv/log_search.hpp"
#include "nanocv/math/abs.hpp"
#include "nanocv/math/random.hpp"
#include "nanocv/math/epsilon.hpp"

namespace test
{
        using namespace ncv;

        void check(scalar_t a, scalar_t b, scalar_t minlog, scalar_t maxlog, scalar_t epslog, size_t splits)
        {
                auto op = [=] (scalar_t x)
                {
                        return (x - a) * (x - a) + b;
                };

                // single-threaded version
                const std::pair<scalar_t, scalar_t> ret1 = ncv::log10_min_search(op, minlog, maxlog, epslog, splits);

                // multi-threaded version
                thread_pool_t pool(splits);
                const std::pair<scalar_t, scalar_t> retx = ncv::log10_min_search_mt(op, pool, minlog, maxlog, epslog, splits);

                const scalar_t epsilon = math::epsilon2<scalar_t>();

                // check optimum result
                BOOST_CHECK_LE(math::abs(ret1.first - b), epsilon);
                BOOST_CHECK_LE(math::abs(retx.first - b), epsilon);

                // check optimum parameters
                BOOST_CHECK_LE(math::abs(ret1.second - a), epsilon);
                BOOST_CHECK_LE(math::abs(retx.second - a), epsilon);
        }
}

BOOST_AUTO_TEST_CASE(test_log_search)
{
        using namespace ncv;

        const size_t n_tests = 16;
        const scalar_t minlog = -6.0;
        const scalar_t maxlog = +6.0;
        const scalar_t epslog = math::epsilon2<scalar_t>();
        const size_t splits = ncv::n_threads();

        for (size_t t = 0; t < n_tests; t ++)
        {
                random_t<scalar_t> agen(+0.1, +1.0);
                random_t<scalar_t> bgen(-2.0, +2.0);

                test::check(agen(), bgen(), minlog, maxlog, epslog, splits);
        }
}


#include "teca_2d_component_area.h"

#include "teca_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh.h"

#include <algorithm>
#include <iostream>
#include <deque>
#include <set>
#define _USE_MATH_DEFINES
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

namespace {

template <typename component_t>
component_t get_max_component_id(unsigned long n, const component_t *labels)
{
    component_t max_component_id = std::numeric_limits<component_t>::lowest();
    for (unsigned long i = 0; i < n; ++i)
    {
        component_t label = labels[i];
        max_component_id = label > max_component_id ? label : max_component_id;
    }
    return max_component_id;
}

// visit each node in the mesh, the node is treated as a cell in
// the dual mesh defined by mid points between nodes. the area of
// the cell is added to the corresponding label. this formulation
// requires a layer of ghost nodes
//
// The exact area of the sperical rectangular patch A_i is given by:
//
// A_i = rho^2(cos(phi_0) - cos(phi_1))(theta_1 - theta_0)
//
//     = rho^2(sin(rad_lat_0) - sin(rad_lat_1))(theta_1 - theta_0)
//
// where
//
//   theta = deg_lon * pi/180
//   phi = pi/2 - rad_lat
//   rad_lat = deg_lat * pi/180
//   sin(rad_lat) = cos(pi/2 - rad_lat)
//
template<typename coord_t, typename component_t, typename container_t>
void component_area(unsigned long nlon, unsigned long nlat,
    const coord_t *deg_lon, const coord_t *deg_lat, const component_t *labels,
    container_t &area)
{
    // This calculation is sensative to floating point precision and
    // should be done in double precision
    using calc_t = double;

    calc_t R_e = 6378.1370; // km
    calc_t R_e_sq = R_e*R_e;
    calc_t rad_per_deg = M_PI/180.0;

    unsigned long nlonm1 = nlon - 1;
    unsigned long nlatm1 = nlat - 1;

    // convert to spherical coordinates in units of radians,
    // move to the dual mesh, and pre-compute factors in A
    calc_t *rho_sq_d_theta = (calc_t*)malloc(nlon*sizeof(calc_t));
    rho_sq_d_theta[0] = calc_t();
    for (unsigned long i = 1; i < nlonm1; ++i)
        rho_sq_d_theta[i] = R_e_sq*calc_t(0.5)*(deg_lon[i + 1] - deg_lon[i - 1])*rad_per_deg;
    rho_sq_d_theta[nlonm1] = calc_t();

    calc_t *rad_lat = (calc_t*)malloc(nlat*sizeof(calc_t));
    for (unsigned long j = 0; j < nlat; ++j)
        rad_lat[j] = deg_lat[j]*rad_per_deg;

    calc_t *d_cos_phi = (calc_t*)malloc(nlat*sizeof(calc_t));
    for (unsigned long j = 1; j < nlatm1; ++j)
    {
        calc_t cos_phi_1 = sin(calc_t(0.5)*(rad_lat[j - 1] + rad_lat[j]));
        calc_t cos_phi_0 = sin(calc_t(0.5)*(rad_lat[j] + rad_lat[j + 1]));
        d_cos_phi[j] = cos_phi_0 - cos_phi_1;
    }
    d_cos_phi[0] = calc_t();
    d_cos_phi[nlatm1] = calc_t();

    // finish off the calc by multiplying the factors
    for (unsigned long j = 1; j < nlatm1; ++j)
    {
        calc_t d_cos_phi_j = d_cos_phi[j];
        unsigned long jj = j*nlon;
        for (unsigned long i = 1; i < nlonm1; ++i)
        {
            area[labels[jj + i]] += rho_sq_d_theta[i]*d_cos_phi_j;
        }
    }

    free(rad_lat);
    free(d_cos_phi);
    free(rho_sq_d_theta);
}

}

// --------------------------------------------------------------------------
teca_2d_component_area::teca_2d_component_area() :
    component_variable(""), contiguous_component_ids(0), background_id(-1)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_2d_component_area::~teca_2d_component_area()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_2d_component_area::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_2d_component_area":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_variable,
            "name of the varibale containing region labels")
        TECA_POPTS_GET(int, prefix, contiguous_component_ids,
            "when the region label ids start at 0 and are consecutive "
            "this flag enables use of an optimization")
        TECA_POPTS_GET(long, prefix, background_id,
            "the label id that corresponds to the background")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_2d_component_area::set_properties(const std::string &prefix,
    variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, component_variable)
    TECA_POPTS_SET(opts, int, prefix, contiguous_component_ids)
    TECA_POPTS_SET(opts, long, prefix, background_id)
}
#endif

// --------------------------------------------------------------------------
int teca_2d_component_area::get_component_variable(std::string &component_var)
{
    if (this->component_variable.empty())
        return -1;

    component_var = this->component_variable;
    return 0;
}

// --------------------------------------------------------------------------
teca_metadata teca_2d_component_area::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::get_output_metadata" << endl;
#endif
    (void) port;

    teca_metadata md = input_md[0];
    return md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_2d_component_area::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::get_upstream_request" << endl;
#endif
    (void) port;
    (void) input_md;

    std::vector<teca_metadata> up_reqs;

    // get the name of the array to request
    std::string component_var;
    if (this->get_component_variable(component_var))
    {
        TECA_FATAL_ERROR("component_variable was not specified")
        return up_reqs;
    }

    // pass the incoming request upstream, and
    // add in what we need
    teca_metadata req(request);
    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);
    arrays.insert(component_var);

    req.set("arrays", arrays);

    // send up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_2d_component_area::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::execute" << endl;
#endif
    (void)port;
    (void)request;

    // get the input
    const_p_teca_cartesian_mesh in_mesh =
        std::dynamic_pointer_cast<const teca_cartesian_mesh>(
            input_data[0]);
    if (!in_mesh)
    {
        TECA_FATAL_ERROR("empty input, or not a cartesian_mesh")
        return nullptr;
    }

    // create output and copy metadata, coordinates, etc
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    // get the input array
    std::string component_var;
    if (this->get_component_variable(component_var))
    {
        TECA_FATAL_ERROR("component_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array component_array
        = out_mesh->get_point_arrays()->get(component_var);
    if (!component_array)
    {
        TECA_FATAL_ERROR("label variable \"" << component_var
            << "\" is not in the input")
        return nullptr;
    }

    // get mesh dimension
    unsigned long extent[6];
    out_mesh->get_extent(extent);

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nxy = nx*ny;

    unsigned long nz = extent[5] - extent[4] + 1;
    if (nz != 1)
    {
        TECA_FATAL_ERROR("This calculation requires 2D data. The current dataset "
            "extents are [" << extent[0] << ", " << extent[1] << ", "
            << extent[2] << ", " << extent[3] << ", " << extent[4] << ", "
            << extent[5] << "]")
        return nullptr;
    }

    // get the coordinate axes
    const_p_teca_variant_array xc = in_mesh->get_x_coordinates();
    const_p_teca_variant_array yc = in_mesh->get_y_coordinates();

    // get the input and output metadata
    teca_metadata &in_metadata =
        const_cast<teca_metadata&>(in_mesh->get_metadata());

    teca_metadata &out_metadata = out_mesh->get_metadata();

    // get the background_id, and pass it through
    long bg_id = this->background_id;
    if (this->background_id == -1)
    {
        if (in_metadata.get("background_id", bg_id))
        {
            TECA_FATAL_ERROR("Metadata is missing the key \"background_id\". "
                "One should specify it via the \"background_id\" algorithm "
                "property")
            return nullptr;
        }
    }
    out_metadata.set("background_id", bg_id);

    // calculate area of components
    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        xc.get(),
        _COORD,
        // the calculation is sensative to floating point precision
        // and should be made in double precision
        using calc_t = double;

        const NT_COORD *p_xc = static_cast<TT_COORD*>(xc.get())->get();
        const NT_COORD *p_yc = static_cast<TT_COORD*>(yc.get())->get();

        NESTED_TEMPLATE_DISPATCH_I(const teca_variant_array_impl,
            component_array.get(),
            _LABEL,

            const NT_LABEL *p_labels = static_cast<TT_LABEL*>(component_array.get())->get();

            unsigned int n_labels = 0;

            bool has_component_id = in_metadata.has("component_ids");
            if (this->contiguous_component_ids || has_component_id)
            {
                // use a contiguous buffer to hold the result, only for
                // contiguous lables that start at 0
                p_teca_variant_array component_id;
                if (has_component_id)
                {
                    in_metadata.get("number_of_components", int(0), n_labels);
                    component_id = in_metadata.get("component_ids");
                }
                else
                {
                    NT_LABEL max_component_id = ::get_max_component_id(nxy, p_labels);
                    n_labels = max_component_id + 1;
                    p_teca_variant_array_impl<NT_LABEL> tmp = teca_variant_array_impl<NT_LABEL>::New(n_labels);
                    for (unsigned int i = 0; i < n_labels; ++i)
                        tmp->set(i, NT_LABEL(i));
                    component_id = tmp;
                }
                std::vector<calc_t> component_area(n_labels);
                ::component_area(nx,ny, p_xc,p_yc, p_labels, component_area);

                // transfer the result to the output
                out_metadata.set("number_of_components", n_labels);
                out_metadata.set("component_ids", component_id);
                out_metadata.set("component_area", component_area);
            }
            else
            {
                // use an associative array to handle any labels
                //std::map<NT_LABEL, NT_COORD> result;
                decltype(std::map<NT_LABEL, calc_t>()) result;
                ::component_area(nx,ny, p_xc,p_yc, p_labels, result);

                // transfer the result to the output
                n_labels = result.size();

                p_teca_variant_array_impl<NT_LABEL> component_id =
                    teca_variant_array_impl<NT_LABEL>::New(n_labels);

                p_teca_variant_array_impl<calc_t> component_area =
                    teca_variant_array_impl<calc_t>::New(n_labels);

                //std::map<NT_LABEL,NT_COORD>::iterator it = result.begin();
                auto it = result.begin();
                for (unsigned int i = 0; i < n_labels; ++i,++it)
                {
                    component_id->set(i, it->first);
                    component_area->set(i, it->second);
                }

                out_metadata.set("number_of_components", n_labels);
                out_metadata.set("component_ids", component_id);
                out_metadata.set("component_area", component_area);
            }
            )
        )

    return out_mesh;
}


//  To use the simple FFT implementation
//  g++ -o demofft -I.. -Wall -O3 FFT.cpp

//  To use the FFTW implementation
//  g++ -o demofft -I.. -DUSE_FFTW -Wall -O3 FFT.cpp -lfftw3 -lfftw3f -lfftw3l

#ifdef USE_FFTW
#include <fftw3.h>
#endif

#include <vector>
#include <complex>
#include <algorithm>
#include <iterator>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/FFT>

using namespace std;
using namespace Eigen;

template <typename T>
T mag2(T a)
{
    return a*a;
}
template <typename T>
T mag2(std::complex<T> a)
{
    return norm(a);
}

template <typename T>
T mag2(const std::vector<T> & vec)
{
    T out=0;
    for (size_t k=0;k<vec.size();++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
T mag2(const std::vector<std::complex<T> > & vec)
{
    T out=0;
    for (size_t k=0;k<vec.size();++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
vector<T> operator-(const vector<T> & a,const vector<T> & b )
{
    vector<T> c(a);
    for (size_t k=0;k<b.size();++k)
        c[k] -= b[k];
    return c;
}

template <typename T>
void RandomFill(std::vector<T> & vec)
{
    for (size_t k=0;k<vec.size();++k)
        vec[k] = T( rand() )/T(RAND_MAX) - .5;
}

template <typename T>
void RandomFill(std::vector<std::complex<T> > & vec)
{
    for (size_t k=0;k<vec.size();++k)
        vec[k] = std::complex<T> ( T( rand() )/T(RAND_MAX) - .5, T( rand() )/T(RAND_MAX) - .5);
}

template <typename T_time,typename T_freq>
void fwd_inv(size_t nfft)
{
    typedef typename NumTraits<T_freq>::Real Scalar;
    vector<T_time> timebuf(nfft);
    RandomFill(timebuf);

    vector<T_freq> freqbuf;
    static FFT<Scalar> fft;
    fft.fwd(freqbuf,timebuf);

    vector<T_time> timebuf2;
    fft.inv(timebuf2,freqbuf);

    long double rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
    cout << "roundtrip rmse: " << rmse << endl;
}

template <typename T_scalar>
void two_demos(int nfft)
{
    cout << "     scalar ";
    fwd_inv<T_scalar,std::complex<T_scalar> >(nfft);
    cout << "    complex ";
    fwd_inv<std::complex<T_scalar>,std::complex<T_scalar> >(nfft);
}

void demo_all_types(int nfft)
{
    cout << "nfft=" << nfft << endl;
    cout << "   float" << endl;
    two_demos<float>(nfft);
    cout << "   double" << endl;
    two_demos<double>(nfft);
    cout << "   long double" << endl;
    two_demos<long double>(nfft);
}

int main()
{
    demo_all_types( 2*3*4*5*7 );
    demo_all_types( 2*9*16*25 );
    demo_all_types( 1024 );
    return 0;
}


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/reduce.hpp>
#include <boost/align/aligned_allocator.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check simd::reduce(f,l,i)", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T,aligned_allocator<T,pack<T>::alignment>> values(2*N);
  std::iota(values.begin(), values.end(),T(0));

  auto ab = values.data();
  auto ae = values.data()+values.size();

  // All aligned
  STF_EQUAL ( (std::accumulate(values.begin(), values.end(), T(3)))
            , (boost::simd::reduce(ab,ae, T(3)))
            );

  // prologue + aligned
  STF_EQUAL ( (std::accumulate(values.begin()+1, values.end(), T(3)))
            , (boost::simd::reduce(ab+1,ae, T(3)))
            );

  // aligned + epilogue
  STF_EQUAL ( (std::accumulate(values.begin(), values.end()-1, T(3)))
            , (boost::simd::reduce(ab,ae-1, T(3)))
            );

  // prologue + epilogue
  STF_EQUAL ( (std::accumulate(values.begin(), values.end()-1, T(3)))
            , (boost::simd::reduce(ab+1,ae-1, T(3)))
            );
}

struct fake_sum
{
  template<typename T> T operator()(T const& a, T const& e) { return a + e; }
};

STF_CASE_TPL( "Check simd::reduce(f,l,i,f,n)", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T,aligned_allocator<T,pack<T>::alignment>> values(2*N);
  std::iota(values.begin(), values.end(),T(0));

  STF_EQUAL ( (std::accumulate(values.begin(), values.end(), T(3), fake_sum{}))
            , (boost::simd::reduce( values.data(), values.data()+values.size()
                                  , T(3)
                                  , fake_sum{}, T(0)
                                  )
              )
            );

}

struct squared_sum
{
  template<typename T> T operator()(T const& a, T const& e) { return a + e*e; }
};

STF_CASE_TPL( "Check simd::reduce(f,l,i,f,n,g)", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T,aligned_allocator<T,pack<T>::alignment>> values(2*N);
  std::iota(values.begin(), values.end(),T(0));

  STF_EQUAL ( (std::accumulate(values.begin(), values.end(), T(3), squared_sum{}))
            , (boost::simd::reduce( values.data(), values.data()+values.size()
                                  , T(3)
                                  , squared_sum{}, T(0)
                                  , boost::simd::plus
                                  )
              )
            );

}


/**
 * @brief 視錐台クラス
 */

#include "View/Frustum.h"

#include <boost/assert.hpp>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <iostream>
#include <vector>

void Frustum::SetupPerspective(float fovy, float aspectRatio, float near,
                               float far) {
  fovy_ = fovy;
  ar_ = aspectRatio;
  near_ = near;
  far_ = far;

  type_ = ProjectionType::Perspective;
}

void Frustum::SetupOrtho(float left, float right, float bottom, float top,
                         float near, float far) {
  left_ = left;
  right_ = right;
  bottom_ = bottom;
  top_ = top;
  near_ = near;
  far_ = far;

  type_ = ProjectionType::Ortho;
}

void Frustum::SetupCorners(const glm::vec3 &eyePt, const glm::vec3 &lookatPt,
                           const glm::vec3 &upVec) {
  corners_[0] = glm::vec3(-1.0, 1.0, 1.0);
  corners_[1] = glm::vec3(1.0, 1.0, 1.0);
  corners_[2] = glm::vec3(1.0, -1.0, 1.0);
  corners_[3] = glm::vec3(-1.0, -1.0, 1.0);

  corners_[4] = glm::vec3(-1.0, 1.0, -1.0);
  corners_[5] = glm::vec3(1.0, 1.0, -1.0);
  corners_[6] = glm::vec3(1.0, -1.0, -1.0);
  corners_[7] = glm::vec3(-1.0, -1.0, -1.0);

  const auto kView = glm::lookAt(eyePt, lookatPt, upVec);
  const auto kProj = GetProjectionMatrix();
  const auto kInvVP = glm::inverse(kProj * kView);
  for (int i = 0; i < 8; i++) {
    const auto corner = kInvVP * glm::vec4(corners_[i], 1.0f);
    corners_[i] = glm::vec3(corner) / corner.w;
  }
}

BSphere Frustum::ComputeBSphere() const {
  glm::vec3 center = glm::vec3(0.0f);
  for (int i = 0; i < 8; i++) {
    center += corners_[i];
  }
  center /= 8.0f;

  float radius = 0.0f;
  for (int i = 0; i < 8; i++) {
    float len = glm::length(corners_[i] - center);
    radius = glm::max(radius, len);
  }
  radius = std::ceil(radius * 16.0f) / 16.0f;

  return {center, radius};
}

glm::mat4 Frustum::GetProjectionMatrix() const {
  if (type_ == ProjectionType::Perspective) {
    return glm::perspective(fovy_, ar_, near_, far_);
  } else {
    return glm::ortho(left_, right_, bottom_, top_, near_, far_);
  }
}


//---------------------------------------------------------------------------
//    $Id: theta_timestepping.cc 28377 2013-02-13 15:22:32Z heister $
//
//    Copyright (C) 2005-2006, 2010, 2012 by the deal.II authors
//
//    This file is subject to QPL and may not be  distributed
//    without copyright and license information. Please refer
//    to the file deal.II/doc/license.html for the  text  and
//    further information on this license.
//
//---------------------------------------------------------------------------

// See documentation of ThetaTimestepping for documentation of this example

#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>

#include <deal.II/algorithms/operator.h>
#include <deal.II/algorithms/theta_timestepping.h>

#include <iostream>

using namespace dealii;
using namespace Algorithms;

class Explicit
  : public Operator<Vector<double> >
{
public:
  Explicit(const FullMatrix<double> &matrix);
  void operator() (NamedData<Vector<double>*> &out,
                   const NamedData<Vector<double>*> &in);

  void initialize_timestep_data(const TimestepData &);
private:
  const TimestepData *timestep_data;
  SmartPointer<const FullMatrix<double>, Explicit> matrix;
  FullMatrix<double> m;
};

class Implicit
  : public Operator<Vector<double> >
{
public:
  Implicit(const FullMatrix<double> &matrix);
  void operator() (NamedData<Vector<double>*> &out,
                   const NamedData<Vector<double>*> &in);

  void initialize_timestep_data(const TimestepData &);
private:
  const TimestepData *timestep_data;
  SmartPointer<const FullMatrix<double>, Implicit> matrix;
  FullMatrix<double> m;
};

// End of declarations

int main()
{
  FullMatrix<double> matrix(2);
  matrix(0,0) = 1.;
  matrix(1,1) = 1.;
  matrix(0,1) = 31.4;
  matrix(1,0) = -31.4;

  OutputOperator<Vector<double> > out;
  out.initialize_stream(std::cout);

  Explicit op_explicit(matrix);
  Implicit op_implicit(matrix);
  ThetaTimestepping<Vector<double> > solver(op_explicit, op_implicit);
  op_explicit.initialize_timestep_data(solver.explicit_data());
  op_implicit.initialize_timestep_data(solver.implicit_data());
  solver.set_output(out);

  Vector<double> value(2);
  value(0) = 1.;
  NamedData<Vector<double>*> indata;
  NamedData<Vector<double>*> outdata;
  Vector<double> *p = &value;
  outdata.add(p, "value");

  solver.notify(Events::initial);
  solver(outdata, indata);
}

Explicit::Explicit(const FullMatrix<double> &M)
  :
  matrix(&M)
{
  m.reinit(M.m(), M.n());
}

void
Explicit::initialize_timestep_data(const TimestepData &t)
{
  timestep_data = &t;
}

void
Explicit::operator() (NamedData<Vector<double>*> &out, const NamedData<Vector<double>*> &in)
{
  if (this->notifications.test(Events::initial) || this->notifications.test(Events::new_timestep_size))
    {
      m.equ(-timestep_data->step, *matrix);
      for (unsigned int i=0; i<m.m(); ++i)
        m(i,i) += 1.;
    }
  this->notifications.clear();
  unsigned int i = in.find("Previous iterate");
  m.vmult(*out(0), *in(i));
}

Implicit::Implicit(const FullMatrix<double> &M)
  :
  matrix(&M)
{
  m.reinit(M.m(), M.n());
}

void
Implicit::initialize_timestep_data(const TimestepData &t)
{
  timestep_data = &t;
}

void
Implicit::operator() (NamedData<Vector<double>*> &out, const NamedData<Vector<double>*> &in)
{
  if (this->notifications.test(Events::initial) || this->notifications.test(Events::new_timestep_size))
    {
      m.equ(timestep_data->step, *matrix);
      for (unsigned int i=0; i<m.m(); ++i)
        m(i,i) += 1.;
      m.gauss_jordan();
    }
  this->notifications.clear();

  unsigned int i = in.find("Previous time");
  m.vmult(*out(0), *in(i));
}


#pragma once

// #include <boost/container/pmr/monotonic_buffer_resource.hpp>
// #include <boost/container/pmr/set.hpp>
// #include <boost/container/pmr/unsynchronized_pool_resource.hpp>
// #include <experimental/memory_resource>
#include <set>

namespace perceive
{
//
// Shortest path on a DAG, using sparse representations
//
template<typename V>
inline vector<V> shortest_path_sparse(
    const V source,
    const V sink,
    std::function<real(const V& u, const V& v)> edge_weight,
    std::function<void(const V& u, std::function<void(const V&)>)>
        for_each_neighbour,
    const real k_max_cost = std::numeric_limits<real>::max())
{
   if(source == sink) return {};

   Expects(edge_weight);
   Expects(for_each_neighbour);

   using Key     = std::pair<real, V>;
   using Compare = std::less<Key>;

   // boost::container::pmr::monotonic_buffer_resource alloc(1024 * 10);
   std::set<Key, Compare> Q;

   std::unordered_map<V, V> parent_v;
   std::unordered_map<V, real> dist_;

   real max_cost = std::isfinite(k_max_cost) ? k_max_cost
                                             : std::numeric_limits<real>::max();

   auto dist = [&](const V u) {
      auto ii = dist_.find(u);
      return (ii == cend(dist_)) ? max_cost : ii->second;
   };

   Q.insert({0.0, source});
   dist_[source]   = 0.0;
   real path_score = dNAN;

   while(Q.size() > 0) {
      const V u = Q.begin()->second;
      Q.erase(Q.begin()); // Remove element
      if(u == sink) {
         Expects(parent_v.find(u) != cend(parent_v));
         break; // We're done
      }
      Expects(dist(u) < std::numeric_limits<real>::max());
      const auto dist_u = dist(u);

      for_each_neighbour(u, [&](const V v) {
         const real w = edge_weight(u, v);
         Expects(std::isfinite(w));
         const auto dist_v = dist(v);
         if(dist_v > dist_u + w) {
            Q.erase({dist_v, v});
            const auto new_dist_v = dist_u + w;
            Q.insert({new_dist_v, v});
            parent_v[v] = u;
            dist_[v]    = new_dist_v;
         }
      });
   }

   if(parent_v.find(sink) == cend(parent_v)) return {};

   // Reconstruct the path
   vector<V> out;
   {
      out.reserve(Q.size() + 2);
      out.push_back(sink);
      while(out.back() != source) {
         const auto ii = parent_v.find(out.back());
         Expects(ii != cend(parent_v));
         out.push_back(ii->second);
      }
      std::reverse(begin(out), end(out));
   }

   return out;
}

} // namespace perceive


//  (C) Copyright John Maddock 2005.
//  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_LOG1P_INCLUDED
#define BOOST_MATH_LOG1P_INCLUDED

#include <cmath>
#include <math.h> // platform's ::log1p
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::fabs; using ::log; }
#endif

namespace boost{ namespace math{

namespace detail{

//
// Functor log1p_series returns the next term in the Taylor series
// pow(-1, k-1)*pow(x, k) / k
// each time that operator() is invoked.
//
template <class T>
struct log1p_series
{
   typedef T result_type;

   log1p_series(T x)
      : k(0), m_mult(-x), m_prod(-1){}

   T operator()()
   {
      m_prod *= m_mult;
      return m_prod / ++k;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_mult;
   T m_prod;
   log1p_series(const log1p_series&);
   log1p_series& operator=(const log1p_series&);
};

} // namespace

//
// Algorithm log1p is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > x > epsilon, which may
// require up to std::numeric_limits<T>::digits+1 terms to be calculated.  It would
// be much more efficient to use the equivalence:
// log(1+x) == (log(1+x) * x) / ((1-x) - 1)
// Unfortunately optimizing compilers make such a mess of this, that it performs
// no better than log(1+x): which is to say not very well at all.
//
template <class T>
T log1p(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif
   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::log(T(1.0) + x);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::log1p_series<T> s(x);
   return detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
// these overloads work around a type deduction bug:
inline float log1p(float z)
{
   return log1p<float>(z);
}
inline double log1p(double z)
{
   return log1p<double>(z);
}
inline long double log1p(long double z)
{
   return log1p<long double>(z);
}
#endif

#ifdef log1p
#  ifndef BOOST_HAS_LOG1P
#     define BOOST_HAS_LOG1P
#  endif
#  undef log1p
#endif

#ifdef BOOST_HAS_LOG1P
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float log1p(float x){ return ::log1pf(x); }
inline long double log1p(long double x){ return ::log1pl(x); }
#else
inline float log1p(float x){ return ::log1p(x); }
#endif
inline double log1p(double x){ return ::log1p(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 *
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <tuple>

using namespace dealii;

class MyTuple
{
  std::tuple<int, int, int> Tuple;

public:
  MyTuple(Triangulation<2> &T)
    : Tuple(std::make_tuple(T.n_levels(), T.n_cells(), T.n_active_cells())) {}

  void print()
  {
    std::cout << "Number of level:\t" << std::get<0>(this->Tuple) << "\n"
              << "Number of cells:\t" << std::get<1>(this->Tuple) << "\n"
              << "Number of active cells:\t" << std::get<2>(this->Tuple) << std::endl;
  }
};

MyTuple helper(Triangulation<2> &T) {return MyTuple(T);}

void
circle_grid()
{
  const Point<2> center(0, 0);
  const double   radius = 1.0;

  Triangulation<2> triangulation;
  GridGenerator::hyper_ball(triangulation, center, radius, false);
  triangulation.refine_global(2);

  std::ofstream out("circle_grid.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to circle_grid.svg" << std::endl;

  Triangulation<2> triangulation2;
  GridGenerator::hyper_ball(triangulation2, center, radius, true);
  triangulation2.refine_global(2);

  std::ofstream out2("circle_grid2.svg");
  GridOut       grid_out2;
  grid_out2.write_svg(triangulation2, out2);
  std::cout << "Grid written to circle_grid2.svg" << std::endl;
}

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);

  std::cout << "Number of original vertices: " << triangulation.n_vertices()
            << std::endl;

  triangulation.refine_global(4);

  std::cout << "Numbe  r of vertices after 4 refinmentss: "
            << triangulation.n_vertices() << std::endl;

  std::ofstream out("grid-1.vtk");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  // triangulation.reset_manifold(0);
  /*
  questo comando fa si che la triangolazione passi dal descrivere
  il Manifold che aveva in precedenza (in questo caso una hyper shell)
  al descrivere un FlatManifold
  */

  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

void
third_grid()
{
  Triangulation<2> triangulation;
  GridGenerator::hyper_L(triangulation, 0., 0.9);
  /*triangulation.refine_global(1);*/
  const Point<2> corner(0.45, 0.45);

  for (unsigned int step = 0; step < 6; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          Point<2>     cell_center          = cell->center();
          const double distance_from_corner = corner.distance(cell_center);

          if (distance_from_corner < 1. / 3)
            {
              cell->set_refine_flag();
            }
        }
      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-3.vtk");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-3.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

int
main()
{
  first_grid();
  second_grid();
  third_grid();

  circle_grid();
}


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;

void testcase()
{
  int n, m, s;
  std::cin >> n >> m >> s;
  assert(n >= 1 && m >= 0 && s >= 1 && s < n);

  Graph G(n + 1);
  auto source = boost::vertex(0, G), sink = boost::vertex(n, G);
  auto add_edge = [&G](int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  std::vector<int> num_stores_by_intersection(n, 0);
  for (int i = 0; i < s; i++)
  {
    int shop_loc;
    std::cin >> shop_loc;
    assert(shop_loc >= 0 && shop_loc < n);
    num_stores_by_intersection.at(shop_loc)++;
  }
  for (int i = 0; i < n; i++)
  {
    if (num_stores_by_intersection.at(i) > 0)
    {
      add_edge(i, sink, num_stores_by_intersection.at(i));
    }
  }

  for (int i = 0; i < m; i++)
  {
    int from, to;
    std::cin >> from >> to;
    assert(from >= 0 && from < n && to >= 0 && to < n);
    add_edge(from, to, 1);
    add_edge(to, from, 1);
  }

  int flow = boost::push_relabel_max_flow(G, source, sink);
  assert(flow >= 0 && flow <= s);
  std::cout << (flow == s ? "yes\n" : "no\n");
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#pragma once

#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <map>
#include <random>
#include <cmath>
#include <numeric>
#include <stack>
#include <assert.h>
#include <tgmath.h>
#include <algorithm>
#include <Eigen/Dense>

using namespace std;
using Mat = Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Vec = Eigen::ArrayXf;

/** This class represents both a decision node and a leaf (when bestPred==-1) in a decision tree. */
class Node {
public:
    struct Node *left, *right;
    int start;    // observation index into tree's data rows where this node's samples start
    int nrows;    // number of rows beyond start index of tree's data rows associated with this node
    int cutcol;   // which column/variable/feature to test if decision node; -1 indicates leaf
    float cutval; // split value for cutcol
    float value;  // prediction value (set even for internal decision nodes)
    float gini;   // uncertainty/impurity of this node

    Node(int start, int nrows, Node *parent);
    bool isTerminal();
};

class FastTree;

class FastForest {
public:
    int NTREE = 5, MIN_NODE = 25;
    const float sampleFraction = 0.8; // fraction of rows to extract from training set to train each tree
    const float PROP_OOB = 0.5;

    Mat X; // all training feature vectors, one row per observation
    Vec y; // all training target values, one per observation
    int nrows, ncols;

    FastTree **trees;

    FastForest(Mat X, Vec y);
    void build();
    FastTree *getTree(int i);
    Vec predict(Mat X);
};

struct CandidateInfo {
    float leftTarget;    // sum of all target values for observations where X[cutcol] < cutval
    float leftSqrTarget; // sum of square of target values to left of cutval
    int leftCount;       // how many observations fall to the left of cutval
    int cutcol;          // which feature/column this candidate tests
    float cutval;        // which split value this candidate tests

    CandidateInfo() { leftSqrTarget = leftTarget = leftCount = 0; }
};

class FastTree {
public:
    const int MAXN = 160, CUTOFF_DIVISOR = 10;

    FastForest* parent;
    default_random_engine* rng; // single random num generator used by code building this tree
    int nrows, ncols;
    float *y;  // subset size nrows of forest's X rows used to train this tree
    float **X; // subset size nrows of forest's y rows used to train this tree (array of ptrs to float)
    int *idxs; // nrows indexes into forest's X/y training observations
    Node *root;

    FastTree(FastForest* parent);
    float predict(Vec X);
    void shuffle();

protected:
    void createIdxsAndOob(float *Xall, float *yall);
    void buildNodes();
    void bestCutoff(Node *node);
    void checkCutoffs(int start, int n, CandidateInfo *candInfo, int ncandidates);
    bool allSame(Node *node);
    static float wgtGini(float leftTarget, float leftSqrTarget, float leftCount,
                         float sumTarget, float sumSqrTarget, float totCount);
    int partition(Node *node);
};

FastForest *trainFF(Mat X, Vec y);

template <typename T> double stdev(T b, T e);
float loss_(float sumTarget, float sumSqrTarget, float n);


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

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/correct_fma.hpp>
#include <boost/simd/function/std.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/oneplus.hpp>
#include <cmath>

STF_CASE_TPL (" correct_fma real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;

  using bs::correct_fma;

  // return type conformity test
  STF_EXPR_IS(correct_fma(T(),T(),T()), T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(correct_fma(bs::Inf<T>(), bs::Inf<T>(), bs::Inf<T>()), bs::Inf<T>());
  STF_IEEE_EQUAL(correct_fma(bs::Minf<T>(), bs::Minf<T>(), bs::Minf<T>()), bs::Nan<T>());
  STF_IEEE_EQUAL(correct_fma(bs::Nan<T>(), bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(correct_fma(bs::Mone<T>(), bs::Mone<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL(correct_fma(bs::One<T>(), bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(correct_fma(bs::One<T>()+bs::Eps<T>(), bs::One<T>()-bs::Eps<T>(),bs::Mone<T>()), -bs::Eps<T>()*bs::Eps<T>());
  STF_EQUAL(correct_fma(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
#ifndef  STF_DONT_CARE_CORRECT_FMA_OVERFLOW
  STF_EQUAL(correct_fma(bs::Valmax<T>(), bs::Two<T>(), -bs::Valmax<T>()), bs::Valmax<T>());
#endif
} // end of test for floating_

STF_CASE_TPL (" correct_fma signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::correct_fma;

  // return type conformity test

  STF_EXPR_IS(correct_fma(T(),T(),T()), T);

  // specific values tests
  STF_EQUAL(correct_fma(bs::Mone<T>(), bs::Mone<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL(correct_fma(bs::One<T>(), bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(correct_fma(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(correct_fma(bs::Valmax<T>(), bs::Two<T>(), bs::oneplus(bs::Valmin<T>())), bs::Valmax<T>());
} // end of test for signed_int_

STF_CASE_TPL (" correct_fma unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::correct_fma;

  // return type conformity test
  STF_EXPR_IS(correct_fma(T(),T(),T()), T);

  // specific values tests
  STF_EQUAL(correct_fma(bs::One<T>(), bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(correct_fma(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" correct_fma std real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;

  using bs::correct_fma;

  // return type conformity test
  STF_EXPR_IS(correct_fma(T(),T(),T()), T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL( bs::std_(correct_fma)(bs::Inf<T>(), bs::Inf<T>(), bs::Inf<T>()), bs::Inf<T>());
  STF_IEEE_EQUAL( bs::std_(correct_fma)(bs::Minf<T>(), bs::Minf<T>(), bs::Minf<T>()), bs::Nan<T>());
  STF_IEEE_EQUAL( bs::std_(correct_fma)(bs::Nan<T>(), bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL( bs::std_(correct_fma)(bs::Mone<T>(), bs::Mone<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL( bs::std_(correct_fma)(bs::One<T>(), bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL( bs::std_(correct_fma)(bs::One<T>()+bs::Eps<T>(), bs::One<T>()-bs::Eps<T>(),bs::Mone<T>()), -bs::Eps<T>()*bs::Eps<T>());
  STF_EQUAL( bs::std_(correct_fma)(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL( bs::std_(correct_fma)(bs::Valmax<T>(), bs::Two<T>(), -bs::Valmax<T>()), bs::Valmax<T>());
} // end of test for floating_


//  (C) Copyright Raffi Enficiaud 2014.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE dataset_example64
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

namespace bdata = boost::unit_test::data;

BOOST_DATA_TEST_CASE(
  test1,
  bdata::xrange(2) * bdata::xrange(3),
  xr1, xr2)
{
  std::cout << "test 1: " << xr1 << ", " << xr2 << std::endl;
  BOOST_TEST((xr1 <= 2 && xr2 <= 3));
}

BOOST_DATA_TEST_CASE(
  test2,
  bdata::xrange(3)
  *
  ( bdata::random(
      bdata::distribution=std::uniform_real_distribution<float>(1, 2))
    ^ bdata::xrange(2)
  ),
  xr, random_sample, index)
{
  std::cout << "test 2: "
    << xr << " / "
    << random_sample << ", " << index
    << std::endl;
  BOOST_TEST(random_sample < 1.7); // 30% chance of failure
}
//]


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

/*!
  @file
  @brief Defines the boost::simd::is_power_of_2 function
**/

#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd
{
  /*!
    @brief Checks if a given value is a power of 2

    @param value Value to test
    @return @c true if value is a non-zero power of 2, @c false otherwise.
  **/
  template<class T> BOOST_FORCEINLINE bool is_power_of_2(T value)
  {
    return (!(value & (value - 1)) && value);
  }
} }

#endif


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    double           array[][3]= {{1., 2., 3.}, {4., 5., 6.}, {7., 8., 9.}};
    dense2D<double>  A(array), B2, B3;

    // Creating a reordering matrix from a vector (or an array respectively)
    int indices[]= {2, 1};
    mat::traits::reorder<>::type R= mat::reorder(indices);
    std::cout << "\nR =\n" << R;

    // Reorder rows
    B2= R * A;
    std::cout << "\nR * A =\n" << B2;

    // Reorder columns
    B3= B2 * trans(R);
    std::cout << "\nB2 * trans(R) =\n" << B3;

    dense_vector<double> v(array[2]), w(R * v);
    std::cout << "\nR * v =\n" << w << "\n";

    return 0;
}


#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/if.hpp>

int main()
{
    using v1 = boost::mpl::vector_c<int, 5, 2, 3, 1, 4>;
    using it1 = boost::mpl::find<v1, boost::mpl::integral_c<int, 3>>::type;
    using index1 = it1::pos;
    using size1 = boost::mpl::size<v1>::type;
    using in_v1 = boost::mpl::less<index1, size1>::type;
    constexpr int r1{ boost::mpl::if_<in_v1, index1, boost::mpl::int_<-1>>::type::value };
    static_assert(r1 == 2);

    using v2 = boost::mpl::vector_c<int, 5, 2, 3>;
    using it2 = boost::mpl::find<v2, boost::mpl::integral_c<int, 6>>::type;
    using index2 = it2::pos;
    using size2 = boost::mpl::size<v2>::type;
    using in_v2 = boost::mpl::less<index2, size2>::type;
    constexpr int r2{ boost::mpl::if_<in_v2, index2, boost::mpl::int_<-1>>::type::value };
    static_assert(r2 == -1);
}