// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH
#define DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH

#include <memory>
#include <algorithm>
#include <vector>
#include <map>
#include <set>
#include <cmath>
#include <limits>
#include <type_traits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/stuff/common/disable_warnings.hh>
# if HAVE_EIGEN
#   include <Eigen/Eigenvalues>
# endif

# include <dune/common/timer.hh>
# include <dune/common/dynmatrix.hh>

# if HAVE_ALUGRID
#   include <dune/grid/alugrid.hh>
# endif

# include <dune/geometry/quadraturerules.hh>
#include <dune/stuff/common/reenable_warnings.hh>

#include <dune/grid/multiscale/provider.hh>

#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/timedlogging.hh>
#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/common/float_cmp.hh>
#include <dune/stuff/common/fixed_map.hh>
#include <dune/stuff/grid/layers.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>
#include <dune/stuff/grid/walker.hh>

#include <dune/pymor/common/exceptions.hh>

#include <dune/gdt/spaces/dg.hh>
#include <dune/gdt/playground/spaces/block.hh>
#include <dune/gdt/playground/localevaluation/swipdg.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/operators/oswaldinterpolation.hh>
#include <dune/gdt/operators/projections.hh>
#include <dune/gdt/spaces/fv/default.hh>
#include <dune/gdt/spaces/rt/pdelab.hh>
#include <dune/gdt/playground/operators/fluxreconstruction.hh>
#include <dune/gdt/playground/products/swipdgpenalty.hh>
#include <dune/gdt/products/boundaryl2.hh>
#include <dune/gdt/products/l2.hh>
#include <dune/gdt/products/h1.hh>
#include <dune/gdt/products/elliptic.hh>
#include <dune/gdt/assembler/system.hh>

#include <dune/hdd/linearelliptic/problems/default.hh>
#include <dune/hdd/linearelliptic/problems/zero-boundary.hh>

#include "base.hh"
#include "swipdg.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Discretizations {

// forward, needed in the Traits
template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder = 1
        , Stuff::LA::ChooseBackend la_backend = Stuff::LA::default_sparse_backend >
class BlockSWIPDG;

namespace internal {

template< class GridType, class RangeFieldType, int dimRange, int polOrder, Stuff::LA::ChooseBackend la_backend >
class LocalDiscretizationsContainer
{
  typedef grid::Multiscale::ProviderInterface< GridType > GridProviderType;
public:
  typedef SWIPDG< GridType, Stuff::Grid::ChooseLayer::local, RangeFieldType, dimRange
                , polOrder, GDT::ChooseSpaceBackend::fem, la_backend > DiscretizationType;
  typedef SWIPDG< GridType, Stuff::Grid::ChooseLayer::local_oversampled, RangeFieldType, dimRange
                , polOrder, GDT::ChooseSpaceBackend::fem, la_backend > OversampledDiscretizationType;
  typedef typename DiscretizationType::ProblemType     ProblemType;
  typedef typename DiscretizationType::TestSpaceType   TestSpaceType;
  typedef typename DiscretizationType::AnsatzSpaceType AnsatzSpaceType;

private:
  typedef Problems::ZeroBoundary< ProblemType > FakeProblemType;
  typedef typename DiscretizationType::GridViewType::Intersection IntersectionType;

public:
  LocalDiscretizationsContainer(const GridProviderType& grid_provider,
                                const ProblemType& prob,
                                const std::vector< std::string >& only_these_products)
    : zero_boundary_problem_(prob)
    , all_dirichlet_boundary_config_(Stuff::Grid::BoundaryInfos::AllDirichlet< IntersectionType >::default_config())
    , all_neumann_boundary_config_(Stuff::Grid::BoundaryInfos::AllNeumann< IntersectionType >::default_config())
    , multiscale_boundary_config_(Stuff::Grid::BoundaryInfoConfigs::IdBased::default_config())
    , local_discretizations_(grid_provider.num_subdomains(), nullptr)
    , oversampled_discretizations_dirichlet_(grid_provider.num_subdomains(), nullptr)
    , oversampled_discretizations_neumann_(grid_provider.num_subdomains(), nullptr)
    , local_test_spaces_(grid_provider.num_subdomains(), nullptr)
    , local_ansatz_spaces_(grid_provider.num_subdomains(), nullptr)
  {
    multiscale_boundary_config_["neumann"] = "7";
    for (size_t ss = 0; ss < grid_provider.num_subdomains(); ++ss) {
      local_discretizations_[ss] = std::make_shared< DiscretizationType >(grid_provider,
                                                                          all_neumann_boundary_config_,
                                                                          zero_boundary_problem_,
                                                                          ss,
                                                                          only_these_products);
      local_test_spaces_[ss]   = std::make_shared< TestSpaceType >(  local_discretizations_[ss]->test_space());
      local_ansatz_spaces_[ss] = std::make_shared< AnsatzSpaceType >(local_discretizations_[ss]->ansatz_space());
    }
  }

protected:
  const FakeProblemType zero_boundary_problem_;
  const Stuff::Common::Configuration all_dirichlet_boundary_config_;
  const Stuff::Common::Configuration all_neumann_boundary_config_;
  Stuff::Common::Configuration multiscale_boundary_config_;
  std::vector< std::shared_ptr< DiscretizationType > > local_discretizations_;
  mutable std::vector< std::shared_ptr< OversampledDiscretizationType > > oversampled_discretizations_dirichlet_;
  mutable std::vector< std::shared_ptr< OversampledDiscretizationType > > oversampled_discretizations_neumann_;
  std::vector< std::shared_ptr< const TestSpaceType > > local_test_spaces_;
  std::vector< std::shared_ptr< const AnsatzSpaceType > > local_ansatz_spaces_;
}; // class LocalDiscretizationsContainer

template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder, Stuff::LA::ChooseBackend la_backend >
class BlockSWIPDGTraits
  : public ContainerBasedDefaultTraits< typename Stuff::LA::Container< RangeFieldImp, la_backend >::MatrixType,
                                        typename Stuff::LA::Container< RangeFieldImp, la_backend >::VectorType >
{
public:
  typedef BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend > derived_type;
  typedef GridImp GridType;
  typedef RangeFieldImp     RangeFieldType;
  static const unsigned int dimRange = rangeDim;
  static const unsigned int polOrder = polynomialOrder;
private:
  friend class BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >;
  typedef grid::Multiscale::ProviderInterface< GridType > GridProviderType;
  typedef LocalDiscretizationsContainer< GridType, RangeFieldType, dimRange, polOrder, la_backend >
      LocalDiscretizationsContainerType;
  typedef typename LocalDiscretizationsContainerType::TestSpaceType   LocalTestSpaceType;
  typedef typename LocalDiscretizationsContainerType::AnsatzSpaceType LocalAnsatzSpaceType;
public:
  typedef GDT::Spaces::Block< LocalTestSpaceType >   TestSpaceType;
  typedef GDT::Spaces::Block< LocalAnsatzSpaceType > AnsatzSpaceType;
  typedef typename TestSpaceType::GridViewType GridViewType;
}; // class BlockSWIPDGTraits

} // namespace internal

/**
 * \attention The given problem is replaced by a Problems::ZeroBoundary.
 * \attention The given boundary info config is replaced by a Stuff::Grid::BoundaryInfos::AllDirichlet.
 * \attention The boundary info for the local oversampled discretizations is hardwired to dirichlet zero atm!
 */
template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder, Stuff::LA::ChooseBackend la_backend >
class BlockSWIPDG
  : internal::LocalDiscretizationsContainer< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >
  , public ContainerBasedDefault< internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim
                                                             , polynomialOrder, la_backend > >

{
  typedef internal::LocalDiscretizationsContainer< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >
    LocalDiscretizationsBaseType;
  typedef ContainerBasedDefault< internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim
                                                            , polynomialOrder, la_backend > > BaseType;
  typedef BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >        ThisType;
public:
  typedef internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend > Traits;
  typedef typename BaseType::ProblemType     ProblemType;
  typedef typename BaseType::GridViewType    GridViewType;
  typedef typename BaseType::TestSpaceType   TestSpaceType;
  typedef typename BaseType::AnsatzSpaceType AnsatzSpaceType;
  typedef typename BaseType::EntityType      EntityType;
  typedef typename BaseType::DomainFieldType DomainFieldType;
  typedef typename BaseType::RangeFieldType  RangeFieldType;
  typedef typename BaseType::MatrixType      MatrixType;
  typedef typename BaseType::VectorType      VectorType;
  typedef typename BaseType::OperatorType    OperatorType;
  typedef typename BaseType::ProductType     ProductType;
  typedef typename BaseType::FunctionalType  FunctionalType;

  static const unsigned int dimDomain = BaseType::dimDomain;
  static const unsigned int dimRange  = BaseType::dimRange;

  typedef grid::Multiscale::ProviderInterface< GridImp > GridProviderType;
  typedef typename GridProviderType::GridType   GridType;
  typedef typename GridProviderType::MsGridType MsGridType;

  typedef typename Traits::LocalDiscretizationsContainerType::DiscretizationType            LocalDiscretizationType;
  typedef typename Traits::LocalDiscretizationsContainerType::OversampledDiscretizationType OversampledDiscretizationType;
  typedef typename TestSpaceType::PatternType PatternType;

private:
  using typename BaseType::AffinelyDecomposedMatrixType;
  using typename BaseType::AffinelyDecomposedVectorType;
  typedef Pymor::LA::AffinelyDecomposedConstContainer< MatrixType > AffinelyDecomposedConstMatrixType;
  typedef Pymor::LA::AffinelyDecomposedConstContainer< VectorType > AffinelyDecomposedConstVectorType;

public:
  typedef typename LocalDiscretizationType::ProblemType LocalProblemType;
  typedef typename LocalDiscretizationType::ProductType LocalProductType;

  static std::string static_id();

  BlockSWIPDG(const GridProviderType& grid_provider,
              const Stuff::Common::Configuration& bound_inf_cfg,
              const ProblemType& prob,
              const std::vector< std::string >& only_these_products = {});

  const std::vector< std::shared_ptr< LocalDiscretizationType > >& local_discretizations() const;

  void init(const bool prune = false);

  ssize_t num_subdomains() const;

  std::vector< ssize_t > neighbouring_subdomains(const ssize_t ss) const;

  VectorType localize_vector(const VectorType& global_vector, const size_t ss) const;

  VectorType globalize_vectors(const std::vector< VectorType >& local_vectors) const;

  VectorType* globalize_vectors_and_return_ptr(const std::vector< VectorType >& local_vectors) const;

  VectorType* localize_vector_and_return_ptr(const VectorType& global_vector, const ssize_t ss) const;

  ProductType get_local_product(const size_t ss, const std::string id) const;

  ProductType* get_local_product_and_return_ptr(const ssize_t ss, const std::string id) const;

  OperatorType get_local_operator(const size_t ss) const;

  OperatorType* get_local_operator_and_return_ptr(const ssize_t ss) const;

  OperatorType get_coupling_operator(const size_t ss, const size_t nn) const;

  OperatorType* get_coupling_operator_and_return_ptr(const ssize_t ss, const ssize_t nn) const;

  FunctionalType get_local_functional(const size_t ss) const;

  FunctionalType* get_local_functional_and_return_ptr(const ssize_t ss) const;

  VectorType solve_for_local_correction(const std::vector< VectorType >& local_vectors,
                                        const size_t subdomain,
                                        const Pymor::Parameter mu = Pymor::Parameter()) const;

  LocalDiscretizationType get_local_discretization(const size_t subdomain) const;

  LocalDiscretizationType* pb_get_local_discretization(const ssize_t subdomain) const;

  OversampledDiscretizationType get_oversampled_discretization(const size_t subdomain,
                                                               const std::string boundary_value_type) const;

  OversampledDiscretizationType* pb_get_oversampled_discretization(const ssize_t subdomain,
                                                                   const std::string boundary_value_type) const;

//  OversampledDiscretizationType* pb_get_oversampled_discretization(const ssize_t subdomain,
//                                                                   const std::string boundary_value_type,
//                                                                   const VectorType& boundary_values) const;

private:
  class CouplingAssembler
  {
    typedef Dune::DynamicMatrix< RangeFieldType > LocalMatrixType;
    typedef Dune::DynamicVector< RangeFieldType > LocalVectorType;
    typedef std::vector< std::vector< LocalMatrixType > > LocalMatricesContainerType;
    typedef std::vector< std::vector< LocalVectorType > > LocalVectorsContainerType;
    typedef std::vector< Dune::DynamicVector< size_t > > IndicesContainer;

    typedef typename LocalDiscretizationType::TestSpaceType LocalTestSpaceType;
    typedef typename LocalDiscretizationType::AnsatzSpaceType LocalAnsatzSpaceType;
    typedef typename MsGridType::CouplingGridPartType CouplingGridPartType;

    class LocalCodim1MatrixAssemblerApplication
    {
    public:
      virtual ~LocalCodim1MatrixAssemblerApplication(){}

      virtual void apply(const LocalTestSpaceType& /*inner_test_space*/,
                         const LocalAnsatzSpaceType& /*inner_ansatz_space*/,
                         const LocalTestSpaceType& /*outer_test_space*/,
                         const LocalAnsatzSpaceType& /*outer_ansatz_space*/,
                         const typename CouplingGridPartType::IntersectionType& /*_intersection*/,
                         LocalMatricesContainerType& /*_localMatricesContainer*/,
                         IndicesContainer& /*indicesContainer*/) const = 0;

      virtual std::vector< size_t > numTmpObjectsRequired() const = 0;
    };

    template< class LocalAssemblerType, class M >
    class LocalCodim1MatrixAssemblerWrapper
      : public LocalCodim1MatrixAssemblerApplication
    {
    public:
      LocalCodim1MatrixAssemblerWrapper(const LocalAssemblerType& localAssembler,
                                        Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix)
        : localMatrixAssembler_(localAssembler)
        , in_in_matrix_(in_in_matrix)
        , out_out_matrix_(out_out_matrix)
        , in_out_matrix_(in_out_matrix)
        , out_in_matrix_(out_in_matrix)
      {}

      virtual void apply(const LocalTestSpaceType& inner_test_space,
                         const LocalAnsatzSpaceType& inner_ansatz_space,
                         const LocalTestSpaceType& outer_test_space,
                         const LocalAnsatzSpaceType& outer_ansatz_space,
                         const typename CouplingGridPartType::IntersectionType& intersection,
                         LocalMatricesContainerType& localMatricesContainer,
                         IndicesContainer& indicesContainer) const
      {
        localMatrixAssembler_.assembleLocal(inner_test_space, inner_ansatz_space,
                                            outer_test_space, outer_ansatz_space,
                                            intersection,
                                            in_in_matrix_, out_out_matrix_, in_out_matrix_, out_in_matrix_,
                                            localMatricesContainer, indicesContainer);
      }

      virtual std::vector< size_t > numTmpObjectsRequired() const
      {
        return localMatrixAssembler_.numTmpObjectsRequired();
      }

    private:
      const LocalAssemblerType& localMatrixAssembler_;
      Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix_;
    }; // class LocalCodim1MatrixAssemblerWrapper

  public:
    CouplingAssembler(const LocalTestSpaceType& inner_test_space,
                      const LocalAnsatzSpaceType& inner_ansatz_space,
                      const LocalTestSpaceType& outer_test_space,
                      const LocalAnsatzSpaceType& outer_ansatz_space,
                      const CouplingGridPartType& grid_part)
      : innerTestSpace_(inner_test_space)
      , innerAnsatzSpace_(inner_ansatz_space)
      , outerTestSpace_(outer_test_space)
      , outerAnsatzSpace_(outer_ansatz_space)
      , grid_part_(grid_part)
    {}

    ~CouplingAssembler()
    {
      clearLocalAssemblers();
    }

    void clearLocalAssemblers()
    {
      for (auto& element: localCodim1MatrixAssemblers_)
        delete element;
    }

    template< class L, class M >
    void addLocalAssembler(const GDT::LocalAssembler::Codim1CouplingMatrix< L >& localAssembler,
                           Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix)
    {
      assert(in_in_matrix.rows() == innerTestSpace_.mapper().size());
      assert(in_in_matrix.cols() == innerAnsatzSpace_.mapper().size());
      assert(in_out_matrix.rows() == innerTestSpace_.mapper().size());
      assert(in_out_matrix.cols() == outerAnsatzSpace_.mapper().size());
      assert(out_in_matrix.rows() == outerTestSpace_.mapper().size());
      assert(out_in_matrix.cols() == innerAnsatzSpace_.mapper().size());
      assert(out_out_matrix.rows() == outerTestSpace_.mapper().size());
      assert(out_out_matrix.cols() == outerAnsatzSpace_.mapper().size());
      localCodim1MatrixAssemblers_.push_back(
            new LocalCodim1MatrixAssemblerWrapper< GDT::LocalAssembler::Codim1CouplingMatrix< L >, M >(
              localAssembler, in_in_matrix, in_out_matrix, out_in_matrix, out_out_matrix));
    }

    void assemble() const
    {
      // only do something, if there are local assemblers
      if (localCodim1MatrixAssemblers_.size() > 0) {
        // common tmp storage for all entities
        // * for the matrix assemblers
        std::vector< size_t > numberOfTmpMatricesNeeded(2, 0);
        for (auto& localCodim1MatrixAssembler : localCodim1MatrixAssemblers_) {
          const auto tmp = localCodim1MatrixAssembler->numTmpObjectsRequired();
          assert(tmp.size() == 2);
          numberOfTmpMatricesNeeded[0] = std::max(numberOfTmpMatricesNeeded[0], tmp[0]);
          numberOfTmpMatricesNeeded[1] = std::max(numberOfTmpMatricesNeeded[1], tmp[1]);
        }
        const size_t maxLocalSize = std::max(innerTestSpace_.mapper().maxNumDofs(),
                                             std::max(innerAnsatzSpace_.mapper().maxNumDofs(),
                                                      std::max(outerTestSpace_.mapper().maxNumDofs(),
                                                               outerAnsatzSpace_.mapper().maxNumDofs())));
        std::vector< LocalMatrixType > tmpLocalAssemblerMatrices( numberOfTmpMatricesNeeded[0],
                                                                  LocalMatrixType(maxLocalSize,
                                                                                maxLocalSize,
                                                                                RangeFieldType(0)));
        std::vector< LocalMatrixType > tmpLocalOperatorMatrices(numberOfTmpMatricesNeeded[1],
                                                                LocalMatrixType(maxLocalSize,
                                                                                maxLocalSize,
                                                                                RangeFieldType(0)));
        std::vector< std::vector< LocalMatrixType > > tmpLocalMatricesContainer;
        tmpLocalMatricesContainer.push_back(tmpLocalAssemblerMatrices);
        tmpLocalMatricesContainer.push_back(tmpLocalOperatorMatrices);
        // * for the global indices
        std::vector< Dune::DynamicVector< size_t > > tmpIndices = {
            Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
        };

        // walk the grid
        const auto entityEndIt = grid_part_.template end< 0 >();
        for(auto entityIt = grid_part_.template begin< 0 >(); entityIt != entityEndIt; ++entityIt ) {
          const auto& entity = *entityIt;
          // walk the intersections
          const auto intersectionEndIt = grid_part_.iend(entity);
          for (auto intersectionIt = grid_part_.ibegin(entity);
               intersectionIt != intersectionEndIt;
               ++intersectionIt) {
            const auto& intersection = *intersectionIt;
            // for a coupling grid part, we can be sure to only get the inner coupling intersetcions
            // so no further check neccesarry then
            assert(intersection.neighbor() && !intersection.boundary());
            // call local matrix assemblers
            for (auto& localCodim1MatrixAssembler : localCodim1MatrixAssemblers_) {
              localCodim1MatrixAssembler->apply(innerTestSpace_, innerAnsatzSpace_,
                                                outerTestSpace_, outerAnsatzSpace_,
                                                intersection,
                                                tmpLocalMatricesContainer, tmpIndices);
            }
          } // walk the intersections
        } // walk the grid
      } // only do something, if there are local assemblers
    } // void assemble() const

  private:
    const LocalTestSpaceType& innerTestSpace_;
    const LocalAnsatzSpaceType& innerAnsatzSpace_;
    const LocalTestSpaceType& outerTestSpace_;
    const LocalAnsatzSpaceType& outerAnsatzSpace_;
    const CouplingGridPartType grid_part_;
    std::vector< LocalCodim1MatrixAssemblerApplication* > localCodim1MatrixAssemblers_;
  }; // class CouplingAssembler

  void add_local_to_global_pattern(const PatternType& local,
                                   const size_t test_subdomain,
                                   const size_t ansatz_subdomain,
                                   PatternType& global) const;

  void copy_local_to_global_matrix(const AffinelyDecomposedConstMatrixType& local_matrix,
                                   const PatternType& local_pattern,
                                   const size_t subdomain,
                                   const size_t neighbor,
                                   AffinelyDecomposedMatrixType& global_matrix) const;

  template< class ML, class MG >
  void copy_local_to_global_matrix(const Stuff::LA::MatrixInterface< ML >& local_matrix,
                                   const PatternType& local_pattern,
                                   const size_t test_subdomain,
                                   const size_t ansatz_subdomain,
                                   Stuff::LA::MatrixInterface< MG >& global_matrix) const
  {
    for (size_t local_ii = 0; local_ii < local_pattern.size(); ++local_ii) {
      const size_t global_ii = this->test_space().mapper().mapToGlobal(test_subdomain, local_ii);
      for (const size_t& local_jj : local_pattern.inner(local_ii)) {
        const size_t global_jj = this->ansatz_space().mapper().mapToGlobal(ansatz_subdomain, local_jj);
        global_matrix.add_to_entry(global_ii, global_jj, local_matrix.get_entry(local_ii, local_jj));
      }
    }
  } // ... copy_local_to_global_matrix(...)

  void copy_local_to_global_vector(const AffinelyDecomposedConstVectorType& local_vector,
                                   const size_t subdomain,
                                   AffinelyDecomposedVectorType& global_vector) const;

  template< class VL, class VG >
  void copy_local_to_global_vector(const Stuff::LA::VectorInterface< VL >& local_vector,
                                   const size_t subdomain,
                                   Stuff::LA::VectorInterface< VG >& global_vector) const
  {
    for (size_t local_ii = 0; local_ii < local_vector.size(); ++local_ii) {
      const size_t global_ii = this->test_space().mapper().mapToGlobal(subdomain, local_ii);
      global_vector.add_to_entry(global_ii, local_vector.get_entry(local_ii));
    }
  } // ... copy_local_to_global_vector(...)

  void assemble_boundary_contributions(const size_t subdomain) const;

  /**
   * \note  We take the matrices as input here becaus we would have to look them up in the maps otherwise. Since that
   *        has already been done above we save a little.
   */
  void assemble_coupling_contributions(const size_t subdomain,
                                       const size_t neighbour,
                                       AffinelyDecomposedMatrixType& inside_inside_matrix,
                                       AffinelyDecomposedMatrixType& inside_outside_matrix,
                                       AffinelyDecomposedMatrixType& outside_inside_matrix,
                                       AffinelyDecomposedMatrixType& outside_outside_matrix) const;

  void build_global_containers();

  template< class AffinelyDecomposedContainerType >
  ssize_t find_component(const AffinelyDecomposedContainerType& container,
                         const Pymor::ParameterFunctional& coefficient) const
  {
    for (size_t qq = 0; qq < boost::numeric_cast< size_t >(container.num_components()); ++qq)
      if (*(container.coefficient(qq)) == coefficient)
        return qq;
    return -1;
  } // ... find_component(...)

  const GridProviderType& grid_provider_;
  std::shared_ptr< const MsGridType > ms_grid_;
  const std::vector< std::string > only_these_products_;
  using BaseType::pattern_;
  std::vector< std::shared_ptr< AffinelyDecomposedMatrixType > > local_matrices_;
  std::vector< std::shared_ptr< AffinelyDecomposedVectorType > > local_vectors_;
  std::vector< std::map< size_t, std::shared_ptr< PatternType > > > inside_outside_patterns_;
  std::vector< std::map< size_t, std::shared_ptr< PatternType > > > outside_inside_patterns_;
  std::vector< std::map< size_t, std::shared_ptr< AffinelyDecomposedMatrixType > > > inside_outside_matrices_;
  std::vector< std::map< size_t, std::shared_ptr< AffinelyDecomposedMatrixType > > > outside_inside_matrices_;
}; // BlockSWIPDG

#if HAVE_ALUGRID && HAVE_DUNE_FEM
# if HAVE_DUNE_ISTL

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::istl_sparse >;

#   if HAVE_MPI

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::istl_sparse >;

#   endif // HAVE_MPI
# endif // HAVE_DUNE_ISTL
# if HAVE_EIGEN

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::eigen_sparse >;

#   if HAVE_MPI

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::eigen_sparse >;

#   endif // HAVE_MPI
# endif // HAVE_EIGEN
#endif // HAVE_ALUGRID && HAVE_DUNE_FEM && HAVE_DUNE_ISTL

template< Stuff::LA::ChooseBackend la, class G, class R, int r = 1, int p = 1 >
BlockSWIPDG< G, R, r, p, la > make_block_swipdg(const grid::Multiscale::ProviderInterface< G >& grid_provider,
                                                const DSC::Configuration& boundary_info,
                                                const ProblemInterface< typename G::template Codim< 0 >::Entity,
                                                                        typename G::ctype, G::dimension,
                                                                        R, r >& problem,
                                                const std::vector< std::string >& only_these_products = {})
{
  return BlockSWIPDG< G, R, r, p, la >(grid_provider, boundary_info, problem, only_these_products);
}

} // namespace Discretizations
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/lightTimeCorrectionPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/firstOrderRelativisticLightTimeCorrectionPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/jw_lighttime_correction_partial.h"

namespace tudat
{

namespace observation_partials
{

//! Function to get the function returning the light-time correction partial for given correction partial and parameter.

std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangePartialReturnType(
        const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool >
getLightTimeParameterPartialFunction(
        const estimatable_parameters::EstimatebleParameterIdentifier parameterId,
        const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial )
{
    // Declare return type, set second part to 0 (no dependency found).
    std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangePartialReturnType(
                const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool > partialFunction;
    partialFunction.second = 0;

    // Check type of light-time correction
    switch( lightTimeCorrectionPartial->getCorrectionType( ) )
    {
    // Correction type of 1st-order relativistic
    case observation_models::first_order_relativistic:
    {
        // Check consistency of input.
        std::shared_ptr< FirstOrderRelativisticLightTimeCorrectionPartial > currentLightTimeCorrectorPartial =
                std::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionPartial >( lightTimeCorrectionPartial );
        if( currentLightTimeCorrectorPartial == nullptr )
        {
            std::string errorMessage = "Error when getting light time correction partial function, type " +
                    std::to_string( lightTimeCorrectionPartial->getCorrectionType( ) ) +
                       "is inconsistent.";
            throw std::runtime_error( errorMessage );
        }

        // Set partial of gravitational parameter
        if( parameterId.first == estimatable_parameters::gravitational_parameter )
        {
            // Retrieve function from FirstOrderRelativisticLightTimeCorrectionPartial if correction depends on
            // body associated with parameter.
            std::vector< std::string > perturbingBodies = currentLightTimeCorrectorPartial->getPerturbingBodies( );
            std::vector< std::string >::iterator findIterator = std::find(
                        perturbingBodies.begin( ), perturbingBodies.end( ), parameterId.second.first );
            if( findIterator != perturbingBodies.end( ) )
            {
                int bodyIndex = std::distance( perturbingBodies.begin( ),  findIterator );
                partialFunction = std::make_pair(
                            std::bind( &FirstOrderRelativisticLightTimeCorrectionPartial::wrtBodyGravitationalParameter,
                                         currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2, bodyIndex ), 1 );
            }
        }
        else if( parameterId.first == estimatable_parameters::ppn_parameter_gamma )
        {
            partialFunction = std::make_pair(
                        std::bind( &FirstOrderRelativisticLightTimeCorrectionPartial::wrtPpnParameterGamma,
                                     currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2 ), 1 );
        }
        break;
    }

    case observation_models::jw_lighttime:
    {
        // Check consistency of input.
        std::shared_ptr< jw_lighttime_correction_partial > currentLightTimeCorrectorPartial =
                std::dynamic_pointer_cast< jw_lighttime_correction_partial >( lightTimeCorrectionPartial );
        if( currentLightTimeCorrectorPartial == nullptr )
        {
            std::string errorMessage = "Error when getting light time correction partial function, type " +
                    std::to_string( lightTimeCorrectionPartial->getCorrectionType( ) ) +
                       "is inconsistent with expected type, jw_lighttime.";
            throw std::runtime_error( errorMessage );
        }

        // Set partial of gravitational parameter
        if( parameterId.first == estimatable_parameters::gravitational_parameter )
        {
            // Retrieve function from jw_lighttime_correction_partial if correction depends on
            // body associated with parameter.
            std::vector< std::string > perturbingBodies = currentLightTimeCorrectorPartial->getPerturbingBodies( );
            std::vector< std::string >::iterator findIterator = std::find(
                        perturbingBodies.begin( ), perturbingBodies.end( ), parameterId.second.first );
            if( findIterator != perturbingBodies.end( ) )
            {
                int bodyIndex = std::distance( perturbingBodies.begin( ),  findIterator );
                partialFunction = std::make_pair(
                            std::bind( &jw_lighttime_correction_partial::wrtBodyGravitationalParameter,
                                         currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2, bodyIndex ), 1 );
            }
        }
        else if( parameterId.first == estimatable_parameters::ppn_parameter_gamma )
        {
            partialFunction = std::make_pair(
                        std::bind( &jw_lighttime_correction_partial::wrtPpnParameterGamma,
                                     currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2 ), 1 );
        }
        break;
    }

    default:
        std::string errorMessage = "Error, light time correction type " + std::to_string(
                    lightTimeCorrectionPartial->getCorrectionType( ) ) + "not found when creating partial ";
        throw std::runtime_error( errorMessage );
    }

    return partialFunction;
}

}

}


#include <math.h>
#include <Eigen/KroneckerProduct>
#include "Core/Utilities/Tools/MatrixDecomposition.h"
USING_QPANDA

static void upper_partition(int order, MatrixOperator &entries)
{
    auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

    for (auto cdx = 0; cdx < order - 1; ++cdx)
    {
        for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
        {
            auto Entry = entries[cdx][rdx];

            Entry.first += order;
            Entry.second[index] = MatrixUnit::SINGLE_P1;

            entries[cdx + order].emplace_back(Entry);
        }
    }
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
    int M = 0;
    while (cdx)
    {
        cdx >>= 1;
        M += cdx ? 1 : 0;
    }

    //if 1 ≤ j ≤ m and cj = lj' = 1 , return true
    auto unit = units[units.size() - udx - 1];
    return udx >= 1
        && udx <= M
        && cdx == 1
        && unit == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
    int M = 0;
    while (cdx)
    {
        cdx >>= 1;
        M += cdx ? 1 : 0;
    }

    //if j = n and none of cn...cm+1 is 1 , return true
    if (udx != units.size() - 1)
    {
        return false;
    }
    else
    {
        auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
        return (units.end() - M) == iter;
    }
}

static void under_partition(int order, MatrixOperator& entries)
{
    auto qubits =(int)std::log2(entries.size() + 1);

    for (auto cdx = 1; cdx < order; ++cdx)
    {
        if (cdx & 1)
        {
            for (auto rdx = 0; rdx < order; ++rdx)
            {
                auto entry = entries[0][rdx + order - 1].first ^ cdx;
                entries[cdx].emplace_back(make_pair(entry, entries[cdx - 1][rdx + order - 1].second));
            }

            auto &units = entries[cdx].back().second;
            for (auto idx = 0; idx < (int)std::log2(order); ++idx)
            {
                if ((cdx >> idx) & 1)
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
                }
                else
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_I2;
                }
            }
        }
        else
        {
            for (auto rdx = 0; rdx < order; ++rdx)
            {
                auto units = entries[0][rdx + order].second;
                auto entry = entries[0][rdx + order].first ^ cdx;

                for (auto udx = 0; udx < qubits; ++udx)
                {
                    bool steps_accord = entry_requirement(units, udx, cdx);
                    bool entry_accord = entry_requirement(units, udx, cdx);

                    if (steps_accord)
                    {
                        units[udx] = MatrixUnit::SINGLE_P1;
                    }
                    else if (entry_accord)
                    {
                        units[udx] = MatrixUnit::SINGLE_P0;
                    }
                    else
                    {
                    }
                }

                entries[cdx].emplace_back(make_pair(entry, units));
            }

            auto &units = entries[0].back().second;
            for (auto idx = 0; idx < (int)std::log2(order); ++idx)
            {
                if ((cdx >> idx) & 1)
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
                }
                else
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_I2;
                }
            }
        }
    }
}

static void voluation(Eigen::MatrixXcf& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.begin() + 1, units.end(), MatrixUnit::SINGLE_P1);
            if (units.end() == iter)
            {
                units.front() = MatrixUnit::SINGLE_P1;;
            }
            else
            {
                units.front() = MatrixUnit::SINGLE_I2;;
            }

            column.emplace_back(make_pair(opt + path + 1, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }
}

static void controller(MatrixSequence &sequence, const Eigen::Matrix2cf U2, Eigen::MatrixXcf &matrix)
{
    Eigen::Matrix2cf P0;
    Eigen::Matrix2cf P1;
    Eigen::Matrix2cf I2;

    P0 << Eigen::scomplex(1, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(0, 0);
    P1 << Eigen::scomplex(0, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(1, 0);
    I2 << Eigen::scomplex(1, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(1, 0);

    std::map<MatrixUnit, std::function<Eigen::Matrix2cf()>> mapping =
    {
        { MatrixUnit::SINGLE_P0, [&]() {return P0; } },
        { MatrixUnit::SINGLE_P1, [&]() {return P1; } },
        { MatrixUnit::SINGLE_I2, [&]() {return I2; } },
        { MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
    };

    auto order = sequence.size();
    Eigen::MatrixXcf Un = Eigen::MatrixXcf::Identity(1, 1);
    Eigen::MatrixXcf In = Eigen::MatrixXcf::Identity(1ull << order, 1ull << order);

    for (const auto &val : sequence)
    {
        Eigen::Matrix2cf M2 = mapping.find(val)->second();
        Un = Eigen::kroneckerProduct(Un, M2).eval();
    }

    matrix = In + Un;
}

static void operation(Eigen::MatrixXcf& matrix, MatrixOperator& entries)
{
    for (auto cdx = 0; cdx < entries.size(); ++cdx)
    {
        for (auto idx = 0; idx < entries[cdx].size(); ++idx)
        {
            auto rdx = entries[cdx][idx].first;
            auto opt = entries[cdx][idx].second;

            if (Eigen::scomplex(0, 0) == matrix(rdx, cdx))
            {
                continue;;
            }
            else
            {
                auto order = opt.size();

                Eigen::Matrix2cf C2; /*placeholder*/
                C2 << Eigen::scomplex(0, 1), Eigen::scomplex(0, 1),
                      Eigen::scomplex(0, 1), Eigen::scomplex(0, 1);

                Eigen::MatrixXcf Cn;
                controller(opt, C2, Cn);

                Qnum indices(2);
                for (Eigen::Index index = 0; index < (1ull << order); ++index)
                {
                    if (Cn(rdx, index) != Eigen::scomplex(0, 0))
                    {
                        indices[index == rdx] = index;
                    }
                }

                Eigen::scomplex C0 = matrix(indices[0], cdx);
                Eigen::scomplex C1 = matrix(indices[1], cdx);

                Eigen::scomplex V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V21 =  C1 / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));

                Eigen::Matrix2cf V2;
                V2 << V11 , V12 , V21 , V22;

                Eigen::MatrixXcf Un;
                controller(opt, V2, Un);

                matrix = Un * matrix;
            }
        }
    }
}

static void partition(Eigen::MatrixXcf& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        Eigen::MatrixXcf corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }
}

static void general_scheme(Eigen::MatrixXcf& matrix)
{
    MatrixOperator entries;
    for (auto idx = 1; idx < matrix.cols(); ++idx)
    {
        ColumnOperator Co;
        entries.emplace_back(Co);
    }

    voluation(matrix, entries);
    partition(matrix, entries);
    operation(matrix, entries);
}

void QMatrix::decompose()
{
    auto order = (int)std::log2(this->size());

    Eigen::MatrixXcf matrix = Eigen::MatrixXcf::Zero(order, order);
    for (auto rdx = 0; rdx < order; ++rdx)
    {
        for (auto cdx = 0; cdx < order; ++cdx)
        {
            matrix(rdx, cdx) = this->at(rdx*order + cdx);
        }
    }

    if (!matrix.isUnitary(1e-3))
    {
        QCERR("Non-unitary matrix");
        return;
    }

    general_scheme(matrix);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "massmatrix.h"
#include "massmatrix_intrinsic.h"
#include "edge_lengths.h"
#include "normalize_row_sums.h"
#include "sparse.h"
#include "doublearea.h"
#include "repmat.h"
#include <Eigen/Geometry>
#include <iostream>

template <typename DerivedV, typename DerivedF, typename Scalar>
IGL_INLINE void igl::massmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const MassMatrixType type,
  Eigen::SparseMatrix<Scalar>& M)
{
  using namespace Eigen;
  using namespace std;

  const int n = V.rows();
  const int m = F.rows();
  const int simplex_size = F.cols();

  MassMatrixType eff_type = type;
  // Use voronoi of for triangles by default, otherwise barycentric
  if(type == MASSMATRIX_TYPE_DEFAULT)
  {
    eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
  }

  // Not yet supported
  assert(type!=MASSMATRIX_TYPE_FULL);

  if(simplex_size == 3)
  {
    // Triangles
    // edge lengths numbered same as opposite vertices
    Matrix<Scalar,Dynamic,3> l;
    igl::edge_lengths(V,F,l);
    return massmatrix_intrinsic(l,F,type,M);
  }else if(simplex_size == 4)
  {
    Matrix<int,Dynamic,1> MI;
    Matrix<int,Dynamic,1> MJ;
    Matrix<Scalar,Dynamic,1> MV;
    assert(V.cols() == 3);
    assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC);
    MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,1);
    MI.block(0*m,0,m,1) = F.col(0);
    MI.block(1*m,0,m,1) = F.col(1);
    MI.block(2*m,0,m,1) = F.col(2);
    MI.block(3*m,0,m,1) = F.col(3);
    MJ = MI;
    // loop over tets
    for(int i = 0;i<m;i++)
    {
      // http://en.wikipedia.org/wiki/Tetrahedron#Volume
      Matrix<Scalar,3,1> v0m3,v1m3,v2m3;
      v0m3.head(V.cols()) = V.row(F(i,0)) - V.row(F(i,3));
      v1m3.head(V.cols()) = V.row(F(i,1)) - V.row(F(i,3));
      v2m3.head(V.cols()) = V.row(F(i,2)) - V.row(F(i,3));
      Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0;
      MV(i+0*m) = v/4.0;
      MV(i+1*m) = v/4.0;
      MV(i+2*m) = v/4.0;
      MV(i+3*m) = v/4.0;
    }
    sparse(MI,MJ,MV,n,n,M);
  }else
  {
    // Unsupported simplex size
    assert(false && "Unsupported simplex size");
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, -1, 1, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
#endif


#include <cfloat>
#include <vector>
#include <algorithm>
#include <iostream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/export.hpp>

#include "core/Common.h"
#include "math/Random.h"
#include "Approximator.h"
#include "ai/AI.h"
#include "TD.h"

namespace OpenNero
{

    /// called right before the agent is born
    bool TDBrain::initialize(const AgentInitInfo& init)
    {
        mInfo = init;
        this->fitness = mInfo.reward.getInstance();

        int bins = action_bins;

        if (num_tiles > 0)
        {
            AssertMsg(action_bins == 0, "action_bins must be 0 for num_tiles > 0");
            AssertMsg(state_bins == 0, "state_bins must be 0 for num_tiles > 0");
            mApproximator.reset(
                new TilesApproximator(mInfo, num_tiles, num_weights));
            bins = 7; // XXX force bins > 0 to discretize action_list below
        }
        else
        {
            AssertMsg(action_bins > 0, "action_bins > 0 for num_tiles == 0");
            AssertMsg(state_bins > 0, "action_bins > 0 for num_tiles == 0");
            mApproximator.reset(
                new TableApproximator(mInfo, action_bins, state_bins));
        }

        // Similar to FeatureVectorInfo::enumerate (from AI.cpp).
        //
        // We want to enumerate all possible actions in a discrete way, so that
        // we can store them in a policy table somehow. So, for each action
        // dimension, if it's discrete we just enumerate the integral values for
        // that dimension, and if it's continuous, we split it into "bins"
        // different values.
        //
        // An example: Suppose some action dimension is continuous and spans the
        // closed interval [-1, 1]. Additionally suppose we want to split
        // continuous action dimensions into 5 bins. We'd like to preserve the
        // actual range of action values, but only store the 5 discrete values
        // that equally partition this space, i.e., {-1, -.5, 0, .5, 1}. So we
        // traverse the action dimension from -1 to 1 (inclusive), adding
        // (hi - lo) / (bins - 1) == (1 - -1) / 4 == 0.5 each time.
        const FeatureVectorInfo& info = init.actions;
        action_list.clear();
        action_list.push_back(info.getInstance());
        for (size_t i = 0; i < info.size(); ++i)
        {
            const double lo = info.getMin(i), hi = info.getMax(i);
            const double inc = info.isDiscrete(i) ? 1.0f : (hi - lo) / (bins - 1);
            std::vector< Actions > new_action_list;
            std::vector< Actions >::const_iterator iter;
            for (iter = action_list.begin(); iter != action_list.end(); ++iter)
            {
                for (double a = lo; a <= hi; a += inc)
                {
                    FeatureVector v = *iter;
                    v[i] = a;
                    new_action_list.push_back(v);
                }
            }
            action_list = new_action_list;
        }

        return true;
    }

    /// called for agent to take its first step
    Actions TDBrain::start(const TimeType& time, const Observations& new_state)
    {
        epsilon_greedy(new_state);
        action = new_action;
        state = new_state;
        return action;
    }

    /// act based on time, sensor arrays, and last reward
    Actions TDBrain::act(const TimeType& time, const Observations& new_state, const Reward& reward)
    {
		AssertMsg(reward.size() == 1, "multi-objective rewards not supported");
        // select new action and estimate its value
        double new_Q = epsilon_greedy(new_state);
        double old_Q = mApproximator->predict(state, action);
        // Q(s_t, a_t) <- Q(s_t, a_t) + \alpha [r_{t+1} + \gamma Q(s_{t+1}, a_{t+1}) - Q(s_t, a_t)
        mApproximator->update(state, action, old_Q + mAlpha * (reward[0] + mGamma * new_Q - old_Q));
        action = new_action;
        state = new_state;
        return action;
    }

    /// called to tell agent about its last reward
    bool TDBrain::end(const TimeType& time, const Reward& reward)
    {
		AssertMsg(reward.size() == 1, "multi-objective rewards not supported");
		// Q(s_t, a_t) <- Q(s_t, a_t) + \alpha [r_{t+1} - Q(s_t, a_t)]
        // LOG_F_DEBUG("ai", "TD FINAL UPDATE s1: " << state << ", a1: " << action << ", r: " << reward);
        double old_Q = mApproximator->predict(state, action);
        mApproximator->update(state, action, old_Q + mAlpha * (reward[0] - old_Q));
        return true;
    }

    /// select action according to the epsilon-greedy policy
    double TDBrain::epsilon_greedy(const Observations& new_state)
    {
        // with chance epsilon, select random action
        if (RANDOM.randF() < mEpsilon)
        {
            new_action = mInfo.actions.getRandom();
            double value = predict(new_state);
            return value;
        }
        // enumerate all possible actions (actions must be discrete!)
        new_action = mInfo.actions.getInstance();
        // select the greedy action in random order
        std::random_shuffle(action_list.begin(), action_list.end());
        double max_value = -DBL_MAX;
        std::vector< Actions >::const_iterator iter;
        for (iter = action_list.begin(); iter != action_list.end(); ++iter)
        {
            double value = mApproximator->predict(new_state, *iter);
            if (value > max_value)
            {
                max_value = value;
                new_action = *iter;
            }
        }
        // Assuming if you choose max value, you will want to update with that as your prediction
        return max_value;
    }

    /// called right before the agent dies
    bool TDBrain::destroy()
    {
        return true;
    }

    /// serialize this brain to a text string
    std::string TDBrain::to_string() const
    {
        std::ostringstream oss;
        boost::archive::text_oarchive oa(oss);
        oa << *this;
        return oss.str();
    }

    /// deserialize this brain from a text string
    void TDBrain::from_string(const std::string& s)
    {
        try {
            std::istringstream iss(s);
            boost::archive::text_iarchive ia(iss);
            ia >> *this;
        } catch (boost::archive::archive_exception const& e) {
            LOG_F_ERROR("ai.rl", "unable to load agent because of error, " << e.what());
        }
    }
}

BOOST_CLASS_EXPORT(OpenNero::TDBrain)


/*
 * Copyright (c) 2015-2017, Princeton University, Johannes M Dieterich, Emily A Carter
 * 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 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 <armadillo>
#include <cmath>
#include "HelperFunctions.hpp"
using namespace arma;

const static double LOWDENSITYFORCUTOFF = 1E-2;
const static double HIGHDENSITYORCUTOFF = 100.0;
const static double HIPOWER = exp(100.0);

template<class GridComputer, template<class> class GridType>
void CutoffFunctions::WGCVacuumCutoff(unique_ptr<GridType<GridComputer> >& density, const double rhoV, const double rhoStep){

    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (2.0*ctP1Sq*rhoStepSq);
    const double cutHi = (HIPOWER-1.0)/(HIPOWER+ct);

    cube* dens = density->getRealGrid();
    const size_t nSlices = dens->n_slices;
    const size_t nRows = dens->n_rows;
    const size_t nCols = dens->n_cols;
    #pragma omp parallel for default(none) shared(dens)
    for(size_t x = 0; x < nSlices; ++x){
        for(size_t col = 0; col < nCols; ++col){
            for(size_t row = 0; row < nRows; ++row){
                const double div = dens->at(row,col,x)/rhoStep;
                if(div < LOWDENSITYFORCUTOFF){
                    // the cutoff function is taylor expanded to 2nd order
                    const double d = dens->at(row,col,x);
                    dens->at(row,col,x) = d/t1 + (-1.0+ct)*d*d/t2;
                } else if(div > HIGHDENSITYORCUTOFF){
                    dens->at(row,col,x) = cutHi; // density is much bigger
                } else {
                    const double powDens = exp(div);
                    dens->at(row,col,x) = (powDens-1.0)/(powDens+ct);
                }
            }
        }
    }
    density->complete(dens);
}

double CutoffFunctions::vacuumCutoff(const double density, const double rhoV, const double rhoStep){

    // XXX not happy about this block, as it is a lot of recomputing but to save memory, this function may still be useful
    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (2.0*ctP1Sq*rhoStepSq);
    const double cutHi = (HIPOWER-1.0)/(HIPOWER+ct);

    const double div = density/rhoStep;
    if(div < LOWDENSITYFORCUTOFF){
        // the cutoff function is taylor expanded to 2nd order
        return density/t1 + (-1.0+ct)*density*density/t2;
    } else if(div > HIGHDENSITYORCUTOFF){
        return cutHi; // density is much bigger
    } else {
        const double powDens = exp(div);
        return (powDens-1.0)/(powDens+ct);
    }
}

template<class GridComputer, template<class> class GridType>
void CutoffFunctions::WGCVacuumCutoffDeriv(unique_ptr<GridType<GridComputer> >& density, const double rhoV, const double rhoStep){

    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double hiCtSq = (HIPOWER+ct)*(HIPOWER+ct);
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (ctP1Sq*rhoStepSq);
    const double cutHi = HIPOWER*(1.0+ct)/(hiCtSq*rhoStep);

    cube* dens = density->getRealGrid();
    const size_t nSlices = dens->n_slices;
    const size_t nRows = dens->n_rows;
    const size_t nCols = dens->n_cols;
    #pragma omp parallel for default(none) shared(dens)
    for(size_t x = 0; x < nSlices; ++x){
        for(size_t col = 0; col < nCols; ++col){
            for(size_t row = 0; row < nRows; ++row){
                const double div = dens->at(row,col,x)/rhoStep;
                if(div < LOWDENSITYFORCUTOFF){
                    // the cutoff function is taylor expanded to 2nd order
                    const double d = dens->at(row,col,x);
                    dens->at(row,col,x) = 1.0/t1 + (-1.0+ct)*d/t2;
                } else if(div > HIGHDENSITYORCUTOFF){
                    dens->at(row,col,x) = cutHi; // density is much bigger
                } else {
                    const double powDens = exp(div);
                    const double powCt = powDens+ct;
                    const double powCtSq = powCt*powCt;
                    dens->at(row,col,x) = powDens*(1.0+ct)/(powCtSq*rhoStep);
                }
            }
        }
    }
}

double CutoffFunctions::vacuumCutoffDeriv(const double density, const double rhoV, const double rhoStep){

    // XXX not happy about this block, as it is a lot of recomputing but to save memory, this function may still be useful
    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double hiCtSq = (HIPOWER+ct)*(HIPOWER+ct);
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (ctP1Sq*rhoStepSq);
    const double cutHi = HIPOWER*(1.0+ct)/(hiCtSq*rhoStep);

    const double div = density/rhoStep;
    if(div < LOWDENSITYFORCUTOFF){
        // the cutoff function is taylor expanded to 2nd order
        return 1.0/t1 + (-1.0+ct)*density/t2;
    } else if(div > HIGHDENSITYORCUTOFF){
        return  cutHi; // density is much bigger
    } else {
        const double powDens = exp(div);
        const double powCt = powDens+ct;
        const double powCtSq = powCt*powCt;
        return powDens*(1.0+ct)/(powCtSq*rhoStep);
    }
}

double MathFunctions::lindhardResponse(const double eta, const double lambda, const double mu){

    if(eta < 0.0){return 0.0;}
    // limit for small eta
    else if(eta < 1e-10){
        const double lind = 1.0 - lambda + eta*eta * (1.0/3.0-3.0*mu);
        return lind;
    } else if(abs(eta-1.0) < 1e-10){
        const double lind = 2.0 - lambda - 3.0*mu + 20.0*(eta-1.0);
        return lind;
    } else if(eta > 3.65){
        // Taylor expansion for high eta
        const double etaSq = eta*eta;
        const double invEtaSq = 1.0/etaSq;
        const double lind = 3.0*(1.0-mu)*etaSq
            - lambda - 0.6
            + invEtaSq *  (-0.13714285714285712
            + invEtaSq * (-6.39999999999999875E-2
            + invEtaSq * (-3.77825602968460128E-2
            + invEtaSq * (-2.51824061652633074E-2
            + invEtaSq * (-1.80879839616166146E-2
            + invEtaSq * (-1.36715733124818332E-2
            + invEtaSq * (-1.07236045520990083E-2
            + invEtaSq * (-8.65192783339199453E-3
            + invEtaSq * (-7.1372762502456763E-3
            + invEtaSq * (-5.9945117538835746E-3
            + invEtaSq * (-5.10997527675418131E-3
            + invEtaSq * (-4.41060829979912465E-3
            + invEtaSq * (-3.84763737842981233E-3
            + invEtaSq * (-3.38745061493813488E-3
            + invEtaSq * (-3.00624946457977689E-3)))))))))))))));
        return lind;
    } else {
        const double lind = 1.0 / (0.5 + 0.25 * (1.-eta*eta) * log((1.0 + eta)
            / abs(1.0-eta))/eta) - 3.0 * mu * eta*eta - lambda;
        return lind;
    }
}

double MathFunctions::derivativeLindhardResponse(const double eta, const double mu){

    if(eta < 0.0){
        return 0.0;
    } else if(eta < 1e-10){
        return 2*eta*(1.0/3.0 - 3.0*mu);
    } else if(abs(eta-1.0) < 1e-10){
        return 40.0;
    } else {
        const double etaSq = eta*eta;
        const double oneMEta = 1-eta;
        const double onePEta = 1+eta;
        const double denom = (0.5 + 0.25*(1-eta*eta) * log(onePEta/abs(oneMEta))/eta);
        const double denomSq = denom*denom;
        const double gprim = ((etaSq + 1)*0.25 / etaSq * log(abs((1.0+eta)/(oneMEta))) - 0.5/eta) / denomSq - 6*eta*mu;

        return gprim;
    }
}