#pragma once
#include "KDTree.hpp"
#include "../util/DistanceFuncs.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/SpectralEmbedding.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <cassert>
#include <cmath>
#include <random>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

namespace fluid {
namespace algorithm {

struct UMAPEmbeddingParamsFunctor
{
  typedef double Scalar;
  enum {
    InputsAtCompileTime = 2,
    ValuesAtCompileTime = 300 // from UMAP python implementation
  };
  typedef Eigen::VectorXd InputType;
  typedef Eigen::VectorXd ValueType;
  typedef Eigen::MatrixXd JacobianType;

  UMAPEmbeddingParamsFunctor(double minDist, double spread = 1.0)
  {
    mX = Eigen::ArrayXd::LinSpaced(values(), 0, 3 * spread);
    mY = (mX <= minDist).select(1, ((-mX + minDist) / spread).exp());
  }

  int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec) const
  {
    fvec = mY - (1 / (1 + x(0) * mX.pow(2 * x(1))));
    return 0;
  }

  int values() const { return ValuesAtCompileTime; }
  int inputs() const { return InputsAtCompileTime; }

  Eigen::ArrayXd mX;
  Eigen::ArrayXd mY;
};

class UMAP
{
public:
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXi = Eigen::ArrayXi;
  using VectorXd = Eigen::VectorXd;
  using SparseMatrixXd = Eigen::SparseMatrix<double>;
  using DataSet = FluidDataSet<std::string, double, 1>;
  template <typename T>
  using Ref = Eigen::Ref<T>;

  void init(RealMatrixView embedding, KDTree tree, index k, double a, double b)
  {
    mEmbedding = _impl::asEigen<Eigen::Array>(embedding);
    mTree = tree;
    mK = k;
    mAB = VectorXd(2);
    mAB << a, b;
    mInitialized = true;
  }

  void getEmbedding(RealMatrixView out) const
  {
    if (mInitialized) out <<= _impl::asFluid(mEmbedding);
  }

  double getA() const { return mInitialized ? mAB(0) : 0; }

  double getB() const { return mInitialized ? mAB(1) : 0; }

  index getK() const { return mInitialized ? mK : 0; }

  KDTree getTree() const { return mTree; }

  void clear()
  {
    mEmbedding.setZero();
    mTree.clear();
    mInitialized = false;
  }

  index dims() const { return mInitialized ? mEmbedding.cols() : 0; }

  index inputDims() const { return mInitialized ? mTree.dims() : 0; }

  index size() const { return mInitialized ? mEmbedding.rows() : 0; }

  bool initialized() const { return mInitialized; }

  DataSet train(DataSet& in, index k = 15, index dims = 2, double minDist = 0.1,
                index maxIter = 200, double learningRate = 1.0)
  {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    SpectralEmbedding      spectralEmbedding;
    index                  n = in.size();
    FluidTensor<string, 1> ids{in.getIds()};
    FluidTensor<string, 1> newIds(n);
    for (index i = 0; i < n; i++) newIds(i) = to_string(i);
    mTree = KDTree(DataSet(newIds, in.getData()));
    SparseMatrixXd knnGraph = SparseMatrixXd(in.size(), in.size());
    ArrayXXd       dists = ArrayXXd::Zero(in.size(), k);
    mK = k;
    makeGraph(in, mK, knnGraph, dists, true);
    ArrayXd sigma = findSigma(k, dists);
    computeHighDimProb(dists, sigma, knnGraph);
    SparseMatrixXd knnGraphT = knnGraph.transpose();
    knnGraph = (knnGraph + knnGraphT) - knnGraph.cwiseProduct(knnGraphT);
    mAB = findAB(minDist);
    mEmbedding = spectralEmbedding.train(knnGraph, dims);
    mEmbedding = normalizeEmbedding(mEmbedding);
    knnGraph.makeCompressed();
    ArrayXi rowIndices(knnGraph.nonZeros());
    ArrayXi colIndices(knnGraph.nonZeros());
    ArrayXd epochsPerSample(knnGraph.nonZeros());
    getGraphIndices(knnGraph, rowIndices, colIndices);
    computeEpochsPerSample(knnGraph, epochsPerSample);
    epochsPerSample = (epochsPerSample == 0).select(-1, epochsPerSample);
    optimizeLayout(mEmbedding, mEmbedding, rowIndices, colIndices,
                   epochsPerSample, true, learningRate, maxIter);
    DataSet out(ids, _impl::asFluid(mEmbedding));
    mInitialized = true;
    return out;
  }

  DataSet transform(DataSet& in, index maxIter = 200, double learningRate = 1.0) const
  {
    if (!mInitialized) return DataSet();
    SparseMatrixXd knnGraph(in.size(), mEmbedding.rows());
    ArrayXXd       dists = ArrayXXd::Zero(in.size(), mK);
    makeGraph(in, mK, knnGraph, dists, false);
    knnGraph.makeCompressed();
    ArrayXd sigma = findSigma(mK, dists);
    computeHighDimProb(dists, sigma, knnGraph);
    normalizeRows(knnGraph);
    ArrayXXd embedding =
        initTransformEmbedding(knnGraph, mEmbedding, in.size());
    ArrayXi rowIndices(knnGraph.nonZeros());
    ArrayXi colIndices(knnGraph.nonZeros());
    ArrayXd epochsPerSample(knnGraph.nonZeros());
    getGraphIndices(knnGraph, rowIndices, colIndices);
    computeEpochsPerSample(knnGraph, epochsPerSample);
    epochsPerSample = (epochsPerSample == 0).select(-1, epochsPerSample);
    optimizeLayout(embedding, mEmbedding, rowIndices, colIndices,
                   epochsPerSample, false, learningRate, maxIter);
    DataSet out(in.getIds(), _impl::asFluid(embedding));
    return out;
  }

  void transformPoint(RealVectorView in, RealVectorView out) const
  {
    if (!mInitialized) return;
    SparseMatrixXd knnGraph(1, mEmbedding.rows());
    ArrayXXd       dists = ArrayXXd::Zero(1, mK);
    knnGraph.reserve(mK);
    auto nearest = mTree.kNearest(in, mK);
    auto nearestIds = nearest.getIds();
    auto distances = nearest.getData().col(0);
    for (index j = 0; j < mK; j++)
    {
      index neighborIndex = stoi(nearestIds(j));
      dists(0, j) = distances(j);
      knnGraph.insert(0, neighborIndex) = distances(j);
    }
    knnGraph.makeCompressed();
    ArrayXd sigma = findSigma(mK, dists);
    computeHighDimProb(dists, sigma, knnGraph);
    normalizeRows(knnGraph);
    ArrayXXd embedding = initTransformEmbedding(knnGraph, mEmbedding, 1);
    ArrayXd  result = embedding.row(0);
    out <<= _impl::asFluid(result);
  }

private:
  template <typename F>
  void traverseGraph(const SparseMatrixXd& graph, F func) const
  {
    for (index i = 0; i < graph.outerSize(); i++)
    {
      for (SparseMatrixXd::InnerIterator it(graph, i); it; ++it) { func(it); }
    }
  }

  double loss(Ref<ArrayXXd> P, Ref<ArrayXXd> Y, double a, double b)
  {
    ArrayXXd D = DistanceMatrix(Y, 2);
    ArrayXXd Q = 1 / (1 + a * D.pow(b));
    Q = Q + epsilon;
    ArrayXXd CE =
        -P * (Q + 0.01).log() - (1 - P) * (1e-6 + (1 - Q + 0.01)).log();
    return CE.sum();
  }

  ArrayXd findSigma(index k, Ref<ArrayXXd> dists, index maxIter = 64,
                    double tolerance = 1e-5) const
  {
    using namespace std;
    double  target = log2(k);
    ArrayXd result = ArrayXd::Zero(dists.rows());
    for (index i = 0; i < dists.rows(); i++)
    {
      index  iter = maxIter;
      double lo = 0;
      double hi = infinity;
      double mid = 1.0;
      double rho = dists(i, 0);
      while (iter-- > 0)
      {
        double pSum = 0;
        for (index j = 1; j < dists.cols(); j++)
        {
          double d = dists(i, j) - rho;
          pSum += (d <= 0 ? 1.0 : exp(-(d / mid)));
        }
        if (abs(pSum - target) < tolerance) break;
        if (pSum > target)
        {
          hi = mid;
          mid = (lo + hi) / 2.0;
        }
        else
        {
          lo = mid;
          mid = (hi == infinity ? mid * 2 : (lo + hi) / 2.0);
        }
      }
      result(i) = mid;
    }
    return result;
  }

  void computeHighDimProb(const Ref<ArrayXXd>& dists, const Ref<ArrayXd>& sigma,
                          SparseMatrixXd& graph) const
  {
    traverseGraph(graph, [&](auto it) {
      it.valueRef() =
          std::exp(-(it.value() - dists(it.row(), 0)) / sigma(it.row()));
    });
  }

  VectorXd findAB(double minDist)
  {
    using namespace Eigen;
    VectorXd ab(2);
    ab << 1.0, 1.0;
    UMAPEmbeddingParamsFunctor                functor(minDist);
    NumericalDiff<UMAPEmbeddingParamsFunctor> numDiff(functor);
    LevenbergMarquardt<NumericalDiff<UMAPEmbeddingParamsFunctor>> lm(numDiff);
    lm.minimize(ab);
    return ab;
  }

  void makeGraph(const DataSet& in, index k, SparseMatrixXd& graph,
                 Ref<ArrayXXd> dists, bool discardFirst) const
  {
    graph.reserve(in.size() * k);
    auto data = in.getData();
    for (index i = 0; i < in.size(); i++)
    {
      auto nearest = mTree.kNearest(data.row(i), discardFirst ? k + 1 : k);
      auto nearestIds = nearest.getIds();
      auto distances = nearest.getData().col(0);
      for (index j = 0; j < k; j++)
      {
        index pos = discardFirst ? j + 1 : j;
        index neighborIndex = stoi(nearestIds(pos));
        dists(i, j) = distances(pos);
        graph.insert(i, neighborIndex) = distances(pos);
      }
    }
  }

  ArrayXXd normalizeEmbedding(const Ref<ArrayXXd>& embedding)
  {
    // based on umap python implementation
    double   expansion = 10.0 / embedding.abs().maxCoeff();
    ArrayXXd noise =
        1e-4 * ArrayXXd::Random(embedding.rows(), embedding.cols()); // uniform
    ArrayXXd result = (embedding * expansion) + noise;
    ArrayXd  min = result.colwise().minCoeff();
    ArrayXd  max = result.colwise().maxCoeff();
    ArrayXd  range = (max - min).max(epsilon);
    result = (result.rowwise() - min.transpose());
    result = result.rowwise() / range.transpose();
    return 10.0 * result;
  }

  void getGraphIndices(const SparseMatrixXd& graph, Ref<ArrayXi> rowIndices,
                       Ref<ArrayXi> colIndices) const
  {
    index p = 0;
    traverseGraph(graph, [&](auto it) {
      rowIndices(p) = static_cast<int>(it.row());
      colIndices(p) = static_cast<int>(it.col());
      p++;
    });
  }

  void computeEpochsPerSample(const SparseMatrixXd& graph,
                              Ref<ArrayXd>          epochsPerSample) const
  {
    index  p = 0;
    double maxVal = graph.coeffs().maxCoeff();
    traverseGraph(graph, [&](auto it) {
      epochsPerSample(p++) = 1.0 / (it.value() / maxVal);
    });
  }

  void optimizeLayout(Ref<ArrayXXd> embedding, Ref<ArrayXXd> reference,
                      Ref<ArrayXi> embIndices, Ref<ArrayXi> refIndices,
                      Ref<ArrayXd> epochsPerSample, bool updateReference,
                      double learningRate, index maxIter, double gamma = 1.0) const
  {
    using namespace std;
    double alpha = learningRate;
    double negativeSampleRate = 5.0;
    auto distance = DistanceFuncs::map()[DistanceFuncs::Distance::kSqEuclidean];
    double                          a = mAB(0);
    double                          b = mAB(1);
    random_device                   rd;
    mt19937                         mt(rd());
    uniform_int_distribution<index> randomInt(0, reference.rows() - 1);
    ArrayXd epochsPerNegativeSample = epochsPerSample / negativeSampleRate;
    ArrayXd nextEpoch = epochsPerSample;
    ArrayXd nextNegEpoch = epochsPerNegativeSample;
    ArrayXd bound = VectorXd::Constant(
        embedding.cols(), 4); // based on umap python implementation
    for (index i = 0; i < maxIter; i++)
    {
      for (index j = 0; j < epochsPerSample.size(); j++)
      {
        if (nextEpoch(j) > i) continue;
        ArrayXd current = embedding.row(embIndices(j));
        ArrayXd other = reference.row(refIndices(j));
        double dist = distance(current, other); // todo: try to have dist member
        double gradCoef = 0;
        ArrayXd grad;
        if (dist > 0)
        {
          gradCoef = -2.0 * a * b * pow(dist, b - 1.0);
          gradCoef /= a * pow(dist, b) + 1.0;
        }
        grad = (gradCoef * (current - other)).cwiseMin(bound).cwiseMax(-bound);
        current += grad * alpha;
        if (updateReference) other += -grad * alpha;
        nextEpoch(j) += epochsPerSample(j);
        index numNegative = static_cast<index>((i - nextNegEpoch(j)) /
                                                 epochsPerNegativeSample(j));
        for (index k = 0; k < numNegative; k++)
        {
          index negativeIndex = randomInt(mt);
          if (negativeIndex == embIndices(j)) continue;
          ArrayXd negative = reference.row(negativeIndex);
          dist = distance(current, negative);
          gradCoef = 0;
          grad = VectorXd::Constant(reference.cols(), 4.0);
          if (dist > 0)
          {
            gradCoef = 2.0 * gamma * b;
            gradCoef /= (0.001 + dist) * (a * pow(dist, b) + 1);
            grad = (gradCoef * (current - negative))
                       .cwiseMin(bound)
                       .cwiseMax(-bound);
          }
          current += grad * alpha;
        }
        nextNegEpoch(j) += numNegative * epochsPerNegativeSample(j);
        embedding.row(embIndices(j)) = current;
        if (updateReference) reference.row(refIndices(j)) = other;
      }
      alpha = learningRate * (1.0 - (i / double(maxIter)));
    }
  }

  ArrayXXd initTransformEmbedding(const SparseMatrixXd& graph,
                                  Ref<const ArrayXXd> reference, index N) const
  {
    ArrayXXd embedding = ArrayXXd::Zero(N, reference.cols());
    traverseGraph(graph, [&](auto it) {
      embedding.row(it.row()) += (reference.row(it.col()) * it.value());
    });
    return embedding;
  }

  void normalizeRows(const SparseMatrixXd& graph) const
  {
    ArrayXd sums = ArrayXd::Zero(graph.innerSize());
    traverseGraph(graph, [&](auto it) { sums(it.row()) += it.value(); });
    traverseGraph(
        graph, [&](auto it) { it.valueRef() = it.value() / sums(it.row()); });
  }

private:
  KDTree   mTree;
  index    mK;
  VectorXd mAB;
  mutable ArrayXXd mEmbedding;
  bool     mInitialized{false};
};
}// namespace algorithm
}// namespace fluid


#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <array>
#include "/Users/drewlewis/software/install/tiledarray/sparse_new_summa_debug/include/tiledarray.h"

using Tensor = TiledArray::Tensor<double>;
using Perm = TiledArray::Permutation;
using Range = TiledArray::Range;
using Matrix =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

int main(int argc, char **argv) {
    auto tensor = Tensor{Range(3, 2, 2)};
    for (auto i = 1; i <= 12; ++i) {
        tensor[i - 1] = i;
    }

    auto const &tsize = tensor.range().size();
    Eigen::Map<Matrix> tmap(tensor.data(), tsize[0], tsize[1] * tsize[2]);
    Eigen::JacobiSVD<Matrix> svd(tmap,
                                 Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto const &vals = svd.singularValues();
    auto rank = 0;
    for (auto i = 0; i < vals.size(); ++i) {
        if (vals[i] > 1e-10) {
            ++rank;
        }
    }
    Matrix mU = svd.matrixU().leftCols(rank);
    Matrix dV = svd.singularValues().asDiagonal();
    Matrix v = dV.block(0,0,rank, rank) * svd.matrixV().transpose().topRows(rank);

    std::cout << "U = \n" << mU << std::endl;
    std::cout << "v = \n" << v << std::endl;
    std::cout << "Approx = \n" << mU * v << std::endl;

    // Resize to 4*2
    v.resize(4,2);
    svd.compute(v, Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto const &vals2 = svd.singularValues();
    rank = 0;
    for (auto i = 0; i < vals2.size(); ++i) {
        if (vals2[i] > 1e-10) {
            ++rank;
        }
    }
    Matrix mU2 = svd.matrixU().leftCols(rank);
    Matrix dV2 = svd.singularValues().asDiagonal();
    Matrix v2 = dV.block(0,0,rank, rank) * svd.matrixV().transpose().topRows(rank);

    std::cout << "U2 = \n" << mU2 << std::endl;
    std::cout << "v2 = \n" << v2 << std::endl;

    mU2.resize(2,4);
    Matrix Ucombo = mU * mU2;

    Ucombo.resize(6,2);
    std::cout << "Ucombo = \n" << Ucombo << std::endl;

    svd.compute(Ucombo, Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto const &vals3 = svd.singularValues();
    rank = 0;
    for (auto i = 0; i < vals2.size(); ++i) {
        if (vals3[i] > 1e-10) {
            ++rank;
        }
    }
    Matrix mU3 = svd.matrixU().leftCols(rank);
    Matrix dV3 = svd.singularValues().asDiagonal();
    Matrix v3 = dV.block(0,0,rank, rank) * svd.matrixV().transpose().topRows(rank);

    std::cout << "U3 = \n" << mU3 << std::endl;
    std::cout << "v3 = \n" << v3 << std::endl;

    return 0;
}


#include <gtest/gtest.h>

#include <Eigen/SparseCore>

#include "../transformationofgalerkinmatrices.h"

namespace TransformationOfGalerkinMatrices::test {

TEST(TransformationOfGalerkinMatrices, TestTransformation) {
  std::cout << "NPDE homework TransformationOfGalerkinMatrices: unit test"
            << std::endl;

  // Create change of basis matrix S
  typedef Eigen::SparseMatrix<double> SpMat;
  std::vector<Eigen::Triplet<double>> S_triplets;

  int N = 4;
  SpMat S(2 * N, 2 * N);
  for (int i = 0; i < N; i++) {
    S_triplets.push_back(Eigen::Triplet<double>(i, i * 2, 1));
    S_triplets.push_back(Eigen::Triplet<double>(i, i * 2 + 1, 1));
    S_triplets.push_back(Eigen::Triplet<double>(N + i, i * 2, 1));
    S_triplets.push_back(Eigen::Triplet<double>(N + i, i * 2 + 1, -1));
  }
  S.setFromTriplets(S_triplets.begin(), S_triplets.end());

  SpMat sol_mat(2 * N, 2 * N);
  SpMat A_mat(2 * N, 2 * N);
  SpMat A_tilde_mat(2 * N, 2 * N);
  std::vector<Eigen::Triplet<double>> A, A_tilde;

  // Case 1 and 2
  A.push_back(Eigen::Triplet<double>(2 * N - 1, 2 * N - 1, 1));  // i, j even
  A.push_back(Eigen::Triplet<double>(3 - 1, 3 - 1, 5));          // i, j odd
  A_mat.setFromTriplets(A.begin(), A.end());
  A_tilde = TransformationOfGalerkinMatrices::transformCOOmatrix(A);
  A_tilde_mat.setFromTriplets(A_tilde.begin(), A_tilde.end());
  sol_mat = S * A_mat * S.transpose();

  ASSERT_TRUE((sol_mat - A_tilde_mat).norm() == 0);

  // Case 3
  A.clear();
  A_tilde.clear();
  A.push_back(Eigen::Triplet<double>(2 * N - 1, 2 * N - 1, 1));
  A.push_back(Eigen::Triplet<double>(4 - 1, 3 - 1, 5));  // i even, j odd
  A_mat.setFromTriplets(A.begin(), A.end());
  A_tilde = TransformationOfGalerkinMatrices::transformCOOmatrix(A);
  A_tilde_mat.setFromTriplets(A_tilde.begin(), A_tilde.end());
  sol_mat = S * A_mat * S.transpose();

  ASSERT_TRUE((sol_mat - A_tilde_mat).norm() == 0);

  // Case 4
  A.clear();
  A_tilde.clear();
  A.push_back(Eigen::Triplet<double>(2 * N - 1, 2 * N - 1, 1));
  A.push_back(Eigen::Triplet<double>(3 - 1, 4 - 1, 5));  // i odd, j even
  A_mat.setFromTriplets(A.begin(), A.end());
  A_tilde = TransformationOfGalerkinMatrices::transformCOOmatrix(A);
  A_tilde_mat.setFromTriplets(A_tilde.begin(), A_tilde.end());
  sol_mat = S * A_mat * S.transpose();

  ASSERT_TRUE((sol_mat - A_tilde_mat).norm() == 0);
}

}  // namespace TransformationOfGalerkinMatrices::test


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

bool sum(const vector<int> & nums, int offset, int target) {
    for (int x = offset; x < nums.size(); x++) {
        for (int y = x + 1; y < nums.size(); y++) {
            if ((nums[x] + nums[y]) == target) {
                return true;
            }
        }
    }
    return false;
}

int range(const vector<int> & nums, int offset, int target) {
    int smallest = INT_MAX;
    int largest = 0;
    int total = 0;
    int i = offset;
    while (total < target && i < nums.size()) {
        total += nums[i];

        smallest = min(smallest, nums[i]);
        largest = max(largest, nums[i]);

        i++;
    }
    if (total == target) {
        return smallest + largest;
    }
    return 0;
}

int main() {
    ifstream file ("2020/9.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    int answer1 = 0;
    int answer2 = 0;

    int preamble = 25;
    vector<int> nums;

    string line;
    while (getline(file, line, '\n')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        int i = stoi(line);

        if (nums.size() > preamble) {
            // Start checking
            if (!sum(nums, nums.size() - preamble, i)) {
                answer1 = i;
                break;
            }
        }

        nums.push_back(i);
    }

    cout << "Answer 9.1: " << answer1 << endl;

    for (int x = 0; x < nums.size(); x++) {
        answer2 = range(nums, x, answer1);
        if (answer2) {
            break;
        }
    }

    cout << "Answer 9.2: " << answer2 << endl;

    file.close();
}


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

/*
 Copyright (C) 2003, 2004 Ferdinando Ametrano
 Copyright (C) 2006 Richard Gould
 Copyright (C) 2007 Mark Joshi

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

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

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

/*! \file sobolrsg.hpp
    \brief Sobol low-discrepancy sequence generator
*/

#ifndef quantlib_sobol_ld_rsg_hpp
#define quantlib_sobol_ld_rsg_hpp

#include <ql/methods/montecarlo/sample.hpp>
#include <vector>
#include <boost/cstdint.hpp>

namespace QuantLib {

    //! Sobol low-discrepancy sequence generator
    /*! A Gray code counter and bitwise operations are used for very
        fast sequence generation.

        The implementation relies on primitive polynomials modulo two
        from the book "Monte Carlo Methods in Finance" by Peter
        Jäckel.

        21 200 primitive polynomials modulo two are provided in QuantLib.
        Jäckel has calculated 8 129 334 polynomials: if you need that many
        dimensions you can replace the primitivepolynomials.cpp file included
        in QuantLib with the one provided in the CD of the "Monte Carlo
        Methods in Finance" book.

        The choice of initialization numbers (also know as free direction
        integers) is crucial for the homogeneity properties of the sequence.
        Sobol defines two homogeneity properties: Property A and Property A'.

        The unit initialization numbers suggested in "Numerical
        Recipes in C", 2nd edition, by Press, Teukolsky, Vetterling,
        and Flannery (section 7.7) fail the test for Property A even
        for low dimensions.

        Bratley and Fox published coefficients of the free direction
        integers up to dimension 40, crediting unpublished work of
        Sobol' and Levitan. See Bratley, P., Fox, B.L. (1988)
        "Algorithm 659: Implementing Sobol's quasirandom sequence
        generator," ACM Transactions on Mathematical Software
        14:88-100. These values satisfy Property A for d<=20 and d =
        23, 31, 33, 34, 37; Property A' holds for d<=6.

        Jäckel provides in his book (section 8.3) initialization
        numbers up to dimension 32. Coefficients for d<=8 are the same
        as in Bradley-Fox, so Property A' holds for d<=6 but Property
        A holds for d<=32.

        The implementation of Lemieux, Cieslak, and Luttmer includes
        coefficients of the free direction integers up to dimension
        360.  Coefficients for d<=40 are the same as in Bradley-Fox.
        For dimension 40<d<=360 the coefficients have
        been calculated as optimal values based on the "resolution"
        criterion. See "RandQMC user's guide - A package for
        randomized quasi-Monte Carlo methods in C," by C. Lemieux,
        M. Cieslak, and K. Luttmer, version January 13 2004, and
        references cited there
        (http://www.math.ucalgary.ca/~lemieux/randqmc.html).
        The values up to d<=360 has been provided to the QuantLib team by
        Christiane Lemieux, private communication, September 2004.

        For more info on Sobol' sequences see also "Monte Carlo
        Methods in Financial Engineering," by P. Glasserman, 2004,
        Springer, section 5.2.3

        The Joe--Kuo numbers and the Kuo numbers are due to Stephen Joe
        and Frances Kuo.

        S. Joe and F. Y. Kuo, Constructing Sobol sequences with better
        two-dimensional projections, preprint Nov 22 2007

        See http://web.maths.unsw.edu.au/~fkuo/sobol/ for more information.

        The Joe-Kuo numbers are available under a BSD-style license
        available at the above link.

        Note that the Kuo numbers were generated to work with a
        different ordering of primitive polynomials for the first 40
        or so dimensions which is why we have the Alternative
        Primitive Polynomials.

        \test
        - the correctness of the returned values is tested by
          reproducing known good values.
        - the correctness of the returned values is tested by checking
          their discrepancy against known good values.
    */
    class SobolRsg {
      public:
        typedef Sample<std::vector<Real> > sample_type;
        enum DirectionIntegers {
            Unit, Jaeckel, SobolLevitan, SobolLevitanLemieux,
            JoeKuoD5, JoeKuoD6, JoeKuoD7,
            Kuo, Kuo2, Kuo3 };
        /*! \pre dimensionality must be <= PPMT_MAX_DIM */
        SobolRsg(Size dimensionality,
                 unsigned long seed = 0,
                 DirectionIntegers directionIntegers = Jaeckel);
        /*! skip to the n-th sample in the low-discrepancy sequence */
        void skipTo(boost::uint_least32_t n);
        const std::vector<boost::uint_least32_t>& nextInt32Sequence() const;

        const SobolRsg::sample_type& nextSequence() const {
            const std::vector<boost::uint_least32_t>& v = nextInt32Sequence();
            // normalize to get a double in (0,1)
            for (Size k=0; k<dimensionality_; ++k)
                sequence_.value[k] = v[k] * normalizationFactor_;
            return sequence_;
        }
        const sample_type& lastSequence() const { return sequence_; }
        Size dimension() const { return dimensionality_; }
      private:
        static const int bits_;
        static const double normalizationFactor_;
        Size dimensionality_;
        mutable boost::uint_least32_t sequenceCounter_;
        mutable bool firstDraw_;
        mutable sample_type sequence_;
        mutable std::vector<boost::uint_least32_t> integerSequence_;
        std::vector<std::vector<boost::uint_least32_t> > directionIntegers_;
    };

}

#endif


<%
cfg['compiler_args'] = ['-std=c++11']
cfg['include_dirs'] = ['/usr/include/eigen3']
setup_pybind11(cfg)
%>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <cmath>
#include <Eigen/LU>

namespace py = pybind11;

using Eigen::MatrixXd;

MatrixXd cdist(MatrixXd xs, MatrixXd ys) {
    int m = xs.rows();
    int n = ys.rows();
    int p = ys.cols();

    MatrixXd res(m, n);

    double s;
    for (int i=0; i<m; i++) {
        for (int j=0; j<n; j++) {
            s = 0;
            for (int k=0; k<p; k++) {
                s += pow(ys(j,k) - xs(i,k), 2);
            }
            res(i,j) = sqrt(s);
        }
    }

    return res;
}

PYBIND11_MODULE(funcs, m) {
    m.doc() = "auto-compiled c++ extension";
    m.def("cdist", &cdist);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3Q.cpp
 * @brief   Rotation (internal: quaternion representation*)
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifdef GTSAM_USE_QUATERNIONS

#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>
#include <cmath>

using namespace std;

namespace gtsam {

  static const Matrix I3 = eye(3);

  /* ************************************************************************* */
  Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
      quaternion_((Eigen::Matrix3d() <<
          col1.x(), col2.x(), col3.x(),
          col1.y(), col2.y(), col3.y(),
          col1.z(), col2.z(), col3.z()).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(double R11, double R12, double R13,
      double R21, double R22, double R23,
      double R31, double R32, double R33) :
        quaternion_((Eigen::Matrix3d() <<
            R11, R12, R13,
            R21, R22, R23,
            R31, R32, R33).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix3& R) :
      quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix& R) :
      quaternion_(Matrix3(R)) {}

//  /* ************************************************************************* */
//   Rot3::Rot3(const Matrix3& R) :
//       quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}

  /* ************************************************************************* */
  Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

  /* ************************************************************************* */
  Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }

  /* ************************************************************************* */
  Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }

  /* ************************************************************************* */
  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
      Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
      Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
      Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
  }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w, double theta) {
    return Quaternion(Eigen::AngleAxisd(theta, w)); }

  /* ************************************************************************* */
  Rot3 Rot3::compose(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = R2.transpose();
    if (H2) *H2 = I3;
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Rot3 Rot3::operator*(const Rot3& R2) const {
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
    if (H1) *H1 = -matrix();
    return Rot3(quaternion_.inverse());
  }

  /* ************************************************************************* */
  Rot3 Rot3::between(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = -(R2.transpose()*matrix());
    if (H2) *H2 = I3;
    return between_default(*this, R2);
  }

  /* ************************************************************************* */
  Point3 Rot3::rotate(const Point3& p,
        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
    Matrix R = matrix();
    if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    if (H2) *H2 = R;
    Eigen::Vector3d r = R * p.vector();
    return Point3(r.x(), r.y(), r.z());
  }

  /* ************************************************************************* */
  Vector3 Rot3::Logmap(const Rot3& R) {
    using std::acos;
    using std::sqrt;
    static const double twoPi = 2.0 * M_PI,
    // define these compile time constants to avoid std::abs:
        NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;

    const Quaternion& q = R.quaternion_;
    const double qw = q.w();
    if (qw > NearlyOne) {
      // Taylor expansion of (angle / s) at 1
      return (2 - 2 * (qw - 1) / 3) * q.vec();
    } else if (qw < NearlyNegativeOne) {
      // Angle is zero, return zero vector
      return Vector3::Zero();
    } else {
      // Normal, away from zero case
      double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
      // Important:  convert to [-pi,pi] to keep error continuous
      if (angle > M_PI)
        angle -= twoPi;
      else if (angle < -M_PI)
        angle += twoPi;
      return (angle / s) * q.vec();
    }
  }

  /* ************************************************************************* */
  Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
    return compose(Expmap(omega));
  }

  /* ************************************************************************* */
  Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
    return Logmap(between(t2));
  }

  /* ************************************************************************* */
  Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}

  /* ************************************************************************* */
  Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();}

  /* ************************************************************************* */
  Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

  /* ************************************************************************* */
  Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }

  /* ************************************************************************* */
  Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }

  /* ************************************************************************* */
  Quaternion Rot3::toQuaternion() const { return quaternion_; }

 /* ************************************************************************* */

} // namespace gtsam

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOG_2_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG_2_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Log_2 Log_2 (function template)

  Generates the constant \f$\log(2)\f$

  @headerref{<boost/simd/constant/log_2.hpp>}

  @par Description

  1.  @code
      template<typename T> T Log_2();
      @endcode

  2.  @code
      template<typename T> T Log_2( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to \f$\log(2)\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
   A value of type @c T that evaluates to ` T(0.6931471805599453094172321214581765680755001343602553)`.

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/log_2.hpp>
#include <boost/simd/constant/simd/log_2.hpp>

#endif


#include <Eigen/Dense>

#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Nodes/ContactSensor.h>

#include "bipedal.h"
#include "utils/ZMP.h"

namespace Bipedal
{

inline Eigen::Vector2f computeModelZMP(const Eigen::Vector3f& com, const Eigen::Vector3f& comAcc, double gravity)
{
    Eigen::Vector2f zmp;
    zmp.x() = com.x() - com.z() / gravity * comAcc.x();
    zmp.y() = com.y() - com.z() / gravity * comAcc.y();
    return zmp;
}

inline Eigen::Vector2f computeMultiBodyZMP(double mass,
                                           double gravity,
                                           const Eigen::Vector3f& com,
                                           const Eigen::Vector3f& linearMomentumDiff,
                                           const Eigen::Vector3f& angularMomentumDiff)
{
    Eigen::Vector2f zmp;
    double norm = mass * gravity + linearMomentumDiff.z();
    zmp.x() = mass * gravity * com.x() - angularMomentumDiff.y();
    zmp.y() = mass * gravity * com.y() + angularMomentumDiff.x();
    zmp /= norm;

    return zmp;
}

MultiBodyZMPEstimator::MultiBodyZMPEstimator(double mass, double gravity)
: mass(mass)
, gravity(gravity)
, estimation(Eigen::Vector2f::Zero())
, linearMomentumDiff(Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero())
, angularMomentumDiff(Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero())
{
    BOOST_ASSERT(gravity > 0);
}

void MultiBodyZMPEstimator::update(const Eigen::Vector3f& com,
            const Eigen::Vector3f& linearMomentum,
            const Eigen::Vector3f& angularMomentum,
            double dt)
{
    linearMomentumDiff.update(linearMomentum, dt);
    angularMomentumDiff.update(angularMomentum, dt);

    estimation = computeMultiBodyZMP(mass, gravity, com, linearMomentumDiff.estimation, angularMomentumDiff.estimation);
}

CartTableZMPEstimator::CartTableZMPEstimator(double gravity)
: gravity(gravity)
, accelerationEstimator(Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero())
{
    BOOST_ASSERT(gravity > 0);
}

void CartTableZMPEstimator::update(const Eigen::Vector3f& com, const Eigen::Vector3f& comVel, double dt)
{
    accelerationEstimator.update(comVel, dt);
    estimation = computeModelZMP(com, accelerationEstimator.estimation, gravity);
}

CoPZMPEstimator::CoPZMPEstimator(const VirtualRobot::ContactSensorPtr& leftFootSensor,
                                 const VirtualRobot::ContactSensorPtr& rightFootSensor)
: leftFootSensor(leftFootSensor)
, rightFootSensor(rightFootSensor)
, estimation(Eigen::Vector2f::Zero())
{
}

void CoPZMPEstimator::update(float dt)
{
    double totalForce = 0.0;
    Eigen::Vector2f pointSum = Eigen::Vector2f::Zero();

    for (const auto& f : leftFootSensor->getContacts().forces)
    {
        if (f.bodyName == "Floor" && f.zForce > 0)
        {
            totalForce += f.zForce;
            pointSum += f.zForce * f.contactPoint.head(2);
        }
    }

    for (const auto& f : rightFootSensor->getContacts().forces)
    {
        if (f.bodyName == "Floor" && f.zForce > 0)
        {
            totalForce += f.zForce;
            pointSum += f.zForce * f.contactPoint.head(2);
        }
    }

    if (totalForce > 0)
    {
        estimation = pointSum / totalForce / 1000.0;
    }
}

}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "NNFuncs.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class NNLayer
{
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;
  using Activation = NNActivations::Activation;
  using LayerData = std::tuple<RealMatrixView, RealVectorView, index>;

public:
  NNLayer(index inputSize, index outputSize, index actType)
  {
    using namespace Eigen;
    mWeights = MatrixXd::Ones(inputSize, outputSize);
    mBiases = VectorXd::Zero(outputSize);
    mActType = actType;
    mActivation = static_cast<Activation>(actType);
  }

  void init(Eigen::Ref<MatrixXd> weights, Eigen::Ref<VectorXd> biases,
            index actType)
  {
    mWeights = weights;
    mBiases = biases;
    mActType = actType;
    mActivation = static_cast<Activation>(actType);
    initGrads();
  }

  void init()
  {
    double dev = std::sqrt(6.0 / (mWeights.rows() + mWeights.cols()));
    mWeights = dev * MatrixXd::Random(mWeights.rows(), mWeights.cols()).array();
    mBiases = VectorXd::Zero(mWeights.cols());
    initGrads();
  }

  MatrixXd getWeights() const { return mWeights; }
  VectorXd getBiases() const { return mBiases; }
  index    getActType() const { return mActType; }

  void initGrads()
  {
    mWeightsGrad = MatrixXd::Zero(mWeights.rows(), mWeights.cols());
    mBiasesGrad = VectorXd::Zero(mWeights.cols());
    mPrevWeightsUpdate = MatrixXd::Zero(mWeights.rows(), mWeights.cols());
    mPrevBiasesUpdate = VectorXd::Zero(mWeights.cols());
  }

  index inputSize() const { return mWeights.rows(); }

  index outputSize() const { return mWeights.cols(); }

  void forward(Eigen::Ref<MatrixXd> in, Eigen::Ref<MatrixXd> out) const
  {
    mInput = in;
    MatrixXd WT = mWeights.transpose();
    MatrixXd IT = mInput.transpose();
    MatrixXd Z = ((WT * IT).colwise() + mBiases).transpose();
    mOutput = MatrixXd::Zero(out.rows(), out.cols());
    NNActivations::activation()[mActivation](Z, mOutput);
    out = mOutput;
  }

  void backward(Eigen::Ref<MatrixXd> outGrad, Eigen::Ref<MatrixXd> inGrad)
  { // going backwards, so out is in
    MatrixXd dAct = MatrixXd::Zero(mOutput.rows(), mOutput.cols());
    NNActivations::derivative()[mActivation](mOutput, dAct);
    MatrixXd actGrad = dAct.array() * outGrad.array();
    double   norm = 1.0 / mInput.rows();
    mWeightsGrad = norm * (mInput.transpose() * actGrad);
    inGrad = actGrad * mWeights.transpose();
    mBiasesGrad = actGrad.colwise().mean();
  }

  void update(double learningRate, double momentum)
  {
    MatrixXd wUpdate = (momentum * mPrevWeightsUpdate) +
                       ((1 - momentum) * learningRate * mWeightsGrad);
    VectorXd bUpdate = (momentum * mPrevBiasesUpdate) +
                       ((1 - momentum) * learningRate * mBiasesGrad);
    mWeights = mWeights - wUpdate;
    mBiases = mBiases - bUpdate;
    mPrevWeightsUpdate = wUpdate;
    mPrevBiasesUpdate = bUpdate;
  }

private:
  MatrixXd   mWeights;
  VectorXd   mBiases;
  index      mActType;
  Activation mActivation;

  MatrixXd mWeightsGrad;
  VectorXd mBiasesGrad;

  MatrixXd mPrevWeightsUpdate;
  VectorXd mPrevBiasesUpdate;

  mutable MatrixXd mInput;
  mutable MatrixXd mOutput;
};
} // namespace algorithm
} // namespace fluid


#include "tpf_intersection.h"

#include "tpf_cuboid.h"
#include "tpf_line.h"
#include "tpf_plane.h"
#include "tpf_point.h"
#include "tpf_polyhedron.h"
#include "tpf_tetrahedron.h"
#include "tpf_triangle.h"

#include "../algorithm/tpf_joaat.h"

#include "../stdext/tpf_comparator.h"

#include "../utility/tpf_optional.h"

#include "Eigen/Dense"

#include <boost/variant/get.hpp>

#include <CGAL/intersections.h>
#include <CGAL/Point_3.h>
#include <CGAL/Triangle_3.h>

#include <algorithm>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace tpf
{
    namespace geometry
    {
        template <typename floatp_t, typename kernel_t>
        inline bool does_intersect_with(const line<floatp_t, kernel_t>& line, const plane<floatp_t, kernel_t>& plane)
        {
            return CGAL::do_intersect(plane.get_internal(), line.get_internal());
        }

        template <typename floatp_t, typename kernel_t>
        inline bool does_intersect_with(const plane<floatp_t, kernel_t>& plane, const cuboid<floatp_t, kernel_t>& cuboid)
        {
            return CGAL::do_intersect(cuboid.get_internal().bbox(), plane.get_internal());
        }

        template <typename floatp_t, typename kernel_t>
        inline utility::optional<point<floatp_t, kernel_t>> intersect_with(const line<floatp_t, kernel_t>& line, const plane<floatp_t, kernel_t>& plane)
        {
            auto intersection = CGAL::intersection(plane.get_internal(), line.get_internal());

            if (intersection)
            {
                const typename kernel_t::Point_3* p = boost::get<typename kernel_t::Point_3>(&*intersection);

                if (p != nullptr)
                {
                    return *p;
                }
                else
                {
                    return utility::nullopt;
                }
            }
            else
            {
                return utility::nullopt;
            }
        }

        template <typename floatp_t, typename kernel_t>
        inline std::vector<point<floatp_t, kernel_t>> intersect_with(const plane<floatp_t, kernel_t>& plane, const cuboid<floatp_t, kernel_t>& cuboid)
        {
            // Extract edges
            std::vector<line<floatp_t, kernel_t>> edges;
            edges.reserve(12);

            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(0), cuboid.get_internal().vertex(1)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(3), cuboid.get_internal().vertex(2)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(5), cuboid.get_internal().vertex(6)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(4), cuboid.get_internal().vertex(7)));

            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(0), cuboid.get_internal().vertex(3)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(1), cuboid.get_internal().vertex(2)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(5), cuboid.get_internal().vertex(4)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(6), cuboid.get_internal().vertex(7)));

            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(0), cuboid.get_internal().vertex(5)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(1), cuboid.get_internal().vertex(6)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(3), cuboid.get_internal().vertex(4)));
            edges.push_back(line<floatp_t, kernel_t>(cuboid.get_internal().vertex(2), cuboid.get_internal().vertex(7)));

            // Intersect edges with plane
            std::vector<point<floatp_t, kernel_t>> intersections;

            for (const line<floatp_t, kernel_t>& edge : edges)
            {
                if (does_intersect_with(edge, plane))
                {
                    auto intersection = intersect_with(edge, plane);

                    if (intersection)
                    {
                        intersections.push_back(*intersection);
                    }
                }
            }

            return intersections;
        }

        template <typename floatp_t, typename kernel_t>
        inline std::vector<point<floatp_t, kernel_t>> intersect_with(const plane<floatp_t, kernel_t>& plane, const tetrahedron<floatp_t, kernel_t>& tetrahedron)
        {
            // Extract edges
            std::vector<line<floatp_t, kernel_t>> edges;
            edges.reserve(6);

            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(0), tetrahedron.get_internal().vertex(1)));
            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(0), tetrahedron.get_internal().vertex(2)));
            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(0), tetrahedron.get_internal().vertex(3)));
            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(1), tetrahedron.get_internal().vertex(2)));
            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(1), tetrahedron.get_internal().vertex(3)));
            edges.push_back(line<floatp_t, kernel_t>(tetrahedron.get_internal().vertex(2), tetrahedron.get_internal().vertex(3)));

            // Intersect edges with plane
            std::vector<point<floatp_t, kernel_t>> intersections;

            for (const line<floatp_t, kernel_t>& edge : edges)
            {
                if (does_intersect_with(edge, plane))
                {
                    auto intersection = intersect_with(edge, plane);

                    if (intersection)
                    {
                        intersections.push_back(*intersection);
                    }
                }
            }

            return intersections;
        }

        template <typename floatp_t, typename kernel_t>
        inline std::vector<point<floatp_t, kernel_t>> intersect_with(const plane<floatp_t, kernel_t>& plane, const polyhedron<floatp_t, kernel_t>& polyhedron)
        {
            // Extract faces
            auto face_predicate = [](const triangle<floatp_t, kernel_t>& triangle) -> std::size_t
            {
                auto points = triangle.get_points();
                std::sort(points.begin(), points.end(), std::less<Eigen::Matrix<floatp_t, 3, 1>>());

                return algorithm::joaat_hash(points[0], points[1], points[2]);
            };

            std::unordered_map<triangle<floatp_t, kernel_t>, std::size_t, decltype(face_predicate)> faces(23, face_predicate);

            for (const auto& tetrahedron : polyhedron.get_internal())
            {
                for (int i = 0; i < 2; ++i)
                {
                    for (int j = i + 1; j < 3; ++j)
                    {
                        for (int k = j + 1; k < 4; ++k)
                        {
                            const triangle<floatp_t, kernel_t> face(tetrahedron.get_internal().vertex(i),
                                tetrahedron.get_internal().vertex(j), tetrahedron.get_internal().vertex(k));

                            if (faces.find(face) == faces.end())
                            {
                                faces[face] = 1;
                            }
                            else
                            {
                                ++faces[face];
                            }
                        }
                    }
                }
            }

            // Filter faces, such that only outer ones remain
            for (auto it = faces.begin(); it != faces.end(); )
            {
                if (it->second != 1)
                {
                    faces.erase(it++);
                }
                else
                {
                    ++it;
                }
            }

            // Extract edges
            auto edge_predicate = [](const line<floatp_t, kernel_t>& line) -> std::size_t
            {
                auto points = line.get_points();
                std::sort(points.begin(), points.end(), std::less<Eigen::Matrix<floatp_t, 3, 1>>());

                return algorithm::joaat_hash(points[0], points[1]);
            };

            std::unordered_set<line<floatp_t, kernel_t>, decltype(edge_predicate)> edges(23, edge_predicate);

            for (const auto& face : faces)
            {
                for (int i = 0; i < 2; ++i)
                {
                    for (int j = i + 1; j < 3; ++j)
                    {
                        const line<floatp_t, kernel_t> edge(face.first.get_internal().vertex(i), face.first.get_internal().vertex(j));

                        if (edges.find(edge) == edges.end())
                        {
                            edges.insert(edge);
                        }
                    }
                }
            }

            // Intersect edges with plane
            std::vector<point<floatp_t, kernel_t>> intersections;

            for (const auto& edge : edges)
            {
                if (does_intersect_with(edge, plane))
                {
                    auto intersection = intersect_with(edge, plane);

                    if (intersection)
                    {
                        intersections.push_back(*intersection);
                    }
                }
            }

            return intersections;
        }
    }
}


#include <iostream>
#include <string>
#include <algorithm>
#include <utility> // std::pair, std::make_pair
#include <cmath> // float comparison
#include <limits>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "persistent_cohomology_multi_field"
#include <boost/test/unit_test.hpp>

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/reader_utils.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/Persistent_cohomology/Multi_field.h>

using namespace Gudhi;
using namespace Gudhi::persistent_cohomology;
using namespace boost::unit_test;

typedef Simplex_tree<> typeST;

std::string test_persistence(int min_coefficient, int max_coefficient, double min_persistence) {
  // file is copied in CMakeLists.txt
  std::ifstream simplex_tree_stream;
  simplex_tree_stream.open("simplex_tree_file_for_multi_field_unit_test.txt");
  typeST st;
  simplex_tree_stream >> st;
  simplex_tree_stream.close();

  // Display the Simplex_tree
  std::clog << "The complex contains " << st.num_simplices() << " simplices" << " - dimension= " << st.dimension()
      << std::endl;

  // Check
  BOOST_CHECK(st.num_simplices() == 58);
  BOOST_CHECK(st.dimension() == 3);

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology<Simplex_tree<>, Multi_field> pcoh(st);

  pcoh.init_coefficients(min_coefficient, max_coefficient); // initializes the coefficient field for homology
  // Compute the persistent homology of the complex
  pcoh.compute_persistent_cohomology(min_persistence); // Minimal lifetime of homology feature to be recorded.

  std::ostringstream ossPers;
  pcoh.output_diagram(ossPers);

  std::string strPers = ossPers.str();
  return strPers;
}

void test_persistence_with_coeff_field(int min_coefficient, int max_coefficient) {
  // there are 2 discontinued ensembles
  std::string value0("  0 0.25 inf");
  std::string value1("  1 0.4 inf");
  // And a big hole - cut in 2 pieces after 0.3
  std::string value2("  0 0.2 0.3");

  // For dim <= 1 =>
  std::string value3("  1 0.25 inf");
  std::string value4("  2 0.25 inf");
  std::string value5("  1 0.3 inf");
  std::string value6("  2 0.3 inf");
  std::string value7("  2 0.4 inf");

  std::clog << "********************************************************************" << std::endl;
  std::clog << "TEST OF PERSISTENT_COHOMOLOGY_MULTI_FIELD MIN_COEFF=" << min_coefficient << " MAX_COEFF=" << max_coefficient << " MIN_PERS=0" << std::endl;

  std::string str_persistence = test_persistence(min_coefficient, max_coefficient, 0.0);
  std::clog << "str_persistence=" << str_persistence << std::endl;

  BOOST_CHECK(str_persistence.find(value0) != std::string::npos); // Check found
  BOOST_CHECK(str_persistence.find(value1) != std::string::npos); // Check found
  BOOST_CHECK(str_persistence.find(value2) != std::string::npos); // Check found

  if ((min_coefficient < 2) && (max_coefficient < 2)) {
    BOOST_CHECK(str_persistence.find(value3) != std::string::npos); // Check found
    BOOST_CHECK(str_persistence.find(value4) != std::string::npos); // Check found
    BOOST_CHECK(str_persistence.find(value5) != std::string::npos); // Check found
    BOOST_CHECK(str_persistence.find(value6) != std::string::npos); // Check found
    BOOST_CHECK(str_persistence.find(value7) != std::string::npos); // Check found
  } else {
    BOOST_CHECK(str_persistence.find(value3) == std::string::npos); // Check not found
    BOOST_CHECK(str_persistence.find(value4) == std::string::npos); // Check not found
    BOOST_CHECK(str_persistence.find(value5) == std::string::npos); // Check not found
    BOOST_CHECK(str_persistence.find(value6) == std::string::npos); // Check not found
    BOOST_CHECK(str_persistence.find(value7) == std::string::npos); // Check not found
  }

}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_0_0) {
  test_persistence_with_coeff_field(0, 0);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_0_1) {
  test_persistence_with_coeff_field(0, 1);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_0_6) {
  test_persistence_with_coeff_field(0, 6);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_1_2) {
  test_persistence_with_coeff_field(1, 2);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_1_3) {
  test_persistence_with_coeff_field(1, 3);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_1_5) {
  test_persistence_with_coeff_field(1, 5);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_2_3) {
  test_persistence_with_coeff_field(2, 3);
}

BOOST_AUTO_TEST_CASE(persistent_cohomology_multi_field_coeff_3_4) {
  test_persistence_with_coeff_field(3, 4);
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2006 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Xing Jin, Wolfgang Bangerth, Texas A&M University, 2006
 */

// @sect3{Include files}

// 以下内容之前都已经介绍过了。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>
#include <iostream>

// 这是唯一一个新的。我们将需要一个定义在GridTools命名空间的库函数，用来计算最小的单元格直径。

#include <deal.II/grid/grid_tools.h>

// 最后一步和以前所有的程序一样。

namespace Step24
{
  using namespace dealii;
// @sect3{The "forward problem" class template}

// 主类的第一部分与 step-23 中的内容完全一致（除了名字）。

  template <int dim>
  class TATForwardProblem
  {
  public:
    TATForwardProblem();
    void run();

  private:
    void setup_system();
    void solve_p();
    void solve_v();
    void output_results() const;

    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;

    Vector<double> solution_p, solution_v;
    Vector<double> old_solution_p, old_solution_v;
    Vector<double> system_rhs_p, system_rhs_v;

    double       time_step, time;
    unsigned int timestep_number;
    const double theta;

// 下面是新的内容：首先，我们需要从吸收边界条件出来的那个边界质量矩阵 $B$ 。同样，由于这次我们考虑的是一个现实的介质，我们必须有一个衡量波速的标准 $c_0$ ，它将进入所有与拉普拉斯矩阵（我们仍然定义为 $(\nabla \phi_i,\nabla \phi_j)$ ）有关的公式。

    SparseMatrix<double> boundary_matrix;
    const double         wave_speed;

// 我们必须注意的最后一件事是，我们想在一定数量的检测器位置评估解决方案。我们需要一个数组来保存这些位置，在这里声明并在构造函数中填充。

    std::vector<Point<dim>> detector_locations;
  };
// @sect3{Equation data}

// 像往常一样，我们必须定义我们的初始值、边界条件和右手边的函数。这次事情有点简单：我们考虑的是一个由初始条件驱动的问题，所以没有右手函数（尽管你可以在 step-23 中查找，看看如何做到这一点）。其次，没有边界条件：域的整个边界由吸收性边界条件组成。这就只剩下初始条件了，这里的事情也很简单，因为对于这个特殊的应用，只规定了压力的非零初始条件，而没有规定速度的非零初始条件（速度在初始时间为零）。

// 所以这就是我们所需要的：一个指定压力初始条件的类。在本程序所考虑的物理环境中，这些是小的吸收器，我们将其建模为一系列的小圆圈，我们假设压力盈余为1，而其他地方没有吸收，因此没有压力盈余。我们是这样做的（注意，如果我们想把这个程序扩展到不仅可以编译，而且可以运行，我们将不得不用三维源的位置来初始化源）。

  template <int dim>
  class InitialValuesP : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> &p,
                         const unsigned int /*component*/ = 0) const override
    {
      static const std::array<Source, 5> sources{
        {Source(Point<dim>(0, 0), 0.025),
         Source(Point<dim>(-0.135, 0), 0.05),
         Source(Point<dim>(0.17, 0), 0.03),
         Source(Point<dim>(-0.25, 0), 0.02),
         Source(Point<dim>(-0.05, -0.15), 0.015)}};

      for (const auto &source : sources)
        if (p.distance(source.location) < source.radius)
          return 1;

      return 0;
    }

    struct Source
    {
      Source(const Point<dim> &l, const double r)
        : location(l)
        , radius(r)
      {}

      const Point<dim> location;
      const double     radius;
    };
  };
// @sect3{Implementation of the TATForwardProblem class}

// 让我们再从构造函数开始。设置成员变量是很直接的。我们使用矿物油的声波速度（单位为毫米/微秒，是实验性生物医学成像中的常用单位），因为我们想和输出的许多实验都是在这里进行的。再次使用Crank-Nicolson方案，即theta被设定为0.5。随后选择时间步长以满足 $k = \frac hc$ ：这里我们把它初始化为一个无效的数字。

  template <int dim>
  TATForwardProblem<dim>::TATForwardProblem()
    : fe(1)
    , dof_handler(triangulation)
    , time_step(std::numeric_limits<double>::quiet_NaN())
    , time(time_step)
    , timestep_number(1)
    , theta(0.5)
    , wave_speed(1.437)
  {

// 构造函数中的第二个任务是初始化存放检测器位置的数组。这个程序的结果与实验进行了比较，其中检测器间距的步长为2.25度，对应160个检测器位置。扫描圆的半径被选为中心和边界之间的一半，以避免不完善的边界条件带来的剩余反射破坏我们的数值结果。

// 然后按顺时针顺序计算探测器的位置。请注意，下面的内容当然只有在我们以2D计算时才有效，我们用一个断言来保护这个条件。如果我们以后想在三维中运行同样的程序，我们将不得不在这里添加代码来初始化三维中的探测器位置。由于断言的存在，我们不可能忘记这样做。

    Assert(dim == 2, ExcNotImplemented());

    const double detector_step_angle = 2.25;
    const double detector_radius     = 0.5;

    for (double detector_angle = 2 * numbers::PI; detector_angle >= 0;
         detector_angle -= detector_step_angle / 360 * 2 * numbers::PI)
      detector_locations.push_back(
        Point<dim>(std::cos(detector_angle), std::sin(detector_angle)) *
        detector_radius);
  }

//  @sect4{TATForwardProblem::setup_system}

// 下面的系统几乎就是我们在  step-23  中已经做过的，但有两个重要的区别。首先，我们必须在原点周围创建一个半径为1的圆形（或球形）网格。这并不新鲜：我们之前在 step-6 和 step-10 中已经这样做了，在那里我们还解释了PolarManifold或SphericalManifold对象如何在细化单元时将新点放在同心圆上，我们在这里也将使用它。

// 我们必须确保的一点是，时间步长满足  step-23  的介绍中讨论的 CFL 条件。在那个程序中，我们通过设置一个与网格宽度相匹配的时间步长来确保这一点，但是这很容易出错，因为如果我们再细化一次网格，我们也必须确保时间步长有所改变。在这里，我们自动做到了这一点：我们向一个库函数询问任何单元的最小直径。然后我们设置 $k=\frac h{c_0}$  。唯一的问题是： $h$ 到底是什么？关键是，对于波浪方程来说，这个问题确实没有好的理论。众所周知，对于由矩形组成的均匀细化网格， $h$ 是最小边长。但对于一般四边形的网格，确切的关系似乎是未知的，也就是说，不知道单元格的什么属性与CFL条件有关。问题是，CFL条件来自于对拉普拉斯矩阵最小特征值的了解，而这只能对简单结构的网格进行分析计算。

// 这一切的结果是，我们并不十分确定我们应该对 $h$ 采取什么措施。函数 GridTools::minimal_cell_diameter 计算了所有单元的最小直径。如果单元格都是正方形或立方体，那么最小边长就是最小直径除以 std::sqrt(dim)  。我们简单地将此概括为非均匀网格的情况，没有理论上的理由。

// 唯一的其他重大变化是我们需要建立边界质量矩阵。我们将在下文中进一步评论这个问题。

  template <int dim>
  void TATForwardProblem<dim>::setup_system()
  {
    const Point<dim> center;
    GridGenerator::hyper_ball(triangulation, center, 1.);
    triangulation.refine_global(7);

    time_step = GridTools::minimal_cell_diameter(triangulation) / wave_speed /
                std::sqrt(1. * dim);

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl;

    dof_handler.distribute_dofs(fe);

    std::cout << "Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl
              << std::endl;

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);

    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);

// 如前所述，与 step-23 的第二个区别是，我们需要建立从吸收性边界条件中生长出来的边界质量矩阵。

// 第一个观察结果是，这个矩阵比常规质量矩阵要稀疏得多，因为没有一个具有纯内部支持的形状函数对这个矩阵有贡献。因此，我们可以根据这种情况优化存储模式，建立第二个稀疏模式，只包含我们需要的非零项。这里有一个权衡：首先，我们必须要有第二个稀疏模式对象，所以这需要花费内存。其次，与该稀疏性模式相连的矩阵将更小，因此需要更少的内存；用它进行矩阵-向量乘法也会更快。然而，最后一个论点是提示规模的论点：我们主要感兴趣的不是单独对边界矩阵进行矩阵-向量运算（尽管我们需要在每个时间步长对右侧向量进行一次运算），而是主要希望将其与两个方程中的第一个方程使用的其他矩阵相加，因为这是CG方法每个迭代都要与之相乘的，即明显更频繁。现在的情况是， SparseMatrix::add 类允许将一个矩阵添加到另一个矩阵中，但前提是它们使用相同的稀疏模式（原因是我们不能在稀疏模式创建后向矩阵添加非零条目，所以我们只是要求这两个矩阵具有相同的稀疏模式）。

// 所以，我们就用这个方法吧。

    boundary_matrix.reinit(sparsity_pattern);

// 第二件要做的事是实际建立矩阵。在这里，我们需要对单元格的面进行积分，所以首先我们需要一个能在 dim-1 维对象上工作的正交对象。其次，FEValues的变体FEFaceValues，正如它的名字所暗示的，它可以在面上工作。最后，其他的变量是组装机器的一部分。所有这些我们都放在大括号里，以便将这些变量的范围限制在我们真正需要它们的地方。
//然后
//组装矩阵的实际行为是相当直接的：我们在所有单元中循环，在每个单元的所有面中循环，然后只在特定的面位于域的边界时做一些事情。像这样。

    {
      const QGauss<dim - 1> quadrature_formula(fe.degree + 1);
      FEFaceValues<dim>     fe_values(fe,
                                  quadrature_formula,
                                  update_values | update_JxW_values);

      const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
      const unsigned int n_q_points    = quadrature_formula.size();

      FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);

      std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

      for (const auto &cell : dof_handler.active_cell_iterators())
        for (const auto &face : cell->face_iterators())
          if (face->at_boundary())
            {
              cell_matrix = 0;

              fe_values.reinit(cell, face);

              for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
                for (unsigned int i = 0; i < dofs_per_cell; ++i)
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    cell_matrix(i, j) += (fe_values.shape_value(i, q_point) *
                                          fe_values.shape_value(j, q_point) *
                                          fe_values.JxW(q_point));

              cell->get_dof_indices(local_dof_indices);
              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                for (unsigned int j = 0; j < dofs_per_cell; ++j)
                  boundary_matrix.add(local_dof_indices[i],
                                      local_dof_indices[j],
                                      cell_matrix(i, j));
            }
    }

    system_matrix.copy_from(mass_matrix);
    system_matrix.add(time_step * time_step * theta * theta * wave_speed *
                        wave_speed,
                      laplace_matrix);
    system_matrix.add(wave_speed * theta * time_step, boundary_matrix);

    solution_p.reinit(dof_handler.n_dofs());
    old_solution_p.reinit(dof_handler.n_dofs());
    system_rhs_p.reinit(dof_handler.n_dofs());

    solution_v.reinit(dof_handler.n_dofs());
    old_solution_v.reinit(dof_handler.n_dofs());
    system_rhs_v.reinit(dof_handler.n_dofs());

    constraints.close();
  }
// @sect4{TATForwardProblem::solve_p and TATForwardProblem::solve_v}

// 下面两个函数，解决压力和速度变量的线性系统，几乎是逐字逐句地从 step-23 中提取的（除了主变量的名字从 $u$ 改为 $p$ ）。

  template <int dim>
  void TATForwardProblem<dim>::solve_p()
  {
    SolverControl solver_control(1000, 1e-8 * system_rhs_p.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(system_matrix, solution_p, system_rhs_p, PreconditionIdentity());

    std::cout << "   p-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

  template <int dim>
  void TATForwardProblem<dim>::solve_v()
  {
    SolverControl solver_control(1000, 1e-8 * system_rhs_v.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(mass_matrix, solution_v, system_rhs_v, PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

//  @sect4{TATForwardProblem::output_results}

// 这里也是如此：该函数来自  step-23  。

  template <int dim>
  void TATForwardProblem<dim>::output_results() const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution_p, "P");
    data_out.add_data_vector(solution_v, "V");

    data_out.build_patches();

    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtu";
    DataOutBase::VtkFlags vtk_flags;
    vtk_flags.compression_level =
      DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{TATForwardProblem::run}

// 这个做大部分工作的函数又和 step-23 中的差不多，尽管我们通过使用介绍中提到的向量G1和G2使事情变得更加清晰。与程序的整体内存消耗相比，引入几个临时向量并没有什么坏处。

// 这个函数唯一的变化是：首先，我们不必为速度 $v$ 预测初始值，因为我们知道它是零。其次，我们在构造函数中计算的检测器位置上评估解决方案。这是用 VectorTools::point_value 函数完成的。然后，这些值被写入我们在函数开始时打开的一个文件中。

  template <int dim>
  void TATForwardProblem<dim>::run()
  {
    setup_system();

    VectorTools::project(dof_handler,
                         constraints,
                         QGauss<dim>(fe.degree + 1),
                         InitialValuesP<dim>(),
                         old_solution_p);
    old_solution_v = 0;

    std::ofstream detector_data("detectors.dat");

    Vector<double> tmp(solution_p.size());
    Vector<double> G1(solution_p.size());
    Vector<double> G2(solution_v.size());

    const double end_time = 0.7;
    for (time = time_step; time <= end_time;
         time += time_step, ++timestep_number)
      {
        std::cout << std::endl;
        std::cout << "time_step " << timestep_number << " @ t=" << time
                  << std::endl;

        mass_matrix.vmult(G1, old_solution_p);
        mass_matrix.vmult(tmp, old_solution_v);
        G1.add(time_step * (1 - theta), tmp);

        mass_matrix.vmult(G2, old_solution_v);
        laplace_matrix.vmult(tmp, old_solution_p);
        G2.add(-wave_speed * wave_speed * time_step * (1 - theta), tmp);

        boundary_matrix.vmult(tmp, old_solution_p);
        G2.add(wave_speed, tmp);

        system_rhs_p = G1;
        system_rhs_p.add(time_step * theta, G2);

        solve_p();

        system_rhs_v = G2;
        laplace_matrix.vmult(tmp, solution_p);
        system_rhs_v.add(-time_step * theta * wave_speed * wave_speed, tmp);

        boundary_matrix.vmult(tmp, solution_p);
        system_rhs_v.add(-wave_speed, tmp);

        solve_v();

        output_results();

        detector_data << time;
        for (unsigned int i = 0; i < detector_locations.size(); ++i)
          detector_data << " "
                        << VectorTools::point_value(dof_handler,
                                                    solution_p,
                                                    detector_locations[i])
                        << " ";
        detector_data << std::endl;

        old_solution_p = solution_p;
        old_solution_v = solution_v;
      }
  }
} // namespace Step24

//  @sect3{The main function}

// 剩下的就是程序的主要功能了。这里没有什么是在前面几个程序中没有展示过的。

int main()
{
  try
    {
      using namespace Step24;

      TATForwardProblem<2> forward_problem_solver;
      forward_problem_solver.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <boost/simd/meta/prev_power_of_2.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/int.hpp>

int main()
{
  using boost::mpl::int_;

  typedef boost::simd::meta::prev_power_of_2_c<0    >::type  a0;
  typedef boost::simd::meta::prev_power_of_2_c<9    >::type  a9;
  typedef boost::simd::meta::prev_power_of_2_c<1055 >::type  a1055;

  BOOST_MPL_ASSERT(( boost::mpl::equal_to<a0    , int_<0>     >::type ));
  BOOST_MPL_ASSERT(( boost::mpl::equal_to<a9    , int_<8>     >::type ));
  BOOST_MPL_ASSERT(( boost::mpl::equal_to<a1055 , int_<1024>  >::type ));
}


#ifndef MI4_NORMALIZER_HPP
#define MI4_NORMALIZER_HPP 1

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

namespace mi4
{
        class Normalizer
        {
        private:
                Normalizer& operator = ( const Normalizer& that ) = delete;
                Normalizer& operator = ( Normalizer&& that ) = delete;
                Normalizer ( const Normalizer& that ) = delete;
                Normalizer ( Normalizer&& that ) = delete;
        private:
                Eigen::Affine3d _mat;
                Eigen::Affine3d _inv;
        public:
                Normalizer ( const Eigen::AlignedBox3d& gbox, const Eigen::AlignedBox3d& lbox = Eigen::AlignedBox3d ( Eigen::Vector3d ( 0, 0, 0 ), Eigen::Vector3d ( 1, 1, 1 ) ) )
                {
                        const auto v0 = this->avoid_zero ( gbox.max() - gbox.min() );
                        const auto v1 = this->avoid_zero ( lbox.max() - lbox.min() );
                        this->_mat = Eigen::Translation3d ( lbox.min() )
                                     * Eigen::Scaling ( v1.x() / v0.x(), v1.y() / v0.y(), v1.z() / v0.z() )
                                     * Eigen::Translation3d ( -gbox.min() );
                        this->_inv = this->_mat.inverse();
                        return;
                }
                Normalizer ( const Eigen::Vector3d& bmin,
                             const Eigen::Vector3d& bmax,
                             const Eigen::Vector3d& lmin = Eigen::Vector3d ( 0, 0, 0 ),
                             const Eigen::Vector3d& lmax = Eigen::Vector3d ( 1, 1, 1 ) )
                {
                        const auto v0 = this->avoid_zero ( bmax - bmin );
                        const auto v1 = this->avoid_zero ( lmax - lmin );
                        this->_mat = Eigen::Translation3d ( lmin )
                                     * Eigen::Scaling ( v1.x() / v0.x(), v1.y() / v0.y(), v1.z() / v0.z() )
                                     * Eigen::Translation3d ( -bmin );
                        this->_inv = this->_mat.inverse();

                        return;
                }

                ~Normalizer ( void ) = default;

                Eigen::Vector3d normalize ( const Eigen::Vector3d& p ) const
                {
                        return this->_mat * p;
                }

                Eigen::Vector3d denormalize ( const Eigen::Vector3d& p ) const
                {
                        return this->_inv * p;
                }

        private:
                // avoid zero-denominator.
                inline Eigen::Vector3d avoid_zero ( const Eigen::Vector3d& v ) const
                {
                        Eigen::Vector3d result;
                        result.x() = this->check_zero ( v.x() );
                        result.y() = this->check_zero ( v.y() );
                        result.z() = this->check_zero ( v.z() );
                        return result;
                }

                inline double check_zero ( const double& v ) const
                {
                        return ( v < 1.0e-40 ) ? 1 : v;
                }
        };
}
#endif


#include "smoothingSpline.h"

#include <Eigen/Geometry>

using namespace BIGSS;

smoothingSpline::smoothingSpline(const Eigen::VectorXd &y, const double lambda)
{
  Eigen::VectorXd x;
  x.setLinSpaced(y.size(), 0, 1);
  createSpline(x, y, lambda);
}

smoothingSpline::smoothingSpline(const Eigen::VectorXd &x, const Eigen::VectorXd &y, const double lambda)
{
  createSpline(x, y, lambda);
}

smoothingSpline::~smoothingSpline()
{

}

double smoothingSpline::evaluate(const double x)
{
  double dx = x - breaks(0);
  int i;
  for (i = 1; i < breaks.size(); i ++)
  {
    if (x - breaks(i) <= 0)
      break;
    dx = x - breaks(i);
  }
  double val = coeffs(i-1,0)*dx*dx*dx + coeffs(i-1,1)*dx*dx + coeffs(i-1,2)*dx + coeffs(i-1,3);
  return val;
}

Eigen::VectorXd smoothingSpline::evaluate(const Eigen::VectorXd &x)
{
  Eigen::VectorXd y = x;
  for (int i = 0; i < x.size(); i++)
  {
    y(i) = evaluate(x(i));
  }
  return y;
}

void smoothingSpline::createSpline(const Eigen::VectorXd &x, const Eigen::VectorXd &y, const double lambda)
{
  Eigen::DenseIndex n = y.size() - 1;
  Eigen::VectorXd h = x.segment(1, n) - x.segment(0, n);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(n-1, n-1);

  R(0, 0) = 2*(h(0) + h(1));
  for (int i = 1; i < n-2; i++)
  {
    R(i, i) = 2*(h(i) + h(i+1));
    R(i-1, i) = h(i);
    R(i, i-1) = h(i);
  }

  Eigen::VectorXd r = 3 / h.array();

  Eigen::MatrixXd Qt = Eigen::MatrixXd::Zero(n-1, n+1);
  for (int i = 0; i < n-1; i++)
  {
    Qt(i, i) = r(i);
    Qt(i, i+1) = -(r(i) + r(i+1));
    Qt(i, i+2) = r(i+1);
  }

  // weights are just the identity matrix
  Eigen::MatrixXd E = Eigen::MatrixXd::Identity(n+1, n+1);

  double mu = 2*(1-lambda)/(3*lambda);

  Eigen::MatrixXd A = mu * Qt * E * Qt.transpose() + R;
  Eigen::VectorXd B = Qt * y;

  Eigen::VectorXd b_1 = A.ldlt().solve(B); // A is guaranteed to be symmetric with 5 diagonal bands
  Eigen::VectorXd b = Eigen::VectorXd::Zero(n+1);
  b.segment(1,n-1) = b_1;

  Eigen::VectorXd d = y - mu * E * Qt.transpose() * b_1;
  Eigen::VectorXd a = (b.segment(1,n) - b.head(n)).array() / (3*h.array());
  // NOTE: There is a typo in the referenced paper for finding the c coefficient.
  //       The formula here is correct.
  Eigen::VectorXd c = (d.segment(1, n) - d.head(n)).array() / h.array() - 1.0 / 3.0 * (b.segment(1, n) + 2 * b.head(n)).array() * h.array();

  coeffs = Eigen::MatrixXd::Zero(n, 4);
  coeffs.col(0) = a;
  coeffs.col(1) = b.head(n);
  coeffs.col(2) = c;
  coeffs.col(3) = d.head(n);

  breaks = x;
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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 "GLHelper.h"

#include <cmath>
#include <Eigen/Dense>

namespace open3d {

namespace GLHelper {

GLMatrix4f LookAt(const Eigen::Vector3d &eye, const Eigen::Vector3d &lookat,
        const Eigen::Vector3d &up)
{
    Eigen::Vector3d front_dir = (eye - lookat).normalized();
    Eigen::Vector3d up_dir = up.normalized();
    Eigen::Vector3d right_dir = up_dir.cross(front_dir).normalized();
    up_dir = front_dir.cross(right_dir).normalized();

    Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
    mat.block<1, 3>(0, 0) = right_dir.transpose();
    mat.block<1, 3>(1, 0) = up_dir.transpose();
    mat.block<1, 3>(2, 0) = front_dir.transpose();
    mat(0, 3) = -right_dir.dot(eye);
    mat(1, 3) = -up_dir.dot(eye);
    mat(2, 3) = -front_dir.dot(eye);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

GLMatrix4f Perspective(double field_of_view_, double aspect,
        double z_near, double z_far)
{
    Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
    double fov_rad = field_of_view_ / 180.0 * M_PI;
    double tan_half_fov = std::tan(fov_rad / 2.0);
    mat(0, 0) = 1.0 / aspect / tan_half_fov;
    mat(1, 1) = 1.0 / tan_half_fov;
    mat(2, 2) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 2) = -1.0;
    mat(2, 3) = -2.0 * z_far * z_near / (z_far - z_near);
    return mat.cast<GLfloat>();
}

GLMatrix4f Ortho(double left, double right, double bottom, double top,
        double z_near, double z_far)
{
    Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
    mat(0, 0) = 2.0 / (right - left);
    mat(1, 1) = 2.0 / (top - bottom);
    mat(2, 2) = -2.0 / (z_far - z_near);
    mat(0, 3) = -(right + left) / (right - left);
    mat(1, 3) = -(top + bottom) / (top - bottom);
    mat(2, 3) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

Eigen::Vector3d Project(const Eigen::Vector3d &point,
        const GLMatrix4f &mvp_matrix, const int width, const int height)
{
    Eigen::Vector4d pos = mvp_matrix.cast<double>() *
            Eigen::Vector4d(point(0), point(1), point(2), 1.0);
    if (pos(3) == 0.0) {
        return Eigen::Vector3d::Zero();
    }
    pos /= pos(3);
    return Eigen::Vector3d(
            (pos(0) * 0.5 + 0.5) * (double)width,
            (pos(1) * 0.5 + 0.5) * (double)height,
            (1.0 + pos(2)) * 0.5);
}

Eigen::Vector3d Unproject(const Eigen::Vector3d &screen_point,
        const GLMatrix4f &mvp_matrix, const int width, const int height)
{
    Eigen::Vector4d point = mvp_matrix.cast<double>().inverse() *
            Eigen::Vector4d(screen_point(0) / (double)width * 2.0 - 1.0,
            screen_point(1) / (double)height * 2.0 - 1.0,
            screen_point(2) * 2.0 - 1.0, 1.0);
    if (point(3) == 0.0) {
        return Eigen::Vector3d::Zero();
    }
    point /= point(3);
    return point.block<3, 1>(0, 0);
}

int ColorCodeToPickIndex(const Eigen::Vector4i &color)
{
    if (color(0) == 255) {
        return -1;
    } else {
        return ((color(0) * 256 + color(1)) * 256 + color(2)) * 256 + color(3);
    }
}

}    // namespace GLHelper

}    // namespace open3d


/**
 * \file dcs/math/stats/distribution/discrete_uniform.hpp
 *
 * \brief The \c discrete_uniform distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_DISCRETE_UNIFORM_HPP
#define DCS_MATH_STATS_DISTRIBUTION_DISCRETE_UNIFORM_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost libraries version >= 1.15."
#endif // DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION

#include <boost/random/uniform_int_distribution.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <iostream>
#include <limits>
#include <vector>

namespace dcs { namespace math { namespace stats {

using ::std::size_t;

/**
 * \brief The discrete uniform distribution with parameter \f$a\f$ (the minimum
 * value) and \f$b\f$ (the maximum value).
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename IntT=int, typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class discrete_uniform_distribution
{
	public: typedef IntT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit discrete_uniform_distribution(support_type a=0, support_type b=std::numeric_limits<support_type>::max())
		: a_(a),
		  b_(b)
	{
		// empty
	}

	/**
	 * \brief Generate a random number distributed according to this discrete
	 * uniform distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this discrete uniform
	 * distribution.
	 *
	 * A \c discrete_uniform random number distribution produces random numbers
	 * \f$x\f$, \f$a \le x \le b\f$, distributed according to the constant
	 * probability density function:
	 * \f[
	 *   \Pr(x|a,b) = \frac{1}{(b - a + 1)}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		value_type rand(UniformRandomGeneratorT& rng) const
	{
		typedef ::boost::random::uniform_int_distribution<support_type> variate_type;
//		typedef ::boost::uniform_int<support_type> rdist_type;
//		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;
//
//		return variate_type(rng, rdist_type(a_, b_))();
		return variate_type(a_, b_)(rng);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * discrete uniform distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A random number distributed according to this discrete uniform
	 * distribution.
	 *
	 * A \c discrete_uniform random number distribution produces random numbers
	 * \f$x\f$, \f$a \le x \le b\f$, distributed according to the constant
	 * probability density function:
	 * \f[
	 *   \Pr(x|a,b) = \frac{1}{(b - a + 1)}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, size_t n)
	{
        typedef ::boost::random::uniform_int_distribution<support_type> variate_type;

		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(variate_type(a_, b_)(rng));
		}

		return rnds;
	}

	public: support_type min() const
	{
		return a_;
	}

	public: support_type max() const
	{
		return b_;
	}

	public: support_type scale() const
	{
		return (b_-a_+1);
	}

	public: support_type location() const
	{
		return a_;
	}

	private: support_type a_;
	private: support_type b_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, discrete_uniform_distribution<RealT,PolicyT> const& dist)
{
	return os << "DUniform("
			  << "min=" <<  dist.min()
			  << ", max=" <<  dist.max()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_DISCRETE_UNIFORM_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ULPDIST_HPP_INCLUDED

#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#include <tuple>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( ulpdist_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return (a0>a1) ? saturated_(minus)(a0,a1) : saturated_(minus)(a1,a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ulpdist_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::uint_<A0> >
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return dist(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ulpdist_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;

      if (a0 == a1)               return Zero<A0>();
      if (is_nan(a0)&&is_nan(a1)) return Zero<A0>();

      i_t e1, e2;
      A0 m1, m2;
      std::tie(m1, e1) = pedantic_(ifrexp)(a0);
      std::tie(m2, e2) = pedantic_(ifrexp)(a1);

      i_t expo = -simd::max(e1, e2);

      A0 e = (e1 == e2) ? simd::abs(m1-m2)
        :   simd::abs( simd::pedantic_(ldexp)(a0, expo)
                              - simd::pedantic_(ldexp)(a1, expo)
                            );
      return e/Eps<A0>();
    }
  };
} } }

#endif


/**
 * Copyright (c) 2020 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file estimate-point-normals.hpp
 * @author Matyas Hollmann <matyas.hollmann@melowntech.com>
 *
 * Estimation of point normals in the point cloud.
 * Code based on window-mesh-legacy.cpp
 *
 * Note: Uses Eigen3 library.
 *
 */

#ifndef ESTIMATE_POINT_NORMALS_HPP_INCLUDED
#define ESTIMATE_POINT_NORMALS_HPP_INCLUDED

#include <Eigen/Dense>

#include "dbglog/dbglog.hpp"
#include "utility/openmp.hpp"

#include "kdtree.hpp"
#include "neighbors.hpp"

namespace geometry {
/**
 * Estimate the normal of a point (in K-dim space) based on the data matrix (N x K)
 * which as rows has point itself and its (N - 1) closest neighbors.
 * The estimation of the normal is based on the principal-component analysis
 * of the data, a SVD of the covariance matrix (K x K) is performed.
 **/
Eigen::VectorXd estimateNormal(const Eigen::MatrixXd& data);

/**
 *  Interface to access dimension values of a point, and to calculate the
 *  difference of two points.
 **/
template <typename T>
struct DefaultAccessor {
    typedef typename T::value_type value_type;
    typedef T difference_type;

    static inline typename T::value_type get(const T& pt, unsigned dim) {
        return pt(dim);
    }

    static inline void set(T& pt, unsigned dim, const typename T::value_type& val) {
        pt(dim) = val;
    }

    // used in Kdtree in calculation of the distance of two points
    static inline T diff(const T& op1, const T& op2) {
        return op1 - op2;
    }
};

/**
 *
 *  Estimate normals of all points in the input point pointcloud.
 *
 *  typename T:     point in a K-dimensional space
 *  unsigned K:     number of dimensions of the space
 *  typename A:     dimension value accessor (has to support get(),
 *                               set() and also diff() [used in Kdtree]
 */
template<typename T, unsigned K = 3, typename A = DefaultAccessor<T>>
std::vector<T> estimateNormals(const std::vector<T>& pointCloud,
                               unsigned nEstimatorPts = 40,
                               double radius = 0)
{
    static_assert(K >= 2,
                  "Estimation of point normals makes sense only in at least "
                  "2-dimensional space.");
    using Neighbor = typename KdTree<T, K, A>::Neighbor;
    using Neighbors = typename KdTree<T, K, A>::Neighbors;

    auto toEigen([](const Neighbor& n) -> Eigen::RowVectorXd {
        Eigen::RowVectorXd vec(K);
        for (unsigned t = 0; t < K; ++t) {
            vec(t) = A::get(n.first, t);
        }
        return vec;
    });

    auto fromEigen([](const Eigen::VectorXd& vec) -> T {
        T res;
        for (unsigned t = 0; t < K; ++t) {
            A::set(res, t, vec(t));
        }
        return res;
    });

    auto setZeros([](T& pt) -> void {
        for (unsigned t = 0; t < K; ++t) {
            A::set(pt, t, 0);
        }
    });

    const size_t nPoints(pointCloud.size());
    // prepare space for normals
    std::vector<T> normals(nPoints);

    LOG(info3) << "Building a kd-tree from the pointcloud of "
               << nPoints << " points.";
    KdTree<T, K, A> kdtree(pointCloud.begin(), pointCloud.end());

    /** per thread accumulative variables **/
    double searchRadiusTotal(0.0);
    size_t pointsProcessed(0);

    UTILITY_OMP(parallel for schedule(static) default(shared)
               firstprivate(searchRadiusTotal, pointsProcessed))
    for (std::int64_t i = 0; i < static_cast<std::int64_t>(nPoints); ++i)
    {
        const T& point(pointCloud[i]);
        T& normal(normals[i]);

        // Mode 1: use provided radius as search radius
        double searchRadius = radius;
        if (radius <= 0.0) {
            // Mode 2: search radius is variable and is based on the average
            // radius needed to reach the specified number of neighbors
            searchRadius = ((searchRadiusTotal && pointsProcessed)
                                ? (searchRadiusTotal / pointsProcessed)
                                : 1.0);
            ++pointsProcessed;
        }

        Neighbors neighbors;
        // find neighbors
        searchRadiusTotal += collectNeighbors(kdtree, point, neighbors
                                              , nEstimatorPts
                                              , searchRadius
                                              , (radius > 0));

        size_t nNeighs = neighbors.size();
        if (nNeighs < nEstimatorPts) {
            // Oops, normal stays zero -> we may deal with this later
            LOG(warn3) << "too few neighbors! (" << nNeighs << ")";
            setZeros(normal);
            continue;
        }

        Eigen::MatrixXd samples(nNeighs, K);
        for (size_t j = 0; j < nNeighs; ++j)
        {
            samples.row(j) = toEigen(neighbors[j]);
        }

        // calculate normal
        normal = fromEigen(estimateNormal(samples));
    }
    return normals;
}

/**
 * @brief Orients point cloud normals to outward directions.
 *
 * @param pointCloud  Input points cloud
 * @param normals     Estimated normals (with undetermined orientation) for each point
 * @param pointRadius Radius of a patch associated with each point. Must be large enough
 *                    for the point patches to overlap and cover the surface.
 */
void reorientNormals(const std::vector<math::Point3>& pointCloud
                     , std::vector<math::Point3>& normals
                     , double pointRadius);

} // geometry

#endif // ESTIMATE_POINT_NORMALS_HPP_INCLUDED