//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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 NT2_CORE_FUNCTIONS_LOGEPS_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_LOGEPS_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/include/constants/logeps.hpp>
#include <nt2/sdk/meta/generative_hierarchy.hpp>
#include <nt2/core/container/dsl/generative.hpp>
#include <nt2/core/functions/common/generative.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

namespace nt2
{
  #if defined(DOXYGEN_ONLY)
  /*!
    @brief Logeps generator

    Create an array full of log(eps).

    @par Semantic:

    logeps() semantic depends of its parameters type and quantity:

    - The following code:
      @code
      double x = logeps();
      @endcode
      is equivalent to
      @code
      double x = log(eps);
      @endcode

    - For any Integer @c n, the following code:
      @code
      auto x = logeps(n);
      @endcode
      generates an expression that evaluates as a @size2d{n,n} table filled with
      @c double log(eps)

    - For any Integer @c sz1,...,szn , the following code:
      @code
      auto x = logeps(sz1,...,szn);
      @endcode
      generates an expression that evaluates as a @sizes{sz1,szn} table filled
      @c double log(eps)

    - For any Expression @c dims evaluating as a row vector of @c N elements,
      the following code:
      @code
      auto x = logeps(dims);
      @endcode
      generates an expression that evaluates as a @sizes{dims(1),dims(N)}
      table filled with @c double log(eps)

    - For any Fusion Sequence @c dims of @c N elements, the following code:
      @code
      auto x = logeps(dims);
      @endcode
      generates an expression that evaluates as a @sizes{at_c<0>(dims),at_c<N-1>(dims)}
      table filled with @c double log(eps)

    - For any type @c T, the following code:
      @code
      T x = logeps( as_<T>() );
      @endcode
      is equivalent to
      @code
      T x = T(2);
      @endcode

    - For any Integer @c n and any type @c T, the following code:
      @code
      auto x = logeps(n, as_<T>());
      @endcode
      generates an expression that evaluates as a @size2d{n,n} table filled with
      type @c T logepss.

    - For any Integer @c sz1,...,szN and any type @c T, the following code:
      @code
      auto x = logeps(sz1,...,szn, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{sz1,szn} table filled
      with type @c T logepss.

    - For any Expression @c dims evaluating as a row vector of @c N elements
      and any type @c T, the following code:
      @code
      auto x = logeps(dims, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{dims(1),dims(N)}
      table filled with type @c T logepss.

    - For any Fusion Sequence @c dims of @c N elements and any type @c T, the
      following code:
      @code
      auto x = logeps(dims, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{at_c<0>(dims),at_c<N-1>(dims)}
      table filled with type @c T logepss.

    @param dims Size of each dimension, specified as one or more integer values
                or as a row vector of integer values. If any @c dims is lesser
                or equal to 0, then the resulting expression is empty.

    @param classname  Type specifier of the output. If left unspecified, the
                      resulting expression behaves as an array of double.

    @return An Expression evaluating as an array of a given type and dimensions
            filled with the @c Logeps constant.

    @sa Logeps
  **/
  template<typename... Args, typename ClassName>
  details::unspecified logeps(Args const&... dims, ClassName const& classname);

  /// @overload
  template<typename... Args> details::unspecified logeps(Args const&... dims);

  /// @overload
  template<typename ClassName> ClassName::type logeps(ClassName const& classname);

  /// @overload
  double logeps();

  #else

  #define M0(z,n,t)                                                            \
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::Logeps,logeps, n)                            \
  /**/

  BOOST_PP_REPEAT_FROM_TO(1,BOOST_PP_INC(BOOST_PP_INC(NT2_MAX_DIMENSIONS)),M0,~)

  #undef M0

  #endif
}

namespace nt2 { namespace ext
{
  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  value_type<tag::Logeps,Domain,N,Expr>
        : meta::generative_value<Expr>
  {};

  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  size_of<tag::Logeps,Domain,N,Expr>
        : meta::generative_size<Expr>
  {};
} }
#endif


#include <iostream>
//#include <iomanip>
//#include <fstream>

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

#include <vector>
#include <cmath>
#include <cstring>

//#include <unistd.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "DM_NRG.hpp"
#include "NRGOpMatRules.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

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

  CNRGCodeHandler ThisCode;

  //CNRGbasisarray AcutN;
  CNRGbasisarray* AcutN;
  CNRGbasisarray AbasisN;

  //CNRGbasisarray AcutNp1;
  CNRGbasisarray AbasisNp1;

  CNRGmatrix** OpArrayN;

  CNRGmatrix* RhoN;
  vector<double> ParamsTemp;
  double betabar, DM,TM;
  // 0 - betabar; 1 - TM; 2 - DM ?

  char CNsites[8];
  char CNmat[8];
  char arqname[32];
  char ext[32];

  int NshellMax=3;
  int NFermiOps=2;

  ThisCode.NumNRGmats=0;
  ThisCode.pAcut=&AcutN[NshellMax];
  ThisCode.pAbasis=&AbasisN;
  //ThisCode.MatArray=MatArray;

  // Set betabar
  betabar=0.727;
  ParamsTemp.push_back(betabar);

  // Tricky

  OpArrayN=new CNRGmatrix* [NFermiOps];
  for (int iop=0;iop<NFermiOps;iop++){
    OpArrayN[iop]=new CNRGmatrix [NshellMax+1];
  }

  AcutN = new CNRGbasisarray [NshellMax+1];
  RhoN = new CNRGmatrix [NshellMax+1];

  ThisCode.ReadGenPars("");

  // This for now
  ThisCode.Nsitesmax=NshellMax;

  cout << "Lambda = " << ThisCode.Lambda << endl;
  cout << "Nsitesmax = " << ThisCode.Nsitesmax << endl;

  // Read last Nshell

  AcutN[NshellMax].Nshell=NshellMax;
  AbasisNp1.Nshell=NshellMax;
  //ThisCode.ReadArrays("test"); // ReadsInto pAcutN
  // Not too handy...

  sprintf(CNsites,"%d",NshellMax);
  strcpy(ext,"test_N");
  strcat(ext,CNsites);
  strcat(ext,".bin");

  // Read Abasis
  strcpy(arqname,"Abasis_");
  strcat(arqname,ext);
  AbasisNp1.ReadBin(arqname);

  // Read Acut
  strcpy(arqname,"Acut_");
  strcat(arqname,ext);
  AcutN[NshellMax].ReadBin(arqname);

  // Read Operators
  for (int iop=0;iop<NFermiOps;iop++){
    sprintf(CNmat,"%d",iop);
    strcpy(arqname,"Mat");
    strcat(arqname,CNmat);
    strcat(arqname,"_");
    strcat(arqname,ext);
    OpArrayN[iop][NshellMax].ReadBin(arqname);
    // Need a better way to do this but for now it will do:
    // Actually, from Mat block it works!
    //OpArrayN[iop][NshellMax].CheckForMatEl=OneChQSz_cd_check;

  }
  // end read operators

  //AcutN[NshellMax].PrintAll();

  // Set density matrix at the LAST NRG iteration

  DM_NRG_SetRhoNmax(ParamsTemp,&AcutN[NshellMax],&RhoN[NshellMax]);

  //RhoN[NshellMax].PrintAllBlocks();

  // Calculate reduced density matrices

  for (int Nshell=NshellMax-1;Nshell>=0;Nshell--){

    AcutN[Nshell].ClearAll();
    AbasisN.ClearAll();
    AcutN[Nshell].Nshell=Nshell;
    AbasisN.Nshell=Nshell;
    //ThisCode.ReadArrays("test"); // ReadsInto pAcutN
    // Not too handy...

    sprintf(CNsites,"%d",Nshell);
    strcpy(ext,"test_N");
    strcat(ext,CNsites);
    strcat(ext,".bin");

    // Read Abasis
    strcpy(arqname,"Abasis_");
    strcat(arqname,ext);
    AbasisN.ReadBin(arqname);

    // Read Acut
    strcpy(arqname,"Acut_");
    strcat(arqname,ext);
    AcutN[Nshell].ReadBin(arqname);

    // Set ChildStates in AcutN
    DM_NRG_SetChildSt(&AcutN[Nshell],&AbasisNp1);

    // Set RhoN from RhoNp1

    DM_NRG_CalcRhoN(ParamsTemp,
		    &AcutN[Nshell],&AcutN[Nshell+1],&AbasisNp1,
		    &RhoN[Nshell],
		    &RhoN[Nshell+1]);

    //RhoN[Nshell].PrintAllBlocks();

    // Update AbasisNp1
    AbasisNp1.ClearAll();
    AbasisNp1=AbasisN;

    // Read Operators
    for (int iop=0;iop<NFermiOps;iop++){
      sprintf(CNmat,"%d",iop); // Get all SavedMatrices
      strcpy(arqname,"Mat");
      strcat(arqname,CNmat);
      strcat(arqname,"_");
      strcat(arqname,ext);
      OpArrayN[iop][Nshell].ReadBin(arqname);
      // Need a better way to do this but for now it will do:
      // Not needed!
      //OpArrayN[iop][Nshell].CheckForMatEl=OneChQSz_cd_check;

    }
    // end read operators

  }
  // end loop in Nshell

  // Given AcutN, rhoN and the Operators, calculate the spectral density

  DM_NRG_CalcSpecFuncs(&ThisCode,AcutN,RhoN,OpArrayN);

  delete[] RhoN;
  delete[] AcutN;
  for (int iop=NFermiOps-1;iop>=0;iop--){
    delete[] OpArrayN[iop];
  }
  delete[] OpArrayN;

}
// end MAIN


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Created by Roman Weissflog & Philipp Schulz

/** \file Bubble.h */

#ifndef AMDIS_BUBBLE_H
#define AMDIS_BUBBLE_H

#include <list>
#include <boost/numeric/mtl/mtl.hpp>
#include "AbstractFunction.h"
#include "BasisFunction.h"
#include "FixVec.h"

namespace AMDiS
{

  /** \ingroup FEMSpace
  * \brief
  * Lagrange basis functions plus Bubble function. Sub class of BasisFunction
  */

  class Bubble : public BasisFunction
  {
  public:
    /// Creator class used in the BasisFunctionCreatorMap.
    class Creator : public BasisFunctionCreator
    {
    public:
      virtual ~Creator() {}

      /// Returns a new Lagrange object.
      BasisFunction* create()
      {
        return getBubble(this->dim, this->dim + 1); // has to be generalized in later versions of bubble functions
      }
    };

  protected:
    /// Constructs lagrange/bubble basis functions with the given dim and degree.
    /// Constructor is protected to avoid multiple instantiation of identical
    /// basis functions. Use \ref getBubble instead.
    Bubble(int dim_, int degree_);

    /** \brief
    * destructor
    */
    virtual ~Bubble();

  public:
    /// Returns a pointer to lagrange and bubble basis functions with the given dim and
    /// degree. Multiple instantiation of identical basis functions is avoided
    /// by rembering once created basis functions in \ref allBasFcts.
    static Bubble* getBubble(int dim, int degree);

    /// Implements BasisFunction::interpol
    void interpol(const ElInfo*, int, const int*,
                  AbstractFunction<double, WorldVector<double>>*,
                  mtl::dense_vector<double>&) const override;

    /// Implements BasisFunction::interpol
    void interpol(const ElInfo*, int,
                  const int* b_no,
                  AbstractFunction<WorldVector<double>, WorldVector<double>>*,
                  mtl::dense_vector<WorldVector<double>>&) const override;

    /// Returns the barycentric coordinates of the i-th basis function.
    DimVec<double>* getCoords(int i) const override;

    /// Implements BasisFunction::getBound
    void getBound(const ElInfo*, BoundaryType*) const override;

    /** \brief
    * Calculates the local vertex indices which are involved in evaluating
    * the nodeIndex-th DOF at the positionIndex-th part of type position
    * (VERTEX/EDGE/FACE/CENTER). nodeIndex determines the permutation
    * of the involved vertices. So in 1d for lagrange4 there are two DOFs at
    * the CENTER (which is an edge in this case). Then vertices[0] = {0, 1} and
    * vertices[1] = {1, 0}. This allows to use the same local basis function
    * for all DOFs at the same position.
    */
    static void setVertices(int dim, int degree,
                            GeoIndex position, int positionIndex, int nodeIndex,
                            int** vertices);

    /// Implements BasisFunction::refineInter
    inline void refineInter(DOFIndexed<double>* drv, RCNeighbourList* list, int n) override
    {
      if (refineInter_fct)
        (*refineInter_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::coarseRestrict
    inline void coarseRestr(DOFIndexed<double>* drv, RCNeighbourList* list, int n) override
    {
      if (coarseRestr_fct)
        (*coarseRestr_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::coarseInter
    inline void coarseInter(DOFIndexed<double>* drv, RCNeighbourList* list, int n) override
    {
      if (coarseInter_fct)
        (*coarseInter_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::getLocalIndices().
    void getLocalIndices(const Element* el,
                         const DOFAdmin* admin,
                         std::vector<DegreeOfFreedom>& dofs) const override;

    /// Implements BasisFunction::getLocalDofPtrVec()
    /// Returns an vector filled with all DOFs per position
    void getLocalDofPtrVec(const Element* el,
                           const DOFAdmin* admin,
                           std::vector<const DegreeOfFreedom*>& vec) const override;

    /// Implements BasisFunction::l2ScpFctBas
    void l2ScpFctBas(Quadrature* q,
                     AbstractFunction<double, WorldVector<double>>* f,
                     DOFVector<double>* fh) override;

    /// Implements BasisFunction::l2ScpFctBas
    void l2ScpFctBas(Quadrature* q,
                     AbstractFunction<WorldVector<double>, WorldVector<double>>* f,
                     DOFVector<WorldVector<double>>* fh) override;

    static void clear();

    /// Implements BasisFunction::isnodal
    bool isNodal() const override
    {
      return false;
    }

  protected:
    /// sets the barycentric coordinates (stored in \ref bary) of the local
    /// basis functions.
    void setBary();

    /// Implements BasisFunction::setNDOF
    void setNDOF() override;

    /// Sets used function pointers
    void setFunctionPointer();

    /// Used by \ref getVec
    int* orderOfPositionIndices(const Element* el,
                                GeoIndex position,
                                int positionIndex) const override;

  private:
    /// barycentric coordinates of the locations of all basis functions
    std::vector<DimVec<double>*>* bary;

    /** \name static dim-degree-arrays
    * \{
    */
    static std::vector<DimVec<double>*> baryDimDegree;
    static DimVec<int>* ndofDimDegree;
    static int nBasFctsDimDegree;
    static std::vector<BasFctType*> phifunc;
    static std::vector<GrdBasFctType*> grdPhifunc;
    static std::vector<D2BasFctType*> D2Phifunc;
    /** \} */

    /// List of all used BasisFunctions in the whole program. Avoids duplicate
    /// instantiation of identical BasisFunctions.
    static Bubble* Singleton;

  protected:
    /// Pointer to the used refineInter function
    void (*refineInter_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    static void  refineInter2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);

    /// Pointer to the used coarseRestr function
    void (*coarseRestr_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    static void  coarseRestr2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);

    /// Pointer to the used coarseInter function
    void (*coarseInter_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    static void  coarseInter2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);

    /// AbstractFunction which implements lagrange/bubble basis functions
    class Phi : public BasFctType
    {
    public:
      /// Constructs the local lagrange/bubble basis function for the given position,
      /// positionIndex and nodeIndex. owner_ is a pointer to the Bubble
      /// object this basis function belongs to.
      Phi(Bubble* owner, GeoIndex position, int positionIndex, int nodeIndex);

      /// Destructor
      virtual ~Phi();

    private:
      /// vertices needed for evaluation of this function
      int* vertices;

      /// Pointer to the evaluating function
      double (*func)(const DimVec<double>& lambda, int* vert);

      /// Returns \ref func(lambda, vertices)
      inline double operator()(const DimVec<double>& lambda) const override
      {
        return func(lambda, vertices);
      }

      // ====== Lagrange, degree = 1 =====================================
      // vertex
      inline static double phi1v(const DimVec<double>& lambda, int* vertices)
      {
        return lambda[vertices[0]];
      }

      // ====== Bubble ===================================================
      // 1d
      inline static double phi2c(const DimVec<double>& lambda, int* vertices)
      {
        return (4.0 * lambda[vertices[0]] * lambda[vertices[1]]);
      }

      // 2d
      inline static double phi3c(const DimVec<double>& lambda, int* vertices)
      {
        return 27.0 * lambda[vertices[0]] * lambda[vertices[1]] * lambda[vertices[2]];
      }

      // 3d
      inline static double phi4c(const DimVec<double>& lambda, int* vertices)
      {
        return 256.0 * lambda[vertices[0]] * lambda[vertices[1]] *
               lambda[vertices[2]] * lambda[vertices[3]];
      }
    };

    /// AbstractFunction which implements gradients of Lagrange/Bubble basis functions.
    /// See \ref Phi
    class GrdPhi : public GrdBasFctType
    {
    public:
      GrdPhi(Bubble* owner, GeoIndex position, int positionIndex, int nodeIndex);

      virtual ~GrdPhi();
    private:
      int* vertices;

      void (*func)(const DimVec<double>& lambda,
                   int* vertices_,
                   mtl::dense_vector<double>& result);

      inline void operator()(const DimVec<double>& lambda,
                             mtl::dense_vector<double>& result) const override
      {
        func(lambda, vertices, result);
      }

      // ====== Lagrange1 ================================================
      // vertex
      inline static void grdPhi1v(const DimVec<double>&,
                                  int* vertices,
                                  mtl::dense_vector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 1.0;
      }

      // ======= Bubble ==================================================
      inline static void grdPhi2c(const DimVec<double>& lambda,
                                  int* vertices,
                                  mtl::dense_vector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 4.0 * lambda[vertices[1]];
        result[vertices[1]] = 4.0 * lambda[vertices[0]];
      }

      inline static void grdPhi3c(const DimVec<double>& lambda,
                                  int* vertices,
                                  mtl::dense_vector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 27.0 * lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[1]] = 27.0 * lambda[vertices[0]] * lambda[vertices[2]];
        result[vertices[2]] = 27.0 * lambda[vertices[0]] * lambda[vertices[1]];
      }

      inline static void grdPhi4c(const DimVec<double>& lambda,
                                  int* vertices,
                                  mtl::dense_vector<double>& result)
      {
        result = 0.0;
        result[0] =
          256.0 * lambda[vertices[1]] * lambda[vertices[2]] * lambda[vertices[3]];
        result[1] =
          256.0 * lambda[vertices[0]] * lambda[vertices[2]] * lambda[vertices[3]];
        result[2] =
          256.0 * lambda[vertices[0]] * lambda[vertices[1]] * lambda[vertices[3]];
        result[3] =
          256.0 * lambda[vertices[0]] * lambda[vertices[1]] * lambda[vertices[2]];
      }
    };

    /// AbstractFunction which implements second derivatives of Lagrange/Bubble basis
    /// functions. See \ref Phi
    class D2Phi : public D2BasFctType
    {
    public:
      D2Phi(Bubble* owner, GeoIndex position, int positionIndex, int nodeIndex);

      virtual ~D2Phi();

    private:
      int* vertices;

      void (*func)(const DimVec<double>& lambda, int* vertices_, DimMat<double>& result);

      inline void operator()(const DimVec<double>& lambda, DimMat<double>& result) const override
      {
        return func(lambda, vertices, result);
      }

      // ===== Lagrange1 ================================================
      // vertex
      inline static void D2Phi1v(const DimVec<double>&, int*, DimMat<double>& result)
      {
        result.set(0.0);
      }

      // ===== Bubble ===================================================
      inline static void D2Phi2c(const DimVec<double>&, int* vertices,
                                 DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] = 4.0;
        result[vertices[1]][vertices[0]] = 4.0;
      }

      inline static void D2Phi3c(const DimVec<double>& lambda, int* vertices,
                                 DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] = 27.0 * lambda[vertices[2]];
        result[vertices[0]][vertices[2]] =
          result[vertices[2]][vertices[0]] = 27.0 * lambda[vertices[1]];
        result[vertices[1]][vertices[2]] =
          result[vertices[2]][vertices[1]] = 27.0 * lambda[vertices[0]];
      }

      inline static void D2Phi4c(const DimVec<double>& lambda, int* vertices,
                                 DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] =
            256.0 * lambda[vertices[2]] * lambda[vertices[3]];
        result[vertices[0]][vertices[2]] =
          result[vertices[2]][vertices[0]] =
            256.0 * lambda[vertices[1]] * lambda[vertices[3]];
        result[vertices[0]][vertices[3]] =
          result[vertices[3]][vertices[0]] =
            256.0 * lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[1]][vertices[2]] =
          result[vertices[2]][vertices[1]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[3]];
        result[vertices[1]][vertices[3]] =
          result[vertices[3]][vertices[1]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[2]];
        result[vertices[2]][vertices[3]] =
          result[vertices[3]][vertices[2]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[1]];
      }
    };
  };
}

#endif // AMDIS_Bubble_H


/*

 Copyright (c) 2005-2015, University of Oxford.
 All rights reserved.

 University of Oxford means the Chancellor, Masters and Scholars of the
 University of Oxford, having an administrative office at Wellington
 Square, Oxford OX1 2JD, UK.

 This file is part of Chaste.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
 this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
 this list of conditions and the following disclaimer in the documentation
 and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
 contributors may be used to endorse or promote products derived from this
 software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 */

/*
 Jessica Cervi and Raymond J. Spiteri
 Numerical Simulation Laboratory
 University of Saskatchewan
 August 2016
 */

#ifndef THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_
#define THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_

#include "UblasIncludes.hpp"

//#include <iostream>
#include <vector>
#include <petscvec.h>

#include "AbstractBidomainSolver.hpp"
#include "HeartConfig.hpp"
#include "BidomainAssembler.hpp"
#include "BidomainMassMatrixAssembler.hpp"
#include "BidomainStiffnessMatrixAssembler.hpp"
#include "BidomainCorrectionTermAssembler.hpp"
#include "BidomainNeumannSurfaceTermAssembler.hpp"
#include "TimeStepper.hpp"

#include <boost/numeric/ublas/vector_proxy.hpp>

//EDIT COMMENTS HERE!!!
/**
 *  A bidomain solver that uses Strang operator splitting of the diffusion (conductivity) term and the reaction
 *  (ionic current) term, instead of solving the full reaction-diffusion PDE. This does NOT refer to operator splitting
 *  of the two PDEs in the bidomain equations. For details see for example Sundnes et al "Computing the Electrical
 *  Activity of the Heart". This solves the PDEs using the SDIRK2O2 method (second order).
 *
 *  The algorithm is, for solving from t=T to T+dt.
 *
 *  (i)   Solve ODEs   dV/dt = Iionic, du/dt = f(u,V)  for t=T to T+dt/2
 *        [giving updated V (internally, and in solution vector) and updated state variables]
 *  (ii)  Solve PDE    dV/dt = div (sigma_i grad V) + div (sigma_i grad phi_e), div (sigma_i grad V) + div ((sigma_i+sigma_e) grad phi_e) = 0
 *        for t=T to dt         [using V from step i, --> updated V]
 *  (iii) Solve ODEs   dV/dt = Iionic, du/dt = f(u,V)  for t=T+dt/2 to T+dt  [using V from step ii, --> final V]
 *
 *  Notes
 *   (a)  Stages (iii) and (i) can normally be solved together in one go, except just before/after printing the voltage to file.
 *        However for simplicity of code this has not been implemented
 *   (b)  Therefore, the effective ODE timestep will be:  min(ode_dt, pde_dt/2), where ode_dt and pde_dt are those
 *        given via HeartConfig.
 *   (c)  This solver is FOR COMPARING ACCURACY, NOT PERFORMANCE. It has not been optimised and may or may not
 *        perform well in parallel.
 *   (d)  Usage: add the follwoing lines to your original code:
 *        HeartConfig::Instance()->SetUseReactionDiffusionOperatorSplittingBidomainSolver();
 * 	  HeartConfig::Instance()->SetUseReactionDiffusionThirdOrderOperatorSplittingBidomainSolver();
 */
template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
class ThirdOrderRDIRKOperatorSplittingBidomainSolver : public AbstractBidomainSolver<ELEMENT_DIM, SPACE_DIM>
{
private:

    /** Coefficient for SDIRK2O3 method */
    double mGamma;

    /** Mass matrix, used to computing the RHS vector (actually: mass-matrix in
     *  voltage-voltage block, zero elsewhere
     */
    Mat mMassMatrix;

    /** Ai matrix, used to computing the RHS vector (actually: Ai-matrix in
     *  voltage-voltage block, zero elsewhere
     */

    Mat mAiMatrix;

    /**
     *  The vector multiplied by the mass matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z1 where b = M z1 + Ai z2.
     */
    Vec mVecForConstructingRhs1;

    /**
     *  The vector multiplied by the Ai matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z2 where b = M z1 + Ai z2.
     */
    Vec mVecForConstructingRhs2;
    /**
     *  The vector multiplied by the mass matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z1 where b = M z1 + Ai z2., Third stage
     */
    Vec mVecForConstructingRhs1_2;

    /**
     *  The vector multiplied by the Ai matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z2 where b = M z1 + Ai z2. Third Stage
     */
    Vec mVecForConstructingRhs2_2;

    /** The bidomain assembler, used to set up the LHS matrix */
    BidomainAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainAssembler;

    /** Assembler for surface integrals coming from any non-zero Neumann boundary conditions */
    BidomainNeumannSurfaceTermAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainNeumannSurfaceTermAssembler;

    /**
     * If using state variable interpolation, points to an assembler to use in
     * computing the correction term to apply to the RHS.
     */
    BidomainCorrectionTermAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainCorrectionTermAssembler;

    /**
     *  The linear system that will be set up and solved first as part of the
     *  PDE solve
     */
    //LinearSystem* mpLinearSystem_first_solve;

    /**
     * If using state variable interpolation, points to an assembler to use in
     * computing the correction term to apply to the RHS.
     */
    //BidomainCorrectionTermAssembler<ELEMENT_DIM,SPACE_DIM>* mpBidomainCorrectionTermAssembler;

    /** Overloaded InitialiseForSolve() which calls base version but also
     *  initialises mMassMatrix and mVecForConstructingRhs
     *
     *  @param initialSolution initial solution
     */
    void InitialiseForSolve(Vec initialSolution);

    /**
     *  Implementation of SetupLinearSystem() which uses the assembler to compute the
     *  LHS matrix, but sets up the RHS vector using the mass-matrix (constructed
     *  using a separate assembler) multiplied by a vector
     *
     *  @param currentSolution Solution at current time
     *  @param computeMatrix Whether to compute the matrix of the linear system
     */
    void SetupLinearSystem(Vec currentSolution, bool computeMatrix);

    /**
     *  Called before setting up the linear system, used to solve the cell models for first half timestep (step (i) above)
     *  @param currentSolution the latest solution vector
     */
    void PrepareForSetupLinearSystem(Vec currentSolution);

    /**
     *  Called after solving the linear system, used to solve the cell models for second half timestep (step (iii) above)
     *  @param currentSolution the latest solution vector (ie the solution of the linear system).
     */
    void FollowingSolveLinearSystem(Vec currentSolution);

    /**
     * Called when we are ready to solve the splitted system
     */
    Vec SolveOS(Vec currentSolution);

    /**
     * Whether we are solving the cell system or not
     */
    bool SolvingCellSystem;

    /**
     * When using third-order OS, the substeps for the ODE and PDE system have to be the following
     */
    double ThirdOrderTimeSteps;

    /**
     * Index for the time-step array
     */
    int TimeSubStepIndex;

    /**
     * Index for the whether solving the cell system of not
     */
    int BackwardTimeIntegrationIndex;

    /**
     * Index for DIRK stage
     */
    int DIRKStage;

    /*
     *Stepper
     */
    TimeStepper stepper();

    /*
     *Stage counter for DIRK method
     */
    int currentStage;

public:
    /**
     * Constructor
     *
     * @param bathSimulation Whether the simulation involves a perfusing bath
     * @param pMesh pointer to the mesh
     * @param pTissue pointer to the tissue
     * @param pBoundaryConditions pointer to the boundary conditions
     */
    ThirdOrderRDIRKOperatorSplittingBidomainSolver(bool bathSimulation,
                                              AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
                                              BidomainTissue<SPACE_DIM>* pTissue,
                                              BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM,2>* pBoundaryConditions);

    ~ThirdOrderRDIRKOperatorSplittingBidomainSolver();

    //  void FinaliseLinearSystem_first_solve(Vec existingSolution);

};

#endif /*THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_*/


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <fstream>
#include <string>
#include <boost/program_options.hpp>

#include <dpMM/dpSubclusterMM.hpp>
#include <dpMM/dirBaseMeasure.hpp>

using namespace Eigen;
using std::string;
namespace po = boost::program_options;

typedef double flt;

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

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("seed", po::value<int>(), "seed for random number generator")
    ("N,N", po::value<int>(), "number of input datapoints")
    ("D,D", po::value<int>(), "number of dimensions of the data")
    ("T,T", po::value<int>(), "iterations")
    ("alpha,a", po::value< vector<double> >()->multitoken(),
      "alpha parameter of the DP (if single value assumes all alpha_i are the "
      "same")
    ("K,K", po::value<int>(), "number of initial clusters ")
    ("base", po::value<string>(),
      "which base measure to use (only DpDir, ")
    ("params,p", po::value< vector<double> >()->multitoken(),
      "parameters of the base measure")
    ("brief", po::value< vector<double> >()->multitoken(),
      "brief parameters of the base measure (ie Delta = delta*I; "
      "theta=t*ones(D)")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different "
      "datapoints)")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  uint64_t seed = time(0);
  if(vm.count("seed"))
    seed = static_cast<uint64_t>(vm["seed"].as<int>());
  boost::mt19937 rndGen(seed);
  uint32_t K=5;
  if (vm.count("K")) K = vm["K"].as<int>();
  // number of iterations
  uint32_t T=100;
  if (vm.count("T")) T = vm["T"].as<int>();
  uint32_t N=100;
  if (vm.count("N")) N = vm["N"].as<int>();
  uint32_t D=2;
  if (vm.count("D")) D = vm["D"].as<int>();
  cout << "T="<<T<<endl;
  // DP alpha parameter
  VectorXd alpha(K);
  alpha.setOnes(K);
  if (vm.count("alpha"))
  {
    vector<double> params = vm["alpha"].as< vector<double> >();
    if(params.size()==1)
      alpha *= params[0];
    else
      for (uint32_t k=0; k<K; ++k)
        alpha(k) = params[k];
  }
  cout << "alpha="<<alpha.transpose()<<endl;

  shared_ptr<MatrixXd> spx(new MatrixXd(D,N));
  MatrixXd& x(*spx);
  string pathIn ="";
  if(vm.count("input")) pathIn = vm["input"].as<string>();
  if (!pathIn.compare(""))
  {
    for(uint32_t i=0; i<N; ++i)
    {
      if(i<N/3)
      {
        x.col(i) << VectorXd::Zero(D/3), VectorXd::Zero(D/3), VectorXd::Ones(D/3)*100;
      }else if(N/3 <= i && i < 2*N/3){
        x.col(i) << VectorXd::Zero(D/3), VectorXd::Ones(D/3)*100, VectorXd::Zero(D/3);
      }else{
        x.col(i) << VectorXd::Ones(D/3)*100, VectorXd::Zero(D/3), VectorXd::Zero(D/3);
      }
//      x.col(i) /= x.col(i).sum();
    }
    cout<<x<<endl;
  }else{
    cout<<"loading data from "<<pathIn<<endl;
    std::ifstream fin(pathIn.data(),std::ifstream::in);
    for (uint32_t j=0; j<D; ++j)
      for (uint32_t i=0; i<N; ++i)
        fin>>x(j,i);
  }

  // which base distribution
  string base = "DpDir";
  if(vm.count("base")) base = vm["base"].as<string>();

//  if(base.compare("DpDir")){
//    // normalize to unit sum
//    int err = 0;
//#pragma omp parallel for
//    for (uint32_t i=0; i<N; ++i)
//      if(fabs(x.col(i).sum() - 1.0) > 1e-1)
//      {
//        err++;
//        cout<<x.col(i).sum() <<endl;
//      }else
//        x.col(i) /= x.col(i).sum();
//    if(err>0) return 0;
//  }

  DpMM<double> *dpmm=NULL;
  if(!base.compare("DpDir"))
  {
  // Dir alpha parameter
    VectorXd gamma(D);
    gamma.setOnes(D);
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      if(params.size() == 1)
        gamma *= params[0];
      else
        for(uint32_t i=0; i<D; ++i)
          gamma(i) = params[i];
      cout <<"gamma="<<gamma<<endl;
    }
    DirMultd dir(gamma, &rndGen);
    shared_ptr<DirMultSampledd> dirSampl(new DirMultSampledd(dir));
    shared_ptr<LrCluster<DirMultSampledd,double> > lrTheta(new
        LrCluster<DirMultSampledd,double>(dirSampl,1.0,&rndGen));
    dpmm = new DpSubclusterMM<DirMultSampledd,double>(alpha(0),
        lrTheta, K, &rndGen);
  }

  string pathOut ="./labels.csv";
  if(vm.count("output"))
    pathOut = vm["output"].as<string>();
  cout<<"output to "<<pathOut<<endl;

  assert(dpmm!=NULL);
  dpmm->initialize(x);

  std::ofstream fout(pathOut.data(),std::ofstream::out);
  std::ofstream foutJointLike((pathOut+"_jointLikelihood.csv").data(),std::ofstream::out);
  std::ofstream foutMeans((pathOut+"_means.csv").data(),std::ofstream::out);
  std::ofstream foutCovs((pathOut+"_covs.csv").data(),std::ofstream::out);

  const VectorXu& z = dpmm->getLabels().transpose();
  for (uint32_t i=0; i<z.size()-1; ++i)
    fout<<z(i)<<" ";
  fout<<z(z.size()-1)<<endl;
  foutJointLike<<dpmm->logJoint()<<endl;
  dpmm->dump(foutMeans,foutCovs);

  for (uint32_t t=0; t<T; ++t)
  {
    cout<<"------------ t="<<t<<" -------------"<<endl;
    //      VectorXd Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
    dpmm->sampleParameters();

    const VectorXu& z = dpmm->getLabels().transpose();
    for (uint32_t i=0; i<z.size()-1; ++i)
      fout<<z(i)<<" ";
    fout<<z(z.size()-1)<<endl;
    foutJointLike<<dpmm->logJoint()<<endl;
    dpmm->dump(foutMeans,foutCovs);

    dpmm->sampleLabels();

    VectorXd Ns = dpmm->getCounts();
    cout<<"--  counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
    cout<<"    K="<<dpmm->getK();
    cout<<"    logJoint= "<<dpmm->logJoint()<<endl;

    dpmm->proposeMerges();
    //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;

    dpmm->proposeSplits();
    //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;

  }
  fout.close();
  foutJointLike.close();

  MatrixXd logLikes;
  MatrixXu inds = dpmm->mostLikelyInds(10,logLikes);
  cout<<"most likely indices"<<endl;
  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;

  fout.open((pathOut+"mlInds.csv").data(),std::ofstream::out);
  fout<<inds<<endl;
  fout.close();
  fout.open((pathOut+"mlLogLikes.csv").data(),std::ofstream::out);
  fout<<logLikes<<endl;
  fout.close();
};


// Filename: mixed_complex.cpp (part of MTL4)

#include <complex>
#include <iostream>
#include <boost/numeric/mtl/operation/extended_complex.hpp>

int main()
{
    std::complex<double>  z(2.0, 3.0);
    std::cout << "2 * z = " << 2 * z << '\n';
    std::cout << "2 + z = " << 2 + z << '\n';
    std::cout << "z / 2 = " << z / 2 << '\n';
    std::cout << "2 / z = " << 2 / z << '\n';
    std::cout << "2 - z = " << 2 - z << '\n';

    return 0;
}


/*Copyright (c) 2021 James Gayvert

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

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

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

/*
 * AOCAP.cpp
 */

#include "AOCAP.h"
#include "grid_radial.h"
#include "bragg.h"
#include <numgrid.h>
#include <omp.h>
#include <chrono>
#include <ctime>
#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <thread>
#include <vector>
#include <cmath>
#include <limits>
#include "BasisSet.h"
#include "gto_ordering.h"
#include "opencap_exception.h"
#include "utils.h"
#include <typeinfo>
#include "boxcap.h"
#include "cap_types.h"

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params)
{
	verify_cap_parameters(params);
	double capx,capy,capz,rcut,radial,angpts;
	std::stringstream capxss(params["cap_x"]);
	std::stringstream capyss(params["cap_y"]);
	std::stringstream capzss(params["cap_z"]);
	std::stringstream rcutss(params["r_cut"]);
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	capxss >> capx;
	capyss >> capy;
	capzss >> capz;
	rcutss >> rcut;
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = params["cap_type"];
	cap_x = capx;
	cap_y= capy;
	cap_z = capz;
	r_cut = rcut;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
    num_atoms = atoms.size();
	if(compare_strings(cap_type,"box"))
		cap_func = box_cap(cap_x,cap_y,cap_z);
	else if(compare_strings(cap_type,"voronoi"))
		cap_func = voronoi_cap(r_cut,atoms);
	else
		opencap_throw("Error: Only box and voronoi caps are implemented natively.");
	if(params.find("do_numerical")!=params.end())
	{
		std::string num_int = params["do_numerical"];
		if(compare_strings(num_int,"true"))
			do_numerical = true;
		else if(compare_strings(num_int,"false") && compare_strings(cap_type,"box"))
			do_numerical = false;
		else
			do_numerical = true;
	}
	else if(compare_strings(cap_type,"box"))
		do_numerical = false;
	else
		do_numerical = true;
}

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params,const std::function<std::vector<double>(std::vector<double> &, std::vector<double> &,
std::vector<double> &, std::vector<double> &)> &custom_cap_func)
{
	verify_cap_parameters(params);
	double radial,angpts;
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = "custom";
	cap_x = 0.0;
	cap_y= 0.0;
	cap_z = 0.0;
	r_cut = 0.0;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
    num_atoms = atoms.size();
	do_numerical = true;
	cap_func = custom_cap_func;
}

void AOCAP::integrate_cap_numerical(Eigen::MatrixXd &cap_mat, BasisSet bs)
{
	// funny business happens with GIL if we try to parallelize over atoms when custom python function is used
	if(cap_type=="custom")
		omp_set_num_threads(1);
    std::cout << "Calculating CAP matrix in AO basis using " << std::to_string(omp_get_max_threads()) << " threads." << std::endl;
    std::cout << std::setprecision(2) << std::scientific  << "Radial precision: " << radial_precision
              << " Angular points: " << angular_points << std::endl;
	size_t num_atoms = atoms.size();
    double x_coords_bohr[num_atoms];
	double y_coords_bohr[num_atoms];
	double z_coords_bohr[num_atoms];
	int nuc_charges[num_atoms];
	for(size_t i=0;i<num_atoms;i++)
	{
		x_coords_bohr[i]=atoms[i].coords[0];
		y_coords_bohr[i]=atoms[i].coords[1];
		z_coords_bohr[i]=atoms[i].coords[2];
		nuc_charges[i]=atoms[i].Z;
		if (atoms[i].Z==0)
			nuc_charges[i]=1; //choose bragg radius for H for ghost atoms
	}
    int min_num_angular_points = angular_points;
    int max_num_angular_points = angular_points;
	#pragma omp parallel for
	for(size_t i=0;i<num_atoms;i++)
	{
        // check parameters
        double alpha_max = bs.alpha_max(atoms[i]);
        std::vector<double> alpha_min = bs.alpha_min(atoms[i]);
        double r_inner = get_r_inner(radial_precision,
                                     alpha_max * 2.0); // factor 2.0 to match DIRAC
        double h = std::numeric_limits<float>::max();
        double r_outer = 0.0;
        for (int l = 0; l <= bs.max_L(); l++)
        {
            if (alpha_min[l] > 0.0)
            {
                r_outer =
                std::max(r_outer,
                         get_r_outer(radial_precision,
                                     alpha_min[l],
                                     l,
                                     4.0 * get_bragg_angstrom(nuc_charges[i])));
                if(r_outer < r_inner)
                {
                    opencap_throw("Error: r_outer < r_inner, grid cannot be allocated for this basis.");
                    //std::cout << "Setting alpha min[l] to 0.01" << std::endl;
                    //alpha_min[l]=0.01;
                }
                else
                {
                    h = std::min(h,
                                 get_h(radial_precision, l, 0.1 * (r_outer - r_inner)));
                    if(r_outer < h)
                    {
                        opencap_throw("Error: r_outer < h, grid cannot be allocated for this basis.");
                    }
                }
            }
        }
        context_t *context = numgrid_new_atom_grid(radial_precision,
		                                 min_num_angular_points,
		                                 max_num_angular_points,
		                                 nuc_charges[i],
		                                 bs.alpha_max(atoms[i]),
		                                 bs.max_L(),
		                                 alpha_min.data());
		int num_points = numgrid_get_num_grid_points(context);
        double *grid_x_bohr = new double[num_points];
        double *grid_y_bohr = new double[num_points];
        double *grid_z_bohr = new double[num_points];
        double *grid_w = new double[num_points];
		double *cap_values = new double[num_points];
        numgrid_get_grid(  context,
                           num_atoms,
                           i,
                           x_coords_bohr,
                           y_coords_bohr,
                           z_coords_bohr,
                           nuc_charges,
                           grid_x_bohr,
                           grid_y_bohr,
                           grid_z_bohr,
                           grid_w);
        int num_radial_points = numgrid_get_num_radial_grid_points(context);
		compute_cap_on_grid(cap_mat,bs,grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points);
	}
}

void AOCAP::compute_cap_on_grid(Eigen::MatrixXd &cap_mat,BasisSet bs,double* x, double* y, double* z,
double *grid_w, int num_points)
{
	Eigen::VectorXd cap_vals; Eigen::MatrixXd bf_values;
	bf_values = Eigen::MatrixXd::Zero(num_points,bs.num_carts());
    std::vector<double> x_vec(x,x+num_points);
    std::vector<double> y_vec(y,y+num_points);
    std::vector<double> z_vec(z,z+num_points);
    std::vector<double> w_vec(grid_w,grid_w+num_points);
    std::vector<double> cap_vec = cap_func(x_vec,y_vec,z_vec,w_vec);
	size_t bf_idx = 0;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(my_shell.l);
		for(size_t j=0;j<my_shell.num_carts();j++)
		{
			std::array<size_t,3> cart = order[j];
			my_shell.evaluate_on_grid(x,y,z,num_points,cart[0],cart[1],cart[2],bf_values.col(bf_idx));
			bf_idx++;
		}
	}
    cap_vals = Eigen::Map<Eigen::VectorXd>(cap_vec.data(),cap_vec.size());
	Eigen::MatrixXd bf_prime;
	bf_prime =  Eigen::MatrixXd::Zero(num_points,bs.num_carts());
	for(size_t i=0;i<bf_prime.cols();i++)
		bf_prime.col(i) = bf_values.col(i).array()*cap_vals.array();
	cap_mat+=bf_prime.transpose()*bf_values;
}

void AOCAP::compute_ao_cap_mat(Eigen::MatrixXd &cap_mat, BasisSet &bs)
{
	if(!do_numerical)
	{
		std::cout << "CAP integrals will be computed analytically." << std::endl;
		eval_box_cap_analytical(cap_mat,bs);
		return;
	}
	else
		integrate_cap_numerical(cap_mat,bs);
}

void AOCAP::eval_box_cap_analytical(Eigen::MatrixXd &cap_mat, BasisSet &bs)
{
	double boxlength[3] = {cap_x,cap_y,cap_z};
	size_t bf1_idx = 0;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell shell1 = bs.basis[i];
		std::vector<std::array<size_t,3>> order1 = opencap_carts_ordering(shell1.l);
		for(size_t j=0;j<shell1.num_carts();j++)
		{
			std::array<size_t,3> l1 = order1[j];
			size_t bf2_idx = 0;
			for (size_t q=0;q<bs.basis.size();q++)
			{
				Shell shell2 = bs.basis[q];
				std::vector<std::array<size_t,3>> order2 = opencap_carts_ordering(shell2.l);
				for(size_t k=0;k<shell2.num_carts();k++)
				{
					std::array<size_t,3> l2 = order2[k];
					cap_mat(bf1_idx+j,bf2_idx+k) = integrate_box_cap(shell1,shell2,l1,l2,boxlength);
				}
				bf2_idx += shell2.num_carts();
			}
		}
		bf1_idx += shell1.num_carts();
	}
}

void AOCAP::verify_cap_parameters(std::map<std::string,std::string> &parameters)
{
	std::vector<std::string> missing_keys;
	if(parameters.find("cap_type")==parameters.end())
		opencap_throw("Error: Missing cap_type keyword.");
	if(compare_strings(parameters["cap_type"],"box"))
	{
		if(parameters.find("cap_x")==parameters.end())
			missing_keys.push_back("cap_x");
		if(parameters.find("cap_y")==parameters.end())
			missing_keys.push_back("cap_y");
		if (parameters.find("cap_z")==parameters.end())
			missing_keys.push_back("cap_z");
	}
	else if (compare_strings(parameters["cap_type"],"voronoi"))
	{
		if(parameters.find("r_cut")==parameters.end())
			missing_keys.push_back("r_cut");
	}
	else if(compare_strings(parameters["cap_type"],"custom"))
		;
	else
		opencap_throw("Error: only box and voronoi CAPs supported.");
	if(missing_keys.size()!=0)
	{
		std::string error_str = "Missing CAP keywords: ";
		for (auto key: missing_keys)
			error_str+=key+" ";
		opencap_throw(error_str);
	}
	std::map<std::string, std::string> defaults = {{"radial_precision", "14"}, {"angular_points", "590"}};
	for (const auto &pair:defaults)
	{
		if(parameters.find(pair.first)==parameters.end())
			parameters[pair.first]=pair.second;
	}
}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/math/Unit.h>
#include <rl/mdl/Dynamic.h>
#include <rl/mdl/XmlFactory.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		rl::mdl::XmlFactory factory;
		std::shared_ptr<rl::mdl::Dynamic> dynamic = std::dynamic_pointer_cast<rl::mdl::Dynamic>(factory.create(argv[1]));

		rl::math::Vector q(dynamic->getDofPosition());
		rl::math::Vector qd(dynamic->getDof());
		rl::math::Vector qdd(dynamic->getDof());

		for (std::size_t i = 0; i < dynamic->getDofPosition(); ++i)
		{
			q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]);
		}

		for (std::size_t i = 0; i < dynamic->getDof(); ++i)
		{
			qd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2 + dynamic->getDofPosition()]);
			qdd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2 + dynamic->getDofPosition() + dynamic->getDof()]);
		}

		std::cout << "===============================================================================" << std::endl;

		// forward position

		dynamic->setPosition(q);
		dynamic->forwardPosition();
		const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
		rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
		std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward velocity

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->forwardVelocity();
		std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// J

		rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
		dynamic->calculateJacobian(J, false);
		std::cout << "J = " << std::endl << J << std::endl;

		// J * qd

		rl::math::Vector Jqd = J * qd;
		std::cout << "xd = J * qd = " << Jqd.transpose() << std::endl;

		// J^{-1}

		rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianInverse(J, invJ);
		std::cout << "J^{-1} = " << std::endl << invJ << std::endl;

		// J^{-1} * xd

		rl::math::Vector invJxd = invJ * Jqd;
		std::cout << "qd = J^{-1} * xd = " << invJxd.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward acceleration

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->forwardVelocity();
		dynamic->forwardAcceleration();
		std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// Jd * qd

		rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianDerivative(Jdqd, false);
		std::cout << "Jd * qd = " << Jdqd.transpose() << std::endl;

		// J * qdd + Jd * qd

		rl::math::Vector JqddJdqd = J * qdd + Jdqd;
		std::cout << "xdd = J * qdd + Jd * qd = " << JqddJdqd.transpose() << std::endl;

		// J^{-1} * (xdd - Jd * qd)

		rl::math::Vector invJxddJdqd = invJ * (JqddJdqd - Jdqd);
		std::cout << "qdd = J^{-1} * (xdd - Jd * qd) = " << invJxddJdqd.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// inverse dynamics

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->inverseDynamics();
		std::cout << "tau = " << dynamic->getTorque().transpose() << std::endl;

		rl::math::Vector tau = dynamic->getTorque();

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// M

		rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrix(M);
		std::cout << "M = " << std::endl << M << std::endl;

		// V

		rl::math::Vector V(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->calculateCentrifugalCoriolis(V);
		std::cout << "V = " << V.transpose() << std::endl;

		// G

		rl::math::Vector G(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateGravity(G);
		std::cout << "G = " << G.transpose() << std::endl;

		// M * qdd + V + G

		rl::math::Vector MqddVG = M * qdd + V + G;
		std::cout << "tau = M * qdd + V + G = " << MqddVG.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward dynamics

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setTorque(tau);
		dynamic->forwardDynamics();
		std::cout << "qdd = " << dynamic->getAcceleration().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// M^{-1}

		rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrixInverse(invM);
		std::cout << "M^{-1} = " << std::endl << invM << std::endl;

		// V

		std::cout << "V = " << V.transpose() << std::endl;

		// G

		std::cout << "G = " << G.transpose() << std::endl;

		// M^{-1} * (tau - V - G)

		rl::math::Vector invMtauVG = invM * (tau - V - G);
		std::cout << "qdd = M^{-1} * (tau - V - G) = " << invMtauVG.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


/*
 * Copyright (c) 2020 Andrew Price
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "mps_voxels/JaccardMatch.h"
#include "mps_voxels/AABB.h"
#include "mps_voxels/util/hungarian.hpp"
#include <boost/bimap.hpp>
#include <utility>

namespace mps
{

JaccardMatch::JaccardMatch(const cv::Mat& labels1, const cv::Mat& labels2)
{

	if (labels1.channels() > 1 || labels1.type() != CV_16U || labels2.channels() > 1 || labels2.type() != CV_16U)
	{
		throw std::logic_error("jaccard() !!! Only works with CV_16U 1-channel Mats");
	}

	boxes1 = getBBoxes(labels1);
	boxes2 = getBBoxes(labels2);

	int count = 0;
	for (const auto& i : boxes1) { lblIndex1.insert({i.first, count++}); }
	count = 0;
	for (const auto& j : boxes2) { lblIndex2.insert({j.first, count++}); }

	intersection = Eigen::MatrixXi::Zero(boxes1.size(), boxes2.size());
	size_t dSize = std::max(boxes1.size(), boxes2.size());
	IOU = Eigen::MatrixXd::Zero(dSize, dSize);

	for (const auto& i : boxes1)
	{
		cv::Mat mask1 = (labels1 == i.first);
		int count1 = cv::countNonZero(mask1);
		iSizes.insert({i.first, count1});
		for (const auto& j : boxes2)
		{
			int intersectionCount = 0;
			double iou = 0;
			// Prune actual comparisons by starting with bounding boxes
			if (intersect(i.second, j.second))
			{
				cv::Mat mask2 = (labels2 == j.first);

				int count2;
				const auto iter = jSizes.find(j.first);
				if (iter == jSizes.end())
				{
					count2 = cv::countNonZero(mask2);
					jSizes.insert({j.first, count2});
				}
				else
				{
					count2 = iter->second;
				}

				cv::Mat intersectionMask = (mask1 & mask2);

				intersectionCount = cv::countNonZero(intersectionMask);
				int unionCount = count1 + count2 - intersectionCount;
//			    cv::Mat unionMask = (mask1 | mask2); // Optimized to be (|A|+|B|-|AB|)
				iou = (intersectionCount) / static_cast<double>(unionCount);
			}
			IOU(lblIndex1.left.at(i.first), lblIndex2.left.at(j.first)) = iou;
			intersection(lblIndex1.left.at(i.first), lblIndex2.left.at(j.first)) = intersectionCount;
		}
	}

	// If an element in J doesn't intersect anything, it won't have a size computed and cached
	for (const auto& j : boxes2)
	{
		const auto iter = jSizes.find(j.first);
		if (iter == jSizes.end())
		{
			cv::Mat mask2 = (labels2 == j.first);
			int count2 = cv::countNonZero(mask2);
			jSizes.insert({j.first, count2});
		}
	}

	// Compute the optimal assignment of patches
	Hungarian H(-IOU);
	const auto& A = H.getAssignment();
	boost::bimap<LabelT, LabelT> matches;
	for (size_t i = 0; i < A.size(); ++i)
	{
		if (i >= boxes1.size()) { continue; }
		if (A[i] >= (int)boxes2.size()) { continue; }
		matches.insert({lblIndex1.right.at(i), lblIndex2.right.at(A[i])});
	}

	match = {-H.getSolutionCost(), matches};
}

double JaccardMatch::symmetricCover() const
{
	double score = 0.0;
	Eigen::VectorXd iMax = IOU.rowwise().maxCoeff();
	for (const auto& pair : lblIndex1.left)
	{
		// pair is (label, index)
		score += iSizes.at(pair.first) * iMax[pair.second];
	}
	Eigen::RowVectorXd jMax = IOU.colwise().maxCoeff();
	for (const auto& pair : lblIndex2.left)
	{
		// pair is (label, index)
		score += jSizes.at(pair.first) * jMax[pair.second];
	}

	double fullSize = 0;
	for (const auto& i : iSizes) { fullSize += i.second; }
	return score / (2.0 * fullSize);
}

double JaccardMatch::cover() const
{
	double score = 0.0;
	Eigen::VectorXd iMax = IOU.rowwise().maxCoeff();
	for (const auto& pair : lblIndex1.left)
	{
		// pair is (label, index)
		score += iSizes.at(pair.first) * iMax[pair.second];
	}

	double fullSize = 0;
	for (const auto& i : iSizes) { fullSize += i.second; }
	return score / fullSize;
}

JaccardMatch3D::LabelBounds
getBBoxes(const OccupancyData& labels)
{
	JaccardMatch3D::LabelBounds boxes;
	if (!labels.objects.empty())
	{
		for (const auto& kv : labels.objects)
		{
			boxes.emplace(kv.first.id, JaccardMatch3D::AABB(kv.second->minExtent, kv.second->maxExtent));
		}
	}
	else
	{
		for (size_t i = 0; i < labels.voxelRegion->num_vertices(); ++i)
		{
			const auto val = labels.vertexState[i];
			if (val != mps::VoxelRegion::FREE_SPACE)
			{
				const auto coord = labels.voxelRegion->coordinate_of(labels.voxelRegion->vertex_at(i));
				boxes[val].extend(coord);
			}
		}
	}
	return boxes;
}

std::set<size_t>
sparse_matches(const VoxelRegion::VertexLabels& labels,
               const VoxelRegion::VertexLabels::value_type & id)
{
	std::set<size_t> res;
	for (size_t i = 0; i < labels.size(); ++i)
	{
		if (labels[i] == id) { res.insert(i); }
	}
	return res;
}

JaccardMatch3D::JaccardMatch3D(const OccupancyData& labels1, const OccupancyData& labels2)
{
//	if (labels1.channels() > 1 || labels1.type() != CV_16U || labels2.channels() > 1 || labels2.type() != CV_16U)
//	{
//		throw std::logic_error("jaccard() !!! Only works with CV_16U 1-channel Mats");
//	}
	assert(labels1.voxelRegion->num_vertices() == labels1.vertexState.size());
	assert(labels2.voxelRegion->num_vertices() == labels2.vertexState.size());
	assert(labels1.voxelRegion->num_vertices() == labels2.voxelRegion->num_vertices());

	boxes1 = getBBoxes(labels1);
	boxes2 = getBBoxes(labels2);

	int count = 0;
	for (const auto& i : boxes1) { lblIndex1.insert({i.first, count++}); }
	count = 0;
	for (const auto& j : boxes2) { lblIndex2.insert({j.first, count++}); }

	intersection = Eigen::MatrixXi::Zero(boxes1.size(), boxes2.size());
	size_t dSize = std::max(boxes1.size(), boxes2.size());
	IOU = Eigen::MatrixXd::Zero(dSize, dSize);

	for (const auto& i : boxes1)
	{
		const auto mask1 = sparse_matches(labels1.vertexState, i.first);
		int count1 = mask1.size();
		iSizes.insert({i.first, count1});
		for (const auto& j : boxes2)
		{
			int intersectionCount = 0;
			double iou = 0;
			// Prune actual comparisons by starting with bounding boxes
			if (i.second.intersects(j.second))
			{
				const auto mask2 = sparse_matches(labels2.vertexState, j.first);

				int count2;
				const auto iter = jSizes.find(j.first);
				if (iter == jSizes.end())
				{
					count2 = mask2.size();
					jSizes.insert({j.first, count2});
				}
				else
				{
					count2 = iter->second;
				}

				std::vector<size_t> intersectionMask;
				std::vector<int> crossover;
				std::set_intersection(mask1.begin(), mask1.end(), mask2.begin(), mask2.end(), std::back_inserter(intersectionMask));

				intersectionCount = intersectionMask.size();
				int unionCount = count1 + count2 - intersectionCount;
//			    cv::Mat unionMask = (mask1 | mask2); // Optimized to be (|A|+|B|-|AB|)
				iou = (intersectionCount) / static_cast<double>(unionCount);
			}
			IOU(lblIndex1.left.at(i.first), lblIndex2.left.at(j.first)) = iou;
			intersection(lblIndex1.left.at(i.first), lblIndex2.left.at(j.first)) = intersectionCount;
		}
	}

	// If an element in J doesn't intersect anything, it won't have a size computed and cached
	for (const auto& j : boxes2)
	{
		const auto iter = jSizes.find(j.first);
		if (iter == jSizes.end())
		{
			const auto mask2 = sparse_matches(labels2.vertexState, j.first);
			int count2 = mask2.size();
			jSizes.insert({j.first, count2});
		}
	}

	// Compute the optimal assignment of patches
	Hungarian H(-IOU);
	const auto& A = H.getAssignment();
	boost::bimap<LabelT, LabelT> matches;
	for (size_t i = 0; i < A.size(); ++i)
	{
		if (i >= boxes1.size()) { continue; }
		if (A[i] >= (int)boxes2.size()) { continue; }
		matches.insert({lblIndex1.right.at(i), lblIndex2.right.at(A[i])});
	}

	match = {-H.getSolutionCost(), matches};
}

double JaccardMatch3D::symmetricCover() const
{
	double score = 0.0;
	Eigen::VectorXd iMax = IOU.rowwise().maxCoeff();
	for (const auto& pair : lblIndex1.left)
	{
		// pair is (label, index)
		score += iSizes.at(pair.first) * iMax[pair.second];
	}
	Eigen::RowVectorXd jMax = IOU.colwise().maxCoeff();
	for (const auto& pair : lblIndex2.left)
	{
		// pair is (label, index)
		score += jSizes.at(pair.first) * jMax[pair.second];
	}

	double fullSize = 0;
	for (const auto& i : iSizes) { fullSize += i.second; }
	return score / (2.0 * fullSize);
}

}