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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2004, 2005, 2006, 2007, 2009 StatPro Italia srl
 Copyright (C) 2015 Peter Caspers

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

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

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

/*! \file blackscholesprocess.hpp
    \brief Black-Scholes processes
*/

#ifndef quantlib_black_scholes_process_hpp
#define quantlib_black_scholes_process_hpp

#include <ql/stochasticprocess.hpp>
#include <ql/processes/eulerdiscretization.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/termstructures/volatility/equityfx/localvoltermstructure.hpp>
#include <ql/quote.hpp>

namespace QuantLib {

    class LocalConstantVol;
    class LocalVolCurve;

    //! Generalized Black-Scholes stochastic process
    /*! This class describes the stochastic process \f$ S \f$ governed by
        \f[
            d\ln S(t) = (r(t) - q(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class GeneralizedBlackScholesProcess : public StochasticProcess1D {
      public:
        GeneralizedBlackScholesProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& dividendTS,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
        //! \name StochasticProcess1D interface
        //@{
        Real x0() const;
        /*! \todo revise extrapolation */
        Real drift(Time t, Real x) const;
        /*! \todo revise extrapolation */
        Real diffusion(Time t, Real x) const;
        Real apply(Real x0, Real dx) const;
        /*! \warning in general raises a "not implemented" exception.
                     It should be rewritten to return the expectation E(S)
                     of the process, not exp(E(log S)).
        */
        Real expectation(Time t0, Real x0, Time dt) const;
        Real stdDeviation(Time t0, Real x0, Time dt) const;
        Real variance(Time t0, Real x0, Time dt) const;
        Real evolve(Time t0, Real x0, Time dt, Real dw) const;
        //@}
        Time time(const Date&) const;
        //! \name Observer interface
        //@{
        void update();
        //@}
        //! \name Inspectors
        //@{
        const Handle<Quote>& stateVariable() const;
        const Handle<YieldTermStructure>& dividendYield() const;
        const Handle<YieldTermStructure>& riskFreeRate() const;
        const Handle<BlackVolTermStructure>& blackVolatility() const;
        const Handle<LocalVolTermStructure>& localVolatility() const;
        //@}
      private:
        Handle<Quote> x0_;
        Handle<YieldTermStructure> riskFreeRate_, dividendYield_;
        Handle<BlackVolTermStructure> blackVolatility_;
        bool forceDiscretization_;
        mutable RelinkableHandle<LocalVolTermStructure> localVolatility_;
        mutable bool updated_, isStrikeIndependent_;
    };

    //! Black-Scholes (1973) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for a stock
        given by
        \f[
            d\ln S(t) = (r(t) - \frac{\sigma(t, S)^2}{2}) dt + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class BlackScholesProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackScholesProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Merton (1973) extension to the Black-Scholes stochastic process
    /*! This class describes the stochastic process ln(S) for a stock or
        stock index paying a continuous dividend yield given by
        \f[
            d\ln S(t, S) = (r(t) - q(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \ingroup processes
    */
    class BlackScholesMertonProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackScholesMertonProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& dividendTS,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Black (1976) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for a
        forward or futures contract given by
        \f[
            d\ln S(t) = -\frac{\sigma(t, S)^2}{2} dt + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class BlackProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Garman-Kohlhagen (1983) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for an exchange
        rate given by
        \f[
            d\ln S(t) = (r(t) - r_f(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class GarmanKohlagenProcess : public GeneralizedBlackScholesProcess {
      public:
        GarmanKohlagenProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& foreignRiskFreeTS,
            const Handle<YieldTermStructure>& domesticRiskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

}

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

/*
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2015 Peter Caspers

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

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

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

#include <ql/termstructures/volatility/equityfx/localvolsurface.hpp>
#include <ql/termstructures/volatility/equityfx/localvolcurve.hpp>
#include <ql/termstructures/volatility/equityfx/localconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

    inline GeneralizedBlackScholesProcess::GeneralizedBlackScholesProcess(
             const Handle<Quote>& x0,
             const Handle<YieldTermStructure>& dividendTS,
             const Handle<YieldTermStructure>& riskFreeTS,
             const Handle<BlackVolTermStructure>& blackVolTS,
             const boost::shared_ptr<discretization>& disc,
             bool forceDiscretization)
    : StochasticProcess1D(disc), x0_(x0), riskFreeRate_(riskFreeTS),
      dividendYield_(dividendTS), blackVolatility_(blackVolTS),
      forceDiscretization_(forceDiscretization), updated_(false) {
        registerWith(x0_);
        registerWith(riskFreeRate_);
        registerWith(dividendYield_);
        registerWith(blackVolatility_);
    }

    inline Real GeneralizedBlackScholesProcess::x0() const {
        return x0_->value();
    }

    inline Real GeneralizedBlackScholesProcess::drift(Time t, Real x) const {
        Real sigma = diffusion(t,x);
        // we could be more anticipatory if we know the right dt
        // for which the drift will be used
        Time t1 = t + 0.0001;
        return riskFreeRate_->forwardRate(t,t1,Continuous,NoFrequency,true)
             - dividendYield_->forwardRate(t,t1,Continuous,NoFrequency,true)
             - 0.5 * sigma * sigma;
    }

    inline Real GeneralizedBlackScholesProcess::diffusion(Time t, Real x) const {
        return localVolatility()->localVol(t, x, true);
    }

    inline Real GeneralizedBlackScholesProcess::apply(Real x0, Real dx) const {
        return x0 * std::exp(dx);
    }

    inline Real GeneralizedBlackScholesProcess::expectation(Time t0,
                                                     Real x0,
                                                     Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return x0 *
                std::exp(dt * (riskFreeRate_->forwardRate(t0, t0 + dt, Continuous,
                                                          NoFrequency, true) -
                             dividendYield_->forwardRate(
                                 t0, t0 + dt, Continuous, NoFrequency, true)));
        } else {
            QL_FAIL("not implemented");
        }
    }

    inline Real GeneralizedBlackScholesProcess::stdDeviation(Time t0, Real x0, Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return std::sqrt(variance(t0,x0,dt));
        }
        else{
            return discretization_->diffusion(*this,t0,x0,dt);
        }
    }

    inline Real GeneralizedBlackScholesProcess::variance(Time t0, Real x0, Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return blackVolatility_->blackVariance(t0 + dt, 0.01) -
                   blackVolatility_->blackVariance(t0, 0.01);
        }
        else{
            return discretization_->variance(*this,t0,x0,dt);
        }
    }

    inline Real GeneralizedBlackScholesProcess::evolve(Time t0, Real x0,
                                                Time dt, Real dw) const {
        localVolatility(); // trigger update
        if (isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            Real var = variance(t0, x0, dt);
            Real drift = (riskFreeRate_->forwardRate(t0, t0 + dt, Continuous,
                                                     NoFrequency, true) -
                          dividendYield_->forwardRate(t0, t0 + dt, Continuous,
                                                      NoFrequency, true)) *
                             dt -
                         0.5 * var;
            return apply(x0, std::sqrt(var) * dw + drift);
        } else
            return apply(x0, discretization_->drift(*this, t0, x0, dt) +
                                 stdDeviation(t0, x0, dt) * dw);
    }

    inline Time GeneralizedBlackScholesProcess::time(const Date& d) const {
        return riskFreeRate_->dayCounter().yearFraction(
                                           riskFreeRate_->referenceDate(), d);
    }

    inline void GeneralizedBlackScholesProcess::update() {
        updated_ = false;
        StochasticProcess1D::update();
    }

    inline const Handle<Quote>&
    GeneralizedBlackScholesProcess::stateVariable() const {
        return x0_;
    }

    inline const Handle<YieldTermStructure>&
    GeneralizedBlackScholesProcess::dividendYield() const {
        return dividendYield_;
    }

    inline const Handle<YieldTermStructure>&
    GeneralizedBlackScholesProcess::riskFreeRate() const {
        return riskFreeRate_;
    }

    inline const Handle<BlackVolTermStructure>&
    GeneralizedBlackScholesProcess::blackVolatility() const {
        return blackVolatility_;
    }

    inline const Handle<LocalVolTermStructure>&
    GeneralizedBlackScholesProcess::localVolatility() const {
        if (!updated_) {
            isStrikeIndependent_=true;

            // constant Black vol?
            boost::shared_ptr<BlackConstantVol> constVol =
                boost::dynamic_pointer_cast<BlackConstantVol>(
                                                          *blackVolatility());
            if (constVol) {
                // ok, the local vol is constant too.
                localVolatility_.linkTo(boost::make_shared<LocalConstantVol>(
                    constVol->referenceDate(),
                    constVol->blackVol(0.0, x0_->value()),
                    constVol->dayCounter()));
                updated_ = true;
                return localVolatility_;
            }

            // ok, so it's not constant. Maybe it's strike-independent?
            boost::shared_ptr<BlackVarianceCurve> volCurve =
                boost::dynamic_pointer_cast<BlackVarianceCurve>(
                                                          *blackVolatility());
            if (volCurve) {
                // ok, we can use the optimized algorithm
                localVolatility_.linkTo(boost::make_shared<LocalVolCurve>(
                    Handle<BlackVarianceCurve>(volCurve)));
                updated_ = true;
                return localVolatility_;
            }

            // ok, so it's strike-dependent. Never mind.
            localVolatility_.linkTo(
                boost::make_shared<LocalVolSurface>(blackVolatility_, riskFreeRate_,
                                                    dividendYield_, x0_->value()));
            updated_ = true;
            isStrikeIndependent_ = false;
            return localVolatility_;

        } else {
            return localVolatility_;
        }
    }

    // specific models

    inline BlackScholesProcess::BlackScholesProcess(
                              const Handle<Quote>& x0,
                              const Handle<YieldTermStructure>& riskFreeTS,
                              const Handle<BlackVolTermStructure>& blackVolTS,
                              const boost::shared_ptr<discretization>& d,
                              bool forceDiscretization)
    : GeneralizedBlackScholesProcess(
             x0,
             // no dividend yield
             Handle<YieldTermStructure>(boost::shared_ptr<YieldTermStructure>(
                  new FlatForward(0, NullCalendar(), 0.0, Actual365Fixed()))),
             riskFreeTS,
             blackVolTS,
             d,forceDiscretization) {}

    inline BlackScholesMertonProcess::BlackScholesMertonProcess(
                              const Handle<Quote>& x0,
                              const Handle<YieldTermStructure>& dividendTS,
                              const Handle<YieldTermStructure>& riskFreeTS,
                              const Handle<BlackVolTermStructure>& blackVolTS,
                              const boost::shared_ptr<discretization>& d,
                              bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,dividendTS,riskFreeTS,blackVolTS,d,
                                     forceDiscretization) {}

    inline BlackProcess::BlackProcess(const Handle<Quote>& x0,
                               const Handle<YieldTermStructure>& riskFreeTS,
                               const Handle<BlackVolTermStructure>& blackVolTS,
                               const boost::shared_ptr<discretization>& d,
                               bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,riskFreeTS,riskFreeTS,blackVolTS,d,
                                     forceDiscretization) {}

    inline GarmanKohlagenProcess::GarmanKohlagenProcess(
                          const Handle<Quote>& x0,
                          const Handle<YieldTermStructure>& foreignRiskFreeTS,
                          const Handle<YieldTermStructure>& domesticRiskFreeTS,
                          const Handle<BlackVolTermStructure>& blackVolTS,
                          const boost::shared_ptr<discretization>& d,
                          bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,foreignRiskFreeTS,domesticRiskFreeTS,
                                     blackVolTS,d,forceDiscretization) {}

}

#endif


//
//  LoudspeakerArray.h
//
//  Created by Marcos F. Sim�n G�lvez on 02/02/2015.
//  Copyright (c) 2014 ISVR, University of Southampton. All rights reserved.
//

// avoid annoying warning about unsafe STL functions.
#ifdef _MSC_VER
#pragma warning(disable: 4996)
#endif

#include "listener_compensation.hpp"

#include <cstdio>

#include <libpanning/defs.h>
#include <libpanning/XYZ.h>

#include <libpml/vector_parameter_config.hpp>

#include <boost/filesystem.hpp>

#include <algorithm>
#include <cmath>

// Uncomment to get debug output
// #define DEBUG_LISTENER_COMPENSATION 1

#ifdef DEBUG_LISTENER_COMPENSATION
#include <iostream>
#include <iterator>
#include <algorithm>
#endif

// TODO: Move definition to a separate file.
#define c_0 344

namespace visr
{
namespace rcl
{
  ListenerCompensation::ListenerCompensation( SignalFlowContext const & context,
                                              char const * name,
                                              CompositeComponent * parent,
                                              panning::LoudspeakerArray const & arrayConfig )
  : AtomicComponent( context, name, parent )
  , m_listenerPos( 0.0f, 0.0f, 0.0f )
  , mNumberOfLoudspeakers( arrayConfig.getNumRegularSpeakers() )
  , mPositionInput( "positionInput", *this, pml::EmptyParameterConfig() )
  , mGainOutput( "gainOutput", *this )
  , mDelayOutput( "delayOutput", *this)
{
  m_array = arrayConfig;
  pml::VectorParameterConfig const vectorConfig( mNumberOfLoudspeakers );

  mGainOutput.setParameterConfig( vectorConfig );
  mDelayOutput.setParameterConfig( vectorConfig );
}

void ListenerCompensation::process()
{
  if( mPositionInput.changed() )
  {
    pml::ListenerPosition const & pos( mPositionInput.data());
    efl::BasicVector<SampleType> & gains( mGainOutput.data());
    efl::BasicVector<SampleType> & delays( mDelayOutput.data());

    if (gains.size() != mNumberOfLoudspeakers or delays.size() != mNumberOfLoudspeakers)
    {
      throw std::invalid_argument("ListenerCompensation::process(): The size of the gain or delay vector does not match the number of loudspeaker channels.");
    }

    setListenerPosition(pos.x(), pos.y(), pos.z());
    if( calcGainComp( gains ) != 0 )
    {
      throw std::runtime_error("ListenerCompensation::process(): calcGainComp() failed.");
    }
    if (calcDelayComp( delays ) != 0)
    {
      throw std::runtime_error("ListenerCompensation::process(): calcDelayComp() failed.");
    }

#ifdef DEBUG_LISTENER_COMPENSATION
    std::cout << "DelayVector Source Gain: ";
    std::copy( gains.data(), gains.data()+gains.size(), std::ostream_iterator<float>(std::cout, " ") );
    std::cout << "Delay [s]: ";
    std::copy( delays.data(), delays.data()+delays.size(), std::ostream_iterator<float>(std::cout, " ") );
    std::cout << std::endl;
#endif // DEBUG_LISTENER_COMPENSATION

    mPositionInput.resetChanged();
    mGainOutput.swapBuffers();
    mDelayOutput.swapBuffers();
  }
}

int ListenerCompensation::calcGainComp( efl::BasicVector<Afloat> & gainComp )
{
  panning::XYZ l1;
  Afloat rad = 0.0f, max_rad = 0.0f, x = 0.0f, y = 0.0f, z = 0.0f;

  //setting listener position
  x = m_listenerPos.x;
  y = m_listenerPos.y;
  z = m_listenerPos.z;

  for (std::size_t i = 0; i < m_array.getNumSpeakers(); i++) {

    l1 = m_array.getPosition( i );

    rad = std::sqrt(std::pow((l1.x - x), 2.0f) + std::pow((l1.y - y), 2.0f) + std::pow((l1.z - z), 2.0f));

    gainComp[i] = rad;

    if ( gainComp[i]>max_rad)
      max_rad = gainComp[i];
  }

  for (std::size_t i = 0; i < m_array.getNumSpeakers(); i++) {

    gainComp[i] = (gainComp[i]/max_rad);
  }
  return 0;
}

int ListenerCompensation::calcDelayComp( efl::BasicVector<Afloat> & delayComp )
{

  Afloat rad=0.0f, max_rad=0.0f, x=0.0f, y=0.0f, z=0.0f;
  panning::XYZ l1;

  //setting listener position
  x = m_listenerPos.x;
  y = m_listenerPos.y;
  z = m_listenerPos.z;

  for (std::size_t i = 0; i < m_array.getNumSpeakers(); i++) {

    l1 = m_array.getPosition( i );

    rad = std::sqrt(std::pow((l1.x-x),2.0f) + std::pow((l1.y-y),2.0f) + std::pow((l1.z-z),2.0f));

    delayComp[i] = rad;

    if (delayComp[i]>max_rad)
      max_rad = delayComp[i];
  }

  for ( std::size_t i = 0; i < m_array.getNumSpeakers(); i++){

    delayComp[i] = std::abs(delayComp[i]-max_rad)/c_0;
  }
  return 0;
}

}// rcl
}//visr


#include "aslam/calibration/target-algorithms.h"

#include <Eigen/Core>
#include <aslam/geometric-vision/pnp-pose-estimator.h>

namespace aslam {
namespace calibration {

bool estimateTargetTransformation(
    const TargetObservation& target_observation,
    const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C) {
  CHECK(camera_ptr);
  CHECK_NOTNULL(T_G_C);
  constexpr bool kRunNonlinearRefinement = true;
  constexpr double kRansacPixelSigma = 1.0;
  constexpr int kRansacMaxIters = 200;
  return estimateTargetTransformation(
      target_observation, camera_ptr, T_G_C, kRunNonlinearRefinement,
      kRansacPixelSigma, kRansacMaxIters);
}

bool estimateTargetTransformation(
    const TargetObservation& target_observation,
    const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C,
    const bool run_nonlinear_refinement, const double ransac_pixel_sigma,
    const int ransac_max_iters) {
  CHECK(camera_ptr);
  CHECK_GT(ransac_pixel_sigma, 0.0);
  CHECK_GT(ransac_max_iters, 0);
  CHECK_NOTNULL(T_G_C);
  const Eigen::Matrix2Xd& observed_corners =
      target_observation.getObservedCorners();
  // Corner positions in target coordinates (global frame).
  const Eigen::Matrix3Xd corner_positions_G =
      target_observation.getCorrespondingTargetPoints();
  aslam::geometric_vision::PnpPoseEstimator pnp(run_nonlinear_refinement);
  std::vector<int> inliers;
  int num_iters = 0;
  bool pnp_success = pnp.absolutePoseRansacPinholeCam(
      observed_corners, corner_positions_G, ransac_pixel_sigma,
      ransac_max_iters, camera_ptr, T_G_C, &inliers, &num_iters);
  if (pnp_success) {
    VLOG(4) << "Found " << inliers.size() << "/" << observed_corners.cols()
            << " inliers in" << num_iters << " iterations.";
  } else {
    LOG(WARNING) << "Target transformation estimation failed.";
  }
  return pnp_success;
}

}  // namespace calibration
}  // namespace aslam


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/two_add.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/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/half.hpp>
#include <utility>

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

  STF_EXPR_IS( (two_add(T(), T()))
                  , (std::pair<T,T>)
                  );

  T inf_    = bs::Inf<T>();
  T zero_   = bs::Zero<T>();
  T one_    = bs::One<T>();
  T half_   = bs::Half<T>();
  T eps_    = bs::Eps<T>();
  T eps_2_  = eps_/T(2);

  std::pair<T,T> p;

  p = two_add(inf_,zero_);
  STF_EQUAL(p.first, inf_);
  STF_EQUAL(p.second, zero_);

  p = two_add(zero_, inf_);
  STF_EQUAL(p.first, inf_);
  STF_EQUAL(p.second, zero_);

  p = two_add(half_+ eps_2_, half_);
  STF_EQUAL(p.first, one_);
  STF_EQUAL(p.second, eps_2_);

  p = two_add(half_, half_+ eps_2_);
  STF_EQUAL(p.first, one_);
  STF_EQUAL(p.second, eps_2_);
}


#pragma once
#include <boost/algorithm/string/split.hpp>
#include <vector>
#include <string>

constexpr auto parse_claim = [](const auto input) {
  std::vector<std::string> nums;
  boost::algorithm::split(
      nums,
      input,
      [](const auto x) { return not std::isdigit(x); },
      boost::token_compress_on);
  std::vector<int> parsed;
  std::for_each(nums.begin(), nums.end(),
      [&parsed](const auto x) { if (x.length()>0) { parsed.push_back(std::stoi(x)); } });
  return parsed;
};

constexpr auto apply_claim = [](auto& claim_counts, const auto& claim) {
  const auto left = claim[1];
  const auto top = claim[2];
  const auto width = claim[3];
  const auto height = claim[4];

  for (auto row = top; row < top+height; ++row) {
    for (auto col = left; col < left+width; ++col) {
      claim_counts[row][col]++;
    }
  }
};

constexpr auto apply_claims = [](auto& claim_counts, const auto& claims) {
  for (const auto& claim_text: claims) {
    const auto claim = parse_claim(claim_text);
    if (5 != claim.size()) continue;
    apply_claim(claim_counts, claim);
  }
};

constexpr auto day3part1 = [](auto&& input) {
  std::vector<std::string> claims;
  while (!input.eof()) {
    std::string line;
    std::getline(input, line);
    claims.push_back(line);
  }

  constexpr auto SIZE = 1000;
  int claim_counts[SIZE][SIZE] = {0};
  apply_claims(claim_counts, claims);
  int overlap_squares = 0;
  for (auto row = 0; row < SIZE; ++row) {
    for (auto col = 0; col < SIZE; ++col) {
      if (claim_counts[row][col] > 1) ++overlap_squares;
    }
  }
  return overlap_squares;
};


#include <boost/math/special_functions/jacobi_elliptic.hpp>


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP

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

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

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/closest_points/services.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace closest_points
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename CalculationType>
struct compute_closest_point_to_segment
{
    template <typename Point, typename PointOfSegment>
    static inline auto
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2)
    {
        // A projected point of points in Integer coordinates must be able to be
        // represented in FP.
        using fp_point_type = model::point
            <
                CalculationType,
                dimension<PointOfSegment>::value,
                typename coordinate_system<PointOfSegment>::type
            >;

        // For convenience
        using fp_vector_type = fp_point_type;

        /*
            Algorithm [p: (px,py), p1: (x1,y1), p2: (x2,y2)]
            VECTOR v(x2 - x1, y2 - y1)
            VECTOR w(px - x1, py - y1)
            c1 = w . v
            c2 = v . v
            b = c1 / c2
            RETURN POINT(x1 + b * vx, y1 + b * vy)
        */

        // v is multiplied below with a (possibly) FP-value, so should be in FP
        // For consistency we define w also in FP
        fp_vector_type v, w, projected;

        geometry::convert(p2, v);
        geometry::convert(p, w);
        geometry::convert(p1, projected);
        subtract_point(v, projected);
        subtract_point(w, projected);

        CalculationType const zero = CalculationType();
        CalculationType const c1 = dot_product(w, v);
        if (c1 <= zero)
        {
            fp_vector_type fp_p1;
            geometry::convert(p1, fp_p1);
            return fp_p1;
        }
        CalculationType const c2 = dot_product(v, v);
        if (c2 <= c1)
        {
            fp_vector_type fp_p2;
            geometry::convert(p2, fp_p2);
            return fp_p2;
        }

        // See above, c1 > 0 AND c2 > c1 so: c2 != 0
        CalculationType const b = c1 / c2;

        multiply_value(v, b);
        add_point(projected, v);

        return projected;
    }
};

}
#endif // DOXYGEN_NO_DETAIL

template
<
    typename CalculationType = void
>
class projected_point
{
public:
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.

    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
            typename select_most_precise
                <
                    typename coordinate_type<Point>::type,
                    typename coordinate_type<PointOfSegment>::type,
                    CalculationType
                >::type
          >
    {};

    template <typename Point, typename PointOfSegment>
    inline auto
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        using calculation_type = typename calculation_type<Point, PointOfSegment>::type;

        return detail::compute_closest_point_to_segment<calculation_type>::apply(p, p1, p2);
    }

};

}} // namespace strategy::closest_points

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP


#include "stdafx.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <fmt/format.h>
#include "contest_types.h"
#include "solver_registry.h"
#include "visual_editor.h"

namespace SimpleMatchingSolver {

constexpr int kNumTrialsPerComponent = 1000000;
constexpr int kMinComponentSize = 10;
constexpr int kMinDegree = 2;
constexpr bool kPruneLeavesOnly = true;

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
auto SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    counter_ = 0;
    if (args.visualize) {
      editor_ = std::make_shared<SVisualEditor>(args.problem, "SimpleMatchingSolver", "visualize");
    }

    hole_ = args.problem->hole_polygon;
    vertices_ = args.problem->vertices;
    edges_ = args.problem->edges;
    epsilon_ = args.problem->epsilon;
    const auto hole_polygon = ToBoostPolygon(hole_);

    N_ = vertices_.size();
    queued_.assign(N_, -1);
    assigned_.assign(N_, -1);
    adjacent_.assign(N_, {});
    std::vector<std::vector<double>> distances(
        N_, std::vector<double>(N_, std::numeric_limits<double>::infinity()));
    for (int i = 0; i < N_; ++i) {
      distances[i][i] = 0.0;
    }
    for (const auto& [a, b] : edges_) {
      const auto squared_distance = SquaredDistance(vertices_[a], vertices_[b]);
      const auto margin = epsilon_ * squared_distance / 1'000'000;
      const auto min = squared_distance - margin;
      const auto max = squared_distance + margin;
      adjacent_[a].emplace_back(b, min, max);
      adjacent_[b].emplace_back(a, min, max);
      distances[a][b] = distances[b][a] = std::sqrt(squared_distance) * (1.0 + 1.0e-6 * epsilon_);
    }
    for (int k = 0; k < N_; ++k) {
      for (int i = 0; i < N_; ++i) {
        for (int j = 0; j < N_; ++j) {
          distances[i][j] = std::min(distances[i][j], distances[i][k] + distances[k][j]);
        }
      }
    }
    vertex_max_distances_.resize(N_);
    for (int i = 0; i < N_; ++i) {
      vertex_max_distances_[i].resize(N_);
      for (int j = 0; j < N_; ++j) {
        vertex_max_distances_[i][j] = std::ceil(std::pow(distances[i][j], 2.0));
      }
    }

    M_ = hole_.size();
    hole_candidates_.clear();
    hole_distances_.resize(M_);
    hole_visibilities_.resize(M_);
    for (int i = 0; i < M_; ++i) {
      hole_candidates_.push_back(i);
      hole_distances_[i].resize(M_);
      hole_visibilities_[i].resize(M_);
      for (int j = 0; j < M_; ++j) {
        const BoostLinestring linestring{ToBoostPoint(hole_[i]), ToBoostPoint(hole_[j])};
        hole_visibilities_[i][j] = bg::covered_by(linestring, hole_polygon);
        hole_distances_[i][j] = SquaredDistance(hole_[i], hole_[j]);
      }
    }

    last_updated_ = 0;
    component_sizes_.assign(M_, M_);
    best_sizes_.assign(M_, 0);
    bool found = false;
    for (int i = 0; i < N_; ++i) {
      queued_[i] = N_;
      vertex_candidates_.push_back(i);
      assigned_counts_.assign(1, 0);
      if (Search()) {
        found = true;
        break;
      }
      vertex_candidates_.pop_back();
      queued_[i] = -1;
    }

    SolverOutputs outputs;
    if (found) {
      auto pose = vertices_;
      const auto assigned_vertices = kPruneLeavesOnly ? CleanupLeaves() : Cleanup();
      for (const int vertex : assigned_vertices) {
        pose[vertex] = hole_[assigned_[vertex]];
      }
      outputs.solution = args.problem->create_solution(pose);
    } else {
      outputs.solution = args.problem->create_solution();
    }
    return outputs;
  }

  std::set<int> Cleanup() const {
    std::set<int> assigned_vertices;
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      assigned_vertices.insert(i);
    }
    while (true) {
      bool converged = true;
      for (int i = 0; i < N_; ++i) {
        if (!assigned_vertices.count(i)) continue;
        int degree = 0;
        for (const auto& [next, min, max] : adjacent_[i]) {
          degree += assigned_vertices.count(next);
        }
        if (degree < kMinDegree) {
          converged = false;
          assigned_vertices.erase(i);
        }
      }
      if (converged) break;
    }
    return assigned_vertices;
  }

  std::set<int> CleanupLeaves() const {
    std::set<int> assigned_vertices;
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      assigned_vertices.insert(i);
    }
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      int degree = 0;
      for (const auto& [next, min, max] : adjacent_[i]) {
        degree += assigned_[next] >= 0;
      }
      if (degree < kMinDegree) {
        assigned_vertices.erase(i);
      }
    }
    return assigned_vertices;
  }

  bool Search() {
    if (hole_candidates_.empty()) return true;
    if (++counter_ % 10000 == 0 && editor_) {
      auto pose = vertices_;
      std::vector<int> marked;
      for (int j = 0; j < N_; ++j) {
        if (assigned_[j] < 0) continue;
        pose[j] = hole_[assigned_[j]];
        marked.push_back(j);
      }
      editor_->set_pose(std::make_shared<SSolution>(pose));
      editor_->set_marked_indices(marked);
      editor_->set_persistent_custom_stat(fmt::format("assigned counts = {}", fmt::join(assigned_counts_, ", ")));
      editor_->show(1);
    }
    const int component_index = assigned_counts_.size() - 1;
    best_sizes_[component_index] = std::max(best_sizes_[component_index], assigned_counts_[component_index]);
    if (counter_ > last_updated_ + kNumTrialsPerComponent) {
      last_updated_ = counter_;
      component_sizes_[component_index] = best_sizes_[component_index];
      if (component_sizes_[component_index] < kMinComponentSize) return true;
    }
    for (int i = vertex_candidates_.size() - 1; i >= 0; --i) {
      const int vertex = vertex_candidates_[i];
      vertex_candidates_.erase(vertex_candidates_.begin() + i);
      for (int j = hole_candidates_.size() - 1; j >= 0; --j) {
        const int hole_vertex = hole_candidates_[j];
        bool feasible = true;
        for (const auto& [next, min, max] : adjacent_[vertex]) {
          if (assigned_[next] < 0) continue;
          const auto squared_distance = hole_distances_[assigned_[next]][hole_vertex];
          const auto visible = hole_visibilities_[assigned_[next]][hole_vertex];
          if (!visible || squared_distance < min || squared_distance > max) {
            feasible = false;
            break;
          }
        }
        for (int other = 0; feasible && other < N_; ++other) {
          if (assigned_[other] < 0) continue;
          if (hole_distances_[assigned_[other]][hole_vertex] > vertex_max_distances_[other][vertex]) {
            feasible = false;
          }
        }
        if (feasible) {
          hole_candidates_.erase(hole_candidates_.begin() + j);
          assigned_[vertex] = hole_vertex;
          ++assigned_counts_.back();
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] >= 0) continue;
            queued_[next] = vertex;
            vertex_candidates_.push_back(next);
          }
          if (Search()) return true;
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] != vertex) continue;
            queued_[next] = -1;
            vertex_candidates_.pop_back();
          }
          --assigned_counts_.back();
          assigned_[vertex] = -1;
          hole_candidates_.insert(hole_candidates_.begin() + j, hole_vertex);
        }
      }
      vertex_candidates_.insert(vertex_candidates_.begin() + i, vertex);
    }
    if (assigned_counts_.back() >= component_sizes_[component_index]) {
      if (assigned_counts_.size() == component_sizes_.size()) return true;
      for (int i = 0; i < N_; ++i) {
        if (queued_[i] >= 0) continue;
        queued_[i] = N_;
        vertex_candidates_.push_back(i);
        assigned_counts_.push_back(0);
        if (Search()) return true;
        assigned_counts_.pop_back();
        vertex_candidates_.pop_back();
        queued_[i] = -1;
      }
    }
    return false;
  }

 private:
  std::vector<Point> hole_;
  std::vector<Point> vertices_;
  std::vector<Edge> edges_;
  integer epsilon_;

  int N_;
  int M_;
  std::vector<int> queued_;
  std::vector<int> assigned_;
  std::vector<int> vertex_candidates_;
  std::vector<int> hole_candidates_;
  std::vector<std::vector<std::tuple<int, integer, integer>>> adjacent_;
  std::vector<std::vector<integer>> hole_distances_;
  std::vector<std::vector<bool>> hole_visibilities_;
  std::vector<std::vector<integer>> vertex_max_distances_;
  std::vector<int> assigned_counts_;
  std::vector<int> component_sizes_;
  std::vector<int> best_sizes_;
  std::size_t last_updated_;
  std::size_t counter_;
  SVisualEditorPtr editor_;
};

}

REGISTER_SOLVER("SimpleMatchingSolver", SimpleMatchingSolver::Solver);
// vim:ts=2 sw=2 sts=2 et ci


#ifndef EIGENGWAS_HPP_
#define EIGENGWAS_HPP_

#include "time.h"

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <math.h>
#include <boost/math/distributions/students_t.hpp>
#include "genotype.h"
#include "Goptions.hpp"
#include "global.h"

using namespace std;

extern Goptions goptions;
extern genotype g;
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

class EigenGWAS {
    public:
    EigenGWAS() {

    }

    void Scan(MatrixXdr IndEigenVec) {
    	clock_t eg_begin = clock();
	    cout << "---------------EigenGWAS scan-------------" << endl;
	    cout << "SNP number: " << g.Nsnp << endl;
	    cout << "Sample size: " << g.Nindv << endl;

    	cout << "Eigenvector number: " << IndEigenVec.cols() <<endl;

	    MatrixXdr evePS(IndEigenVec.rows(), IndEigenVec.cols());
	    cout << "Standardization eigenvec" << endl;
	    for (int i = 0; i < evePS.cols(); i++) {
		    double sum = 0, sd = 0, eSq = 0, m = 0;
		    for (int j = 0; j < evePS.rows(); j++) {
			    sum += IndEigenVec(j, i);
			    eSq += IndEigenVec(j, i) * IndEigenVec(j, i);
		    }
		    m = sum / IndEigenVec.rows();
		    sd = sqrt((eSq - m * m * IndEigenVec.rows()) / (IndEigenVec.rows() - 1));
		    for (int j = 0; j < IndEigenVec.rows(); j++) {
			    evePS(j, i) = (IndEigenVec(j, i) - m) / sd;
		    }
	    }

	//using mailman
	    MatrixXdr EgBeta(g.Nsnp, IndEigenVec.cols());
	    multiply_y_pre(evePS, evePS.cols(), EgBeta, true);

    	EgBeta = EgBeta / g.Nindv;
	    for (int i = 0; i < EgBeta.cols(); i++) {
		    cout << EgBeta(0, i) <<endl;
	    }

    	using boost::math::students_t;
	    students_t Tdist(g.Nindv - 1);

    	for (int i = 0; i < EgBeta.cols(); i++) {
			cout << "Scanning eigenvector " << i + 1 << endl;
			ofstream e_file;
			e_file.open((goptions.GetGenericOutFile() + string("eg."+std::to_string(i+1)+".txt")).c_str());
			e_file << "CHR\tSNP\tPOS\tBP\tA1\tA2\tBeta\tSE\tT-stat\tP" << endl;
			for (int j = 0; j <EgBeta.rows(); j++) {
			    double seB = sqrt((1 - pow(EgBeta(j, i), 2))/(g.Nindv - 1));
			    double t_stat = EgBeta(j, i) / seB;
				if (goptions.IsGenericDebug()) {
				    cout << "MK: " << j << " infor=" << g.get_bim_info(j) << " egB=" << EgBeta(j, i) << " seB=" << seB << " t=" << t_stat << endl;
				}
			    double pt2tail = cdf(complement(Tdist, fabs(t_stat))) * 2;
			    e_file << g.get_bim_info(j) << "\t" << std::setprecision(8) << EgBeta(j, i) << "\t" << seB << "\t" << t_stat << "\t" << pt2tail <<endl;
		    }
    		e_file.close();
	    }

    	clock_t eg_end = clock();
	    double eg_time = double(eg_end - eg_begin) / CLOCKS_PER_SEC;
	    cout << "EigenGWAS total time " << eg_time << endl;
    }
};

#endif

/*
void EigenGWAS(MatrixXdr IndEigenVec) {

	clock_t eg_begin = clock();
	cout << "---------------EigenGWAS scan-------------" << endl;
	cout << "SNP number: " << g.Nsnp << endl;
	cout << "Sample size: " << g.Nindv << endl;

	cout << "Eigenvector number: " << IndEigenVec.cols() <<endl;

	MatrixXdr evePS(IndEigenVec.rows(), IndEigenVec.cols());
	cout << "Standardization eigenvec" << endl;
	for (int i = 0; i < evePS.cols(); i++) {
		double sum = 0, sd = 0, eSq = 0, m = 0;
		for (int j = 0; j < evePS.rows(); j++) {
			sum += IndEigenVec(j, i);
			eSq += IndEigenVec(j, i) * IndEigenVec(j, i);
		}
		m = sum / IndEigenVec.rows();
		sd = sqrt((eSq - m * m * IndEigenVec.rows()) / (IndEigenVec.rows() - 1));
		for (int j = 0; j < IndEigenVec.rows(); j++) {
			evePS(j, i) = (IndEigenVec(j, i) - m) / sd;
		}
	}

	//using mailman
	MatrixXdr EgBeta(g.Nsnp, IndEigenVec.cols());
	multiply_y_pre(evePS, evePS.cols(), EgBeta, true);

	EgBeta = EgBeta / g.Nindv;
	for (int i = 0; i < EgBeta.cols(); i++) {
		cout << EgBeta(0, i) <<endl;
	}

	using boost::math::students_t;
	students_t Tdist(g.Nindv - 1);

	for (int i = 0; i < EgBeta.cols(); i++) {
		cout << "Scanning eigenvector " << i + 1 << endl;
		ofstream e_file;
		e_file.open((goptions.GetGenericOutFile() + string("eg."+std::to_string(i+1)+".txt")).c_str());
		e_file << "CHR\tSNP\tPOS\tBP\tA1\tA2\tBeta\tSE\tT-stat\tP" << endl;
		for (int j = 0; j <EgBeta.rows(); j++) {
			double seB = sqrt((1 - pow(EgBeta(j, i), 2))/(g.Nindv - 1));
			double t_stat = EgBeta(j, i) / seB;
			cout << "MK: " << j << " infor=" << g.get_bim_info(j) << " egB=" << EgBeta(j, i) << " seB=" << seB << " t=" << t_stat << endl;
			double pt2tail = cdf(complement(Tdist, fabs(t_stat))) * 2;
			e_file << g.get_bim_info(j) << "\t" << std::setprecision(8) << EgBeta(j, i) << "\t" << seB << "\t" << t_stat << "\t" << pt2tail <<endl;
		}
		e_file.close();
	}

	clock_t eg_end = clock();
	double eg_time = double(eg_end - eg_begin) / CLOCKS_PER_SEC;
	cout << "EigenGWAS total time " << eg_time << endl;
}
*/


#include "compi.hpp"

#include <limits>

#include <boost/math/quadrature/trapezoidal.hpp>

extern "C" {
    #include "integration_routines.h"
}
#include "integration_routines_template.hpp"
#include "IntegrandFunctionWrapper.hpp"

struct TrapezoidParamerters: public RoutineParametersBase {
    Real x_min, x_max;

    TrapezoidParamerters(PyObject* routine_args, PyObject* routine_kwargs):RoutineParametersBase{std::numeric_limits<Real>::epsilon(),12}{
        constexpr auto keywords = generate_keyword_list<IntegralRange::finite>();

        if(!PyArg_ParseTupleAndKeywords(routine_args,routine_kwargs,"Odd|OO$pId",const_cast<char**>(keywords.data()),
                &integrand,&x_min,&x_max,
                &args,&kw,
                &full_output, &max_levels,&tolerance)){
            throw could_not_parse_arguments("Unable to parse python arguments to C variables");
        }
    }
};

TrapezoidParamerters::result_type run_integration_routine(const compi_internal::IntegrandFunctionWrapper& f, const TrapezoidParamerters& params){
    TrapezoidParamerters::result_type result;

    result.result = boost::math::quadrature::trapezoidal(f,params.x_min, params.x_max,params.tolerance,params.max_levels, &(result.err),&(result.l1));

    return result;
}

template<>
PyObject* generate_full_output_dict(const TrapezoidParamerters::result_type& result,const TrapezoidParamerters& params)noexcept{
    return Py_BuildValue("{sd}", "L1 norm", result.l1);
}

extern "C" PyObject* trapezoidal(PyObject* self, PyObject* args, PyObject* kwargs){
    return integration_routine<TrapezoidParamerters>(args,kwargs);
}


//  Copyright John Maddock 2017.
//  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_STANDALONE

#include <boost/math/concepts/std_real_concept.hpp>
#include <boost/math/interpolators/barycentric_rational.hpp>

void compile_and_link_test()
{
   boost::math::concepts::std_real_concept x[] = { 1, 2, 3 };
   boost::math::concepts::std_real_concept y[] = { 13, 15, 17 };
   boost::math::interpolators::barycentric_rational<boost::math::concepts::std_real_concept> s(x, y, 3, 3);
   s(1.0);
}

#endif


/*
 * Copyright 2019 Forschungszentrum Juelich GmbH
 * Copyright 2009-2019 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE linalg_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local private VOTCA includes
#include "kokkos_linalg.hpp"

BOOST_AUTO_TEST_SUITE(linalg_test)

BOOST_AUTO_TEST_CASE(cross) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = -1;
    a(1) = 7;
    a(2) = 4;
    Kokkos::View<double[3]> b("b");
    b(0) = -5;
    b(1) = 8;
    b(2) = 4;

    auto result = kokkos_linalg_3d::cross(a, b);
    BOOST_CHECK_CLOSE(result[0], -4, 1e-9);
    BOOST_CHECK_CLOSE(result[1], -16, 1e-9);
    BOOST_CHECK_CLOSE(result[2], 27, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(dot) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = -1;
    a(1) = 7;
    a(2) = 4;
    Kokkos::View<double[3]> b("b");
    b(0) = -5;
    b(1) = 8;
    b(2) = 4;

    auto result = kokkos_linalg_3d::dot(a, b);
    BOOST_CHECK_CLOSE(result, 77, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(gemv) {
  Kokkos::initialize();
  {

    Kokkos::View<double[3]> a("a");
    a(0) = 1;
    a(1) = 2;
    a(2) = 3;
    Kokkos::View<double[9]> A("A");
    A(0) = 5;
    A(1) = 1;
    A(2) = 3;
    A(3) = 1;
    A(4) = 1;
    A(5) = 1;
    A(6) = 1;
    A(7) = 2;
    A(8) = 1;

    auto result = kokkos_linalg_3d::gemv(A, a);
    BOOST_CHECK_CLOSE(result[0], 16, 1e-9);
    BOOST_CHECK_CLOSE(result[1], 6, 1e-9);
    BOOST_CHECK_CLOSE(result[2], 8, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(norm) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = -1;
    a(1) = 7;
    a(2) = 4;
    auto result = kokkos_linalg_3d::norm(a);
    BOOST_CHECK_CLOSE(result, std::sqrt(66), 1e-5);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(trace) {
  Kokkos::initialize();
  {
    Kokkos::View<double[9]> A("A");
    A(0) = 5;
    A(1) = 1;
    A(2) = 3;
    A(3) = 1;
    A(4) = 1;
    A(5) = 1;
    A(6) = 1;
    A(7) = 2;
    A(8) = 1;

    auto result = kokkos_linalg_3d::trace(A);
    BOOST_CHECK_CLOSE(result, 7, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(cross_matrix_product) {
  Kokkos::initialize();
  {
    Kokkos::View<double[9]> A("A");
    A(0) = 5;
    A(1) = 1;
    A(2) = 3;
    A(3) = 1;
    A(4) = 1;
    A(5) = 1;
    A(6) = 1;
    A(7) = 2;
    A(8) = 1;

    Kokkos::View<double[9]> B("B");
    B(0) = -1;
    B(1) = 1;
    B(2) = 3;
    B(3) = 1;
    B(4) = 1;
    B(5) = 1;
    B(6) = 1;
    B(7) = 2;
    B(8) = 1;
    auto result = kokkos_linalg_3d::cross_matrix_product(A, B);
    BOOST_CHECK_CLOSE(result[0], 0, 1e-9);
    BOOST_CHECK_CLOSE(result[1], -6, 1e-9);
    BOOST_CHECK_CLOSE(result[2], 6, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(scale_3d) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> v("v");
    v(0) = -1.2;
    v(1) = 2.4;
    v(2) = 1000.0;
    double s = 0.25;

    auto result = kokkos_linalg_3d::scale_3d(s, v);

    BOOST_CHECK_CLOSE(result[0], -0.3, 1e-9);
    BOOST_CHECK_CLOSE(result[1], 0.6, 1e-9);
    BOOST_CHECK_CLOSE(result[2], 250.0, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(dualbase_3d) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = 1.0;
    a(1) = 1.0;
    a(2) = 1.0;

    Kokkos::View<double[3]> b("b");
    b(0) = -1.0;
    b(1) = 1.0;
    b(2) = 1.0;

    Kokkos::View<double[3]> c("c");
    c(0) = 1.0;
    c(1) = -1.0;
    c(2) = 1.0;

    auto result = kokkos_linalg_3d::dualbase_3d(a, b, c);

    BOOST_CHECK_CLOSE(result[0][0], 0.5, 1e-9);
    BOOST_CHECK_CLOSE(result[0][1], 0.5, 1e-9);
    BOOST_CHECK_CLOSE(result[0][2], 0.0, 1e-9);

    BOOST_CHECK_CLOSE(result[1][0], -0.5, 1e-9);
    BOOST_CHECK_CLOSE(result[1][1], 0.0, 1e-9);
    BOOST_CHECK_CLOSE(result[1][2], 0.5, 1e-9);

    BOOST_CHECK_CLOSE(result[2][0], 0.0, 1e-9);
    BOOST_CHECK_CLOSE(result[2][1], -0.5, 1e-9);
    BOOST_CHECK_CLOSE(result[2][2], 0.5, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(add_to) {
  Kokkos::initialize();
  {
    Kokkos::View<double[5]> a("a");
    a(0) = 1.2;
    a(1) = -0.4;
    a(2) = 3.7;
    a(3) = -2.8;
    a(4) = 0.6;
    Kokkos::View<double[5]> b("b");
    b(0) = -1.2;
    b(1) = 1.4;
    b(2) = -4.7;
    b(3) = 1.8;
    b(4) = 1.0;
    kokkos_linalg_3d::add_to(a, b);

    BOOST_CHECK_CLOSE(a(0), 0.0, 1e-9);
    BOOST_CHECK_CLOSE(a(1), 1.0, 1e-9);
    BOOST_CHECK_CLOSE(a(2), -1.0, 1e-9);
    BOOST_CHECK_CLOSE(a(3), -1.0, 1e-9);
    BOOST_CHECK_CLOSE(a(4), 1.6, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(subtract_from) {
  Kokkos::initialize();
  {
    Kokkos::View<double[5]> a("a");
    a(0) = 1.2;
    a(1) = -0.4;
    a(2) = 3.7;
    a(3) = -2.8;
    a(4) = 0.6;
    Kokkos::View<double[5]> b("b");
    b(0) = -1.2;
    b(1) = 1.4;
    b(2) = -4.7;
    b(3) = 1.8;
    b(4) = 1.0;
    kokkos_linalg_3d::subtract_from(a, b);

    BOOST_CHECK_CLOSE(a(0), 2.4, 1e-9);
    BOOST_CHECK_CLOSE(a(1), -1.8, 1e-9);
    BOOST_CHECK_CLOSE(a(2), 8.4, 1e-9);
    BOOST_CHECK_CLOSE(a(3), -4.6, 1e-9);
    BOOST_CHECK_CLOSE(a(4), -0.4, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(add) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = 1.2;
    a(1) = -0.4;
    a(2) = 3.7;
    Kokkos::View<double[3]> b("b");
    b(0) = -1.2;
    b(1) = 1.4;
    b(2) = -4.7;
    auto result = kokkos_linalg_3d::add(a, b);

    BOOST_CHECK_CLOSE(result[0], 0.0, 1e-9);
    BOOST_CHECK_CLOSE(result[1], 1.0, 1e-9);
    BOOST_CHECK_CLOSE(result[2], -1.0, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_CASE(subtract) {
  Kokkos::initialize();
  {
    Kokkos::View<double[3]> a("a");
    a(0) = 1.2;
    a(1) = -0.4;
    a(2) = 3.7;
    Kokkos::View<double[3]> b("b");
    b(0) = -1.2;
    b(1) = 1.4;
    b(2) = -4.7;
    auto result = kokkos_linalg_3d::subtract(a, b);

    BOOST_CHECK_CLOSE(result[0], 2.4, 1e-9);
    BOOST_CHECK_CLOSE(result[1], -1.8, 1e-9);
    BOOST_CHECK_CLOSE(result[2], 8.4, 1e-9);
  }
  Kokkos::finalize();
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright John Maddock 2016.

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

//
// This file takes way too long to run to be part of the main regression test suite,
// but is useful in probing for errors in cpp_bin_float's rounding code.
// It cycles through every single value of type float, and rounds those numbers
// plus some closely related ones and compares the results to those produced by MPFR.
//
#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/special_functions/next.hpp>
#include "test.hpp"

using namespace boost::multiprecision;

typedef number<mpfr_float_backend<35> >                                                     good_type;
typedef number<cpp_bin_float<std::numeric_limits<good_type>::digits, digit_base_2>, et_off> test_type;

int main()
{
   float f = (std::numeric_limits<float>::max)();

   do
   {
      float     fr1, fr2;
      good_type gf(f), gf2(f);
      test_type tf(f), tf2(f);
      fr1 = gf.convert_to<float>();
      fr2 = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);
      // next represenation:
      gf = boost::math::float_next(gf2);
      tf = boost::math::float_next(tf2);
      BOOST_CHECK_NE(gf, gf2);
      BOOST_CHECK_NE(tf, tf2);
      fr1 = gf.convert_to<float>();
      fr2 = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);
      // previous representation:
      gf = boost::math::float_prior(gf2);
      tf = boost::math::float_prior(tf2);
      BOOST_CHECK_NE(gf, gf2);
      BOOST_CHECK_NE(tf, tf2);
      fr1 = gf.convert_to<float>();
      fr2 = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);

      // Create ands test ties:
      int e;
      std::frexp(f, &e);
      float extra = std::ldexp(1.0f, e - std::numeric_limits<float>::digits - 1);
      gf          = gf2 += extra;
      tf          = tf2 += extra;
      fr1         = gf.convert_to<float>();
      fr2         = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);
      // next represenation:
      gf = boost::math::float_next(gf2);
      tf = boost::math::float_next(tf2);
      BOOST_CHECK_NE(gf, gf2);
      BOOST_CHECK_NE(tf, tf2);
      fr1 = gf.convert_to<float>();
      fr2 = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);
      // previous representation:
      gf = boost::math::float_prior(gf2);
      tf = boost::math::float_prior(tf2);
      BOOST_CHECK_NE(gf, gf2);
      BOOST_CHECK_NE(tf, tf2);
      fr1 = gf.convert_to<float>();
      fr2 = tf.convert_to<float>();
      BOOST_CHECK_EQUAL(fr1, fr2);

      f = boost::math::float_prior(f);
   } while (f);

   return boost::report_errors();
}


#include <gtest/gtest.h>
#include <Eigen/Geometry>
#include <plucker/mat_relational.h>

namespace
{

template<typename T>
class MatRelationalTest
    : public ::testing::Test
{
protected:
    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, float>::value, U>::type
    absolute_tolerance(){ return 1e-4f; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, double>::value, U>::type
    absolute_tolerance(){ return 1e-8; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, float>::value, U>::type
    relative_tolerance(){ return 1e-5f; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, double>::value, U>::type
    relative_tolerance(){ return 1e-5; }
};

using MyTypes = ::testing::Types<float, double>;
TYPED_TEST_SUITE(MatRelationalTest, MyTypes);

TYPED_TEST(MatRelationalTest, almost_equal)
{
    using Matrix1 = Eigen::Matrix<TypeParam, 1, 1>;

    constexpr auto atol = MatRelationalTest<TypeParam>::absolute_tolerance();

    {
        constexpr auto val1 = atol;
        constexpr auto val2 = TypeParam(0);
        const auto s1 = Matrix1(val1);
        const auto s2 = Matrix1(val2);

        EXPECT_TRUE(plucker::detail::almost_equal(s1, s2, atol));
        EXPECT_TRUE(plucker::detail::almost_equal(s2, s1, atol));
    }
    {
        constexpr auto val1 = 10 * atol;
        constexpr auto val2 = TypeParam(0);
        const auto s1 = Matrix1(val1);
        const auto s2 = Matrix1(val2);

        EXPECT_FALSE(plucker::detail::almost_equal(s1, s2, atol));
        EXPECT_FALSE(plucker::detail::almost_equal(s2, s1, atol));
    }
    {
        constexpr auto val1 = TypeParam(1000);
        constexpr auto val2 = TypeParam(999);
        const auto s1 = Matrix1(val1);
        const auto s2 = Matrix1(val2);

        EXPECT_FALSE(plucker::detail::almost_equal(s1, s2, TypeParam(1e-5)));
        EXPECT_TRUE(plucker::detail::almost_equal(s1, s2, TypeParam(1e-3)));
        EXPECT_TRUE(plucker::detail::almost_equal(s1, s2, TypeParam(1e-3), TypeParam(1e-5)));
    }
}

TYPED_TEST(MatRelationalTest, almost_zero)
{
    using Matrix1 = Eigen::Matrix<TypeParam, 1, 1>;

    constexpr auto atol = MatRelationalTest<TypeParam>::absolute_tolerance();

    {
        constexpr auto val = atol;
        const auto s = Matrix1(val);

        EXPECT_TRUE(plucker::detail::almost_zero(s, atol));
    }
    {
        constexpr auto val = 10 * atol;
        const auto s = Matrix1(val);

        EXPECT_FALSE(plucker::detail::almost_zero(s, atol));
    }
}

}   // namespace


#ifndef EMPIRICALCPP_SRC_MESH_HPP_
#define EMPIRICALCPP_SRC_MESH_HPP_

#include <empiricalcpp/src/constants.hpp>
#include <empiricalcpp/src/quadrature.hpp>
#include <array>
#include <functional>
#include <vector>
#include <tuple>
#include <boost/multi_array.hpp>

namespace empirical {
    namespace mesh {

        class Mesh1D {
        public:
            typedef std::vector<Scalar> vector_type;
            typedef vector_type::size_type size_type;
            typedef std::function<vector_type(const size_type, const Scalar, const Scalar)> generator_function;

        protected:
            Scalar minVal, maxVal;
            vector_type points;
            generator_function generator;

            void recalculate(const size_type N, const Scalar min, const Scalar max) {
                minVal = min;
                maxVal = max;
                points = generator(N, min, max);
            }

        public:
            Mesh1D(generator_function generator, const size_type N, const Scalar min = -1, const Scalar max = 1)
                : minVal(min), maxVal(max), points(1), generator(generator) {
                recalculate(N, min, max);
            }
            Mesh1D(const Quadrature& quadrature);
            Mesh1D(std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> pointFunction,
                const size_type N, const Scalar min = -1, const Scalar max = 1);

            size_type size() const { return points.size(); }
            Scalar min() const { return minVal; }
            Scalar max() const { return maxVal; }

            vector_type::iterator begin() { return points.begin(); }
            vector_type::iterator end() { return points.end(); }
            vector_type::const_iterator begin() const { return points.begin(); }
            vector_type::const_iterator end() const { return points.end(); }

            vector_type getPoints() { return points; }
            const vector_type getPoints() const { return points; }

            Scalar operator[](const size_type i) const { return points[i]; }
            Scalar& operator[](const size_type i) { return points[i]; }

            Scalar operator()(const size_type i) const { return points[i]; }
            Scalar& operator()(const size_type i) { return points[i]; }

            void resize(const size_type N, const Scalar min, const Scalar max) {
                if (points.size() == N && min == minVal && max == maxVal) {
                    return;
                }
                recalculate(N, min, max);
            }

            void resize(const size_type N) {
                resize(N, minVal, maxVal);
            }
        };

        class Mesh2D {
        public:
            typedef boost::multi_array<Scalar, 3> array_type;
            typedef array_type::size_type size_type;
            typedef std::array<size_type, 2> element_index_type;
            typedef std::array<Scalar, 2> point_type;
            typedef std::function<array_type*(const element_index_type, const point_type, const point_type)> generator_function;

        protected:
            point_type minVal, maxVal;
            std::unique_ptr<array_type> points;
            generator_function generator;

            void recalculate(const element_index_type N, const point_type min, const point_type max);

        public:
            Mesh2D(generator_function generator, const element_index_type N, const point_type min, const point_type max)
                : minVal(min), maxVal(max), generator(generator) {
                recalculate(N, min, max);
            }
            Mesh2D(generator_function generator, const element_index_type N) : generator(generator) {
                minVal[0] = minVal[1] = -1;
                maxVal[0] = maxVal[1] = 1;
                recalculate(N, minVal, maxVal);
            }
            Mesh2D(const Quadrature& xQuadrature, const Quadrature& yQuadrature);
            Mesh2D(std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> xFunction,
                std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> yFunction,
                const element_index_type N, const point_type min, const point_type max);

            size_type sizeX() const { return points->shape()[0]; }
            size_type sizeY() const { return points->shape()[1]; }
            point_type min() const { return minVal; }
            point_type max() const { return maxVal; }

            array_type::iterator begin();// { return points.begin(); }
            array_type::iterator end();// { return points.end(); }
            array_type::const_iterator begin() const;// { return points.begin(); }
            array_type::const_iterator end() const;// { return points.end(); }

            array_type getPoints() { return *points; }
            const array_type getPoints() const { return *points; }

            array_type::const_reference operator[](const size_type i) const { return (*points)[i]; }
            array_type::reference operator[](const size_type i) { return (*points)[i]; }

            point_type operator()(const size_type i, const size_type j) const { return{ { (*points)[i][j][0], (*points)[i][j][1] } }; }

            void resize(const element_index_type N, const point_type min, const point_type max) {
                if (points->shape()[0] == N[0] && points->shape()[1] == N[1] &&
                    min[0] == minVal[0] && min[1] == minVal[1] &&
                    max[0] == maxVal[0] && max[1] == maxVal[1]) {
                    return;
                }
                recalculate(N, min, max);
            }

            void resize(const element_index_type N) {
                resize(N, minVal, maxVal);
            }
        };

#ifndef EMPIRICAL_NO_OSTREAM_DEFINITIONS
        inline std::ostream& operator<<(std::ostream& os, const Mesh1D& mesh) {
            os << "Mesh1D {";
            os << std::fixed << std::setw(11) << std::setprecision(6);
            for (Scalar val : mesh) {
                os << val << ", ";
            }
            return os << "}";
        }
        std::ostream& operator<<(std::ostream& os, const Mesh2D& mesh);
#endif
    }

    typedef mesh::Mesh1D Mesh1D;
}

#endif /* EMPIRICALCPP_SRC_MESH_HPP_ */


/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_R3xSO3_HPP
#define INTERPOL_R3xSO3_HPP

#include <Eigen/Geometry>

namespace interpol {

class R3xSO3
{
public:
    Eigen::Quaterniond quat;
    Eigen::Vector3d tra;

    R3xSO3(const Eigen::Quaterniond& q, const Eigen::Vector3d& t) : quat(q), tra(t) {}

    R3xSO3& operator *=(const R3xSO3& other){
        quat *= other.quat;
        tra += other.tra;
        return *this;
    }
};

static Eigen::Vector3d tra(const R3xSO3& rr){
    return rr.tra;
}
static Eigen::Quaterniond rot(const R3xSO3& rr){
    return rr.quat;
}

} // ns interpol

#endif // INTERPOL_R3xSO3_HPP


//  (C) Copyright Matt Borland 2021.
//  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)
//
//  Constepxr implementation of abs (see c.math.abs secion 26.8.2 of the ISO standard)

#ifndef BOOST_MATH_CCMATH_ABS
#define BOOST_MATH_CCMATH_ABS

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

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T abs_impl(T x)
{
    return boost::math::ccmath::isnan(x) ? std::numeric_limits<T>::quiet_NaN() :
           boost::math::ccmath::isinf(x) ? std::numeric_limits<T>::infinity() :
           x == -0 ? T(0) :
           x == (std::numeric_limits<T>::min)() ? std::numeric_limits<T>::quiet_NaN() :
           x > 0 ? x : -x;
}

} // Namespace detail

template <typename T, std::enable_if_t<!std::is_unsigned_v<T>, bool> = true>
inline constexpr T abs(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return detail::abs_impl<T>(x);
    }
    else
    {
        using std::abs;
        return abs(x);
    }
}

// If abs() is called with an argument of type X for which is_unsigned_v<X> is true and if X
// cannot be converted to int by integral promotion (7.3.7), the program is ill-formed.
template <typename T, std::enable_if_t<std::is_unsigned_v<T>, bool> = true>
inline constexpr T abs(T x)
{
    if constexpr (std::is_convertible_v<T, int>)
    {
        return detail::abs_impl<int>(static_cast<int>(x));
    }
    else
    {
        static_assert(sizeof(T) == 0, "Taking the absolute value of an unsigned value not covertible to int is UB.");
        return T(0); // Unreachable, but suppresses warnings
    }
}

} // Namespaces

#endif // BOOST_MATH_CCMATH_ABS


/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#include <eosio/chain/asset.hpp>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <fc/reflect/variant.hpp>

namespace eosio { namespace chain {
typedef boost::multiprecision::int128_t  int128_t;

uint8_t asset::decimals()const {
   return sym.decimals();
}

string asset::symbol_name()const {
   return sym.name();
}

int64_t asset::precision()const {
   return sym.precision();
}

string asset::to_string()const {
   string result = fc::to_string( static_cast<int64_t>(amount) / precision());
   if( decimals() )
   {
      auto fract = static_cast<int64_t>(amount) % precision();
      result += "." + fc::to_string(precision() + fract).erase(0,1);
   }
   return result + " " + symbol_name();
}

asset asset::from_string(const string& from)
{
   try {
      string s = fc::trim(from);
      auto dot_pos = s.find(".");
      FC_ASSERT(dot_pos != string::npos, "dot missing in asset from string");
      auto space_pos = s.find(" ", dot_pos);
      FC_ASSERT(space_pos != string::npos, "space missing in asset from string");

      asset result;

      auto intpart = s.substr(0, dot_pos);
      result.amount = fc::to_int64(intpart);
      string symbol_part;
      if (dot_pos != string::npos && space_pos != string::npos) {
         symbol_part = eosio::chain::to_string(space_pos - dot_pos - 1);
         symbol_part += ',';
         symbol_part += s.substr(space_pos + 1);
      }
      result.sym = symbol::from_string(symbol_part);
      if (dot_pos != string::npos) {
         auto fractpart = "1" + s.substr(dot_pos + 1, space_pos - dot_pos - 1);

         result.amount *= int64_t(result.precision());
         result.amount += int64_t(fc::to_int64(fractpart));
         result.amount -= int64_t(result.precision());
      }

      return result;
   }
   FC_CAPTURE_LOG_AND_RETHROW( (from) )
}

} }  // eosio::types


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

This file contains the implementation of the edit distance and hamming distance metrics.
*/

#pragma once

#include "core.h"
#include "utils.hpp"
#include <boost/utility/string_ref.hpp>
#include <cassert>
#include <cstdint>
#include <iostream>
#include <string>
#include <x86intrin.h>

namespace campers {

// [1] Helpful description: Section 3.3 in http://www.cs.uta.fi/~helmu/pubs/spire03a.pdf
// [2] Other helpful description: http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf, Figure 5
// [3] http://apps.topcoder.com/forums/;jsessionid=2E79B293F6EA2F509795A6C303F74CAA?module=Thread&threadID=639500&start=0&mc=8#1095148

/// Custom levenshtein distance. This is essentially the algorithm given in [1], Figure 5 which
/// was originally developed by Myeres et. al (G. Myers. A fast bit-vector algorithm for approximate
/// string matching based on dynamic progamming. Journal of the ACM, 46(3): 395-415, 1999.).
/// An example implementation is given at [2]. We found [3] helpful in understanding the original
/// paper better.
///
/// We improved the algorithm by replacing the inner loop with sse instructions effectively
/// removing it
///
/// [1] http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf
/// [2] http://apps.topcoder.com/forums/?module=Thread&threadID=639500&start=0&mc=8#1095148
/// [3] http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf (especially Figure 5)
static inline uint64_t similarity_levenshtein(boost::string_ref a,boost::string_ref b)
{
    unsigned qlen=a.length();
    unsigned blen=b.length();
    const char* qptr=a.data();
    const char* bptr=b.data();
    if (qlen>blen) {
        std::swap(qlen,blen);
        std::swap(qptr,bptr);
    }
    __m128i brep1,brep2;
    uint64_t bmask;
    if (blen<=16) {
        brep1=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr));
        bmask=(1<<blen)-1;
    } else {
        brep1=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr));
        brep2=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr)+1);
        bmask=(blen==32)?0xFFFFFFFF:((1<<blen)-1);
    }

    uint64_t dmj=blen;
    uint64_t vp = (uint64_t)-1;
    uint64_t vn = 0;
    uint64_t hp = 0;
    uint64_t hn = 0;
    for (unsigned j = 0; j < qlen; j++) {
        uint64_t pm;
        __m128i c=_mm_set1_epi8(qptr[j]);
        if (blen<=16) {
            pm=_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep1))&bmask;
        } else {
            pm=(_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep1))|(_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep2))<<16))&bmask;
        }
        uint64_t d = ((((pm & vp) + vp)) ^ vp) | pm | vn;
        hp = (vn | ~(d | vp));
        uint64_t hpw = (hp << 1) | 1;
        hn = d & vp;

        int m=blen-1;
        if (hp&(1<<m)) ++dmj;
        if (hn&(1<<m)) --dmj;

        vp = (hn << 1) | ~(d | hpw);
        vn = d & hpw;
    }
    return dmj;
}

/// Hamming distance using sse. Both words must be padded and aligned to 16 bytes and loaded
/// into sse registers a and b. Then, we compute the xor of the two registers and use saturation
/// arithmetics to propagte differences from the differing bit to the while byte.
static inline unsigned similarity_hamming(__m128i a,__m128i b)
{
    __m128i mask=_mm_set1_epi8(254);
    union { __m128i a; uint64_t b[2]; } x;
    x.a=_mm_adds_epu8(_mm_xor_si128(a,b),mask);
    return (_mm_popcnt_u64(x.b[0])+_mm_popcnt_u64(x.b[1]))-(128-16);
}

}


#include <boost/test/unit_test.hpp>

#include "Werk/Utility/Units.hpp"

BOOST_AUTO_TEST_SUITE(UnitsTest) //haha

BOOST_AUTO_TEST_CASE(TestBasic)
{
	BOOST_REQUIRE_EQUAL(Werk::parseUnits("124", Werk::STORAGE_UNITS), 124);
	BOOST_REQUIRE_EQUAL(Werk::parseUnits("723", Werk::STORAGE_UNITS), 723);

	BOOST_REQUIRE_EQUAL(Werk::parseUnits("1.5K", Werk::STORAGE_UNITS), 1024 + 512);
	BOOST_REQUIRE_EQUAL(Werk::parseUnits("4M", Werk::STORAGE_UNITS), 4 * 1024 * 1024);

	BOOST_REQUIRE_EQUAL(Werk::parseUnits("10us", Werk::TIME_UNITS), 10000);
	BOOST_REQUIRE_EQUAL(Werk::parseUnits("123.456ms", Werk::TIME_UNITS), 123456000);
}

BOOST_AUTO_TEST_SUITE_END()


#include <fstream>
#include <vector>
#include <Eigen/Dense>

int loadData(Eigen::MatrixXd &mat, std::string filename, size_t row, size_t col)
{

	std::ifstream input;
	input.open(filename, std::fstream::in | std::fstream::binary);
	if (input.is_open())
	{
		std::vector<unsigned char> vec((std::istreambuf_iterator<char>(input)),
				std::istreambuf_iterator<char>() );
                mat= Eigen::Map<Eigen::Matrix<unsigned char, Eigen::Dynamic,Eigen::Dynamic> >(vec.data(),row,col).template cast<double>();
		input.close();
		return 1;
	}
	return 0;

}
int storeData(Eigen::MatrixXd &mat, std::string filename)
{
	Eigen::Matrix<char, Eigen::Dynamic, Eigen::Dynamic> pic= mat.template cast<char>();
	std::ofstream output;
	output.open(filename,std::fstream::trunc);
	if (output.is_open())
	{
		output.write(pic.data(),pic.size());
		output.close();
		return 1;
	}
	return 0;

}


#define BOOST_TEST_MODULE Tests

#ifdef BOOST_TEST_DYN_LINK
#    include <boost/test/unit_test.hpp>
#else
#    include <boost/test/included/unit_test.hpp>
#endif

#include <boost/test/tools/output_test_stream.hpp>

boost::test_tools::output_test_stream output;

#define CHECK_OUTPUT(msg) BOOST_CHECK(output.is_equal(msg))

class coutRedirect {
    std::streambuf* old;
  public:
    coutRedirect(std::streambuf* buf) : old(std::cout.rdbuf(buf)) {}
    ~coutRedirect() { std::cout.rdbuf(old); }
};

#include "Array.hpp"
#include "AVL.hpp"
#include "util.hpp"

BOOST_AUTO_TEST_SUITE(InsertionSort)

BOOST_AUTO_TEST_CASE(test1)
{
    Array<int, 5> x = { 4, 2, 4, 5, 1 };
    x.sort("insertion");
    output << x;
    CHECK_OUTPUT("1 2 4 4 5");
}

BOOST_AUTO_TEST_CASE(test2)
{
    Array<int, 100> a(2, 99);
    a.sort("insertion");
    for (std::size_t i = 0; i < a.size-1; ++i) {
        BOOST_CHECK_LE(a[i], a[i+1]);
    }
}

BOOST_AUTO_TEST_CASE(test3)
{
    Array<double, 10> x{ 10.2, 8.7, 8.3, 7.1, 6.9, 5.1, 4.3, 3.9, 2, 1.111 };
    x.sort("insertion");
    BOOST_CHECK_EQUAL(x[0], 1.111);
    BOOST_CHECK_EQUAL(x[1], 2);
    BOOST_CHECK_EQUAL(x[2], 3.9);
    BOOST_CHECK_EQUAL(x[3], 4.3);
    BOOST_CHECK_EQUAL(x[4], 5.1);
    BOOST_CHECK_EQUAL(x[5], 6.9);
    BOOST_CHECK_EQUAL(x[6], 7.1);
    BOOST_CHECK_EQUAL(x[7], 8.3);
    BOOST_CHECK_EQUAL(x[8], 8.7);
    BOOST_CHECK_EQUAL(x[9], 10.2);
}

BOOST_AUTO_TEST_CASE(test4)
{
    Array<int, 100> a(2, 99);
    a.sort("insertion");
    BOOST_CHECK(isSorted(a));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(MergeSort)

BOOST_AUTO_TEST_CASE(test1)
{
    Array<int, 5> x = { 4, 2, 4, 5, 1 };
    x.sort("merge");
    output << x;
    CHECK_OUTPUT("1 2 4 4 5");
}

BOOST_AUTO_TEST_CASE(test2)
{
    Array<int, 100> a(2, 99);
    a.sort("merge");
    for (std::size_t i = 0; i < a.size-1; ++i) {
        BOOST_CHECK_LE(a[i], a[i+1]);
    }
}

BOOST_AUTO_TEST_CASE(test3)
{
    Array<double, 10> x{ 10.2, 8.7, 8.3, 7.1, 6.9, 5.1, 4.3, 3.9, 2, 1.111 };
    x.sort("merge");
    BOOST_CHECK_EQUAL(x[0], 1.111);
    BOOST_CHECK_EQUAL(x[1], 2);
    BOOST_CHECK_EQUAL(x[2], 3.9);
    BOOST_CHECK_EQUAL(x[3], 4.3);
    BOOST_CHECK_EQUAL(x[4], 5.1);
    BOOST_CHECK_EQUAL(x[5], 6.9);
    BOOST_CHECK_EQUAL(x[6], 7.1);
    BOOST_CHECK_EQUAL(x[7], 8.3);
    BOOST_CHECK_EQUAL(x[8], 8.7);
    BOOST_CHECK_EQUAL(x[9], 10.2);
}

BOOST_AUTO_TEST_CASE(test4)
{
    Array<int, 100> a(2, 99);
    a.sort("merge");
    BOOST_CHECK(isSorted(a));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(HeapSort)

BOOST_AUTO_TEST_CASE(test1)
{
    Array<int, 5> x = { 4, 2, 4, 5, 1 };
    x.sort("heap");
    output << x;
    CHECK_OUTPUT("1 2 4 4 5");
}

BOOST_AUTO_TEST_CASE(test2)
{
    Array<int, 100> a(2, 99);
    a.sort("heap");
    for (std::size_t i = 0; i < a.size-1; ++i) {
        BOOST_CHECK_LE(a[i], a[i+1]);
    }
}

BOOST_AUTO_TEST_CASE(test3)
{
    Array<double, 10> x{ 10.2, 8.7, 8.3, 7.1, 6.9, 5.1, 4.3, 3.9, 2, 1.111 };
    x.sort("heap");
    BOOST_CHECK_EQUAL(x[0], 1.111);
    BOOST_CHECK_EQUAL(x[1], 2);
    BOOST_CHECK_EQUAL(x[2], 3.9);
    BOOST_CHECK_EQUAL(x[3], 4.3);
    BOOST_CHECK_EQUAL(x[4], 5.1);
    BOOST_CHECK_EQUAL(x[5], 6.9);
    BOOST_CHECK_EQUAL(x[6], 7.1);
    BOOST_CHECK_EQUAL(x[7], 8.3);
    BOOST_CHECK_EQUAL(x[8], 8.7);
    BOOST_CHECK_EQUAL(x[9], 10.2);
}

BOOST_AUTO_TEST_CASE(test4)
{
    Array<int, 100> a(2, 99);
    a.sort("heap");
    BOOST_CHECK(isSorted(a));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(AVL_tree)

BOOST_AUTO_TEST_CASE(test1)
{
    coutRedirect guard(output.rdbuf());
    AVL<int> x;
    x.insert(10);
    x.insert(20);
    x.insert(30);
    x.insert(40);
    x.insert(50);
    x.insert(25);
    x.print("pre");
    CHECK_OUTPUT("30 20 10 25 40 50 \n");
}

BOOST_AUTO_TEST_CASE(test2)
{
    coutRedirect guard(output.rdbuf());
    AVL<int> x;
    x.insert(9);
    x.insert(5);
    x.insert(10);
    x.insert(0);
    x.insert(6);
    x.insert(11);
    x.insert(-1);
    x.insert(1);
    x.insert(2);
    x.print("pre");
    CHECK_OUTPUT("9 1 0 -1 5 2 6 10 11 \n");
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         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_ARITHMETIC_FUNCTIONS_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_AVERAGE_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  average generic tag

      Represents the average function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct average_ : ext::elementwise_<average_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<average_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_average_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::average_, Site> dispatching_average_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::average_, Site>();
    }
    template<class... Args>
    struct impl_average_;
  }
  /*!
    Computes the arithmetic mean of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = average(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = (x+y)/T(2);
    @endcode

    for integer types  it returns a rounded value at a distance guaranteed
    less or equal to 0.5 of the average floating value,  but can differ
    of one unity from the truncation given by (x1+x2)/T(2).

    @par Note:

    This function does not overflow.

    @see  @funcref{meanof}
    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::average_, average, 2)
} }

#endif


///////////////////////////////////////////////////////////////////////////////
// example::iterator::range_cycle.cpp                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/assign/std/vector.hpp>
#include <boost/range.hpp>
#include <vector>
#include <boost/iterator/cycle_iterator_ext.hpp>
#include <boost/iterator/range_cycle.hpp>
#include <libs/iterator/example/range_cycle.h>

void example_range_cycle(std::ostream& out){
    out << "->example_iterator_cycle_range : ";

    using namespace boost;

    typedef unsigned                                    val_;
    typedef std::vector<val_>                           vals_;
    typedef range_size<vals_>::type                     size_;

    typedef boost::range_cycle<>                        range_cycle_;
    typedef range_cycle_::apply<vals_>::type            cycle_;
    const size_ n = 5;
    const size_ k = 2;

    vals_ vals;
    {
        using namespace assign;
        for(unsigned i = 0; i<k; i++){
            vals.push_back(i);
        }
    }

    cycle_ cycle = range_cycle_::make(vals,0,n);
    BOOST_ASSERT( !cycle.is_singular() );
    BOOST_ASSERT(size( cycle ) == n);
    for(unsigned i = 0; i<n; i++){
        BOOST_ASSERT(
         *next(boost::begin(cycle),i) == (i%k)
        );
    }
    cycle_ cycle2;
    BOOST_ASSERT( cycle2.is_singular() );
    cycle2 = cycle;
    for(unsigned i = 0; i<n; i++){
        BOOST_ASSERT(
         *next(boost::begin(cycle2),i) == (i%k)
        );
    }

//    copy(
//        boost::begin(cycle2),
//        boost::end(cycle2),
//        std::ostream_iterator<val_>(out," ")
//    );

    out << "<-" << std::endl;
}


#ifndef MYFRAMEMAIN_HPP
#define MYFRAMEMAIN_HPP

#include "mna.h"
#include <mutex>
#include <future>

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <wx/rawbmp.h>

using fl = long double; //boost::multiprecision::cpp_bin_float_100;

class MyFrameMain :
    public FrameMain
{
    wxBitmap m_bitmap;
    bool in_on_timer = false;
    void m_timerUpdateScreenOnTimer(wxTimerEvent& evnt) override;
    void FrameMainOnPaint(wxPaintEvent& evnt) override;
    std::mutex drawing_lock;
    std::future<void> main_loop_thread;
    volatile bool stop_now = false;
    volatile bool changed_dimensions = false;
    static void separate_thread_to_compute_rows(wxNativePixelData& data, wxNativePixelData::Iterator pixel, const unsigned y_start, const unsigned rows_to_compute, MyFrameMain& this_ref, wxSize frame_size);
    static void main_loop(MyFrameMain& this_ref);
    void FrameMainOnLeftDClick(wxMouseEvent& evnt) override;
    boost::multiprecision::cpp_bin_float_100 m_magnification = 1;
    fl m_zoom = 0.5;
    fl m_start_x_mandel = -2;
    fl m_start_y_mandel = -1;
    fl m_len_x_mandel = 3;
    fl m_len_y_mandel = 2;
public:
    void start_main_loop();
    MyFrameMain(wxWindow *const parent, const wxWindowID id, const wxString& title);
    ~MyFrameMain() override;
};

#endif


//
// Created by ziqwang on 2020-02-03.
//

#include <catch2/catch.hpp>
#include "Interlocking/InterlockingSolver_Clp.h"
#include "IO/JsonIOReader.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/SparseQR>

TEST_CASE("Bunny Example Clp")
{
    vector<shared_ptr<PolyMesh<double>>> meshList;
    vector<bool> atboundary;
    shared_ptr<InputVarList> varList = make_shared<InputVarList>();
    InitVar(varList.get());

    //Read all Parts
    for(int id = 1; id <= 80; id++){
        char number[50];
        sprintf(number, "%d.obj", id);
        std::string part_filename = "data/Voxel/bunny/part_";
        part_filename += number;
        shared_ptr<PolyMesh<double>> polyMesh = make_shared<PolyMesh<double>>(varList);
        polyMesh->readOBJModel(part_filename.c_str(), false);

        meshList.push_back(polyMesh);
        atboundary.push_back(false);
    }

    SECTION("fix key and second part"){
        // set the first and second part to be on the boundires
        // then the structure is interlocking
        atboundary[0] = true;
        atboundary[1] = true;

        // construct the contact graph
        shared_ptr<ContactGraph<double>>graph = make_shared<ContactGraph<double>>(varList);
        graph->buildFromMeshes(meshList, atboundary, 1e-3);

        // solve the interlocking problem by using CLP library
        InterlockingSolver_Clp<double> solver(graph, varList);
        shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
        REQUIRE(solver.isRotationalInterlocking(interlockData) == true);
    }

    SECTION("fix key"){
        // if only set the key to be fixed
        // the reset parts could move together, therefore the structure is not interlocking
        atboundary[0] = true;

        // construct the contact graph
        shared_ptr<ContactGraph<double>>graph = make_shared<ContactGraph<double>>(varList);
        graph->buildFromMeshes(meshList, atboundary);

        // solve the interlocking problem by using CLP library
        InterlockingSolver_Clp<double> solver(graph, varList);
        shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
        REQUIRE(solver.isRotationalInterlocking(interlockData) == false);
    }

    SECTION("fix key and merge key and second part"){
        // if only set the key to be fixed
        // the reset parts could move together, therefore the structure is not interlocking
        atboundary[0] = true;

        // construct the contact graph
        shared_ptr<ContactGraph<double>>graph = make_shared<ContactGraph<double>>(varList);
        graph->buildFromMeshes(meshList, atboundary);

        //merge
        graph->mergeNode(graph->nodes[0], graph->nodes[1]);

        // solve the interlocking problem by using CLP library
        InterlockingSolver_Clp<double> solver(graph, varList);
        shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
        REQUIRE(solver.isRotationalInterlocking(interlockData) == true);
    }
}

TEST_CASE("Ania Example Clp"){
    std::string file_name[5] = { "piece4_tri.obj", "piece0.obj", "piece1.obj", "piece3.obj", "piece2.obj"};

    vector<shared_ptr<PolyMesh<double>>> meshList;
    vector<bool> atboundary;
    shared_ptr<InputVarList> varList = make_shared<InputVarList>();
    InitVar(varList.get());

    for(int id = 0; id < 5; id++){
        char number[50];
        std::string part_filename = "data/Mesh/Ania_200127_betweenbars/";
        part_filename += file_name[id];
        shared_ptr<PolyMesh<double>> polyMesh = make_shared<PolyMesh<double>>(varList);
        polyMesh->readOBJModel(part_filename.c_str(), false);
        meshList.push_back(polyMesh);
        atboundary.push_back(true);
    }

    atboundary[0] = false;

    shared_ptr<ContactGraph<double>>graph = make_shared<ContactGraph<double>>(varList);
    graph->buildFromMeshes(meshList, atboundary, 1e-3);

//    SECTION("Simplex Method"){
//        // solve the interlocking problem by using CLP library
//        InterlockingSolver_Clp<double> solver(graph, varList, SIMPLEX);
//        shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
//        REQUIRE(solver.isRotationalInterlocking(interlockData) == false);
//    }

    SECTION("Barrier Method"){
        // solve the interlocking problem by using CLP library
        InterlockingSolver_Clp<double> solver(graph, varList, BARRIER);
        shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
        REQUIRE(solver.isRotationalInterlocking(interlockData) == false);
    }

}

TEST_CASE("Ania Example: Special Case"){
    //Read all Parts

    std::string file_name[3] = {"piece0.obj", "piece1.obj", "piece4_tri.obj"};
    vector<shared_ptr<PolyMesh<double>>> meshList;
    vector<bool> atboundary;
    shared_ptr<InputVarList> varList = make_shared<InputVarList>();
    InitVar(varList.get());
    bool textureModel;

    for(int id = 0; id < 3; id++){
        char number[50];
        std::string part_filename = "data/Mesh/Ania_200127_betweenbars/";
        part_filename += file_name[id];
        shared_ptr<PolyMesh<double>> polyMesh = make_shared<PolyMesh<double>>(varList);
        polyMesh->readOBJModel(part_filename.c_str(), false);
        meshList.push_back(polyMesh);
        atboundary.push_back(false);
    }

    atboundary[0] = true;
    atboundary[1] = true;

    shared_ptr<ContactGraph<double>>graph = make_shared<ContactGraph<double>>(varList);
    graph->buildFromMeshes(meshList, atboundary, 1e-3);

    // solve the interlocking problem by using CLP library
    InterlockingSolver_Clp<double> solver(graph, varList);
    shared_ptr<typename InterlockingSolver<double>::InterlockingData> interlockData;
    REQUIRE(solver.isRotationalInterlocking(interlockData) == false);
}