#include <Eigen/Core>

#include <vector>
#include <memory>

#include <CGAL/Boolean_set_operations_2.h>

// standard includes
#include <iostream>

#include "SDOT/PolygonRasterize.h"
#include "SDOT/RegularGrid.h"
#include "SDOT/LaguerreDiagram.h"

using namespace sdot;

int main(int argc, char* argv[])
{
  int numPts = 4;
  Eigen::VectorXd costs = Eigen::VectorXd::Ones(numPts);

  Eigen::Matrix2Xd pts(2,numPts);
  pts <<  0.680375,  0.566198,  0.823295,  0.329554,//  0.444451, 0.0452059,  0.270431,  0.904459,  0.271423,  0.716795,
          0.211234,   0.59688,  0.604897,  0.536459;//,   0.10794,  0.257742, 0.0268018,   0.83239,  0.434594,  0.213938;

  std::cout << "Points = \n";
  std::cout << "[[" << pts(0,0) << "," << pts(1,0) << "]";
  for(int i=1; i<numPts; ++i){
    std::cout << ", [" << pts(0,i) << "," << pts(1,i) << "]";
  }
  std::cout << "]" << std::endl;

  Eigen::Matrix2Xd domain(2,4);
  domain << 0.0, 1.0, 1.0, 0.0,
            0.0, 0.0, 1.0, 1.0;
  // domain << 0.0, 1.0, 1.0, 0.0,
  //           0.0, 0.0, 1.0, 1.0;

  LaguerreDiagram diag(domain(0,0), domain(0,1), domain(1,0), domain(1,2), pts, costs);

  auto grid = std::make_shared<RegularGrid>(domain(0,0),domain(1,0), domain(0,2), domain(1,2), 10, 10);

  double area = 0.0;
  Eigen::VectorXd localAreas = Eigen::VectorXd::Zero(numPts);
  Eigen::MatrixXd cellAreas = Eigen::MatrixXd::Zero(grid->NumCells(0), grid->NumCells(1));

  for(int polyInd=0; polyInd<numPts; ++polyInd){
    std::cout << "\n\n==================================\n";
    std::cout << "Polygon " << polyInd << std::endl;
    std::shared_ptr<PolygonRasterizeIter::Polygon_2> poly = diag.GetCell(polyInd)->ToCGAL();

    auto vertIt = poly->vertices_begin();
    std::cout << "[[" << vertIt->x() << "," << vertIt->y() << "]";
    vertIt++;
    for(;  vertIt != poly->vertices_end(); ++vertIt){
      std::cout << ", [" << vertIt->x() << "," << vertIt->y() << "]";
    }
    std::cout << "]" << std::endl;

    PolygonRasterizeIter gridIter(grid,poly);

    //
    // unsigned int oldYInd = gridIter.Indices().second;
    // std::cout << "yind = " << oldYInd << std::endl;
    // std::cout << "yval = " << grid->yMin + oldYInd*grid->dy << std::endl;
    // std::cout << "    ";

    while(gridIter.IsValid()){

      // if(gridIter.Indices().second != oldYInd){
      //   oldYInd = gridIter.Indices().second;
      //   std::cout << "\nyind = " << oldYInd << std::endl;
      //   std::cout << "yval = " << grid->yMin + oldYInd*grid->dy << std::endl;
      //   std::cout << "    ";
      // }

      double cellArea;

      if(gridIter.IsBoundary()){

        cellArea = CGAL::to_double( gridIter.OverlapPoly()->area() );
        //std::cout << "  " << gridIter.Indices().first << ", " << gridIter.Indices().second << " -> Overlap poly = " << *gridIter.OverlapPoly() << std::endl;
        // unsigned int indX = gridIter.Indices().first;
        // unsigned int indY = gridIter.Indices().second;
        //
        // PolygonRasterizeIter::Polygon_2 tempPoly;
        // tempPoly.push_back(PolygonRasterizeIter::Point_2(grid->xMin + indX*grid->dx, grid->yMin+indY*grid->dy));
        // tempPoly.push_back(PolygonRasterizeIter::Point_2(grid->xMin + (indX+1)*grid->dx, grid->yMin+indY*grid->dy));
        // tempPoly.push_back(PolygonRasterizeIter::Point_2(grid->xMin + (indX+1)*grid->dx, grid->yMin+(indY+1)*grid->dy));
        // tempPoly.push_back(PolygonRasterizeIter::Point_2(grid->xMin + indX*grid->dx, grid->yMin+(indY+1)*grid->dy));
        //
        // // Compute the intersection of P and Q.
        // std::list<PolygonRasterizeIter::Polygon_with_holes_2> interList;
        // CGAL::intersection(*poly, tempPoly, std::back_inserter(interList));
        //
        // double trueArea = CGAL::to_double( interList.begin()->outer_boundary().area() );
        //
        // //std::cout << *gridIter.OverlapPoly() << std::endl;
        // double error = cellArea-trueArea;
        // if(std::abs(error)>std::numeric_limits<double>::epsilon()){
        //   // std::cout << "  Cell area = " << cellArea << " with error " << error << std::endl;
        // }
      }else{
        cellArea = grid->dx*grid->dy;
      }

      std::cout << "    area(" << gridIter.Indices().first << "," << gridIter.Indices().second << ")= " << cellArea << std::endl;
      cellAreas(gridIter.Indices().first, gridIter.Indices().second) += cellArea;
      localAreas(polyInd) += std::abs(cellArea);
      area += std::abs(cellArea);

      gridIter.Increment();
    }

    std::cout << "  Polygon area = " << localAreas(polyInd) << std::endl;
    std::cout << "  Polygon area error = " << localAreas(polyInd) - CGAL::to_double( poly->area() ) << std::endl;
    // std::cout << "\n\n";
  }

  // Eigen::VectorXd trueAreas(3);
  // trueAreas << 0.15*0.15, (domain(0,2)-0.15)*0.15 + 0.5*(domain(1,2)-0.15)*(domain(0,2)-0.15), 0.15*(domain(1,2)-0.15) + 0.5*(domain(1,2)-0.15)*(domain(0,2)-0.15);

  // List all cells with errors
  for(unsigned int yInd=0; yInd<grid->NumCells(1); ++yInd){
    for(unsigned int xInd=0; xInd<grid->NumCells(0); ++xInd){
      if(std::abs(cellAreas(xInd,yInd) - grid->dx*grid->dy)>1e-15){
        std::cout << "Cell " << xInd << "," << yInd << " error = " << cellAreas(xInd,yInd) - grid->dx*grid->dy << std::endl;
      }
    }
  }

  std::cout << "Local areas = " << localAreas.transpose() << std::endl;

  //std::cout << "dx*dy = " << grid->dx * grid->dy << std::endl;
  std::cout << "True total area = " << grid->dx*grid->dy*grid->NumCells() << std::endl;
  std::cout << "Total Area = " << area << std::endl;
  std::cout << "Error = " << area - grid->dx*grid->dy*grid->NumCells() << std::endl;
  return 0;
}


#ifndef CALIBRATOR_PROCESSES_GENERAL_VASICEK_HPP
#define CALIBRATOR_PROCESSES_GENERAL_VASICEK_HPP

#include <boost/bind.hpp>

#include <ql/math/randomnumbers/sobolrsg.hpp>
#include <ql/models/shortrate/onefactormodel.hpp>
#include <ql/math/integrals/kronrodintegral.hpp>

#include <calibrator/global.hpp>
#include <calibrator/processes/generalornsteinuhlenbeckprocess.hpp>

namespace HJCALIBRATOR
{
	//! Time-dependent %Hull-White model class
	/*! This class implements the time-dependent Hull-White model defined by
	\f[
	dr(t) = (\theta(t) - a(t)r(t))dt + \sigma(t) dW_t ,
	\f]
	a risk premium \f$ \lambda \f$ can also be specified.

	\ingroup shortrate
	*/
	class GeneralizedHullWhite : public OneFactorAffineModel, public TermStructureConsistentModel
	{
	public:
		GeneralizedHullWhite( const Handle<YieldTermStructure>& termStructure,
							  const IntegrableParameter& a,
							  const Parameter& sigma );

		// OnFactorModel virtual override
		boost::shared_ptr<Lattice> tree( const TimeGrid& grid ) const override
		{
			return boost::shared_ptr<Lattice>();
		};
		virtual boost::shared_ptr<ShortRateDynamics> dynamics() const override;

		virtual Real discountBondOption( Option::Type type,
										 Real strike,
										 Time maturity,
										 Time bondMaturity ) const override;

		virtual Real discountBondOption( Option::Type type, Real strike,
										 Time maturity, Time bondStart,
										 Time bondMaturity ) const override;

		Parameter a() const { return a_; }
		Parameter sigma() const { return sigma_; }

		Real a( Time t ) const { return a_( t ); }
		Real sigma( Time t ) const { return sigma_( t ); }

		/*! Futures convexity bias (i.e., the difference between
		futures implied rate and forward rate) calculated as in
		G. Kirikos, D. Novak, "Convexity Conundrums", Risk
		Magazine, March 1997.
		http://www.powerfinance.com/convexity/

		\note t and T should be expressed in yearfraction using
		deposit day counter, F_quoted is futures' market price.
		*/
		// to be implemented with variable a & \sigma
		/*
		static Rate convexityBias( Real futurePrice,
								   Time t,
								   Time T,
								   Real sigma,
								   Real a );
		*/

		static std::vector<bool> FixedReversion() {
			std::vector<bool> c( 2 );
			c[0] = true; c[1] = false;
			return c;
		}

	protected:
		void generateArguments();

		virtual Real A( Time t, Time T ) const;
		virtual Real B( Time t, Time T ) const;

		Parameter& a_;
		Parameter& sigma_;

	private:
		class Dynamics;
		class FittingParameter;

		boost::shared_ptr<FittingParameter> alpha_;
	};

	class GeneralizedHullWhite::FittingParameter : public TermStructureFittingParameter
	{
	private:
		class Impl : public Parameter::Impl
		{
		public:
			Impl( const Handle<YieldTermStructure>& termStructure,
				  const IntegrableParameter& a,
				  const Parameter& sigma );
			~Impl()
			{
				gsl_integration_workspace_free( int_wrkspcs_ );
			}

			Real value( const Array&, Time t ) const override;

			Real B( Time t, Time T ) const;
			Real variance( Time s, Time t ) const;

		private:
			Real E( Time t0, Time t1, Real multiplier = 1. ) const;
			Real integrand( Time u, Time t ) const;
			Real integrandVr( Time t ) const;
			Real value_integral( Time t ) const;

			gsl_integration_workspace* int_wrkspcs_;
			//gsl_function integrand_Vr_;
			//gsl_function OneOverE_;

			GaussKronrodAdaptive integrator_;
			//SimpsonIntegral integrator_;
			//boost::function<Real( Real )> OneOverEintegrand_; // Eq. 31 integrand
			//boost::function<Real( Real )> Vrintegrand_; // Eq. 37 integrand

			Handle<YieldTermStructure> termStructure_;
			IntegrableParameter a_;
			Parameter sigma_;
		};
	public:
		FittingParameter( const Handle<YieldTermStructure>& termStructure,
						  const IntegrableParameter& a,
						  const Parameter& sigma )
			: TermStructureFittingParameter( boost::shared_ptr<Parameter::Impl>( new FittingParameter::Impl( termStructure, a, sigma ) ) )
		{}

		Real B( Time t, Time T ) const
		{
			return boost::static_pointer_cast<Impl>(implementation())->B( t, T );
		}

		Real variance( Time t, Time T ) const
		{
			return boost::static_pointer_cast<Impl>(implementation())->variance( t, T );
		}
	};

	//! Short-rate dynamics in the time-dependent Hull-White model
	/*! The short-rate follows an time-dependent Hull-White process */
	class GeneralizedHullWhite::Dynamics : public OneFactorModel::ShortRateDynamics {
	public:
		Dynamics( const FittingParameter& fitting,
				  const IntegrableParameter& a,
				  const Parameter& sigma )
			: ShortRateDynamics( boost::shared_ptr<StochasticProcess1D>(
				new GeneralizedOrnsteinUhlenbeckProcess( a, sigma ) ) )
			, fitting_( fitting )
		{}

		virtual Real variable( Time t, Rate r ) const {
			return r - fitting_( t );
		}
		virtual Real shortRate( Time t, Real x ) const {
			return x + fitting_( t );
		}

		FittingParameter fitting_;
	};

	// inline definitions
	inline boost::shared_ptr<OneFactorModel::ShortRateDynamics>
		GeneralizedHullWhite::dynamics() const
	{
		return boost::shared_ptr<ShortRateDynamics>(
			new Dynamics( *alpha_, static_cast<IntegrableParameter>(a()), sigma() ) );
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::value( const Array&, Time t ) const
	{
		/*
		Rate forwardRate = termStructure_->forwardRate( t, t, Continuous, NoFrequency );
		Real Et = E( 0, t );

		boost::function<Real( Real )> I_t;
		I_t = boost::bind( &GeneralizedHullWhite::FittingParameter::Impl::integrand, this, _1, t );
		gsl_function tmpf = convertToGslFunction( I_t );

		Real result, error;
		gsl_integration_qags( &tmpf, 0, t, 0, 1e-7, 1000, int_wrkspcs_, &result, &error );

		return forwardRate;// +result;
		*/

		Rate forwardRate = termStructure_->forwardRate( t, t, Continuous, NoFrequency );

		Real intsum = value_integral( t );

		return forwardRate + intsum;
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::integrand( Time u, Time t ) const
	{
		/* eq.36 */
		Real sigma_u = sigma_( u );

		return E( 0, u ) * sigma_u * sigma_u * B( u, t );
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::integrandVr( Time t ) const
	{
		Real sigma_t = sigma_( t );
		Real E_t = E( 0, t );

		return E_t * E_t * sigma_t * sigma_t;
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::E( Time t0, Time t1, Real multiplier ) const
	{
		/* eq. 30 */
		return exp( multiplier * a_.integral( t0, t1 ) );
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::B( Time t, Time T ) const
	{
		/* eq. 31 */

		auto lambda = [&, t]( Time u )
		{
			return 1 / E( t, u );
		};

		return integrator_( lambda, t, T );
	}

	inline Real GeneralizedHullWhite::FittingParameter::Impl::variance( Time s, Time t ) const
	{
		/* eq. 37 */
		const Parameter& sigma_i = sigma_;

		auto integrand = [&, sigma_i,  t]( Time u )
		{
			Real sigma = sigma_i( u );
			Real Eut = E( u, t );
			return sigma * sigma / Eut / Eut;
		};

		return integrator_( integrand, s, t );
	}
}

#endif // !CALIBRATOR_PROCESSES_GENERAL_VASICEK_HPP


/**
 * @file expressions.hpp
 *
 */

#pragma once

#include "parameter.hpp"
#include "variable.hpp"

#include <Eigen/Sparse>

namespace Eigen
{

    template <>
    struct NumTraits<cvx::Scalar>
        : NumTraits<double>
    {
        using Real = cvx::Scalar;
        using NonInteger = cvx::Scalar;
        using Nested = cvx::Scalar;

        enum
        {
            IsComplex = 0,
            IsInteger = 0,
            IsSigned = 1,
            RequireInitialization = 1,
            ReadCost = 10,
            AddCost = 200,
            MulCost = 200,
        };
    };

} // namespace Eigen

namespace cvx
{
    class Constraint;
    class OptimizationProblem;
    class Scalar;

    namespace internal
    {
        class Affine;
        class SOCPWrapperBase;
        class QPWrapperBase;

        class Term
        {
        public:
            Term();

            Parameter parameter;
            Variable variable;

            bool operator==(const Term &other) const;
            Term &operator*=(const Parameter &param);
            Term &operator/=(const Parameter &param);

            operator Affine() const;

            friend std::ostream &operator<<(std::ostream &os, const Term &term);
            double evaluate() const;
        };

        class Affine
        {
        public:
            bool operator==(const Affine &other) const;

            Parameter constant = Parameter(0.);
            std::vector<Term> terms;

            friend std::ostream &operator<<(std::ostream &os, const Affine &affine);
            double evaluate() const;
            Affine &operator+=(const Affine &other);
            Affine &operator-=(const Affine &other);
            Affine &operator*=(const Parameter &param);
            Affine &operator/=(const Parameter &param);
            // Affine operator+(const Affine &other) const;
            Affine operator-(const Affine &other) const;
            Affine operator-() const;

            void cleanUp();

            bool isZero() const;
            bool isConstant() const;
            bool isFirstOrder() const;
        };

        class Product
        {
        public:
            explicit Product(const Affine &term);
            Product(const Affine &lhs, const Affine &rhs);
            Affine &firstTerm();
            Affine &secondTerm();
            const Affine &firstTerm() const;
            const Affine &secondTerm() const;
            void toSquaredTerm();
            double evaluate() const;
            bool isSquare() const;

            bool operator==(const Product &other) const;

            friend std::ostream &operator<<(std::ostream &os, const Product &product);

        private:
            std::vector<Affine> factors;
        };

    } // namespace internal

    class Scalar
    {
    public:
        Scalar() = default;
        explicit Scalar(int x);
        Scalar(double x);
        explicit Scalar(double *x);

        Scalar &operator+=(const Scalar &other);
        Scalar &operator-=(const Scalar &other);
        Scalar &operator*=(const Scalar &other);
        Scalar &operator/=(const Scalar &other);
        Scalar operator-() const;
        friend Scalar operator+(const Scalar &lhs, const Scalar &rhs);
        friend Scalar operator-(const Scalar &lhs, const Scalar &rhs);
        friend Scalar operator*(const Scalar &lhs, const Scalar &rhs);
        friend Scalar operator/(const Scalar &lhs, const Scalar &rhs);

        bool operator==(const cvx::Scalar &other) const;

        double evaluate() const;
        size_t getOrder() const;
        bool isNorm() const;

        friend OptimizationProblem;
        friend internal::SOCPWrapperBase;
        friend internal::QPWrapperBase;

        explicit operator double() const;

        friend Scalar sqrt(const Scalar &scalar);
        friend Scalar square(const Scalar &scalar);
        friend Scalar abs2(const Scalar &scalar);

        // friend internal::Parameter::operator Scalar() const;
        friend internal::Variable::operator Scalar() const;

        friend Constraint equalTo(const Scalar &lhs, const Scalar &rhs);
        friend Constraint lessThan(const Scalar &lhs, const Scalar &rhs);
        friend Constraint greaterThan(const Scalar &lhs, const Scalar &rhs);
        friend Constraint box(const Scalar &lower, const Scalar &middle, const Scalar &upper);

    private:
        internal::Affine affine;
        std::vector<internal::Product> products;
        bool norm = false;

        friend std::ostream &operator<<(std::ostream &os, const Scalar &scalar);
    };

    using MatrixX = Eigen::Matrix<cvx::Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorX = Eigen::Matrix<cvx::Scalar, Eigen::Dynamic, 1>;

    /**
     * @brief Creates a constant parameter.
     *
     * @param p The value of the parameter
     * @return Scalar The constant parameter
     */
    Scalar par(double p);

    /**
     * @brief Creates a dynamic parameter.
     *
     * @details Internally stores pointers to the original values.
     *
     * @warning Do not delete the source before the parameter is no longer required.
     *
     * @param p The value of the parameter
     * @return Scalar The constant parameter
     */
    Scalar dynpar(double &p);

    /**
     * @brief Creates a constant parameter from a dense Eigen type.
     *
     * @tparam Derived
     * @param m A dense Eigen type containing problem parameters
     * @return auto A dense Eigen type with cvx::Scalar as scalar type
     */
    template <typename Derived>
    inline auto par(const Eigen::MatrixBase<Derived> &m)
    {
        return m.template cast<Scalar>().eval();
    }

    /**
     * @brief Creates a constant parameter from a sparse Eigen type.
     *
     * @tparam Derived
     * @param m A sparse Eigen type containing problem parameters
     * @return auto A sparse Eigen type with cvx::Scalar as scalar type
     */
    template <typename Derived>
    inline auto par(const Eigen::SparseMatrixBase<Derived> &m)
    {
        return m.template cast<Scalar>().eval();
    }

    /**
     * @brief Creates a dynamic parameter from a dense Eigen type.
     *
     * @details Internally stores pointers to the original values.
     *
     * @warning Do not delete the source before the parameter is no longer required.
     *
     * @tparam Derived
     * @param m A dense Eigen type containing problem parameters
     * @return auto A dense Eigen type with cvx::Scalar as scalar type
     */
    template <typename Derived>
    auto dynpar(Eigen::MatrixBase<Derived> &m)
    {
        auto result = m.template cast<Scalar>().eval();

        for (int row = 0; row < m.rows(); row++)
        {
            for (int col = 0; col < m.cols(); col++)
            {
                result.coeffRef(row, col) = dynpar(m.coeffRef(row, col));
            }
        }

        return result;
    }

    /**
     * @brief Creates a dynamic parameter from a sparse Eigen type.
     *
     * @details Internally stores pointers to the original values.
     *
     * @warning Do not delete the source before the parameter is no longer required.
     *
     * @tparam T
     * @param m A sparse Eigen type containing problem parameters
     * @return auto A sparse Eigen type with cvx::Scalar as scalar type
     */
    template <typename T>
    auto dynpar(Eigen::SparseMatrix<T> &m)
    {
        auto result = m.template cast<Scalar>().eval();

        for (int k = 0; k < result.nonZeros(); k++)
        {
            result.valuePtr()[k] = dynpar(m.valuePtr()[k]);
        }

        return result;
    }

    /**
     * @brief Evaluates the scalar.
     *
     * @param s The scalar to be evaluated
     * @return double The value of the scalar
     */
    double eval(const Scalar &s);

    /**
     * @brief Evaluates a dense Eigen type
     *
     * @tparam Derived Has to be cvx::Scalar
     * @param m A dense Eigen type to be evaluated
     * @return auto The evaluated dense Eigen type
     */
    template <typename Derived>
    inline auto eval(const Eigen::MatrixBase<Derived> &m)
    {
        return m.template cast<double>();
    }

    /**
     * @brief Evaluates a sparse Eigen type
     *
     * @tparam Derived Has to be cvx::Scalar
     * @param m A sparse Eigen type to be evaluated
     * @return auto The evaluated sparse Eigen type
     */
    template <typename Derived>
    inline auto eval(const Eigen::SparseMatrixBase<Derived> &m)
    {
        return m.template cast<double>();
    }

    inline const Scalar &conj(const Scalar &x) { return x; }
    inline const Scalar &real(const Scalar &x) { return x; }
    inline Scalar imag(const Scalar &) { return Scalar(0.); }
    inline Scalar square(const Scalar &x)
    {
        Scalar new_scalar;
        new_scalar.products = {internal::Product(x.affine)};
        return new_scalar;
    }
    inline Scalar abs2(const Scalar &x)
    {
        return square(x);
    }

} // namespace cvx


//Copyright (c) 2014 - 2020, The Trustees of Indiana University.
//
//Licensed under the Apache License, Version 2.0 (the "License");
//you may not use this file except in compliance with the License.
//You may obtain a copy of the License at
//
//    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.

#include <cmath>

#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/normal.hpp>

#include "common/util/logger.hpp"
#include "ms/spec/peak.hpp"
#include "ms/spec/env_peak.hpp"
#include "ms/spec/raw_ms_util.hpp"
#include "ms/feature/peak_cluster.hpp"

namespace toppic {

int PeakCluster::even_charge_idx_ = 0;
int PeakCluster::odd_charge_idx_ = 1;
double PeakCluster::win_size_ = 6.0;

double getBcDistance(std::vector<double> &v1, std::vector<double> &v2) {
  if (v1.size() != v2.size() || v1.size() == 0) {
    LOG_ERROR("Two vectors have different sizes!");
    exit(EXIT_FAILURE);
  }

  double s1 = 0.0;
  double s2 = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    s1 += v1[i];
    s2 += v2[i];
  }

  if (!(s1 > 0) || !(s2 > 0)) return 10.0;

  double bc = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    double p = v1[i]/ s1;
    double q = v2[i]/ s2;
    bc += std::sqrt(p * q);
  }
  if (bc > std::exp(-10)) {
    return -std::log(bc);
  }
  else {
    return 10;
  }
}

double getPearsonCorr(std::vector<double> &v1, std::vector<double> &v2) {
  if (v1.size() != v2.size() || v1.size() == 0) {
    LOG_ERROR("Two vectors have different sizes or empty vector!");
    exit(EXIT_FAILURE);
  }
  if (v1.size() == 1) {
    return 1.0;
  }
  // Compute means
  double m1 = 0.0;
  double m2 = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    m1 += v1[i];
    m2 += v2[i];
  }
  m1 /= v1.size();
  m2 /= v2.size();

  // compute Pearson correlation
  double cov = 0.0;
  double s1 = 0.0;
  double s2 = 0.0;

  for (size_t i = 0; i < v1.size(); i++) {
    double d1 = v1[i] - m1;
    double d2 = v2[i] - m2;
    cov += d1 * d2;
    s1 += d1 * d1;
    s2 += d2 * d2;
  }

  if (s1 <= 0 || s2 <= 0) return 0;

  return cov < 0 ? 0 : cov / std::sqrt(s1 * s2);
}

double getPoissonPValue(PeakPtrVec &win_peaks, double win_size,
                        std::vector<double> &env_peak_intensities) {
  double win_high_inte = raw_ms_util::getHighestPeakInte(win_peaks);
  double inte_thresh = win_high_inte  * 0.1;
  int match_peak_num = 0;
  for (size_t i = 0; i < env_peak_intensities.size(); i++) {
    if (env_peak_intensities[i] > inte_thresh) {
      match_peak_num++;
    }
  }
  int inte_peak_num = 0;
  for (size_t i = 0; i < win_peaks.size(); i++) {
    if (win_peaks[i]->getIntensity() > inte_thresh) {
      inte_peak_num++;
    }
  }
  int possible_peak_num = std::ceil(win_size * 100);

  double n = possible_peak_num;
  double k = env_peak_intensities.size();
  double n1 = inte_peak_num;
  double k1 = match_peak_num;
  double lambda = n1 / n * k;

  boost::math::poisson_distribution<> pd(lambda);

  double pvalue = 1.0 - boost::math::cdf(pd, k1);
  return pvalue;
}

double compRankSumPValue(double n1, double n2, double r1) {
  double u1 = n1 * n2 + n1 * (n1 + 1) * 0.5 - r1;

  double mean_u = 0.5 * (n1 * n2);
  double log_sig_u = 0.5 * (std::log(n1) + std::log(n2) + std::log(n1 + n2 + 1) - std::log(12));
  double sig_u = std::exp(log_sig_u);
  LOG_DEBUG("log sig u " << log_sig_u << " sig u " << sig_u << " n1 " << n1 << " n2 " << n2 << " r1 " << r1);
  // all peaks are matched
  if (sig_u == 0.0) {
    return 0.0;
  }
  boost::math::normal_distribution<> nd(mean_u, sig_u);
  double p_value = boost::math::cdf(nd, u1);
  p_value = std::min(p_value, 1-p_value);
  return std::abs(p_value);
}

double getRankSumPValue(PeakPtrVec win_peaks, std::vector<double> &env_peak_intensities) {
  int rank_sum = 0;
  int match_peak_num = 0;

  std::sort(win_peaks.begin(), win_peaks.end(), Peak::cmpInteDec);

  for (size_t i = 0; i < env_peak_intensities.size(); i++) {
    double peak_inte = env_peak_intensities[i];
    if (peak_inte > 0.0) {
      int rank = win_peaks.size();
      for (size_t j = 0; j < win_peaks.size(); j++) {
        if (peak_inte >= win_peaks[j]->getIntensity()) {
          rank = j+1;
          break;
        }
      }
      rank_sum += rank;
      match_peak_num++;
    }
  }
  int peak_num = win_peaks.size();
  double pvalue = compRankSumPValue(peak_num, match_peak_num, rank_sum);
  return pvalue;
}

PeakCluster::PeakCluster(EnvelopePtr theo_env) {
  theo_env_ = theo_env;
  rep_mass_ = theo_env_->getMonoNeutralMass();
  rep_charge_ = theo_env_->getCharge();

  int peak_num = theo_env_->getPeakNum();
  rep_summed_intensities_.resize(peak_num, 0.0);

  clearScores();

  flag_ = 0;
  init_score_ = false;
  smoother_ = std::make_shared<SavitzkyGolay>(9, 2);
}

void PeakCluster::addEnvelopes(FracFeaturePtr feature_ptr,
                               RealEnvPtrVec envs) {

  int row_num = feature_ptr->getMaxCharge() - feature_ptr->getMinCharge() + 1;
  int col_num = feature_ptr->getMaxMs1Id() - feature_ptr->getMinMs1Id() + 1;

  min_charge_ = feature_ptr->getMinCharge();
  max_charge_ = feature_ptr->getMaxCharge();

  min_ms1_id_ = feature_ptr->getMinMs1Id();
  max_ms1_id_ = feature_ptr->getMaxMs1Id();

  scan_begin_ = feature_ptr->getScanBegin();
  scan_end_ = feature_ptr->getScanEnd();
  LOG_DEBUG("add env row " << row_num << " col " << col_num);

  real_envs_.resize(row_num);

  for (int i = 0; i < row_num; i++) {
    real_envs_[i].resize(col_num);
  }

  for (size_t i = 0; i < envs.size(); i++) {
    int row = envs[i]->getCharge() - min_charge_;
    int col = envs[i]->getSpId() - min_ms1_id_;
    if (row >= 0 && row < row_num && col >= 0 && col < col_num) {
      real_envs_[row][col] = envs[i];
    }
  }
}

void PeakCluster::clearScores() {
  inte_distr_.resize(2, 0.0);
  best_corr_scores_.resize(2, 0.0);
  best_inte_scores_.resize(2, 0.0);
  best_dist_scores_.resize(2, 1.0);

  best_charges_.resize(2, 0);
  sum_dist_scores_.resize(2, 1.0);
  sum_corr_scores_.resize(2, 0.0);
  sum_inte_scores_.resize(2, 0.0);
  xic_corr_between_best_charges_.resize(2, 0.0);
}

void PeakCluster::updateScore(PeakPtrVec2D &raw_peaks, bool check_pvalue) {
  int row_num = max_charge_ - min_charge_ + 1;
  int col_num = max_ms1_id_ - min_ms1_id_ + 1;
  int ref_idx = theo_env_->getReferIdx();

  clearScores();

  std::vector<double> best_charge_dists{10.0, 10.0};

  // sum up peak intensities
  int peak_num = theo_env_->getPeakNum();
  std::vector<double> theo_intensities = theo_env_->getIntensities();
  std::vector<double> summed_intensities(peak_num, 0);

  int xic_len = col_num + 18;
  int xic_start_idx = 9;

  std::vector<std::vector<double>> xic2(2);
  xic2[0].resize(xic_len, 0.0);
  xic2[1].resize(xic_len, 0.0);

  std::vector<std::vector<double>> charge_xic(row_num);

  double tmp_best_bc_dist = 10.0;
  double rep_env_bc_dist = 10.0;
  RealEnvPtr rep_env(nullptr);

  double rep_env_bc_dist_2 = 10.0;
  RealEnvPtr rep_env_2(nullptr);

  std::vector<double> tmp_best_dist_scores{10.0, 10.0};
  std::vector<double> tmp_best_inte_scores(2, 0.0);
  std::vector<double> tmp_best_corr_scores(2, 0.0);

  for (int i = 0; i < row_num; i++) {
    int charge = i + min_charge_;
    double ref_neutral_mass = theo_env_->getRefNeutralMass();
    double ref_mz = Peak::compMz(ref_neutral_mass, charge);
    std::fill(summed_intensities.begin(), summed_intensities.end(), 0.0);

    charge_xic[i].resize(xic_len, 0.0);

    int charge_idx = (charge % 2 == 0) ? even_charge_idx_:odd_charge_idx_;
    // summed_most_abu_isotope_intensity
    double summed_iso_high_inte = 0.0;
    //summed_referenc_intensity
    double summed_win_high_inte = 0.0;

    for (int j = 0; j < col_num; j++) {
      RealEnvPtr env = real_envs_[i][j];
      if (env == nullptr) continue;

      // sum peak inte
      for (int k = 0; k < peak_num; k++) {
        summed_intensities[k] += env->getIntensity(k);
      }

      int ms1_id = min_ms1_id_ + j;
      PeakPtrVec all_peaks = raw_peaks[ms1_id];
      PeakPtrVec win_peaks = raw_ms_util::getPeaksInWindow(all_peaks, ref_mz, win_size_);
      double win_high_inte = raw_ms_util::getHighestPeakInte(win_peaks);
      double win_median_inte = raw_ms_util::getMedianPeakInte(win_peaks);

      if (env->isExist(ref_idx)) {
        summed_iso_high_inte += env->getIntensity(ref_idx);
        summed_win_high_inte += win_high_inte;
      }
      double env_inte_sum = env->getIntensitySum();
      inte_distr_[charge_idx] += env_inte_sum;

      std::vector<double> real_intensities = env->getIntensities();

      double new_bc_dist = getBcDistance(theo_intensities, real_intensities);
      double new_corr = getPearsonCorr(theo_intensities, real_intensities);

      bool good_env = (new_bc_dist < 0.07 || new_corr > 0.7);
      if (good_env) {
        xic2[charge_idx][xic_start_idx + j] += env_inte_sum;
        charge_xic[i][xic_start_idx+j] = env_inte_sum;
      }

      bool level_one_env = true;
      bool level_two_env = true;
      if (check_pvalue) {
        double poisson_pvalue = getPoissonPValue(win_peaks, win_size_, real_intensities);
        double rank_sum_pvalue = getRankSumPValue(win_peaks, real_intensities);
        level_one_env = (rank_sum_pvalue < 0.01 && poisson_pvalue < 0.01);
        //levelTwoEnvelope = (rankSumPValue < 0.05 || poissonPValue < 0.05);
      }
      if (level_one_env ) {
        if (new_bc_dist < best_dist_scores_[charge_idx]) {
          best_dist_scores_[charge_idx] = new_bc_dist;
          double new_inte_score = 1.0;
          if (win_median_inte > 0.0) {
            new_inte_score = env->getReferIntensity()/win_high_inte;
          }
          best_inte_scores_[charge_idx] = std::max(best_inte_scores_[charge_idx], new_inte_score);
        }
        best_corr_scores_[charge_idx] = std::max(best_corr_scores_[charge_idx], new_corr);

        if (new_bc_dist < rep_env_bc_dist) {
          rep_env_bc_dist = new_bc_dist;
          rep_env = env;
        }
      }
      if (level_two_env) {
        if (new_bc_dist < tmp_best_dist_scores[charge_idx]) {
          tmp_best_dist_scores[charge_idx] = new_bc_dist;
          double new_inte_score = 1.0;
          if (win_median_inte > 0.0) {
            new_inte_score = env->getReferIntensity()/win_high_inte;
          }
          tmp_best_inte_scores[charge_idx] = std::max(tmp_best_inte_scores[charge_idx], new_inte_score);
        }
        tmp_best_corr_scores[charge_idx] = std::max(tmp_best_corr_scores[charge_idx], new_corr);

        if (new_bc_dist < rep_env_bc_dist_2) {
          rep_env_bc_dist_2 = new_bc_dist;
          rep_env_2 = env;
        }
      }

      double bc_dist = getBcDistance(theo_intensities, summed_intensities);
      sum_dist_scores_[charge_idx] = std::min(sum_dist_scores_[charge_idx], bc_dist);
      double pc = getPearsonCorr(theo_intensities, summed_intensities);
      sum_corr_scores_[charge_idx] = std::max(sum_corr_scores_[charge_idx], pc);

      if (best_charges_[charge_idx] < 1 || bc_dist < best_charge_dists[charge_idx]) {
        best_charges_[charge_idx] = charge;
        best_charge_dists[charge_idx] = bc_dist;
        if (summed_win_high_inte > 0.0) {
          sum_inte_scores_[charge_idx] = summed_iso_high_inte/summed_win_high_inte;
        }
      }

      if (bc_dist < tmp_best_bc_dist) {
        tmp_best_bc_dist = bc_dist;
        rep_summed_intensities_ = summed_intensities;
      }
    }
  }

  // when good envelope is observed at only even charge...
  if (best_corr_scores_[0] > 0.7 && best_corr_scores_[1] < 0.5) {
    int idx = 1;
    best_corr_scores_[idx] = tmp_best_corr_scores[idx];
    best_inte_scores_[idx] = tmp_best_inte_scores[idx];
    best_dist_scores_[idx] = tmp_best_dist_scores[idx];
  }

  // when good envelope is observed at only odd charge...
  if (best_corr_scores_[1] > 0.7 && best_corr_scores_[0] < 0.5) {
    int idx = 0;
    best_corr_scores_[idx] = tmp_best_corr_scores[idx];
    best_inte_scores_[idx] = tmp_best_inte_scores[idx];
    best_dist_scores_[idx] = tmp_best_dist_scores[idx];
  }

  // normalize intensities
  double s = inte_distr_[0] + inte_distr_[1];
  if (s > 0) {
    inte_distr_[0] = inte_distr_[0] / s;
    inte_distr_[1] = inte_distr_[1] / s;
  }

  if (col_num > 1) {
    int even_best_charge = best_charges_[even_charge_idx_] - min_charge_;
    int odd_best_charge = best_charges_[odd_charge_idx_] - min_charge_;
    if (even_best_charge >= 0 && odd_best_charge >= 0) {
      std::vector<double> v1 = smoother_->smooth(charge_xic[even_best_charge]);
      std::vector<double> v2 = smoother_->smooth(charge_xic[odd_best_charge]);
      xic_corr_between_best_charges_[0] = getPearsonCorr(v1, v2);
      v1 = smoother_->smooth(xic2[even_charge_idx_]);
      v2 = smoother_->smooth(xic2[odd_charge_idx_]);
      xic_corr_between_best_charges_[1] = getPearsonCorr(v1, v2);
    }
  }

  if (rep_env == nullptr && rep_env_2 != nullptr) {
    rep_env = rep_env_2;
  }
  if (rep_env != nullptr) {
    rep_charge_ = rep_env->getCharge();
    rep_mz_ = rep_env->getMonoMz();
    rep_ms1_id_ = rep_env->getSpId();
  }

  init_score_ = true;
}

}


//=======================================================================
// Copyright 2015 Tsinghua University.
// Authors: Fuan Pu (Pu.Fuan@gmail.com)
//
// Dung's abstract argumentation framework
//=======================================================================

#ifndef DUNG_REASONER_HPP
#define DUNG_REASONER_HPP

#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <algorithm>
#include <iosfwd>

//boost
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

#include "config/config.hpp"
#include "ArgumentProperty.hpp"
#include "AttackProperty.hpp"
#include "bitmatrix/bitvector.hpp"
#include "bitmatrix/bitmatrix.hpp"
#include "DungAF.hpp"

namespace argumatrix{

using namespace std;

class Reasoner {
public:
	Reasoner(const DungAF& daf, streambuf* osbuff = std::cout.rdbuf());
	virtual ~Reasoner() {}
	/**
	 * @brief Get the attacked arguments by arguments in _bv, \f$R^+(S)\f$
	 * \f$R^+(S) = {x|x is attacked by S}\f$.
	 * \f$R^+(S_bv) = D*S_bv\f$
	 * @param _bv the bitvector with respect to the set \f$S\f$.
	 * @return a set of arguments in bitvector form.
	 */
	bitvector getAttacked(const bitvector& _bv);
	//bitvector R_plus(const bitvector& _bv);

	/**
	 * @brief The characteristic function of an abstract argumentation framework:
	 * \f$F_{AF}(S) = {A|A is acceptable wrt. S}\f$.
	 * F_AF(S_bv) = not{ R^+[ not( R^+(S_bv) ) ] }
	 * @param extension an extension (a set of arguments), the default is empty set.
	 * @return a set of arguments in bitvector form.
	 */
	bitvector characteristic(const bitvector& _bv);

	/**
	 * @brief The characteristic function of an abstract argumentation framework with
	 * the initialization of empty set
	 * @return a set of arguments in bitvector form.
	 */
	bitvector characteristic();

	/**
	 * @brief The neutrality function of an abstract argumentation framework:
	 * N_AF(S) = {A| All arguments that not attacked by S}.
	 * N_AF(S_bv) = not( R^+(S_bv) )
	 * @param extension an extension (a set of arguments).
	 * @return a set of arguments in bitvector form.
	 */
	bitvector neutrality(const bitvector& _bv);

	/**
	 * @brief Argument set S is conflict-free?
	 * S is said to be conflict-free iff for any two arguments a,b in S such
	 * that a does not attack b
	 * @param extension an extension (a set of arguments).
	 * @return true if S is conflict-free.
	 */
	bool is_conflict_free(const bitvector& _bv);
	bool is_conflict_free(const set<string>& argset);

	/**
	 * @brief Argument set A is acceptable w.r.t S? Alternately, S defends A?
	 * A can be an argument or an argument set.
	 * S defends argument (set) A iff for any argument x in X, if attacks A
	 * (or a in A) then there is an argument y in S such that y attacks x.
	 * @param S: a set of arguments.
	 * @param A: an argument or an argument set
	 * @return true if S defends A.
	 */
	bool is_acceptable(const bitvector& S, const bitvector& A);

	/**
	 * @brief Argument a is self-attacking iff it attacks itself.
	 * @param idx: the index of the argument w.r.t the attack matrix
	 * @return true if S defends A.
	 */
	bool is_self_attacking(size_type idx);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * an admissible extension.
	 * $S$ is an admissible extension iff $S \subseteq F(S) \cap N(S)$.
	 * @return true if _bv is admissible; otherwise returns false.
	 */
	bool is_admissible(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a complete extension.
	 * $S$ is a complete extension iff $S == F(S) \cap N(S)$.
	 * @return true if _bv is complete; otherwise returns false.
	 */
	bool is_complete(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a stable extension.
	 * $S$ is a stable extension iff $S == N(S)$.
	 * @return true if _bv is stable; otherwise returns false.
	 */
	bool is_stable(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a grounded extension. $S$ is a grounded extension iff it is the least
	 * fixed point of the characteristic function F.
	 * @return true if _bv is grounded; otherwise returns false.
	 */
	bool is_grounded(const bitvector& _bv);

	/**
	 * @brief Get all arguments which are self-attacking. Obviously, if an argument
	 * i attacks itself, the entry[i][i] of the attack matrix must be 1 (true).
	 * Therefore to get the set of self-attacking arguments is to get the diagonal
	 * elements of the attack matrix.
	 * @return a set of arguments in bitvector form, if bitvector[i] = true
	 * representing the argument (with index) i is a self-attacking argument,
	 * otherwise is not.
	 */
	bitvector getSelfAttackingArguments();

	/**
	 * @brief Get all extensions given a specific semantics in format of bitvector.
	 * Each extension is a set of arguments which can "survive the conflict
	 * together". Here, we use a bitvector to represent an extension.
	 * Therefore, all extensions are formed an set of bit vectors, i.e.,
	 * set<bitvector>. Each bitvector in set is an extension w.r.t.
	 * a given semantics. The computed extensions are stored in m_extensions,
	 * therefore, to get all extensions, it must first invoke the function
	 * computeExtension()
	 * @return a set of bitvector, i.e., set<bitvector>.
	 */
	const set<bitvector>& getBvExtensions();

	/**
	 * @brief Get a vector of integers {0, 1, 2}: 2 -- Unknown, 1 -- in grounded
	 * extension, and 0 -- attacked by the grounded extension.
	 * @return a vector of integers {0, 1, 2}.
	 */
	vector<int> getGroundedIntVector();

	/**
	 * @brief Redirect the output stream to streambuf* strbuf or ostream& os. If strbuf = cout.rdbuf(),
	 * then the output is standard output. It can also redirect the output to
	 * a file by the following codes:
	 * ~~~~~{.cpp}
	 *   ofstream ofile("output.txt");
	 *   streambuf* oldsb = xxx.setOutput(ofile.rdbuf());
	 * ~~~~~
	 * or
	 * ~~~~~{.cpp}
	 *   streambuf* oldsb = xxx.setOutput(ofile);
	 * ~~~~~
	 * @param strbuf is a streambuf pointer.
	 * @return return the old streambuf, which can be used to redirect the old
	 * streambuf.
	 */
	streambuf* setOutput(streambuf* strbuf);

	/**
	 * Method:    setOutput
	 * FullName:  public  argumatrix::Reasoner::setOutput
	 * @see       streambuf* setOutput(streambuf* strbuf = std::cout.rdbuf());
	 * @param     ostream & os
	 * @return    streambuf*
	 */
	streambuf* setOutput(ostream& os = std::cout);

	/**
	 * @brief Print an extension with the form of bitvector. Assume the bitvector
	 * is [0, 1, 0, 1], the arguments corresponding to entry 1 will
	 * be print. The member *m_output* will determine where to print.
	 * @param bitvector& bv_ext : The bv_ext is a bitvector, which represents
	 * an extension.
	 * @return no return.
	 * @see [setOutput]
	 */
	void printLabSet(const bitvector& bv_ext);

	/**
	 * @brief Print a set of arguments. The member *m_output* will determine
	 * where to print.
	 * @param const std::set<string>& labset
	 * @return no return.
	 * @see setOutput(streambuf* strbuf = std::cout.rdbuf());
	 */
	void printLabSet(const std::set<string>& labset);

	/**
	 * @brief Output all extensions given a specific semantics with an ostream.
	 * Each extension is a set of arguments which can "survive the conflict
	 * together". Here, we return a string to represent all
	 * extensions. For example, if there are two extensions,
	 * [a,b] and [b, d], for some problem w.r.t. a given semantics, then we
	 * return string "[[a,b],[d,c]]". If extensions is not existing, the string
	 * "[]" will return.
	 * @param ostream& os = std::cout, output the resluts into the ostream os.
	 * @return a set of bitvector, i.e., set<bitvector>.
	 */
	void printBvExts();

	/**
	 * @brief Get the grounded extension
	 * @return a set of arguments in bitvector form.
	 */
	bitvector getGroundedExtension();

	/**
	 * @brief Print the grounded extension
	 * @return no return.
	 */
	void printGroundedExt();

	/**
	 * @brief Convert a set of argument (string) labels to an integer vector of {0,1,2},
	 * All arguments in these set, the indices is 1; otherwise 2 (representing unknown)
	 * @return no return.
	 */
	vector<int> labelSet2IntVector(const std::set<string>& label_set);

public:
	/**
	 * @brief Problem [EE-$\sigma$]
	 * Print all extensions
	 * @param no argument
	 * @return no return.
	 */
	virtual void task_EE() { cerr << "Unimplemented!" << endl; }
	virtual void task_EX() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [EC-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$ and
	 * an argument $s \in \mathcal{X}$ (respectively, a set of arguments
	 * $S\subseteq \mathcal{X}$), enumerate all sets $E\subseteq \mathcal{X}$
	 * such that $E \in \mathcal{E}_\sigma(AF)$ and $s\in E$ (respectively,
	 * $S \subseteq E$).
	 * @param set<string>, or a Boolean vector: a set of argument
	 * @return no return.
	 */
	virtual void task_EC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [SC-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$ and an
	 * argument $s \in \mathcal{X}$ (respectively, a set of arguments
	 * $S\subseteq \mathcal{X}$), enumerate some set $E\subseteq \mathcal{X}$
	 * such that $E \in \mathcal{E}_\sigma(AF)$ and $s\in E$ (respectively,
	 * $S \subseteq E$).
	 * @param a set of arguments.
	 * @return no return.
	 */
	virtual void task_SC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [SE-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$, enumerate
	 * some set $E\subseteq \mathcal{X}$ that are in $\mathcal{E}_\sigma(AF)$.
	 * @param a set of arguments.
	 * @return no return.
	 */
	virtual void task_SE() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DE-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and a set of arguments $S \subseteq X$. Decide whether S is
	 * a \sigma-extension of AF, i.e., S \in E_\sigma(AF).
	 * @param a set of arguments.
	 * @return true if argset is a \sigma-extension; otherwise return false.
	 * @note For this task, the *set<string>& argset* can be empty, which means
	 * to decide whether the empty set is a $\sigma$-extension of AF. This indicates
	 * that the option -a is not necessary, then the default *set<string>& argset*
	 * is empty.
	 */
	virtual void task_DE(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DN-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and a set of arguments $S \subseteq X$. Decide whether
	 * there exist a non-empty \sigma-extension for AF
	 * @return true if there exist a non-empty \sigma-extension; otherwise return false.
	 */

	virtual void task_DN() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DC-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and an argument s \in X (respectively, a set of
	 * arguments $S \subseteq X$). Decide whether s contained (respectively, S included)
	 * in some E \in E_\sigma(AF) (i.e., credulously justified).
	 */
	virtual void task_DC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DS-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and an argument s \in X (respectively, a set of
	 * arguments $S \subseteq X$). Decide whether s contained (respectively, S included)
	 * in each E \in E_\sigma(AF) (i.e., skeptically justified).
	 */
	virtual void task_DS(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

protected:
	bitmatrix m_BmAtkMtx;  /**< The bitmatrix of the Dung Abstract argumentation framework. We can
	* access all attackers of an argument. The attackers of the argument
	* with index i is m_BmAtkMtx[i].
	*/

	size_type m_argNum; /**< The number of arguments */

	const DungAF&	  m_daf;  /**< Dung's abstract argumentation framework */

	set< bitvector > m_extensions;

	vector< std::string > m_argLabels; /**< Dung's abstract argumentation framework */

	std::ostream m_output; /**< Where to output */
};

Reasoner::Reasoner(const DungAF& daf, streambuf* osbuff /*= std::cout.rdbuf()*/):
	m_daf(daf), m_output(osbuff)
{
	m_argNum = m_daf.getNumberOfArguments();
	m_BmAtkMtx = m_daf.getAttackMatrix();
	m_argLabels = m_daf.getArgumentLabels();
}

__inline
argumatrix::bitvector Reasoner::getAttacked(const bitvector& _bv)
{
	//assert( _bv.size() == m_argNum );

	return m_BmAtkMtx * _bv;
}

__inline
argumatrix::bitvector Reasoner::characteristic(const bitvector& _bv)
{
	// return ~(m_BmAtkMtx * (~(m_BmAtkMtx * _bv)));
	return neutrality(neutrality(_bv));
}

__inline
argumatrix::bitvector Reasoner::characteristic()
{
	return characteristic(bitvector::EmptySet(m_argNum));
}

__inline
bool Reasoner::is_conflict_free(const bitvector& _bv)
{
	// S is conflict-free iff $S \intersect R^+(S) = \emptyset$.
	return !_bv.intersects( getAttacked(_bv) );
}

__inline
bool Reasoner::is_conflict_free(const set<string>& argset)
{
	return is_conflict_free(m_daf.labelSet2bv(argset));
}

__inline
bool Reasoner::is_acceptable(const bitvector& S, const bitvector& A)
{
	return A.is_subset_of( characteristic(S) );
}

__inline
const set<bitvector>& Reasoner::getBvExtensions()
{
	return m_extensions;
}

__inline
bool Reasoner::is_self_attacking(size_type idx)
{
	// assert(idx < m_argNum);

	return m_BmAtkMtx[idx][idx];
}

__inline
argumatrix::bitvector Reasoner::getSelfAttackingArguments()
{
	//bitvector _bv(m_argNum, false);
	//for (size_type i=0; i<m_argNum; i++)
	//{
	//	_bv[i] = m_BmAtkMtx[i][i];
	//}

	//return _bv;

	return m_BmAtkMtx.diag();
}

__inline
void Reasoner::printBvExts()
{
	m_daf.outputBvSet(m_extensions, m_output);
}

void Reasoner::printLabSet(const bitvector& bv_ext)
{
	bool first = true;

	m_output << LEFT_LIMITER;
	for ( size_type i = bv_ext.find_first();
		i != bitvector::npos;
		i = bv_ext.find_next(i) )
	{
		if(first){
			first = false;
		}else{
			m_output << DELIMITER; // ","
		}
		m_output << m_argLabels[i];
	}
	m_output << RIGHT_LIMITER;

	// m_daf.outputBv(bv_ext, m_output);
}

void Reasoner::printLabSet(const std::set<string>& labset)
{
	bool first = true;

	m_output << LEFT_LIMITER;
	set<string>::iterator itr = labset.begin();
	for ( ; itr != labset.end(); ++itr )
	{
		if(first){
			first = false;
		}else{
			m_output << DELIMITER; // ","
		}
		m_output << *itr;
	}
	m_output << RIGHT_LIMITER;
}

__inline
streambuf* Reasoner::setOutput(streambuf* strbuf)
{
	return m_output.rdbuf(strbuf);
}

__inline
streambuf* Reasoner::setOutput(ostream& os /*= std::cout*/)
{
	return m_output.rdbuf(os.rdbuf());
}

argumatrix::bitvector Reasoner::getGroundedExtension()
{
	bitvector _bv = bitvector::EmptySet(m_argNum);
	bitvector _bv_next = characteristic(_bv);

	while (_bv != _bv_next)
	{
		_bv = _bv_next;
		_bv_next = characteristic(_bv);
	}

	return _bv_next;
}

vector<int> Reasoner::getGroundedIntVector()
{
	// create m_argNum length vector with initial value 2 -- Unknown
	vector<int> vecii(m_argNum, 2);

	bitvector gr_ext = bitvector::EmptySet(m_argNum);
	bitvector gr_out;
	bitvector _bv_last;
	do {
		_bv_last = gr_ext;
		//gr_out = (m_BmAtkMtx * _bv_last); // S =  R^+(X)
		//gr_ext = ~(m_BmAtkMtx * (~gr_out)); // G = ~R^+(~S)
		gr_out = getAttacked( _bv_last ); // S =  R^+(X)
		gr_ext = neutrality( ~gr_out ); // G = ~R^+(~S)
	} while (gr_ext != _bv_last);

	// cout << "gr_ext" << gr_ext << endl;

	for(size_type i=0; i<m_argNum; ++i)
	{
		if( gr_ext[i] ) { vecii[i] = 1; }
		else if ( gr_out[i] ) { vecii[i] = 0; }
	}

	return vecii;
}

vector<int> Reasoner::labelSet2IntVector(const std::set<string>& label_set)
{
	vector<int> vecI( m_argNum, 2 );
	set<string>::iterator _ls_itr;
	for (_ls_itr = label_set.begin(); _ls_itr != label_set.end(); ++_ls_itr)
	{
		vecI[ m_daf.getArgumentIdx( *_ls_itr ) ] = 1;
	}

	return vecI;
}

__inline
void Reasoner::printGroundedExt()
{
	m_output << LEFT_LIMITER;  // [
	printLabSet( getGroundedExtension() );
	m_output << RIGHT_LIMITER; // ]
}

__inline
argumatrix::bitvector Reasoner::neutrality(const bitvector& _bv)
{
	return ~getAttacked(_bv);
}

__inline
bool Reasoner::is_admissible(const bitvector& _bv)
{
	// $S$ is an admissible extension iff $S \subseteq F(S) \cap N(S)$.
	bitvector neu_s = neutrality(_bv);
	bitvector f_s = neutrality( neu_s );
	f_s &= neu_s;

	return _bv.is_subset_of(f_s);
}

__inline
bool Reasoner::is_complete(const bitvector& _bv)
{
	// // $S$ is a complete extension iff $S$ is conflict-free and $S == F(S)$.
	// if ( !is_conflict_free(_bv) )
	//	  return false;
	// bitvector neu_s = neutrality(_bv);
	// bitvector f_s = neutrality( neu_s );
	// return _bv == f_s;

	// This way may be more efficient.
	return is_conflict_free(_bv) && (_bv == characteristic(_bv));

	//// $S$ is a complete extension iff $S == F(S) \cap N(S)$.
	//bitvector neu_s = neutrality(_bv);
	//bitvector f_s = neutrality( neu_s );
	//f_s &= neu_s;

	//return _bv == f_s;
}

__inline
bool Reasoner::is_stable(const bitvector& _bv)
{
	// $S$ is a stable extension iff $S == N(S)$.
	bitvector neu_s = neutrality(_bv);

	return _bv == neu_s;
}

__inline
bool Reasoner::is_grounded(const bitvector& _bv)
{
	bitvector gr_ext = getGroundedExtension();

	return (_bv == gr_ext);
}

} // namespace argumatrix

#endif  //DUNG_REASONER_HPP


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

#include <arlib/esx.hpp>
#include <arlib/graph_utils.hpp>
#include <arlib/multi_predecessor_map.hpp>
#include <arlib/onepass_plus.hpp>
#include <arlib/path.hpp>
#include <arlib/penalty.hpp>
#include <arlib/routing_kernels/types.hpp>

#include <iostream>
#include <string>
#include <string_view>
#include <vector>

// Create type-aliases for the Graph type
using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
                                    boost::bidirectionalS, boost::no_property,
                                    boost::property<boost::edge_weight_t, int>>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

/// Run the alternative routing algorithm named @p name.
template <typename WeightMap, typename MultiPredecessorMap>
void run_alt_routing(std::string_view name, Graph const &G,
                     WeightMap const &weight, MultiPredecessorMap &predecessors,
                     Vertex s, Vertex t, int k, double theta) {
  using arlib::routing_kernels;
  if (name == "onepass_plus") {
    arlib::onepass_plus(G, weight, predecessors, s, t, k, theta);
  } else if (name == "esx") {
    arlib::esx(G, weight, predecessors, s, t, k, theta,
               routing_kernels::astar);
  } else if (name == "penalty") {
    double p = 0.1, r = 0.1;
    int max_nb_updates = 10, max_nb_steps = 100000;
    arlib::penalty(G, weight, predecessors, s, t, k, theta, p, r,
                   max_nb_updates, max_nb_steps,
                   routing_kernels::bidirectional_dijkstra);
  } else {
    std::cout << "Unknown algorithm '" << name << "'. Exiting...\n";
    std::exit(1);
  }
}

/// Define a convenient function to compute the alternative routes and return
/// them as a view.
std::vector<arlib::Path<Graph>> get_alternative_routes(std::string_view alg,
                                                       Graph const &G, Vertex s,
                                                       Vertex t) {
  // Make output MultiPredecessorMap
  auto predecessors = arlib::multi_predecessor_map<Vertex>{};

  int k = 3;                                       // Nb alternative routes
  double theta = 0.5;                              // Overlapping threshold
  auto weight = boost::get(boost::edge_weight, G); // Get Edge WeightMap

  run_alt_routing(alg, G, weight, predecessors, s, t, k, theta);
  auto alt_routes = arlib::to_paths(G, predecessors, weight, s, t);
  return alt_routes;
}

void print_path(arlib::Path<Graph> const &path,
                std::vector<std::string> const &name) {
  using namespace boost;

  for (auto [v_it, v_end] = vertices(path); v_it != v_end; ++v_it) {
    for (auto [e_it, e_end] = out_edges(*v_it, path); e_it != e_end; ++e_it) {
      std::cout << name[source(*e_it, path)] << " -- "
                << name[target(*e_it, path)] << "\n";
    }
  }
}

int main() {
  // Make convenient labels for the vertices
  enum { S, N1, N2, N3, N4, N5, T };
  const long unsigned num_vertices = T;
  const auto name =
      std::vector<std::string>{"s", "n1", "n2", "n3", "n4", "n5", "t"};

  // Writing out the edges in the graph
  const auto edges = std::vector<std::pair<int, int>>{
      {S, N1},  {N1, S},  {S, N2},  {N2, S},  {S, N3},  {N3, S},
      {N1, T},  {T, N1},  {N3, N1}, {N1, N3}, {N3, N5}, {N5, N3},
      {N3, N2}, {N2, N3}, {N3, N4}, {N4, N3}, {N2, N4}, {N4, N2},
      {N5, T},  {T, N5},  {N5, N4}, {N4, N5}, {N4, T},  {T, N4}};

  const auto weights = std::vector<int>{6, 6, 4, 4, 3, 3, 6, 6, 2, 2, 3, 3,
                                        3, 3, 5, 5, 5, 5, 2, 2, 1, 1, 2, 2};
  auto G = Graph{edges.begin(), edges.end(), weights.begin(), num_vertices};

  //=-------------------------------------------------------------------------=

  // OnePass+
  auto res_opplus = get_alternative_routes("onepass_plus", G, S, T);
  // ESX
  auto res_esx = get_alternative_routes("esx", G, S, T);
  // Penalty
  auto res_penalty = get_alternative_routes("penalty", G, S, T);

  std::cout << "OnePass+ solutions...\n";
  for (auto const &route : res_opplus) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  std::cout << "ESX solutions...\n";
  for (auto const &route : res_esx) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  std::cout << "Penalty solutions...\n";
  for (auto const &route : res_penalty) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  return 0;
}


#pragma once
#ifndef VECTORMATH_NEW_H
#define VECTORMATH_NEW_H

#include <vector>
#include <memory>

#include <Eigen/Core>

#include <data/Spin_System.hpp>
#include <engine/Vectormath_Defines.hpp>

namespace Engine
{
    namespace Vectormath
    {
        /////////////////////////////////////////////////////////////////
        //////// Single Vector Math

        // Rotate a vector around an axis by a certain degree (Implemented with Rodrigue's formula)
        void rotate(const Vector3 & v, const Vector3 & axis, const scalar & angle, Vector3 & v_out);
        void rotate( const vectorfield & v, const vectorfield & axis, const scalarfield & angle,
                     vectorfield & v_out );

        // Decompose a vector into numbers of translations in a basis
        Vector3 decompose(const Vector3 & v, const std::vector<Vector3> & basis);

        /////////////////////////////////////////////////////////////////
        //////// Translating across the lattice

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations)
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations[0];
            int db = translations[1];
            int dc = translations[2];

            return da*N + db*N*Na + dc*N*Na*Nb;
        }

        #ifndef USE_CUDA

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations_i, const std::array<int, 3> translations)
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations_i[0] + translations[0];
            int db = translations_i[1] + translations[1];
            int dc = translations_i[2] + translations[2];

            if (translations[0] < 0)
                da += N*Na;
            if (translations[1] < 0)
                db += N*Na*Nb;
            if (translations[2] < 0)
                dc += N*Na*Nb*Nc;

            int idx = (da%Na)*N + (db%Nb)*N*Na + (dc%Nc)*N*Na*Nb;

            return idx;
        }

        inline bool boundary_conditions_fulfilled(const intfield & n_cells, const intfield & boundary_conditions, const std::array<int, 3> & translations_i, const std::array<int, 3> & translations_j)
        {
            int da = translations_i[0] + translations_j[0];
            int db = translations_i[1] + translations_j[1];
            int dc = translations_i[2] + translations_j[2];
            return ((boundary_conditions[0] || (0 <= da && da < n_cells[0])) &&
                    (boundary_conditions[1] || (0 <= db && db < n_cells[1])) &&
                    (boundary_conditions[2] || (0 <= dc && dc < n_cells[2])));
        }

        #endif
        #ifdef USE_CUDA

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations_i, const int translations[3])
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations_i[0] + translations[0];
            int db = translations_i[1] + translations[1];
            int dc = translations_i[2] + translations[2];

            if (translations[0] < 0)
                da += N*Na;
            if (translations[1] < 0)
                db += N*Na*Nb;
            if (translations[2] < 0)
                dc += N*Na*Nb*Nc;

            int idx = (da%Na)*N + (db%Nb)*N*Na + (dc%Nc)*N*Na*Nb;

            return idx;
        }

        inline bool boundary_conditions_fulfilled(const intfield & n_cells, const intfield & boundary_conditions, const std::array<int, 3> & translations_i, const int translations_j[3])
        {
            int da = translations_i[0] + translations_j[0];
            int db = translations_i[1] + translations_j[1];
            int dc = translations_i[2] + translations_j[2];
            return ((boundary_conditions[0] || (0 <= da && da < n_cells[0])) &&
                    (boundary_conditions[1] || (0 <= db && db < n_cells[1])) &&
                    (boundary_conditions[2] || (0 <= dc && dc < n_cells[2])));
        }

        __inline__ __device__ bool cu_check_atom_type(int atom_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we check for
                //		vacancies (type < 0)
                if (atom_type >= 0) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        __inline__ __device__ bool cu_check_atom_type(int atom_type, int reference_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we do a check if
                //		atom types match.
                if (atom_type == reference_type) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        // Calculates, for a spin i, a pair spin's index j.
        // This function takes into account boundary conditions and atom types and returns `-1` if any condition is not met.
        __inline__ __device__ int cu_idx_from_pair(int ispin, const int * boundary_conditions, const int * n_cells, int N, const int * atom_types, const Pair & pair, bool invert=false)
        {
            // Invalid index if atom type of spin i is not correct
            if ( pair.i != ispin%N || !cu_check_atom_type(atom_types[ispin]) )
                return -1;

            // Number of cells
            auto& Na = n_cells[0];
            auto& Nb = n_cells[1];
            auto& Nc = n_cells[2];

            // Invalid index if translations reach out over the lattice bounds
            if (std::abs(pair.translations[0]) > Na ||
                std::abs(pair.translations[1]) > Nb ||
                std::abs(pair.translations[2]) > Nc )
                return -1;

            // Translations (cell) of spin i
            int nic = ispin / (N*Na*Nb);
            int nib = (ispin - nic*N*Na*Nb) / (N*Na);
            int nia = ispin - nic*N*Na*Nb - nib*N*Na;

            // Translations (cell) of spin j (possibly outside of non-periodical domain)
            int pm = 1;
            if (invert)
                pm = -1;
            int nja = nia + pm*pair.translations[0];
            int njb = nib + pm*pair.translations[1];
            int njc = nic + pm*pair.translations[2];

            // Check boundary conditions: a
            if ( boundary_conditions[0] || (0 <= nja && nja < Na) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (nja < 0)
                    nja += Na;
                // Calculate the correct index
                if (nja >= Na)
                    nja -= Na;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: b
            if ( boundary_conditions[1] || (0 <= njb && njb < Nb) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njb < 0)
                    njb += Nb;
                // Calculate the correct index
                if (njb >= Nb)
                    njb -= Nb;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: c
            if ( boundary_conditions[2] || (0 <= njc && njc < Nc) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njc < 0)
                    njc += Nc;
                // Calculate the correct index
                if (njc >= Nc)
                    njc -= Nc;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Calculate the index of spin j according to it's translations
            int jspin = pair.j + (nja)*N + (njb)*N*Na + (njc)*N*Na*Nb;

            // Invalid index if atom type of spin j is not correct
            if ( pair.j != jspin%N || !cu_check_atom_type(atom_types[jspin]) )
                return -1;

            // Return a valid index
            return jspin;
        }

        #endif

        inline std::array<int, 3> translations_from_idx(const intfield & n_cells, const int n_cell_atoms, int idx)
        {
            std::array<int, 3> ret;
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            ret[2] = idx / (N*Na*Nb);
            ret[1] = (idx - ret[2] * N*Na*Nb) / (N*Na);
            ret[0] = (idx - ret[2] * N*Na*Nb - ret[1] * N*Na) / N;
            return ret;
        }

        // Check atom types
        inline bool check_atom_type(int atom_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we check for
                //		vacancies (type < 0)
                if (atom_type >= 0) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }
        inline bool check_atom_type(int atom_type, int reference_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we do a check if
                //		atom types match.
                if (atom_type == reference_type) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        // Calculates, for a spin i, a pair spin's index j.
        // This function takes into account boundary conditions and atom types and returns `-1` if any condition is not met.
        inline int idx_from_pair(int ispin, const intfield & boundary_conditions, const intfield & n_cells, int N, const intfield & atom_types, const Pair & pair, bool invert=false)
        {
            // Invalid index if atom type of spin i is not correct
            if ( pair.i != ispin%N || !check_atom_type(atom_types[ispin]) )
                return -1;

            // Number of cells
            auto& Na = n_cells[0];
            auto& Nb = n_cells[1];
            auto& Nc = n_cells[2];

            // Invalid index if translations reach out over the lattice bounds
            if (std::abs(pair.translations[0]) > Na ||
                std::abs(pair.translations[1]) > Nb ||
                std::abs(pair.translations[2]) > Nc )
                return -1;

            // Translations (cell) of spin i
            int nic = ispin / (N*Na*Nb);
            int nib = (ispin - nic*N*Na*Nb) / (N*Na);
            int nia = (ispin - nic*N*Na*Nb - nib*N*Na) / N;

            int pm = 1;
            if (invert)
                pm = -1;
            // Translations (cell) of spin j (possibly outside of non-periodical domain)
            int nja = nia + pm*pair.translations[0];
            int njb = nib + pm*pair.translations[1];
            int njc = nic + pm*pair.translations[2];

            // Check boundary conditions: a
            if ( boundary_conditions[0] || (0 <= nja && nja < Na) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (nja < 0)
                    nja += Na;
                // Calculate the correct index
                if (nja >= Na)
                    nja -= Na;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: b
            if ( boundary_conditions[1] || (0 <= njb && njb < Nb) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njb < 0)
                    njb += Nb;
                // Calculate the correct index
                if (njb >= Nb)
                    njb -= Nb;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: c
            if ( boundary_conditions[2] || (0 <= njc && njc < Nc) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njc < 0)
                    njc += Nc;
                // Calculate the correct index
                if (njc >= Nc)
                    njc -= Nc;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Calculate the index of spin j according to it's translations
            int jspin = pair.j + (nja)*N + (njb)*N*Na + (njc)*N*Na*Nb;

            // Invalid index if atom type of spin j is not correct
            if ( !check_atom_type(atom_types[jspin]) )
                return -1;

            // Return a valid index
            return jspin;
        }

        /////////////////////////////////////////////////////////////////
        //////// Vectorfield Math - special stuff

        // Build an array of spin positions and atom types. TODO: find a better name for this function
        void Build_Spins(vectorfield & positions, intfield & atom_types,
                         const std::vector<Vector3> & cell_atoms, const intfield & cell_atom_types,
                         const std::vector<Vector3> & translation_vectors, const intfield & n_cells);
        // Calculate the mean of a vectorfield
        std::array<scalar, 3> Magnetization(const vectorfield & vf);
        // Calculate the topological charge inside a vectorfield
        scalar TopologicalCharge(const vectorfield & vf, const vectorfield & vf_pos, const std::vector<std::array<int, 3>> & triangulation);

        // Utility function for the SIB Solver - maybe create a MathUtil namespace?
        void transform(const vectorfield & spins, const vectorfield & force, vectorfield & out);

        void get_random_vector(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec);
        void get_random_vectorfield(std::mt19937 & prng, vectorfield & xi);
        void get_random_vector_unitsphere(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec);
        void get_random_vectorfield_unitsphere(std::mt19937 & prng, vectorfield & xi);

        // Calculate a gradient scalar distribution according to a starting value, direction and inclination
        void get_gradient_distribution(const Data::Geometry & geometry, Vector3 gradient_direction, scalar gradient_start, scalar gradient_inclination, scalarfield & distribution, scalar range_min, scalar range_max);

        // Calculate the spatial gradient of a vectorfield in a certain direction.
        //      This requires to know the underlying geometry, as well as the boundary conditions.
        void directional_gradient(const vectorfield & vf, const Data::Geometry & geometry, const intfield & boundary_conditions, const Vector3 & direction, vectorfield & gradient);

        /////////////////////////////////////////////////////////////////
        //////// Vectormath-like operations

        // sets sf := s
        // sf is a scalarfield
        // s is a scalar
        void fill(scalarfield & sf, scalar s);

        // TODO: Add the test
        void fill(scalarfield & sf, scalar s, const intfield & mask);

        // Scale a scalarfield by a given value
        void scale(scalarfield & sf, scalar s);

        // Add a scalar to all entries of a scalarfield
        void add(scalarfield & sf, scalar s);

        // Sum over a scalarfield
        scalar sum(const scalarfield & sf);

        // Calculate the mean of a scalarfield
        scalar mean(const scalarfield & sf);

        // Cut off all values to remain in a certain range
        void set_range(scalarfield & sf, scalar sf_min, scalar sf_max);

        // sets vf := v
        // vf is a vectorfield
        // v is a vector
        void fill(vectorfield & vf, const Vector3 & v);
        void fill(vectorfield & vf, const Vector3 & v, const intfield & mask);

        // Normalize the vectors of a vectorfield
        void normalize_vectors(vectorfield & vf);

        // Get the norm of a vectorfield
        void norm( const vectorfield & vf, scalarfield & norm );

        // Pair of Minimum and Maximum of any component of any vector of a vectorfield
        std::pair<scalar, scalar> minmax_component(const vectorfield & v1);

        // Maximum absolute component of a vectorfield
        scalar max_abs_component(const vectorfield & vf);

        // Scale a vectorfield by a given value
        void scale(vectorfield & vf, const scalar & sc);

        // Sum over a vectorfield
        Vector3 sum(const vectorfield & vf);

        // Calculate the mean of a vectorfield
        Vector3 mean(const vectorfield & vf);

        // divide two scalarfields
        void divide( const scalarfield & numerator, const scalarfield & denominator, scalarfield & out );

        // TODO: move this function to manifold??
        // computes the inner product of two vectorfields v1 and v2
        scalar dot(const vectorfield & vf1, const vectorfield & vf2);

        // computes the inner products of vectors in v1 and v2
        // v1 and v2 are vectorfields
        void dot(const vectorfield & vf1, const vectorfield & vf2, scalarfield & out);

        // TODO: find a more appropriate name
        // computes the product of scalars in sf1 and sf2
        // sf1 and sf2 are vectorfields
        void dot(const scalarfield & sf1, const scalarfield & sf2, scalarfield & out);

        // computes the vector (cross) products of vectors in v1 and v2
        // v1 and v2 are vector fields
        void cross(const vectorfield & vf1, const vectorfield & vf2, vectorfield & out);

        // out[i] += c*a
        void add_c_a(const scalar & c, const Vector3 & a, vectorfield & out);
        // out[i] += c*a[i]
		void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out);
		void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask);
        // out[i] += c[i]*a[i]
        void add_c_a( const scalarfield & c, const vectorfield & vf, vectorfield & out );

        // out[i] = c*a
        void set_c_a(const scalar & c, const Vector3 & a, vectorfield & out);
        void set_c_a(const scalar & c, const Vector3 & a, vectorfield & out, const intfield & mask);
        // out[i] = c*a[i]
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out);
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask);
        // out[i] = c[i]*a[i]
        void set_c_a( const scalarfield & sf, const vectorfield & vf, vectorfield & out );

        // out[i] += c * a*b[i]
        void add_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out);
        // out[i] += c * a[i]*b[i]
        void add_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out);

        // out[i] = c * a*b[i]
        void set_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out);
        // out[i] = c * a[i]*b[i]
        void set_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out);

        // out[i] += c * a x b[i]
        void add_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out);
        // out[i] += c * a[i] x b[i]
        void add_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out);
        // out[i] += c[i] * a[i] x b[i]
        void add_c_cross(const scalarfield & c, const vectorfield & a, const vectorfield & b, vectorfield & out);

        // out[i] = c * a x b[i]
        void set_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out);
        // out[i] = c * a[i] x b[i]
        void set_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out);

    }
}

#endif


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <complex>
#include <cmath>

using namespace boost::numeric::ublas ;

template<class E1, class E2>
struct component_prod{
	typedef E1			argument_type1 ;
	typedef E2			argument_type2 ;
	typedef E1			result_type ;

	static
	result_type apply(argument_type1 t1, argument_type2 t2) {
		return element_prod( t1, t2 ) ;
	}
};

template<class E1, class E2>
typename vector_binary_traits<E1, E2, component_prod<typename E1::value_type,
	typename E2::value_type> >::result_type
layer_prod( const vector_expression<E1>& e1, const vector_expression<E2>& e2 ) {
    typedef typename vector_binary_traits<E1, E2, component_prod<
    	typename E1::value_type, typename E2::value_type> >::expression_type expression_type ;
    return expression_type( e1(), e2() ) ;
}

template<class E>
struct matrix_inverse {
	typedef E		argument_type ;
	typedef argument_type	result_type ;
	typedef typename E::value_type	value_type ;

	static
	result_type apply(argument_type t) {
		argument_type tmp(t) ;
		value_type d = t(0,0)*t(1,1) - t(0,1)*t(1,0) ;
		tmp(0,0) = t(1,1) / d ;
		tmp(1,0) = -t(1,0) / d ;
		tmp(0,1) = -t(0,1) / d ;
		tmp(1,1) = t(0,0) / d ;
		return tmp ;
	}
};

template<class E>
typename vector_unary_traits<E, matrix_inverse<typename E::value_type> >::result_type
inverse( const vector_expression<E>& e ) {
	typedef typename vector_unary_traits<E, matrix_inverse<typename E::value_type> >::expression_type
		expression_type ;
	return expression_type( e() ) ;
}

/** ========== VECTOR UNARY SPECIAL ========== **/
template<class E, std::size_t I1, std::size_t I2>
struct nested_index {
	typedef E			argument_type ;
	typedef typename E::value_type		result_type ;

	static
	result_type apply(argument_type t) {
		return t(I1,I2) ;
	}
};

template<class E1, class E2>
struct nested_plus_assign {
	typedef E1		argument_type1 ;
	typedef E2		argument_type2 ;
	typedef argument_type1	result_type ;
	typedef typename E1::size_type	size_type ;

	static
	result_type apply(argument_type1 t1, argument_type2 t2) {
		size_type size( t2.size() ) ;
		for(size_type i=0; i<size; ++i)
			t1 += t2(i) ;
		return t1 ;
	}

};

template<class E>
struct component_determinant{
	typedef E			argument_type ;
	typedef typename E::value_type			result_type ;

	static
	result_type apply(argument_type t) {
		return t(0,0)*t(1,1) - t(0,1)*t(1,0) ;
	}
};

template<class E, class F>
class vector_unary_special :
	public vector_expression<vector_unary_special<E,F> >
{
	public:
		typedef F	functor_type ;
		typedef E	expression_type ;
		typedef typename E::size_type	size_type ;
		typedef typename E::value_type::value_type	value_type ;
		typedef value_type	const_reference ;
		typedef typename E::iterator		iterator ;
		typedef typename E::const_iterator			const_iterator ;

		explicit
		vector_unary_special( const expression_type& e ) : __e(e) {}

		const expression_type& expression() const {
			return __e ;
		}

		const size_type size() const {
			return expression().size() ;
		}

		const_reference operator() (size_type i) const {
			return functor_type::apply( __e(i) ) ;
		}

		const_iterator begin() const {
			return expression().begin() ;
		}

		const_iterator end() const {
			return expression().end() ;
		}

	private:
		expression_type	__e ;
};

template<class E, class F>
struct vector_unary_special_traits {
	typedef vector_unary_special<E,F>	result_type ;
	typedef result_type		expression_type ;
};

template<class E>
typename vector_unary_special_traits<E,	component_determinant<typename E::value_type> >::result_type
layer_determinant( const vector_expression<E>& e ) {
    typedef typename vector_unary_special_traits<E, component_determinant<
    	typename E::value_type> >::expression_type expression_type ;
    return expression_type( e() ) ;
}

template<template<class E1, class E2>class F, class V, class E>
void nested_vector_assign( V& v, const vector_expression<E>& e) {
	typedef F<V,E>		functor_type ;
    typedef typename V::size_type size_type ;
    v = functor_type::apply( v, e () ) ;
}

template<class E, std::size_t I1, std::size_t I2>
typename vector_unary_special_traits<E, nested_index<typename E::value_type, I1, I2> >::result_type
nested_access( const vector_expression<E>& e ) {
	typedef typename vector_unary_special_traits<E, nested_index<typename E::value_type,
		I1, I2> >::expression_type	expression_type ;
	return expression_type( e() ) ;
}

template<class T>
struct scalar_abs:
    public scalar_real_unary_functor<T> {
    typedef typename scalar_real_unary_functor<T>::argument_type
		 argument_type;
    typedef typename scalar_real_unary_functor<T>::result_type
		 result_type;

    static inline result_type apply(argument_type t) {
        return type_traits<result_type>::abs(t);
    }
};

/**
 * Magnitude of a complex vector.
 */
template<class E> BOOST_UBLAS_INLINE
    typename vector_unary_traits<E,
    scalar_abs<typename E::value_type> >::result_type
abs(const vector_expression<E> &e) {
    typedef typename vector_unary_traits<E,
    scalar_abs<typename E::value_type> >::expression_type
        expression_type;
    return expression_type( e() );
}

int main() {

	typedef std::complex<double>	complex_num ;

	bool full_test = false ;

	vector<complex_num> vc(3) ;
	vc(0) = complex_num(5, 10) ;
	vc(1) = complex_num(-3, 2) ;
	vc(2) = complex_num(1, -1) ;

	std::cout << "vc:" << vc << std::endl ;
//	std::cout << "abs(vc):" << abs(vc) << std::endl ;

	if( full_test ) {
		vector<double> v1(4) ;
		vector<double> v2(4) ;
		vector<double> v3(4) ;
		vector<double> v4(4) ;
		for(unsigned i=0; i<v1.size(); ++i) {
			v1(i) = (i+1)*1 ;
			v2(i) = (i+1)*2 ;
			v3(i) = (i+1)*6 ;
			v4(i) = (i+1)*4 ;
		}
		vector<vector<double> > vv1(2) ;
		vv1(0) = v1 ;
		vv1(1) = v2 ;
		vector<vector<double> > vv2(2) ;
		vv2(0) = v3 ;
		vv2(1) = v4 ;
		vector<vector<double> > v_diff = layer_prod(vv2, vv1) ;

		std::cout << "vv1: " << vv1 << std::endl ;
		std::cout << "vv2: " << vv2 << std::endl ;
		std::cout << "v_diff: " << v_diff << std::endl ;
		vector<double> vtest (4,0.0) ;
		nested_vector_assign<nested_plus_assign>(vtest, v_diff) ;
		std::cout << "vtest: " << vtest << std::endl ;

		matrix<double> m1(2,2) ;
		matrix<double> m2(2,2) ;
		matrix<double> m3(2,2) ;
		matrix<double> m4(2,2) ;
		for(unsigned i=0; i<m1.size1(); ++i) {
			for(unsigned j=0; j<m1.size2(); ++j) {
				m1(i,j) = (i+1)*1 + j * 1 ;
				m2(i,j) = (i+1)*6 + j * 2 ;
				m3(i,j) = (i+1)*5 + j * 3 ;
				m4(i,j) = (i+1)*4 + j * 4 ;
			}
		}
		vector<matrix<double> > vm1(2) ;
		vm1(0) = m1 ;
		vm1(1) = m2 ;
		vector<matrix<double> > vm2(2) ;
		vm2(0) = m3 ;
		vm2(1) = m4 ;
		vector<matrix<double> > m_diff = vm1 - vm2 ;

		std::cout << "vm1: " << vm1 << std::endl ;
		std::cout << "vm2: " << vm2 << std::endl ;
		std::cout << "m_diff: " << m_diff << std::endl ;

		vector<matrix<double> > test_inverse = inverse(m_diff) ;
		std::cout << "test_inverse:" << test_inverse << std::endl ;

		vector<double> test_access = nested_access<vector<matrix<double> >,1,1>(m_diff) ;
		std::cout << "test_access:" << test_access << std::endl ;

		vector<double> det = layer_determinant(m_diff) ;
		std::cout << "det: " << det << std::endl ;

		double result = prod( m1, m2 )(0,0) ;
		std::cout << "result: " << result << std::endl ;
	}
}