// Copyright (C) 2011-2012 by the BEM++ Authors
//
// 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.

#include "bempp/common/config_ahmed.hpp"
#include "bempp/common/config_trilinos.hpp"

#include "aca_global_assembler.hpp"

#include "assembly_options.hpp"
#include "block_coalescer.hpp"
#include "cluster_construction_helper.hpp"
#include "context.hpp"
#include "evaluation_options.hpp"
#include "index_permutation.hpp"
#include "discrete_boundary_operator_composition.hpp"
#include "discrete_sparse_boundary_operator.hpp"

#include "../common/armadillo_fwd.hpp"
#include "../common/auto_timer.hpp"
#include "../common/boost_shared_array_fwd.hpp"
#include "../common/chunk_statistics.hpp"
#include "../common/to_string.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/local_assembler_for_integral_operators.hpp"
#include "../fiber/local_assembler_for_potential_operators.hpp"
#include "../fiber/serial_blas_region.hpp"
#include "../fiber/scalar_traits.hpp"
#include "../space/space.hpp"

#include <stdexcept>
#include <fstream>
#include <iostream>

#include <boost/type_traits/is_complex.hpp>

#include <tbb/atomic.h>
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/concurrent_queue.h>

#ifdef WITH_AHMED
#include "ahmed_aux.hpp"

#ifdef __INTEL_COMPILER
#pragma warning(disable : 381)
#endif

#include <apprx.h>

#ifdef __INTEL_COMPILER
#pragma warning(default : 381)
#endif

#include "discrete_aca_boundary_operator.hpp"
#include "modified_aca.hpp"
#include "potential_operator_aca_assembly_helper.hpp"
#include "scattered_range.hpp"
#include "weak_form_aca_assembly_helper.hpp"
#endif

// This is a workaround of the problem of the abs() function being declared
// both in Epetra and in AHMED. It relies of the implementation detail (!) that
// in Epetra the declaration of abs is put between #ifndef __IBMCPP__ ...
// #endif. So it may well break in future versions of Trilinos. The ideal
// solution would be for AHMED to use namespaces.

// Note: this particular instance of this kludge could be removed --
// we could move permuteEpetraMatrix() to another file
#ifndef __IBMCPP__
#define __IBMCPP__
#include <Epetra_CrsMatrix.h>
#include <EpetraExt_MatrixMatrix.h>
#undef __IBMCPP__
#else
#include <Epetra_CrsMatrix.h>
#include <EpetraExt_MatrixMatrix.h>
#endif

// #define DUMP_DENSE_BLOCKS // if defined, contents and DOF lists of blocks
// stored as dense matrices will be printed to the
// screen

namespace Bempp {

// Body of parallel loop
namespace {

#ifdef WITH_AHMED
template <typename BasisFunctionType, typename ResultType,
          typename AcaAssemblyHelper>
class AcaAssemblerLoopBody {
  typedef typename Fiber::ScalarTraits<ResultType>::RealType CoordinateType;
  typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
  typedef bbxbemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
  typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;

public:
  typedef tbb::concurrent_queue<size_t> LeafClusterIndexQueue;

  AcaAssemblerLoopBody(AcaAssemblyHelper *helper,
                       AcaAssemblyHelper *admissibleHelper,
                       AhmedLeafClusterArray &leafClusters,
                       AhmedLeafClusterArray &localLeafClusters,
                       LeafClusterIndexQueue &leafClusterIndexQueue,
                       boost::shared_array<AhmedMblock *> blocks,
                       boost::shared_array<AhmedMblock *> flatLocalBlocks,
                       BlockCoalescer<ResultType> *coalescer,
                       const AcaOptions &options, tbb::atomic<size_t> &done,
                       bool verbose, bool symmetric,
                       std::vector<ChunkStatistics> &stats)
      : m_helper(helper), m_admissibleHelper(admissibleHelper),
        m_leafClusters(leafClusters), m_localLeafClusters(localLeafClusters),
        m_leafClusterIndexQueue(leafClusterIndexQueue), m_blocks(blocks),
        m_flatLocalBlocks(flatLocalBlocks), m_coalescer(coalescer),
        m_options(options), m_done(done), m_verbose(verbose),
        m_symmetric(symmetric), m_stats(stats) {}

  template <typename Range> void operator()(const Range &r) const {
    const char *TEXT = "Approximating ... ";
    for (typename Range::const_iterator i = r.begin(); i != r.end(); ++i) {
      size_t leafClusterIndex = 0;
      if (!m_leafClusterIndexQueue.try_pop(leafClusterIndex)) {
        std::cerr << "AcaWeakFormAssemblerLoopBody::operator(): "
                     "Warning: try_pop failed; this shouldn't happen!"
                  << std::endl;
        continue;
      }
      m_stats[leafClusterIndex].valid = true;
      m_stats[leafClusterIndex].chunkStart = r.begin();
      m_stats[leafClusterIndex].chunkSize = r.size();
      m_stats[leafClusterIndex].startTime = tbb::tick_count::now();

      AhmedBemBlcluster *cluster =
          dynamic_cast<AhmedBemBlcluster *>(m_leafClusters[leafClusterIndex]);
      AhmedBemBlcluster *localCluster = dynamic_cast<AhmedBemBlcluster *>(
          m_localLeafClusters[leafClusterIndex]);
      bool globalAssembly = m_options.mode != AcaOptions::HYBRID_ASSEMBLY ||
                            !localCluster->isadm();
      AcaAssemblyHelper *helper =
          globalAssembly ? m_helper : m_admissibleHelper;
      AhmedMblock **blocks =
          globalAssembly ? m_blocks.get() : m_flatLocalBlocks.get();
      if (!globalAssembly)
        cluster = localCluster;
      if (m_symmetric)
        apprx_sym(*helper, blocks[cluster->getidx()], cluster, m_options.eps,
                  m_options.maximumRank, true /* complex_sym */);
      else {
        if (m_options.useAhmedAca)
          apprx_unsym(*helper, blocks[cluster->getidx()], cluster,
                      m_options.eps, m_options.maximumRank);
        else
          apprx_unsym_shooting(*helper, blocks[cluster->getidx()], cluster,
                               m_options.eps, m_options.maximumRank);
      }
      if (m_leafClusters[leafClusterIndex]->isadm() &&
          !blocks[cluster->getidx()]->isLrM()) {
        // // Show clusters for which ACA failed
        //                std::cout << "global assembly: " << globalAssembly <<
        // "\n";
        //                typedef ExtendedBemCluster<AhmedDofType>
        // AhmedBemCluster;
        //                if (globalAssembly) {
        //                    std::cout << "Global cluster 1:\n";
        //                    dynamic_cast<AhmedBemCluster&>(
        //                                *cluster->getcl1()).printBboxes();
        //                    std::cout << "Global cluster 2:\n";
        //                    dynamic_cast<AhmedBemCluster&>(
        //                                *cluster->getcl2()).printBboxes();
        //                } else {
        //                    std::cout << "Local cluster 1:\n";
        //                    dynamic_cast<AhmedBemCluster&>(
        //                                *localCluster->getcl1()).printBboxes();
        //                    std::cout << "Local cluster 2:\n";
        //                    dynamic_cast<AhmedBemCluster&>(
        //                                *localCluster->getcl2()).printBboxes();
        //                }
      }
      if (!globalAssembly)
        m_coalescer->coalesceBlock(cluster->getidx());
      m_stats[leafClusterIndex].endTime = tbb::tick_count::now();
      // TODO: recompress
      const int HASH_COUNT = 20;
      if (m_verbose)
        progressbar(std::cout, TEXT, (++m_done) - 1, m_leafClusters.size(),
                    HASH_COUNT, true);
    }
  }

private:
  AcaAssemblyHelper *m_helper;
  AcaAssemblyHelper *m_admissibleHelper;
  AhmedLeafClusterArray &m_leafClusters;
  AhmedLeafClusterArray &m_localLeafClusters;
  boost::shared_array<AhmedMblock *> m_blocks;
  boost::shared_array<AhmedMblock *> m_flatLocalBlocks;
  BlockCoalescer<ResultType> *m_coalescer;
  const AcaOptions &m_options;
  tbb::atomic<size_t> &m_done;
  bool m_verbose;
  LeafClusterIndexQueue &m_leafClusterIndexQueue;
  bool m_symmetric;
  std::vector<ChunkStatistics> &m_stats;
};

void reallyGetClusterIds(const cluster &clusterTree,
                         const std::vector<unsigned int> &p2oDofs,
                         std::vector<unsigned int> &clusterIds,
                         unsigned int &id) {
  if (clusterTree.isleaf())
    for (unsigned int nDof = clusterTree.getnbeg();
         nDof < clusterTree.getnend(); ++nDof)
      clusterIds[p2oDofs[nDof]] = id;
  else
    for (unsigned int nSon = 0; nSon < clusterTree.getns(); ++nSon)
      reallyGetClusterIds(*clusterTree.getson(nSon), p2oDofs, clusterIds, ++id);
}

void getClusterIds(const cluster &clusterTree,
                   const std::vector<unsigned int> &p2oDofs,
                   std::vector<unsigned int> &clusterIds) {
  clusterIds.resize(p2oDofs.size());
  unsigned int id = 0;
  reallyGetClusterIds(clusterTree, p2oDofs, clusterIds, id);
}

template <typename T>
void save_arma_matrix(const arma::Mat<T> &a, const std::string &fname) {
  std::ofstream out;
  out.precision(17);
  out.open(fname.c_str());
  arma::diskio::save_raw_ascii(a, out);
  out.close();
}
template <typename T>
void save_arma_matrix(const arma::Mat<std::complex<T>> &a,
                      const std::string &fname) {
  std::ofstream out;
  out.precision(17);
  out.open((fname + "-real.txt").c_str());
  arma::Mat<T> component = arma::real(a);
  arma::diskio::save_raw_ascii(component, out);
  out.close();
  out.open((fname + "-imag.txt").c_str());
  component = arma::imag(a);
  arma::diskio::save_raw_ascii(component, out);
  out.close();
}

template <typename ValueType>
void dumpDenseBlocks(
    typename DiscreteAcaBoundaryOperator<ValueType>::AhmedBemBlcluster *
        clusterTree,
    typename DiscreteAcaBoundaryOperator<ValueType>::AhmedMblockArray &blocks,
    const std::vector<unsigned int> &p2oRows,
    const std::vector<unsigned int> &p2oCols,
    const std::vector<
        Point3D<typename Fiber::ScalarTraits<ValueType>::RealType>> &rowDofs,
    const std::vector<
        Point3D<typename Fiber::ScalarTraits<ValueType>::RealType>> &colDofs,
    int singleClusterIndex = -1) {
  if (!clusterTree)
    return;
  typedef typename Fiber::ScalarTraits<ValueType>::RealType CoordinateType;
  typedef DiscreteAcaBoundaryOperator<ValueType> AcaOp;
  typedef typename AcaOp::AhmedDofType AhmedDofType;
  typedef typename AcaOp::AhmedMblock AhmedMblock;
  typedef bbxbemcluster<AhmedDofType> Cluster;
  if (clusterTree->isleaf()) {
    unsigned int idx = clusterTree->getidx();
    if ((singleClusterIndex < 0 && clusterTree->isGeM(blocks.get()) &&
         clusterTree->isadm()) ||
        idx == singleClusterIndex) {
      std::cout << "Dense block; " << clusterTree->getb1() << " "
                << clusterTree->getb2() << " " << clusterTree->getn1() << " "
                << clusterTree->getn2() << "\n";
      // if (clusterTree->getn1() < 500 || clusterTree->getn2() < 500)
      //     return;
      Cluster *clRow = clusterTree->getcl1();
      assert(clRow);
      std::cout << "Row center of mass: (" << clRow->getcom(0) << ", "
                << clRow->getcom(1) << ", " << clRow->getcom(2) << ")\n";
      std::cout << "Row icm: " << clRow->geticom() - clusterTree->getb1()
                << std::endl;
      for (unsigned int nDof = clRow->getnbeg(); nDof < clRow->getnend();
           ++nDof) {
        assert(nDof < p2oRows.size());
        assert(p2oRows[nDof] < rowDofs.size());
        const Point3D<CoordinateType> dofPos = rowDofs[p2oRows[nDof]];
        std::cout << "  Row dof #" << p2oRows[nDof] << " at " << dofPos.x
                  << ", " << dofPos.y << ", " << dofPos.z << "\n";
      }
      Cluster *clCol = clusterTree->getcl2();
      assert(clCol);
      std::cout << "Col center of mass: (" << clCol->getcom(0) << ", "
                << clCol->getcom(1) << ", " << clCol->getcom(2) << ")\n";
      std::cout << "Column icm: " << clCol->geticom() - clusterTree->getb2()
                << std::endl;
      for (unsigned int nDof = clCol->getnbeg(); nDof < clCol->getnend();
           ++nDof) {
        assert(nDof < p2oCols.size());
        assert(p2oCols[nDof] < colDofs.size());
        const Point3D<CoordinateType> dofPos = colDofs[p2oCols[nDof]];
        std::cout << "  Col dof #" << p2oCols[nDof] << " at " << dofPos.x
                  << ", " << dofPos.y << ", " << dofPos.z << "\n";
      }
      //            AhmedMblock* block = blocks[idx];
      //            arma::Mat<ValueType> ablock(clusterTree->getn1(),
      //                                        clusterTree->getn2());
      //            for (size_t i = 0; i < block->nvals(); ++i)
      //                ablock[i] = block->getdata()[i];
      //            save_arma_matrix(ablock, "block-" + toString(idx) + ".txt");
    }
  } else
    for (unsigned int nRowSon = 0; nRowSon < clusterTree->getnrs(); ++nRowSon)
      for (unsigned int nColSon = 0; nColSon < clusterTree->getncs(); ++nColSon)
        dumpDenseBlocks<ValueType>(
            dynamic_cast<typename AcaOp::AhmedBemBlcluster *>(
                clusterTree->getson(nRowSon, nColSon)),
            blocks, p2oRows, p2oCols, rowDofs, colDofs, singleClusterIndex);
}

shared_ptr<const Epetra_CrsMatrix>
permuteEpetraCrsMatrix(const Epetra_CrsMatrix &mat,
                       const IndexPermutation &p2oCols,
                       const IndexPermutation &o2pRows) {
  shared_ptr<const Epetra_CrsMatrix> p2oColsMat = p2oCols.permutationMatrix();
  shared_ptr<const Epetra_CrsMatrix> o2pRowsMat = o2pRows.permutationMatrix();

  shared_ptr<Epetra_CrsMatrix> tmp(
      new Epetra_CrsMatrix(Copy, mat.RowMap(), p2oColsMat->ColMap(),
                           3 /* three entries per row  -- just a guess */));
  EpetraExt::MatrixMatrix::Multiply(mat, false, *p2oColsMat, false, *tmp);
  shared_ptr<Epetra_CrsMatrix> result(
      new Epetra_CrsMatrix(Copy, o2pRowsMat->RowMap(), tmp->ColMap(),
                           3 /* three entries per row  -- just a guess */));
  EpetraExt::MatrixMatrix::Multiply(*o2pRowsMat, false, *tmp, false, *result);

  return result;
}

// Reorder elements of array leafCluster so that for each i
// leafClusters[i]->getidx() == refLeafClusters[i]->getidx()
void reorderIdentically(AhmedLeafClusterArray &leafClusters,
                        const AhmedLeafClusterArray &refLeafClusters) {
  const size_t size = leafClusters.size();
  if (refLeafClusters.size() != size)
    throw std::invalid_argument("reorderIdentically(): "
                                "arrays must have the same size");
  std::vector<blcluster *> idx2pblcluster(size);
  for (size_t i = 0; i < size; ++i) {
    unsigned idx = leafClusters[i]->getidx();
    if (idx >= size)
      throw std::invalid_argument("reorderIdentically(): "
                                  "invalid cluster index encountered");
    idx2pblcluster[leafClusters[i]->getidx()] = leafClusters[i];
  }
  for (size_t i = 0; i < size; ++i)
    leafClusters[i] = idx2pblcluster[refLeafClusters[i]->getidx()];
  for (size_t i = 0; i < size; ++i)
    assert(leafClusters[i]->getidx() == refLeafClusters[i]->getidx());
}

template <typename AcaAssemblyHelper, typename BasisFunctionType,
          typename ResultType>
std::unique_ptr<DiscreteAcaBoundaryOperator<ResultType>> assembleAcaOperator(
    AcaAssemblyHelper *helper, AcaAssemblyHelper *admissibleHelper,
    const shared_ptr<typename DiscreteAcaBoundaryOperator<
        ResultType>::AhmedBemBlcluster> &blclusterTree,
    const shared_ptr<typename DiscreteAcaBoundaryOperator<
        ResultType>::AhmedBemBlcluster> &localBlclusterTree,
    const ParallelizationOptions &parallelOptions, const AcaOptions &acaOptions,
    bool verbosityAtLeastDefault, bool verbosityAtLeastHigh, bool symmetric,
    const shared_ptr<IndexPermutation> &test_o2pPermutation,
    const shared_ptr<IndexPermutation> &trial_o2pPermutation,
    const shared_ptr<const Epetra_CrsMatrix> &permutedTestGlobalToLocalMap,
    const shared_ptr<const Epetra_CrsMatrix> &permutedTrialGlobalToLocalMap
#ifdef DUMP_DENSE_BLOCKS
    ,
    const shared_ptr<IndexPermutation> &test_p2oPermutation,
    const shared_ptr<IndexPermutation> &trial_p2oPermutation,
    const std::vector<Point3D<
        typename Fiber::ScalarTraits<ResultType>::RealType>> &testDofCenters,
    const std::vector<Point3D<
        typename Fiber::ScalarTraits<ResultType>::RealType>> &trialDofCenters
#endif // DUMP_DENSE_BLOCKS
    ) {
  const bool indexWithGlobalDofs =
      acaOptions.mode != AcaOptions::HYBRID_ASSEMBLY;

  typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;
  boost::shared_array<AhmedMblock *> blocks =
      allocateAhmedMblockArray<ResultType>(blclusterTree.get());
  boost::shared_array<AhmedMblock *> decomposedBlocks;
  if (!indexWithGlobalDofs)
    decomposedBlocks =
        allocateAhmedMblockArray<ResultType>(localBlclusterTree.get());

  const size_t testDofCount = test_o2pPermutation->size();
  const size_t trialDofCount = trial_o2pPermutation->size();

  AhmedLeafClusterArray leafClusters(blclusterTree.get());
  leafClusters.sortAccordingToClusterSize();
  if (acaOptions.firstClusterIndex >= 0)
    leafClusters.startWithClusterOfIndex(acaOptions.firstClusterIndex);
  const size_t leafClusterCount = leafClusters.size();

  AhmedLeafClusterArray localLeafClusters(localBlclusterTree.get());
  reorderIdentically(localLeafClusters, leafClusters);

  int maxThreadCount = 1;
  if (!parallelOptions.isOpenClEnabled()) {
    if (parallelOptions.maxThreadCount() == ParallelizationOptions::AUTO)
      maxThreadCount = tbb::task_scheduler_init::automatic;
    else
      maxThreadCount = parallelOptions.maxThreadCount();
  }
  tbb::task_scheduler_init scheduler(maxThreadCount);
  tbb::atomic<size_t> done;
  done = 0;

#ifdef DUMP_DENSE_BLOCKS
  if (acaOptions.firstClusterIndex >= 0)
    dumpDenseBlocks<ResultType>(
        bemBlclusterTree.get(), blocks, test_p2oPermutation->permutedIndices(),
        trial_p2oPermutation->permutedIndices(), testDofCenters,
        trialDofCenters, acaOptions.firstClusterIndex);
#endif

  std::vector<ChunkStatistics> chunkStats(leafClusterCount);

  typedef AcaAssemblerLoopBody<BasisFunctionType, ResultType, AcaAssemblyHelper>
  Body;
  typename Body::LeafClusterIndexQueue leafClusterIndexQueue;
  for (size_t i = 0; i < leafClusterCount; ++i)
    leafClusterIndexQueue.push(i);

  std::unique_ptr<BlockCoalescer<ResultType>> coalescer;
  if (!indexWithGlobalDofs)
    coalescer.reset(new BlockCoalescer<ResultType>(
        blclusterTree.get(), localBlclusterTree.get(),
        permutedTestGlobalToLocalMap, permutedTrialGlobalToLocalMap, blocks,
        decomposedBlocks, acaOptions));
  if (verbosityAtLeastDefault)
    std::cout << "About to start the ACA assembly loop" << std::endl;
  tbb::tick_count loopStart = tbb::tick_count::now();
  {
    Fiber::SerialBlasRegion region; // if possible, ensure that BLAS is
                                    // single-threaded
    tbb::parallel_for(tbb::blocked_range<size_t>(0, leafClusterCount),
                      Body(helper, admissibleHelper, leafClusters,
                           localLeafClusters, leafClusterIndexQueue, blocks,
                           decomposedBlocks, coalescer.get(), acaOptions, done,
                           verbosityAtLeastDefault, symmetric, chunkStats));
  }
  tbb::tick_count loopEnd = tbb::tick_count::now();
  if (verbosityAtLeastDefault) {
    std::cout << "\n"; // the progress bar doesn't print the final \n
    std::cout << "ACA loop took " << (loopEnd - loopStart).seconds() << " s"
              << std::endl;
  }

  // TODO: parallelise!
  if (acaOptions.recompress) {
    if (verbosityAtLeastDefault)
      std::cout << "About to start ACA agglomeration" << std::endl;
    agglH(blclusterTree.get(), blocks.get(), acaOptions.eps,
          acaOptions.maximumRank);
    if (verbosityAtLeastDefault)
      std::cout << "Agglomeration finished" << std::endl;
  }

  // // Dump timing data of individual chunks
  //    std::cout << "\nChunks:\n";
  //    for (int i = 0; i < leafClusterCount; ++i)
  //        if (chunkStats[i].valid) {
  //            int blockIndex = leafClusters[i]->getidx();
  //            std::cout << chunkStats[i].chunkStart << "\t"
  //                      << chunkStats[i].chunkSize << "\t"
  //                      << (chunkStats[i].startTime - loopStart).seconds() <<
  // "\t"
  //                      << (chunkStats[i].endTime - loopStart).seconds() <<
  // "\t"
  //                      << (chunkStats[i].endTime -
  // chunkStats[i].startTime).seconds() << "\t"
  //                      << blocks[blockIndex]->getn1() << "\t"
  //                      << blocks[blockIndex]->getn2() << "\t"
  //                      << blocks[blockIndex]->islwr() << "\t"
  //                      << (blocks[blockIndex]->islwr() ?
  // blocks[blockIndex]->rank() : 0) << "\n";
  //        }

  if (verbosityAtLeastDefault) {
#ifdef CHECK_ACA_ERROR // a define from include/AHMED/apprx.h
    std::cout << "ACA finished. Max error: " << ACA_error_max << std::endl;
#endif
    size_t totalEntryCount = testDofCount * trialDofCount;
    size_t origMemory = sizeof(ResultType) * totalEntryCount;
    size_t ahmedMemory = sizeH(blclusterTree.get(), blocks.get());
    int maximumRank = Hmax_rank(blclusterTree.get(), blocks.get());
    size_t accessedEntryCount = helper->accessedEntryCount();
    double accessedFraction = double(accessedEntryCount) / totalEntryCount;
    std::cout << "\nNeeded storage: " << ahmedMemory / 1024. / 1024. << " MB.\n"
              << "Without approximation: " << origMemory / 1024. / 1024.
              << " MB.\n"
              << "Compressed to " << (100. * ahmedMemory) / origMemory << "%.\n"
              << "Maximum rank: " << maximumRank << ".\n";
    // We only display this information in global and local ACA modes;
    // in hybrid modes the percentage is not really well defined,
    // since some blocks are accessed using global indexing, and others
    // using local indexing
    if (indexWithGlobalDofs)
      std::cout << "Accessed " << 100. * accessedFraction
                << "% matrix entries.\n";

    tbb::tick_count::interval_t localAdmTime, globalAdmTime, inadmTime;
    for (size_t i = 0; i < leafClusterCount; ++i)
      if (localLeafClusters[i]->isadm())
        localAdmTime += chunkStats[i].endTime - chunkStats[i].startTime;
      else if (leafClusters[i]->isadm())
        globalAdmTime += chunkStats[i].endTime - chunkStats[i].startTime;
      else
        inadmTime += chunkStats[i].endTime - chunkStats[i].startTime;

    if (verbosityAtLeastHigh) {
      std::cout << "CPU time spent on assembly of admissible local blocks: "
                << localAdmTime.seconds() << " s\n";
      std::cout << "CPU time spent on assembly of admissible global blocks: "
                << globalAdmTime.seconds() << " s\n";
      std::cout << "CPU time spent on assembly of inadmissible blocks: "
                << inadmTime.seconds() << "\n";
    }
    std::cout << std::endl;
  }

  if (acaOptions.outputPostscript) {
    if (verbosityAtLeastDefault)
      std::cout << "Writing matrix partition ..." << std::flush;
    std::ofstream os(acaOptions.outputFname.c_str());
    if (symmetric)
      // psoutputHeH() seems to work also for symmetric matrices
      psoutputHeH(os, blclusterTree.get(), trialDofCount, blocks.get());
    else
      psoutputGeH(os, blclusterTree.get(),
                  std::max(testDofCount, trialDofCount), blocks.get());
    os.close();
    if (verbosityAtLeastDefault)
      std::cout << " done." << std::endl;
  }

  int outSymmetry = NO_SYMMETRY;
  if (symmetric) {
    outSymmetry = SYMMETRIC;
    if (!boost::is_complex<ResultType>())
      outSymmetry |= HERMITIAN;
  }
  typedef DiscreteAcaBoundaryOperator<ResultType> DiscreteAcaLinOp;
  std::unique_ptr<DiscreteAcaLinOp> acaOp(new DiscreteAcaLinOp(
      testDofCount, trialDofCount, acaOptions.eps, acaOptions.maximumRank,
      outSymmetry, blclusterTree, blocks, *trial_o2pPermutation, // domain
      *test_o2pPermutation,                                      // range
      parallelOptions));
  return acaOp;
}

#endif

} // namespace

template <typename BasisFunctionType, typename ResultType>
std::unique_ptr<DiscreteBoundaryOperator<ResultType>>
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
    const Space<BasisFunctionType> &testSpace,
    const Space<BasisFunctionType> &trialSpace,
    const std::vector<LocalAssemblerForIntegralOperators *> &localAssemblers,
    const std::vector<LocalAssemblerForIntegralOperators *> &
        localAssemblersForAdmissibleBlocks,
    const std::vector<const DiscreteBndOp *> &sparseTermsToAdd,
    const std::vector<ResultType> &denseTermMultipliers,
    const std::vector<ResultType> &sparseTermMultipliers,
    const Context<BasisFunctionType, ResultType> &context, int symmetry) {
#ifdef WITH_AHMED
  typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
  typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;
  typedef bbxbemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
  typedef DiscreteAcaBoundaryOperator<ResultType> DiscreteAcaLinOp;

  const AssemblyOptions &options = context.assemblyOptions();
  const AcaOptions &acaOptions = options.acaOptions();
  const bool indexWithGlobalDofs =
      acaOptions.mode != AcaOptions::HYBRID_ASSEMBLY;
  const bool verbosityAtLeastDefault =
      (options.verbosityLevel() >= VerbosityLevel::DEFAULT);
  const bool verbosityAtLeastHigh =
      (options.verbosityLevel() >= VerbosityLevel::HIGH);

  // Currently we don't support Hermitian ACA operators. This is because we
  // don't have the means to really test them -- we would need complex-valued
  // basis functions for that. (Assembly of such a matrix would be very easy
  // -- just change complex_sym from true to false in the call to apprx_sym()
  // in AcaWeakFormAssemblerLoopBody::operator() -- but operations on
  // symmetric/Hermitian matrices are not always trivial and we do need to be
  // able to test them properly.)
  bool symmetric = symmetry & SYMMETRIC;
  if (symmetry & HERMITIAN && !(symmetry & SYMMETRIC) &&
      verbosityAtLeastDefault)
    std::cout << "Warning: assembly of non-symmetric Hermitian H-matrices "
                 "is not supported yet. A general H-matrix will be assembled"
              << std::endl;

#ifndef WITH_TRILINOS
  if (!indexWithGlobalDofs)
    throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                             "local-mode ACA assembly requires BEM++ to be "
                             "linked with Trilinos");
#endif // WITH_TRILINOS

  const size_t testDofCount = indexWithGlobalDofs
                                  ? testSpace.globalDofCount()
                                  : testSpace.flatLocalDofCount();
  const size_t trialDofCount = indexWithGlobalDofs
                                   ? trialSpace.globalDofCount()
                                   : trialSpace.flatLocalDofCount();

  if (symmetric && testDofCount != trialDofCount)
    throw std::invalid_argument(
        "AcaGlobalAssembler::assembleDetachedWeakForm(): "
        "you cannot generate a symmetric weak form "
        "using test and trial spaces with different "
        "numbers of DOFs");

  // Construct cluster trees indexed with global indices

  // o2p: map of original indices to permuted indices
  // p2o: map of permuted indices to original indices
  typedef ClusterConstructionHelper<BasisFunctionType> CCH;
  shared_ptr<AhmedBemCluster> testClusterTree;
  shared_ptr<IndexPermutation> test_o2pPermutation, test_p2oPermutation;
  CCH::constructBemCluster(testSpace, true /*indexWithGlobalDofs*/, acaOptions,
                           testClusterTree, test_o2pPermutation,
                           test_p2oPermutation);
  shared_ptr<AhmedBemCluster> trialClusterTree;
  shared_ptr<IndexPermutation> trial_o2pPermutation, trial_p2oPermutation;
  if (symmetric || &testSpace == &trialSpace) {
    trialClusterTree = testClusterTree;
    trial_o2pPermutation = test_o2pPermutation;
    trial_p2oPermutation = test_p2oPermutation;
  } else
    CCH::constructBemCluster(trialSpace, true /*indexWithGlobalDofs*/,
                             acaOptions, trialClusterTree, trial_o2pPermutation,
                             trial_p2oPermutation);

  // If necessary, construct cluster trees indexed with flat local indices
  shared_ptr<AhmedBemCluster> testLocalClusterTree = testClusterTree;
  shared_ptr<IndexPermutation> testLocal_o2pPermutation = test_o2pPermutation;
  shared_ptr<IndexPermutation> testLocal_p2oPermutation = test_p2oPermutation;
  shared_ptr<AhmedBemCluster> trialLocalClusterTree = trialClusterTree;
  shared_ptr<IndexPermutation> trialLocal_o2pPermutation = trial_o2pPermutation;
  shared_ptr<IndexPermutation> trialLocal_p2oPermutation = trial_p2oPermutation;
  if (!indexWithGlobalDofs) {
    if (!testSpace.isDiscontinuous())
      CCH::constructBemCluster(testSpace, false /*indexWithGlobalDofs*/,
                               acaOptions, testLocalClusterTree,
                               testLocal_o2pPermutation,
                               testLocal_p2oPermutation);
    if (symmetric || &testSpace == &trialSpace) {
      trialLocalClusterTree = testLocalClusterTree;
      trialLocal_o2pPermutation = testLocal_o2pPermutation;
      trialLocal_p2oPermutation = testLocal_p2oPermutation;
    } else if (!trialSpace.isDiscontinuous())
      CCH::constructBemCluster(trialSpace, false /*indexWithGlobalDofs*/,
                               acaOptions, trialLocalClusterTree,
                               trialLocal_o2pPermutation,
                               trialLocal_p2oPermutation);
  }

  //    // Export VTK plots showing the disctribution of leaf cluster ids
  //    std::vector<unsigned int> testClusterIds;
  //    getClusterIds(*testClusterTree, test_p2oPermutation->permutedIndices(),
  // testClusterIds);
  //    testSpace.dumpClusterIds("testClusterIds", testClusterIds,
  //                             indexWithGlobalDofs ? GLOBAL_DOFS :
  // FLAT_LOCAL_DOFS);
  //    std::vector<unsigned int> trialClusterIds;
  //    getClusterIds(*trialClusterTree,
  // trial_p2oPermutation->permutedIndices(), trialClusterIds);
  //    trialSpace.dumpClusterIds("trialClusterIds", trialClusterIds,
  //                              indexWithGlobalDofs ? GLOBAL_DOFS :
  // FLAT_LOCAL_DOFS);

  if (verbosityAtLeastHigh)
    std::cout << "Test cluster count: " << testClusterTree->getncl()
              << "\nTrial cluster count: " << trialClusterTree->getncl()
              << std::endl;

  // Create block cluster trees
  unsigned int blockCount = 0;
  bool useStrongAdmissibilityCondition =
      !indexWithGlobalDofs ||
      // experiments indicate that for spaces with discontinuous basis
      // functions one gets faster assembly (although *slightly* higher memory
      // consumption) with the strong admissibility condition
      (testSpace.isDiscontinuous() && trialSpace.isDiscontinuous());
  shared_ptr<AhmedBemBlcluster> blclusterTree(CCH::constructBemBlockCluster(
      acaOptions, symmetric, *testClusterTree, *trialClusterTree,
      useStrongAdmissibilityCondition, blockCount).release());
  shared_ptr<AhmedBemBlcluster> localBlclusterTree = blclusterTree;
  if (!indexWithGlobalDofs &&
      (!testSpace.isDiscontinuous() || !trialSpace.isDiscontinuous())) {
    unsigned int localBlockCount = 0;
    localBlclusterTree.reset(CCH::constructBemBlockCluster(
        acaOptions, symmetric, *testLocalClusterTree, *trialLocalClusterTree,
        useStrongAdmissibilityCondition, localBlockCount).release());
    CCH::truncateBemBlockCluster(localBlclusterTree.get(), blclusterTree.get());
    if (localBlclusterTree->nleaves() != blclusterTree->nleaves())
      throw std::runtime_error(
          "AcaGlobalAssembler::assembleDetachedWeakForm(): "
          "internal error: truncated local-dof cluster tree is not "
          "identical to global-dof cluster tree");
  }

  if (verbosityAtLeastHigh)
    std::cout << "Mblock count: " << blockCount << std::endl;

#ifdef DUMP_DENSE_BLOCKS
  std::vector<Point3D<CoordinateType>> testDofCenters, trialDofCenters;
  if (indexWithGlobalDofs) {
    testSpace.getGlobalDofPositions(testDofCenters);
    trialSpace.getGlobalDofPositions(trialDofCenters);
  } else {
    testSpace.getFlatLocalDofPositions(testDofCenters);
    trialSpace.getFlatLocalDofPositions(trialDofCenters);
  }
#endif // DUMP_DENSE_BLOCKS

  typedef WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>
  AcaAssemblyHelper;

  // Construct assembly helpers

  // It will be possible to remove this kludge if we get rid of the concept
  // of flat local indices and just use a separate discontinuous space.
  AssemblyOptions assemblyOptionsWithGlobalIndices = options;
  AcaOptions acaOptionsWithGlobalIndices = acaOptions;
  acaOptionsWithGlobalIndices.mode = AcaOptions::GLOBAL_ASSEMBLY;
  assemblyOptionsWithGlobalIndices.switchToAcaMode(acaOptionsWithGlobalIndices);

  // TODO: It might be better (more efficient and elegant)
  // to pass p2oPermutation than p2oDofs.
  // Also, it might be more logical to rename IndexPermutation to IndexMapping
  // and permute/unpermute to map/unmap.
  shared_ptr<AcaAssemblyHelper> helper(new AcaAssemblyHelper(
      testSpace, trialSpace, test_p2oPermutation->permutedIndices(),
      trial_p2oPermutation->permutedIndices(), localAssemblers,
      sparseTermsToAdd, denseTermMultipliers, sparseTermMultipliers,
      assemblyOptionsWithGlobalIndices));
  shared_ptr<AcaAssemblyHelper> admissibleHelper = helper;
  if (!indexWithGlobalDofs)
    admissibleHelper.reset(new AcaAssemblyHelper(
        testSpace, trialSpace, testLocal_p2oPermutation->permutedIndices(),
        trialLocal_p2oPermutation->permutedIndices(),
        localAssemblersForAdmissibleBlocks, sparseTermsToAdd,
        denseTermMultipliers, sparseTermMultipliers, options));

  // If necessary, construct maps between (permuted) flat local and global
  // indices
  shared_ptr<const Epetra_CrsMatrix> testGlobalToLocal, trialGlobalToLocal;
  if (!indexWithGlobalDofs) {
    typedef DiscreteSparseBoundaryOperator<ResultType> SparseOp;
    if (!testSpace.isDiscontinuous()) {
      shared_ptr<SparseOp> op = constructOperatorMappingGlobalToFlatLocalDofs<
          BasisFunctionType, ResultType>(testSpace);
      testGlobalToLocal = op->epetraMatrix();
      testGlobalToLocal = permuteEpetraCrsMatrix(
          *testGlobalToLocal, *test_p2oPermutation, *testLocal_o2pPermutation);
    }
    if (!trialSpace.isDiscontinuous()) {
      shared_ptr<SparseOp> op = constructOperatorMappingGlobalToFlatLocalDofs<
          BasisFunctionType, ResultType>(trialSpace);
      trialGlobalToLocal = op->epetraMatrix();
      trialGlobalToLocal =
          permuteEpetraCrsMatrix(*trialGlobalToLocal, *trial_p2oPermutation,
                                 *trialLocal_o2pPermutation);
    }
  }

  std::unique_ptr<DiscreteAcaBoundaryOperator<ResultType>> acaOp =
      assembleAcaOperator<AcaAssemblyHelper, BasisFunctionType, ResultType>(
          helper.get(), admissibleHelper.get(), blclusterTree,
          localBlclusterTree, options.parallelizationOptions(), acaOptions,
          verbosityAtLeastDefault, verbosityAtLeastHigh, symmetric,
          test_o2pPermutation, trial_o2pPermutation, testGlobalToLocal,
          trialGlobalToLocal
#ifdef DUMP_DENSE_BLOCKS
          ,
          test_p2oPermutation, trial_p2oPermutation, testDofCenters,
          trialDofCenters
#endif // DUMP_DENSE_BLOCKS
          );

  std::unique_ptr<DiscreteBndOp> result;
  result = acaOp;
  return result;

#else  // without Ahmed
  throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                           "To enable assembly in ACA mode, recompile BEM++ "
                           "with the symbol WITH_AHMED defined.");
#endif // WITH_AHMED
}

template <typename BasisFunctionType, typename ResultType>
std::unique_ptr<DiscreteBoundaryOperator<ResultType>>
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
    const Space<BasisFunctionType> &testSpace,
    const Space<BasisFunctionType> &trialSpace,
    LocalAssemblerForIntegralOperators &localAssembler,
    LocalAssemblerForIntegralOperators &localAssemblerForAdmissibleBlocks,
    const Context<BasisFunctionType, ResultType> &context, int symmetry) {
  typedef LocalAssemblerForIntegralOperators Assembler;
  std::vector<Assembler *> localAssemblers(1, &localAssembler);
  std::vector<Assembler *> localAssemblersForAdmissibleBlocks(
      1, &localAssemblerForAdmissibleBlocks);
  std::vector<const DiscreteBndOp *> sparseTermsToAdd;
  std::vector<ResultType> denseTermsMultipliers(1, 1.0);
  std::vector<ResultType> sparseTermsMultipliers;

  return assembleDetachedWeakForm(testSpace, trialSpace, localAssemblers,
                                  localAssemblersForAdmissibleBlocks,
                                  sparseTermsToAdd, denseTermsMultipliers,
                                  sparseTermsMultipliers, context, symmetry);
}

template <typename BasisFunctionType, typename ResultType>
std::unique_ptr<DiscreteBoundaryOperator<ResultType>>
AcaGlobalAssembler<BasisFunctionType, ResultType>::assemblePotentialOperator(
    const arma::Mat<CoordinateType> &points,
    const Space<BasisFunctionType> &trialSpace,
    const std::vector<LocalAssemblerForPotentialOperators *> &localAssemblers,
    const std::vector<ResultType> &termMultipliers,
    const EvaluationOptions &options) {
#ifdef WITH_AHMED
  const int symmetric = false;

  typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
  typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;
  typedef bbxbemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
  typedef DiscreteAcaBoundaryOperator<ResultType> DiscreteAcaLinOp;

  const AcaOptions &acaOptions = options.acaOptions();
  const bool indexWithGlobalDofs =
      acaOptions.mode != AcaOptions::HYBRID_ASSEMBLY;
  const bool verbosityAtLeastDefault =
      (options.verbosityLevel() >= VerbosityLevel::DEFAULT);
  const bool verbosityAtLeastHigh =
      (options.verbosityLevel() >= VerbosityLevel::HIGH);

#ifndef WITH_TRILINOS
  if (!indexWithGlobalDofs)
    throw std::runtime_error("AcaGlobalAssembler::assemblePotentialOperator(): "
                             "local-mode ACA assembly requires BEM++ to be "
                             "linked with Trilinos");
#endif // WITH_TRILINOS

  if (localAssemblers.empty())
    throw std::runtime_error("AcaGlobalAssembler::assemblePotentialOperator(): "
                             "the 'localAssemblers' vector must not be empty");

  const size_t pointCount = points.n_cols;
  const int componentCount = localAssemblers[0]->resultDimension();
  const size_t testDofCount = pointCount * componentCount;
  const size_t trialDofCount = indexWithGlobalDofs
                                   ? trialSpace.globalDofCount()
                                   : trialSpace.flatLocalDofCount();

  // o2p: map of original indices to permuted indices
  // p2o: map of permuted indices to original indices
  typedef ClusterConstructionHelper<BasisFunctionType> CCH;
  shared_ptr<AhmedBemCluster> testClusterTree;
  shared_ptr<IndexPermutation> test_o2pPermutation, test_p2oPermutation;
  CCH::constructBemCluster(points, componentCount, acaOptions, testClusterTree,
                           test_o2pPermutation, test_p2oPermutation);
  shared_ptr<AhmedBemCluster> trialClusterTree;
  shared_ptr<IndexPermutation> trial_o2pPermutation, trial_p2oPermutation;
  CCH::constructBemCluster(trialSpace, indexWithGlobalDofs, acaOptions,
                           trialClusterTree, trial_o2pPermutation,
                           trial_p2oPermutation);

// Print the distribution of cluster ids
#ifdef DUMP_DENSE_BLOCKS
  std::vector<Point3D<CoordinateType>> testDofCenters, trialDofCenters;
  CCH::getComponentDofPositions(points, componentCount, testDofCenters);
  if (indexWithGlobalDofs)
    trialSpace.getGlobalDofPositions(trialDofCenters);
  else
    trialSpace.getFlatLocalDofPositions(trialDofCenters);
#endif // DUMP_DENSE_BLOCKS

  if (verbosityAtLeastHigh)
    std::cout << "Test cluster count: " << testClusterTree->getncl()
              << "\nTrial cluster count: " << trialClusterTree->getncl()
              << std::endl;

  unsigned int blockCount = 0;
  bool useStrongAdmissibilityCondition = !indexWithGlobalDofs;
  shared_ptr<AhmedBemBlcluster> bemBlclusterTree(CCH::constructBemBlockCluster(
      acaOptions, false /* symmetric */, *testClusterTree, *trialClusterTree,
      useStrongAdmissibilityCondition, blockCount).release());

  if (verbosityAtLeastHigh)
    std::cout << "Mblock count: " << blockCount << std::endl;

  std::vector<unsigned int> p2oPoints = test_p2oPermutation->permutedIndices();
  std::vector<unsigned int> p2oTrialDofs =
      trial_p2oPermutation->permutedIndices();
  typedef PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>
  AcaAssemblyHelper;
  AcaAssemblyHelper helper(points, trialSpace, p2oPoints, p2oTrialDofs,
                           localAssemblers, termMultipliers, options);

  // If necessary, construct maps between (permuted) flat local and global
  // indices
  shared_ptr<const Epetra_CrsMatrix> testGlobalToLocal, trialGlobalToLocal;

  std::unique_ptr<DiscreteAcaBoundaryOperator<ResultType>> acaOp =
      assembleAcaOperator<AcaAssemblyHelper, BasisFunctionType, ResultType>(
          &helper, &helper, bemBlclusterTree, bemBlclusterTree,
          options.parallelizationOptions(), options.acaOptions(),
          verbosityAtLeastDefault, verbosityAtLeastHigh, symmetric,
          test_o2pPermutation, trial_o2pPermutation, testGlobalToLocal,
          trialGlobalToLocal
#ifdef DUMP_DENSE_BLOCKS
          ,
          test_p2oPermutation, trial_p2oPermutation, testDofCenters,
          trialDofCenters
#endif // DUMP_DENSE_BLOCKS
          );

  std::unique_ptr<DiscreteBndOp> result;
  if (indexWithGlobalDofs)
    result = acaOp;
  else {
#ifdef WITH_TRILINOS
    // without Trilinos, this code will never be reached -- an exception
    // will be thrown earlier in this function
    typedef DiscreteBoundaryOperatorComposition<ResultType> DiscreteBndOpComp;
    shared_ptr<DiscreteBndOp> acaOpShared(acaOp.release());
    shared_ptr<DiscreteBndOp> trialGlobalToLocal =
        constructOperatorMappingGlobalToFlatLocalDofs<BasisFunctionType,
                                                      ResultType>(trialSpace);
    result.reset(new DiscreteBndOpComp(acaOpShared, trialGlobalToLocal));
#endif // WITH_TRILINOS
  }
  return result;

#else  // without Ahmed
  throw std::runtime_error("AcaGlobalAssembler::assemblePotentialOperator(): "
                           "To enable assembly in ACA mode, recompile BEM++ "
                           "with the symbol WITH_AHMED defined.");
#endif // WITH_AHMED
}

template <typename BasisFunctionType, typename ResultType>
std::unique_ptr<DiscreteBoundaryOperator<ResultType>>
AcaGlobalAssembler<BasisFunctionType, ResultType>::assemblePotentialOperator(
    const arma::Mat<CoordinateType> &points,
    const Space<BasisFunctionType> &trialSpace,
    LocalAssemblerForPotentialOperators &localAssembler,
    const EvaluationOptions &options) {
  std::vector<LocalAssemblerForPotentialOperators *> localAssemblers(
      1, &localAssembler);
  std::vector<ResultType> termMultipliers(1, 1.0);

  return assemblePotentialOperator(points, trialSpace, localAssemblers,
                                   termMultipliers, options);
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(AcaGlobalAssembler);

} // namespace Bempp


/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** 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 chemkit project 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.
**
******************************************************************************/

// The formulae for sphere intersection area and volume are
// derived from those presented in: "Measuring Space Filling
// Diagrams and Voids" by Herbert Edelsbrunner and Ping Fu.

#include "molecularsurface.h"

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include "atom.h"
#include "foreach.h"
#include "vector3.h"
#include "geometry.h"
#include "molecule.h"
#include "alphashape.h"
#include "concurrent.h"
#include "delaunaytriangulation.h"

namespace chemkit {

namespace {

const Real pi = chemkit::constants::Pi;

Real angleDihedral(const Point3 &s, const Point3 &t, const Point3 &u, const Point3 &v)
{
    Vector3 mu = (u - s).cross(u - t);
    Vector3 mv = (v - s).cross(v - t);

    Vector3 nu = mu.normalized();
    Vector3 nv = mv.normalized();

    return acos(nu.dot(nv)) / (2.0 * pi);
}

} // end anonymous namespace

// === MolecularSurfacePrivate ============================================= //
class MolecularSurfacePrivate
{
public:
    const Molecule *molecule;
    MolecularSurface::SurfaceType surfaceType;
    Real probeRadius;
    std::vector<Point3> points;
    std::vector<Real> radii;
    AlphaShape *alphaShape;
    Real volume;
    Real surfaceArea;
    bool volumeCalculated;
    bool surfaceAreaCalculated;
};

// === MolecularSurface ==================================================== //
/// \class MolecularSurface molecularsurface.h chemkit/molecularsurface.h
/// \ingroup chemkit
/// \brief  The MolecularSurface class represents a molecular
///         surface.
///
/// The following example shows how to calculate the solvent
/// accessible surface area of a molecule.
/// \code
/// // create the surface object for the molecule
/// MolecularSurface surface(molecule);
///
/// // set the surface type to solvent accessible
/// surface.setSurfaceType(MolecularSurface::SolventAccessible);
///
/// // set the solvent probe radius to 1.4 angstroms
/// surface.setProbeRadius(1.4);
///
/// // calculate the surface area
/// double area = surface.surfaceArea();
/// \endcode

/// \enum MolecularSurface::SurfaceType
/// Provides names for each of the available surface types:
///     - \c VanDerWaals
///     - \c SolventAccessible
///     - \c SolventExcluded

// --- Construction and Destruction ---------------------------------------- //
/// Creates a new molecular surface for \p molecule.
MolecularSurface::MolecularSurface(const Molecule *molecule, SurfaceType type)
    : d(new MolecularSurfacePrivate)
{
    d->molecule = molecule;
    d->surfaceType = type;
    d->probeRadius = 1.4;

    if(molecule){
        foreach(const Atom *atom, molecule->atoms()){
            d->points.push_back(atom->position());
            d->radii.push_back(atom->vanDerWaalsRadius());
        }
    }

    d->alphaShape = 0;
    d->volumeCalculated = false;
    d->surfaceAreaCalculated = false;
}

/// Destroys the molecular surface object.
MolecularSurface::~MolecularSurface()
{
    delete d->alphaShape;
    delete d;
}

// --- Properties ---------------------------------------------------------- //
/// Sets the molecule for the surface.
void MolecularSurface::setMolecule(const Molecule *molecule)
{
    d->molecule = molecule;

    // update atom positions and radii
    if(molecule){
        d->points.resize(molecule->size());
        d->radii.resize(molecule->size());

        for(size_t i = 0; i < molecule->size(); i++){
            const Atom *atom = molecule->atom(i);

            d->points[i] = atom->position();
            d->radii[i] = atom->vanDerWaalsRadius();
        }
    }

    setCalculated(false);
}

/// Returns the molecule for the surface.
const Molecule* MolecularSurface::molecule() const
{
    return d->molecule;
}

/// Sets the surface type to \p type.
void MolecularSurface::setSurfaceType(SurfaceType type)
{
    d->surfaceType = type;

    setCalculated(false);
}

/// Returns the surface type.
MolecularSurface::SurfaceType MolecularSurface::surfaceType() const
{
    return d->surfaceType;
}

/// Sets the probe radius to \p radius.
void MolecularSurface::setProbeRadius(Real radius)
{
    d->probeRadius = radius;

    setCalculated(false);
}

/// Returns the probe radius.
///
/// The default probe radius is 1.4 Angstroms which approximates
/// the radius of a water molecule.
Real MolecularSurface::probeRadius() const
{
    return d->probeRadius;
}

const AlphaShape* MolecularSurface::alphaShape() const
{
    if(!d->alphaShape){
        // calculate weights (weight = radius sqaured)
        std::vector<Real> weights(d->points.size());
        for(unsigned int i = 0; i < d->points.size(); i++){
            weights[i] = pow(radius(i), 2);
        }

        d->alphaShape = new AlphaShape(d->points, weights);
    }

    return d->alphaShape;
}

// --- Geometry ------------------------------------------------------------ //
/// Returns the position of the sphere at \p index.
Point3 MolecularSurface::position(int index) const
{
    return d->points[index];
}

/// Returns the radius of the sphere at \p index.
Real MolecularSurface::radius(int index) const
{
    if(d->surfaceType == VanDerWaals)
        return d->radii[index];
    else
        return d->radii[index] + d->probeRadius;
}

/// Returns the total volume of the surface. The returned volume
/// is in Angstroms cubed (\f$ \AA^{3} \f$).
Real MolecularSurface::volume() const
{
    if(!d->volumeCalculated){
        d->volume = 0;

        const AlphaShape *alphaShape = this->alphaShape();

        // add volume and area for each vertex
        for(unsigned int i = 0; i < d->points.size(); i++){
            Real r = radius(i);

            d->volume += (4.0/3.0) * pi * r*r*r;
        }

        // subtract volume from each edge
        foreach(const AlphaShape::Edge &edge, alphaShape->edges()){
            d->volume -= intersectionVolume(edge[0], edge[1]);
        }

        // add volume from each triangle
        foreach(const AlphaShape::Triangle &triangle, alphaShape->triangles()){
            d->volume += intersectionVolume(triangle[0], triangle[1], triangle[2]);
        }

        // subtract volume from each tetrahedron
        foreach(const std::vector<int> &tetrahedron, alphaShape->tetrahedra()){
            d->volume -= intersectionVolume(tetrahedron[0], tetrahedron[1], tetrahedron[2], tetrahedron[3]);
        }

        d->volumeCalculated = true;
    }

    return d->volume;
}

/// Runs the volume() method asynchronously and returns a future
/// containing the result.
///
/// \internal
boost::shared_future<Real> MolecularSurface::volumeAsync() const
{
    return chemkit::concurrent::run(boost::bind(&MolecularSurface::volume, this));
}

/// Returns the total surface area of the surface. The returned
/// area is in Angstroms squared (\f$ \AA^{2} \f$).
Real MolecularSurface::surfaceArea() const
{
    if(!d->surfaceAreaCalculated){
        d->surfaceArea = 0;

        const AlphaShape *alphaShape = this->alphaShape();

        // add volume and area for each vertex
        for(unsigned int i = 0; i < d->points.size(); i++){
            Real r = radius(i);

            d->surfaceArea += 4.0 * pi * r*r;
        }

        // subtract volume and area from each edge
        foreach(const AlphaShape::Edge &edge, alphaShape->edges()){
            d->surfaceArea -= intersectionArea(edge[0], edge[1]);
        }

        // add volume and area from each triangle
        foreach(const AlphaShape::Triangle &triangle, alphaShape->triangles()){
            d->surfaceArea += intersectionArea(triangle[0], triangle[1], triangle[2]);
        }

        // subtract volume and area from each tetrahedron
        foreach(const std::vector<int> &tetrahedron, alphaShape->tetrahedra()){
            d->surfaceArea -= intersectionArea(tetrahedron[0], tetrahedron[1], tetrahedron[2], tetrahedron[3]);
        }

        d->surfaceAreaCalculated = true;
    }

    return d->surfaceArea;
}

/// Runs the surfaceArea() method asynchronously and returns a future
/// containing the result.
///
/// \internal
boost::shared_future<Real> MolecularSurface::surfaceAreaAsync() const
{
    return chemkit::concurrent::run(boost::bind(&MolecularSurface::surfaceArea, this));
}

// --- Internal Methods ---------------------------------------------------- //
void MolecularSurface::setCalculated(bool calculated) const
{
    if(calculated == false){
        delete d->alphaShape;
        d->alphaShape = 0;
        d->volumeCalculated = false;
        d->surfaceAreaCalculated = false;
    }
}

/// Returns the area of intersection between spheres \p i and \p j.
Real MolecularSurface::intersectionArea(int i, int j) const
{
    return 2.0 * pi * (radius(i) * capHeight(i, j) + radius(j) * capHeight(j, i));
}

/// Returns the area of intersection between spheres \p i, \p j and
/// \p k.
Real MolecularSurface::intersectionArea(int i, int j, int k) const
{
    return cap2Area(i, j, k) + cap2Area(j, i, k) + cap2Area(k, i, j);
}

/// Returns the area of intersection between spheres \p i, \p j, \p k
/// and \p l.
Real MolecularSurface::intersectionArea(int i, int j, int k, int l) const
{
    return cap3Area(i, j, k, l) + cap3Area(j, i, k, l) + cap3Area(k, i, j, l) + cap3Area(l, i, j, k);
}

/// Returns the volume of intersection between spheres \p i, and
/// \p j.
Real MolecularSurface::intersectionVolume(int i, int j) const
{
    return capVolume(i, j) + capVolume(j, i);
}

/// Returns the volume of intersection between spheres \p i, \p j,
/// and \p k.
Real MolecularSurface::intersectionVolume(int i, int j, int k) const
{
    return cap2Volume(i, j, k) + cap2Volume(j, i, k) + cap2Volume(k, i, j);
}

/// Returns the volume of intersection between spheres \p i, \p j,
/// \p k and \p l.
Real MolecularSurface::intersectionVolume(int i, int j, int k, int l) const
{
    return cap3Volume(i, j, k, l) + cap3Volume(j, i, k, l) + cap3Volume(k, i, j, l) + cap3Volume(l, i, j, k);
}

Real MolecularSurface::ballArea(int index) const
{
    Real r = radius(index);

    return 4.0 * pi * r*r;
}

Real MolecularSurface::capHeight(int i, int j) const
{
    const Point3 &s = position(i);

    Point3 y = d->alphaShape->orthocenter(i, j);

    // check if vertex i is attached to vertex j
    if(d->alphaShape->vertexAttached(i, j)){
        return radius(i) + chemkit::geometry::distance(s, y);
    }
    else{
        return radius(i) - chemkit::geometry::distance(s, y);
    }
}

Real MolecularSurface::capArea(int i, int j) const
{
    return 2.0 * pi * radius(i) * capHeight(i, j);
}

Real MolecularSurface::capVolume(int i, int j) const
{
    Real s = radius(i) * capArea(i, j);
    Real c = (radius(i) - capHeight(i, j)) * diskArea(i, j);

    return (1.0/3.0) * (s - c);
}

Real MolecularSurface::cap2Area(int i, int j, int k) const
{
    Point3 pjk = triangleDual(i, j, k);

    Real lj = segmentAngle(i, j, k);
    Real lk = segmentAngle(i, k, j);

    const Point3 &s = position(i);
    const Point3 &t = position(j);
    const Point3 &u = position(k);

    Real r = radius(i);
    Real phi = (1.0/2.0) - angleDihedral(s, pjk, t, u);

    Real a1 = ballArea(i) * phi;
    Real a2 = 2.0 * pi * r * lj * (r - capHeight(i, j));
    Real a3 = 2.0 * pi * r * lk * (r - capHeight(i, k));

    return a1 - a2 - a3;
}

Real MolecularSurface::cap2Volume(int i, int j, int k) const
{
    Real s2 = (1.0/3.0) * radius(i) * cap2Area(i, j, k);
    Real cj = (1.0/3.0) * (radius(i) - capHeight(i, j)) * segmentArea(i, j, k);
    Real ck = (1.0/3.0) * (radius(i) - capHeight(i, k)) * segmentArea(i, k, j);

    return s2 - cj - ck;
}

Real MolecularSurface::cap3Area(int i, int j, int k, int l) const
{
    if(!ccw(i, j, k, l)){
        std::swap(k, l);
    }

    const Point3 &s = position(i);
    const Point3 &t = position(j);
    const Point3 &u = position(k);
    const Point3 &v = position(l);

    Point3 pkj = triangleDual(i, k, j);
    Point3 plk = triangleDual(i, l, k);
    Point3 pjl = triangleDual(i, j, l);

    Real lj = segment2Angle(i, j, k, l);
    Real lk = segment2Angle(i, k, l, j);
    Real ll = segment2Angle(i, l, j, k);

    Real rho_kj = (1.0/2.0) - angleDihedral(s, pkj, u, t);
    Real rho_lk = (1.0/2.0) - angleDihedral(s, plk, v, u);
    Real rho_jl = (1.0/2.0) - angleDihedral(s, pjl, t, v);

    Real a1 = (1.0/2.0) * ballArea(i) * (rho_kj + rho_lk + rho_jl - (1.0/2.0));
    Real a2 = 2.0 * pi * radius(i) * lj * (radius(i) - capHeight(i, j));
    Real a3 = 2.0 * pi * radius(i) * lk * (radius(i) - capHeight(i, k));
    Real a4 = 2.0 * pi * radius(i) * ll * (radius(i) - capHeight(i, l));

    return a1 - a2 - a3 - a4;
}

Real MolecularSurface::cap3Volume(int i, int j, int k, int l) const
{
    Real s3 = (1.0/3.0) * radius(i) * cap3Area(i, j, k, l);
    Real cj = (1.0/3.0) * (radius(i) - capHeight(i, j)) * segment2Area(i, j, k, l);
    Real ck = (1.0/3.0) * (radius(i) - capHeight(i, k)) * segment2Area(i, k, j, l);
    Real cl = (1.0/3.0) * (radius(i) - capHeight(i, l)) * segment2Area(i, l, j, k);

    return s3 - cj - ck - cl;
}

Real MolecularSurface::diskArea(int i, int j) const
{
    return (1.0/2.0) * diskRadius(i, j) * diskLength(i, j);
}

Real MolecularSurface::diskLength(int i, int j) const
{
    return 2.0 * pi * diskRadius(i, j);
}

Real MolecularSurface::diskRadius(int i, int j) const
{
    return sqrt(capHeight(i, j) * (2.0 * radius(i) - capHeight(i, j)));
}

Point3 MolecularSurface::triangleDual(int i, int j, int k) const
{
    Point3 y = d->alphaShape->orthocenter(i, j, k);

    const Point3 &s = d->points[i];
    const Point3 &t = d->points[j];
    const Point3 &u = d->points[k];

    Vector3 n = (t - s).cross(u - s);

    Vector3 ys = y - s;

    Real s1 = (ys).dot(n);
    Real s2 = n.dot(n);
    Real s3 = (ys).dot(ys);

    Real r = radius(i);

    Real xi = (-s1 + sqrt(s1*s1 - s3 * s2 + r*r * s2)) / s2;

    return y + (n * xi);
}

Real MolecularSurface::segmentArea(int i, int j, int k) const
{
    Real s = (1.0/2.0) * diskRadius(i, j) * segmentLength(i, j, k);

    Point3 pjk = triangleDual(i, j, k);
    Point3 pkj = triangleDual(i, k, j);

    Real h = diskRadius(i, j) - segmentHeight(i, j, k);
    Real t = (1.0/2.0) * h * chemkit::geometry::distance(pjk, pkj);

    return s - t;
}

Real MolecularSurface::segmentAngle(int i, int j, int k) const
{
    Point3 pjk = triangleDual(i, j, k);

    const Point3 &s = d->points[i];
    const Point3 &t = d->points[j];
    const Point3 &u = d->points[k];

    return 2.0 * angleDihedral(s, t, u, pjk);
}

Real MolecularSurface::segmentLength(int i, int j, int k) const
{
    return segmentAngle(i, j, k) * diskLength(i, j);
}

Real MolecularSurface::segmentHeight(int i, int j, int k) const
{
    Point3 y2 = d->alphaShape->orthocenter(i, j);
    Point3 y3 = d->alphaShape->orthocenter(i, j, k);

    // check if vertex k is attached to the edge (i, j)
    if(d->alphaShape->edgeAttached(i, j, k)){
        return diskRadius(i, j) + chemkit::geometry::distance(y2, y3);
    }
    else{
        return diskRadius(i, j) - chemkit::geometry::distance(y2, y3);
    }
}

Real MolecularSurface::segment2Area(int i, int j, int k, int l) const
{
    if(!ccw(i, j, k, l))
        std::swap(k, l);

    Point3 pkj = triangleDual(i, k, j);
    Point3 pjl = triangleDual(i, j, l);

    Point3 y = d->alphaShape->orthocenter(i, j, k, l);

    Real hk = segmentHeight(i, j, k);
    Real hl = segmentHeight(i, j, l);

    Real rij = diskRadius(i, j);

    Real s = (1.0/2.0) * rij * segment2Length(i, j, k, l);
    Real tk = (1.0/2.0) * (rij - hk) * chemkit::geometry::distance(pkj, y);
    Real tl = (1.0/2.0) * (rij - hl) * chemkit::geometry::distance(pjl, y);

    return s - tk - tl;
}

Real MolecularSurface::segment2Angle(int i, int j, int k, int l) const
{
    Point3 pjl = triangleDual(i, j, l);
    Point3 pkj = triangleDual(i, k, j);

    const Point3 &s = d->points[i];
    const Point3 &t = d->points[j];
    const Point3 &u = d->points[k];
    const Point3 &v = d->points[l];

    return angleDihedral(s, t, u, pkj) + angleDihedral(s, t, v, pjl) - angleDihedral(s, t, u, v);
}

Real MolecularSurface::segment2Length(int i, int j, int k, int l) const
{
    return segment2Angle(i, j, k, l) * diskLength(i, j);
}

bool MolecularSurface::ccw(int i, int j, int k, int l) const
{
    const Point3 &a = position(i);
    const Point3 &b = position(j);
    const Point3 &c = position(k);
    const Point3 &d = position(l);

    return chemkit::geometry::planeOrientation(a, b, c, d) > 0;
}

} // end chemkit namespace


// Copyright (c) 2019, Torsten Sattler
// 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.
//
//     * 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 <COPYRIGHT HOLDER> 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.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iostream>
#include <limits>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include <RansacLib/ransac.h>
#include "line_estimator.h"

// Assumes that inlier threshold << 0.5.
void GenerateRandomInstance(const int num_inliers, const int num_outliers,
                            double inlier_threshold, Eigen::Matrix2Xd* points) {
  const int kNumPoints = num_inliers + num_outliers;
  points->resize(2, kNumPoints);

  std::vector<int> indices(kNumPoints);
  std::iota(indices.begin(), indices.end(), 0);

  std::random_device rand_dev;
  std::mt19937 rng(rand_dev());

  std::shuffle(indices.begin(), indices.end(), rng);

  // Generates num_inliers points along the x-axis in the interval [0, 1] with
  // a y-value in the range (-inlier_threshold, inlier_threshold) choosen
  // at random.
  std::uniform_real_distribution<double> distr(-inlier_threshold,
                                               inlier_threshold);

  const double kXStep = 1.0 / static_cast<double>(num_inliers);
  double x = 0.0;
  for (int i = 0; i < num_inliers; ++i, x += kXStep) {
    const int kIndex = indices[i];
    points->col(kIndex)[0] = x;
    while (true) {
      points->col(kIndex)[1] = distr(rng);
      if (points->col(kIndex)[1] > -inlier_threshold) {
        break;
      }
    }
  }

  // Randomly generates outliers in the range [0, 1] x [-0.5, 0.5].
  std::uniform_real_distribution<double> distr_x(0.0, 1.0);
  std::uniform_real_distribution<double> distr_y(-0.5, 0.5);
  for (int i = num_inliers; i < kNumPoints; ++i) {
    double x = distr_x(rng);
    double y = distr_y(rng);
    while (std::fabs(y) < 5.0 * inlier_threshold) {
      y = distr_y(rng);
    }

    const int kIndex = indices[i];
    points->col(kIndex)[0] = x;
    points->col(kIndex)[1] = y;
  }

  // Randomly rotates and translates the points.
  const double kAngle = distr_y(rng) * M_PI;
  Eigen::Matrix2d R;
  R << std::cos(kAngle), -std::sin(kAngle), std::sin(kAngle), std::cos(kAngle);
  Eigen::Vector2d t(distr_y(rng) * 10.0, distr_y(rng) * 10.0);

  for (int i = 0; i < kNumPoints; ++i) {
    Eigen::Vector2d p = R * points->col(i) + t;
    points->col(i) = p;
  }
}

int main(int argc, char** argv) {
  ransac_lib::LORansacOptions options;
  options.min_num_iterations_ = 100u;
  options.max_num_iterations_ = 100000u;
  options.squared_inlier_threshold_ = 0.01 * 0.01;

  std::random_device rand_dev;
  options.random_seed_ = rand_dev();

  // Generates random instances for outlier ratios 10%, 20%, 30%, ..., 90%,
  // and then applies RANSAC on it.
  // kNumDataPoints data points are used.
  const int kNumDataPoints = 10000;
  std::vector<double> outlier_ratios = {0.1, 0.2, 0.3, 0.4,  0.5,  0.6,
                                        0.7, 0.8, 0.9, 0.95, 0.99, 0.999};
  for (const double outlier_ratio : outlier_ratios) {
    std::cout << " Inlier ratio: " << 1.0 - outlier_ratio << std::endl;
    int num_outliers =
        static_cast<int>(static_cast<double>(kNumDataPoints) * outlier_ratio);
    int num_inliers = kNumDataPoints - num_outliers;

    Eigen::Matrix2Xd data;
    GenerateRandomInstance(num_inliers, num_outliers, 0.5 * 0.01, &data);
    std::cout << "   ... instance generated" << std::endl;

    ransac_lib::LineEstimator solver(data);
    ransac_lib::LocallyOptimizedMSAC<Eigen::Vector3d,
                                     std::vector<Eigen::Vector3d>,
                                     ransac_lib::LineEstimator>
        lomsac;
    ransac_lib::RansacStatistics ransac_stats;

    std::cout << "   ... running LOMSAC" << std::endl;
    Eigen::Vector3d best_model;
    int num_ransac_inliers =
        lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);
    std::cout << "   ... LOMSAC found " << num_ransac_inliers << " inliers in "
              << ransac_stats.num_iterations << " iterations with an inlier "
              << "ratio of " << ransac_stats.inlier_ratio << std::endl;
  }
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 15:03:20

#ifndef NUHMSSM_SLHA_IO_H
#define NUHMSSM_SLHA_IO_H

#include "NUHMSSM_mass_eigenstates.hpp"
#include "NUHMSSM_model_slha.hpp"
#include "NUHMSSM_info.hpp"
#include "NUHMSSM_observables.hpp"
#include "NUHMSSM_physical.hpp"
#include "problems.hpp"
#include "spectrum_generator_problems.hpp"
#include "standard_model_two_scale_model.hpp"
#include "slha_io.hpp"
#include "ckm.hpp"
#include "ew_input.hpp"
#include "lowe.h"

#include <Eigen/Core>
#include <string>
#include <tuple>
#include <utility>

#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/adapted/std_tuple.hpp>

#define Pole(p) physical.p
#define PHYSICAL(p) model.get_physical().p
#define PHYSICAL_SLHA(p) model.get_physical_slha().p
#define LOCALPHYSICAL(p) physical.p
#define MODEL model
#define MODELPARAMETER(p) model.get_##p()
#define EXTRAPARAMETER(p) model.get_##p()
#define OBSERVABLES observables
#define LowEnergyConstant(p) Electroweak_constants::p
#define SCALES(p) scales.p

namespace flexiblesusy {

struct NUHMSSM_input_parameters;
class Spectrum_generator_settings;

template <class T>
class NUHMSSM;

struct NUHMSSM_scales {
   double HighScale{0.}, SUSYScale{0.}, LowScale{0.};
   double pole_mass_scale{0.};
};

class NUHMSSM_slha_io {
public:
   NUHMSSM_slha_io();

   void clear();

   void fill(softsusy::QedQcd& qedqcd) const { slha_io.fill(qedqcd); }
   void fill(NUHMSSM_input_parameters&) const;
   void fill(NUHMSSM_mass_eigenstates&) const;
   template <class Model> void fill(NUHMSSM_slha<Model>&) const;
   void fill(Physical_input&) const;
   void fill(Spectrum_generator_settings&) const;
   double get_parameter_output_scale() const;
   const SLHA_io& get_slha_io() const { return slha_io; }
   void read_from_file(const std::string&);
   void read_from_source(const std::string&);
   void read_from_stream(std::istream&);
   void set_block(const std::string& str, SLHA_io::Position position = SLHA_io::back) { slha_io.set_block(str, position); }
   void set_blocks(const std::vector<std::string>& vec, SLHA_io::Position position = SLHA_io::back) { slha_io.set_blocks(vec, position); }
   template <class Model> void set_extra(const NUHMSSM_slha<Model>&, const NUHMSSM_scales&, const NUHMSSM_observables&);
   void set_input(const NUHMSSM_input_parameters&);
   void set_modsel(const SLHA_io::Modsel&);
   void set_physical_input(const Physical_input&);
   void set_settings(const Spectrum_generator_settings&);
   void set_sminputs(const softsusy::QedQcd&);
   template <class... Ts> void set_spectrum(const std::tuple<Ts...>&);
   template <class Model> void set_spectrum(const NUHMSSM_slha<Model>&);
   template <class T> void set_spectrum(const NUHMSSM<T>&);
   void set_spectrum(const standard_model::Standard_model& m) { slha_io.set_spectrum(m); }
   void set_spinfo(const Spectrum_generator_problems&);
   void set_spinfo(const Problems&);
   void set_spinfo(const std::vector<std::string>&, const std::vector<std::string>&);
   void set_print_imaginary_parts_of_majorana_mixings(bool);
   void write_to(const std::string&) const;
   void write_to_file(const std::string& file_name) const { slha_io.write_to_file(file_name); }
   void write_to_stream(std::ostream& ostr = std::cout) const { slha_io.write_to_stream(ostr); }

   static void fill_minpar_tuple(NUHMSSM_input_parameters&, int, double);
   static void fill_extpar_tuple(NUHMSSM_input_parameters&, int, double);
   static void fill_imminpar_tuple(NUHMSSM_input_parameters&, int, double);
   static void fill_imextpar_tuple(NUHMSSM_input_parameters&, int, double);

   template <class Model>
   static void fill_slhaea(SLHAea::Coll&, const NUHMSSM_slha<Model>&, const softsusy::QedQcd&, const NUHMSSM_scales&, const NUHMSSM_observables&);

   template <class Model>
   static SLHAea::Coll fill_slhaea(const NUHMSSM_slha<Model>&, const softsusy::QedQcd&, const NUHMSSM_scales&, const NUHMSSM_observables&);

private:
   SLHA_io slha_io; ///< SLHA io class
   bool print_imaginary_parts_of_majorana_mixings;

   void set_extpar(const NUHMSSM_input_parameters&);
   void set_imminpar(const NUHMSSM_input_parameters&);
   void set_imextpar(const NUHMSSM_input_parameters&);
   void set_minpar(const NUHMSSM_input_parameters&);
   void set_mass(const NUHMSSM_physical&, bool);
   void set_mixing_matrices(const NUHMSSM_physical&, bool);
   template <class Model> void set_model_parameters(const NUHMSSM_slha<Model>&);
   void set_ckm(const Eigen::Matrix<std::complex<double>,3,3>&, double);
   void set_pmns(const Eigen::Matrix<std::complex<double>,3,3>&, double);
   double read_scale() const;
   void fill_drbar_parameters(NUHMSSM_mass_eigenstates&) const;
   void fill_physical(NUHMSSM_physical&) const;
};

/**
 * Reads DR-bar parameters, pole masses and mixing matrices from a
 * SLHA output file.
 */
template <class Model>
void NUHMSSM_slha_io::fill(NUHMSSM_slha<Model>& model) const
{
   fill(static_cast<NUHMSSM_mass_eigenstates&>(model));
   fill_physical(model.get_physical_slha());
}

template <class Model>
void NUHMSSM_slha_io::fill_slhaea(
   SLHAea::Coll& slhaea, const NUHMSSM_slha<Model>& model,
   const softsusy::QedQcd& qedqcd, const NUHMSSM_scales& scales,
   const NUHMSSM_observables& observables)
{
   NUHMSSM_slha_io slha_io;
   const NUHMSSM_input_parameters& input = model.get_input();
   const auto& problems = model.get_problems();
   const bool error = problems.have_problem();

   slha_io.set_spinfo(problems);
   slha_io.set_sminputs(qedqcd);
   slha_io.set_input(input);
   if (!error) {
      slha_io.set_spectrum(model);
      slha_io.set_extra(model, scales, observables);
   }

   slhaea = slha_io.get_slha_io().get_data();
}

template <class Model>
SLHAea::Coll NUHMSSM_slha_io::fill_slhaea(
   const NUHMSSM_slha<Model>& model, const softsusy::QedQcd& qedqcd,
   const NUHMSSM_scales& scales, const NUHMSSM_observables& observables)
{
   SLHAea::Coll slhaea;
   NUHMSSM_slha_io::fill_slhaea(slhaea, model, qedqcd, scales, observables);

   return slhaea;
}

/**
 * Stores the model (DR-bar) parameters in the SLHA object.
 *
 * @param model model class
 */
template <class Model>
void NUHMSSM_slha_io::set_model_parameters(const NUHMSSM_slha<Model>& model)
{
   {
      std::ostringstream block;
      block << "Block gauge Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (MODELPARAMETER(g1) * 0.7745966692414834), "g1 * 0.7745966692414834")
            << FORMAT_ELEMENT(2, (MODELPARAMETER(g2)), "g2")
            << FORMAT_ELEMENT(3, (MODELPARAMETER(g3)), "g3")
      ;
      slha_io.set_block(block);
   }
   slha_io.set_block("Yu", ToMatrix(MODELPARAMETER(Yu_slha)), "Yu", model.get_scale());
   slha_io.set_block("Yd", ToMatrix(MODELPARAMETER(Yd_slha)), "Yd", model.get_scale());
   slha_io.set_block("Ye", ToMatrix(MODELPARAMETER(Ye_slha)), "Ye", model.get_scale());
   slha_io.set_block("Te", MODELPARAMETER(TYe_slha), "TYe", model.get_scale());
   slha_io.set_block("Td", MODELPARAMETER(TYd_slha), "TYd", model.get_scale());
   slha_io.set_block("Tu", MODELPARAMETER(TYu_slha), "TYu", model.get_scale());
   {
      std::ostringstream block;
      block << "Block HMIX Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (MODELPARAMETER(Mu)), "Mu")
            << FORMAT_ELEMENT(101, (MODELPARAMETER(BMu)), "BMu")
            << FORMAT_ELEMENT(102, (MODELPARAMETER(vd)), "vd")
            << FORMAT_ELEMENT(103, (MODELPARAMETER(vu)), "vu")
      ;
      slha_io.set_block(block);
   }
   slha_io.set_block("MSQ2", MODELPARAMETER(mq2_slha), "mq2", model.get_scale());
   slha_io.set_block("MSE2", MODELPARAMETER(me2_slha), "me2", model.get_scale());
   slha_io.set_block("MSL2", MODELPARAMETER(ml2_slha), "ml2", model.get_scale());
   slha_io.set_block("MSU2", MODELPARAMETER(mu2_slha), "mu2", model.get_scale());
   slha_io.set_block("MSD2", MODELPARAMETER(md2_slha), "md2", model.get_scale());
   {
      std::ostringstream block;
      block << "Block MSOFT Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(21, (MODELPARAMETER(mHd2)), "mHd2")
            << FORMAT_ELEMENT(22, (MODELPARAMETER(mHu2)), "mHu2")
            << FORMAT_ELEMENT(1, (MODELPARAMETER(MassB)), "MassB")
            << FORMAT_ELEMENT(2, (MODELPARAMETER(MassWB)), "MassWB")
            << FORMAT_ELEMENT(3, (MODELPARAMETER(MassG)), "MassG")
      ;
      slha_io.set_block(block);
   }

   {
      std::ostringstream block;
      block << "Block Phases Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (Re(MODELPARAMETER(PhaseGlu))), "Re(PhaseGlu)")
      ;
      slha_io.set_block(block);
   }
   {
      std::ostringstream block;
      block << "Block IMPhases Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (Im(MODELPARAMETER(PhaseGlu))), "Im(PhaseGlu)")
      ;
      slha_io.set_block(block);
   }

}

/**
 * Writes extra SLHA blocks
 *
 * @param model model class
 * @param scales struct of boundary condition scales
 * @param observables struct of observables
 */
template <class Model>
void NUHMSSM_slha_io::set_extra(
   const NUHMSSM_slha<Model>& model, const NUHMSSM_scales& scales,
   const NUHMSSM_observables& observables)
{
   const NUHMSSM_physical physical(model.get_physical_slha());

   {
      std::ostringstream block;
      block << "Block FlexibleSUSYLowEnergy Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (OBSERVABLES.a_muon), "Delta(g-2)_muon/2 FlexibleSUSY")
      ;
      slha_io.set_block(block);
   }
   {
      std::ostringstream block;
      block << "Block EFFHIGGSCOUPLINGS" << '\n'
            << FORMAT_RANK_THREE_TENSOR(25, 22, 22, (Abs(OBSERVABLES.eff_cp_higgs_photon_photon(0))), "Abs(effective H-Photon-Photon coupling)")
            << FORMAT_RANK_THREE_TENSOR(35, 22, 22, (Abs(OBSERVABLES.eff_cp_higgs_photon_photon(1))), "Abs(effective H-Photon-Photon coupling)")
            << FORMAT_RANK_THREE_TENSOR(25, 21, 21, (Abs(OBSERVABLES.eff_cp_higgs_gluon_gluon(0))), "Abs(effective H-Gluon-Gluon coupling)")
            << FORMAT_RANK_THREE_TENSOR(35, 21, 21, (Abs(OBSERVABLES.eff_cp_higgs_gluon_gluon(1))), "Abs(effective H-Gluon-Gluon coupling)")
            << FORMAT_RANK_THREE_TENSOR(36, 22, 22, (Abs(OBSERVABLES.eff_cp_pseudoscalar_photon_photon)), "Abs(effective A-Photon-Photon coupling)")
            << FORMAT_RANK_THREE_TENSOR(36, 21, 21, (Abs(OBSERVABLES.eff_cp_pseudoscalar_gluon_gluon)), "Abs(effective A-Gluon-Gluon coupling)")
      ;
      slha_io.set_block(block);
   }

}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices of
 * all given models in the SLHA object.
 *
 * @todo Use generic lambda instead of Set_spectrum in C++14
 *
 * @param models model classes
 */
template <class... Ts>
void NUHMSSM_slha_io::set_spectrum(const std::tuple<Ts...>& models)
{
   Set_spectrum<NUHMSSM_slha_io> ss(this);
   boost::fusion::for_each(models, ss);
}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices in
 * the SLHA object.
 *
 * @param model model class in BPMZ convention
 */
template <class T>
void NUHMSSM_slha_io::set_spectrum(const NUHMSSM<T>& model)
{
   set_spectrum(NUHMSSM_slha<NUHMSSM<T> >(model));
}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices in
 * the SLHA object.
 *
 * @param model model class in SLHA convention
 */
template <class Model>
void NUHMSSM_slha_io::set_spectrum(const NUHMSSM_slha<Model>& model)
{
   const NUHMSSM_physical physical(model.get_physical_slha());
   const bool write_sm_masses = model.do_calculate_sm_pole_masses();

   set_model_parameters(model);
   set_mass(physical, write_sm_masses);
   set_mixing_matrices(physical, write_sm_masses);

   if (slha_io.get_modsel().quark_flavour_violated)
      set_ckm(model.get_ckm_matrix(), model.get_scale());

   if (slha_io.get_modsel().lepton_flavour_violated)
      set_pmns(model.get_pmns_matrix(), model.get_scale());
}

} // namespace flexiblesusy

#undef Pole
#undef PHYSICAL
#undef PHYSICAL_SLHA
#undef LOCALPHYSICAL
#undef MODEL
#undef MODELPARAMETER
#undef EXTRAPARAMETER
#undef OBSERVABLES
#undef LowEnergyConstant
#undef SCALES

#endif