/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseDataIOFactory.h>
#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <itkEvaluateDirectionImagesFilter.h>
#include <itkTractsToVectorImageFilter.h>
#include <usAny.h>
#include <itkImageFileWriter.h>
#include <mitkIOUtil.h>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>
#include <itksys/SystemTools.hxx>
#include <mitkTestingMacros.h>
#include <mitkCompareImageDataFilter.h>
#include <mitkFiberBundleXWriter.h>

#define _USE_MATH_DEFINES
#include <math.h>

using namespace std;

int mitkLocalFiberPlausibilityTest(int argc, char* argv[])
{
    MITK_TEST_BEGIN("mitkLocalFiberPlausibilityTest");
    MITK_TEST_CONDITION_REQUIRED(argc==8,"check for input data")

            string fibFile = argv[1];
    vector< string > referenceImages;
    referenceImages.push_back(argv[2]);
    referenceImages.push_back(argv[3]);
    string LDFP_ERROR_IMAGE = argv[4];
    string LDFP_NUM_DIRECTIONS = argv[5];
    string LDFP_VECTOR_FIELD = argv[6];
    string LDFP_ERROR_IMAGE_IGNORE = argv[7];

    float angularThreshold = 25;

    try
    {
        typedef itk::Image<unsigned char, 3>                                    ItkUcharImgType;
        typedef itk::Image< itk::Vector< float, 3>, 3 >                         ItkDirectionImage3DType;
        typedef itk::VectorContainer< unsigned int, ItkDirectionImage3DType::Pointer >   ItkDirectionImageContainerType;
        typedef itk::EvaluateDirectionImagesFilter< float >                     EvaluationFilterType;

        // load fiber bundle
        mitk::FiberBundleX::Pointer inputTractogram = dynamic_cast<mitk::FiberBundleX*>(mitk::IOUtil::LoadDataNode(fibFile)->GetData());

        // load reference directions
        ItkDirectionImageContainerType::Pointer referenceImageContainer = ItkDirectionImageContainerType::New();
        for (unsigned int i=0; i<referenceImages.size(); i++)
        {
            try
            {
                mitk::Image::Pointer img = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(referenceImages.at(i))->GetData());
                typedef mitk::ImageToItk< ItkDirectionImage3DType > CasterType;
                CasterType::Pointer caster = CasterType::New();
                caster->SetInput(img);
                caster->Update();
                ItkDirectionImage3DType::Pointer itkImg = caster->GetOutput();
                referenceImageContainer->InsertElement(referenceImageContainer->Size(),itkImg);
            }
            catch(...){ MITK_INFO << "could not load: " << referenceImages.at(i); }
        }

        ItkUcharImgType::Pointer itkMaskImage = ItkUcharImgType::New();
        ItkDirectionImage3DType::Pointer dirImg = referenceImageContainer->GetElement(0);
        itkMaskImage->SetSpacing( dirImg->GetSpacing() );
        itkMaskImage->SetOrigin( dirImg->GetOrigin() );
        itkMaskImage->SetDirection( dirImg->GetDirection() );
        itkMaskImage->SetLargestPossibleRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->SetBufferedRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->SetRequestedRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->Allocate();
        itkMaskImage->FillBuffer(1);

        // extract directions from fiber bundle
        itk::TractsToVectorImageFilter<float>::Pointer fOdfFilter = itk::TractsToVectorImageFilter<float>::New();
        fOdfFilter->SetFiberBundle(inputTractogram);
        fOdfFilter->SetMaskImage(itkMaskImage);
        fOdfFilter->SetAngularThreshold(cos(angularThreshold*M_PI/180));
        fOdfFilter->SetNormalizeVectors(true);
        fOdfFilter->SetUseWorkingCopy(false);
        fOdfFilter->SetNumberOfThreads(1);
        fOdfFilter->Update();
        ItkDirectionImageContainerType::Pointer directionImageContainer = fOdfFilter->GetDirectionImageContainer();

        // Get directions and num directions image
        ItkUcharImgType::Pointer numDirImage = fOdfFilter->GetNumDirectionsImage();
        mitk::Image::Pointer mitkNumDirImage = mitk::Image::New();
        mitkNumDirImage->InitializeByItk( numDirImage.GetPointer() );
        mitkNumDirImage->SetVolume( numDirImage->GetBufferPointer() );
        mitk::FiberBundleX::Pointer testDirections = fOdfFilter->GetOutputFiberBundle();

        // evaluate directions with missing directions
        EvaluationFilterType::Pointer evaluationFilter = EvaluationFilterType::New();
        evaluationFilter->SetImageSet(directionImageContainer);
        evaluationFilter->SetReferenceImageSet(referenceImageContainer);
        evaluationFilter->SetMaskImage(itkMaskImage);
        evaluationFilter->SetIgnoreMissingDirections(false);
        evaluationFilter->Update();

        EvaluationFilterType::OutputImageType::Pointer angularErrorImage = evaluationFilter->GetOutput(0);
        mitk::Image::Pointer mitkAngularErrorImage = mitk::Image::New();
        mitkAngularErrorImage->InitializeByItk( angularErrorImage.GetPointer() );
        mitkAngularErrorImage->SetVolume( angularErrorImage->GetBufferPointer() );

        // evaluate directions without missing directions
        evaluationFilter->SetIgnoreMissingDirections(true);
        evaluationFilter->Update();

        EvaluationFilterType::OutputImageType::Pointer angularErrorImageIgnore = evaluationFilter->GetOutput(0);
        mitk::Image::Pointer mitkAngularErrorImageIgnore = mitk::Image::New();
        mitkAngularErrorImageIgnore->InitializeByItk( angularErrorImageIgnore.GetPointer() );
        mitkAngularErrorImageIgnore->SetVolume( angularErrorImageIgnore->GetBufferPointer() );

        mitk::Image::Pointer gtAngularErrorImageIgnore = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(LDFP_ERROR_IMAGE_IGNORE)->GetData());
        mitk::Image::Pointer gtAngularErrorImage = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(LDFP_ERROR_IMAGE)->GetData());
        mitk::Image::Pointer gtNumTestDirImage = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(LDFP_NUM_DIRECTIONS)->GetData());
        mitk::FiberBundleX::Pointer gtTestDirections = dynamic_cast<mitk::FiberBundleX*>(mitk::IOUtil::LoadDataNode(LDFP_VECTOR_FIELD)->GetData());

        if (!testDirections->Equals(gtTestDirections))
        {
            MITK_INFO << "SAVING FILES TO " << mitk::IOUtil::GetTempPath();
            std::string out1 = mitk::IOUtil::GetTempPath().append("test.fib");
            std::string out2 = mitk::IOUtil::GetTempPath().append("reference.fib");

            mitk::FiberBundleXWriter::Pointer fibWriter = mitk::FiberBundleXWriter::New();
            fibWriter->SetFileName(out1.c_str());
            fibWriter->DoWrite(testDirections.GetPointer());

            fibWriter->SetFileName(out2.c_str());
            fibWriter->DoWrite(gtTestDirections.GetPointer());
        }

        MITK_TEST_CONDITION_REQUIRED(mitk::Equal(gtAngularErrorImageIgnore, mitkAngularErrorImageIgnore, 0.01, true), "Check if error images are equal (ignored missing directions).");
        MITK_TEST_CONDITION_REQUIRED(mitk::Equal(gtAngularErrorImage, mitkAngularErrorImage, 0.01, true), "Check if error images are equal.");
        MITK_TEST_CONDITION_REQUIRED(testDirections->Equals(gtTestDirections), "Check if vector fields are equal.");
        MITK_TEST_CONDITION_REQUIRED(mitk::Equal(gtNumTestDirImage, mitkNumDirImage, 0.0001, true), "Check if num direction images are equal.");
    }
    catch (itk::ExceptionObject e)
    {
        MITK_INFO << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        MITK_INFO << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        MITK_INFO << "ERROR!?!";
        return EXIT_FAILURE;
    }
    MITK_TEST_END();
}


#ifndef RSVD_BENCHMARK_BENCH_RUNNER_HPP_
#define RSVD_BENCHMARK_BENCH_RUNNER_HPP_

#include <chrono>
#include <cstdlib>
#include <rsvd/Prelude.hpp>
#include <sstream>

#include "DataStructures.hpp"
#include <Eigen/Dense>

using Benchmark::BenchConfig;
using Benchmark::RandomizedSvdConfig;
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
using Eigen::Index;
using Eigen::JacobiSVD;

namespace Benchmark {

template <typename MatrixType> MatrixType generateTestMatrix(const BenchConfig &conf) {
  std::srand(conf.prngSeed);

  const auto a = MatrixType::Random(conf.numRows, conf.rank);
  const auto b = MatrixType::Random(conf.rank, conf.numCols);

  return a * b;
}

template <typename MatrixType> class BenchRunnerBase {
  using ScalarType = typename MatrixType::Scalar;
  using RealType = typename Eigen::NumTraits<ScalarType>::Real;

public:
  explicit BenchRunnerBase(const BenchConfig &benchConf)
      : m_testMatrix(generateTestMatrix<MatrixType>(benchConf)), m_benchConfig(benchConf),
        m_stats() {}

  virtual void run() = 0;

  virtual void pushAsCsv(std::stringstream &) const = 0;

  void displayResults() const { m_stats.display(); }

protected:
  const MatrixType m_testMatrix;
  const BenchConfig &m_benchConfig;
  BenchStats<RealType> m_stats;
};

template <typename MatrixType> class BenchRunnerSvd : public BenchRunnerBase<MatrixType> {
  using ScalarType = typename MatrixType::Scalar;
  using RealType = typename Eigen::NumTraits<ScalarType>::Real;

public:
  explicit BenchRunnerSvd(const BenchConfig &benchConf) : BenchRunnerBase<MatrixType>(benchConf) {}

  void run() {
    const auto tic = std::chrono::steady_clock::now();
    JacobiSVD<MatrixType> svd(this->m_testMatrix, ComputeThinU | ComputeThinV);
    const auto toc = std::chrono::steady_clock::now();

    this->m_stats.runtime = std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic);

    const MatrixType reconstructed =
        svd.matrixU() * svd.singularValues().asDiagonal() * svd.matrixV().adjoint();

    this->m_stats.relativeApproximationError =
        Rsvd::relativeFrobeniusNormError(this->m_testMatrix, reconstructed);
  }

  void pushAsCsv(std::stringstream &ss) const {
    this->m_benchConfig.pushAsCsv(ss);
    ss << ",,,,,";
    this->m_stats.pushAsCsv(ss);
  }
};

template <typename MatrixType, Rsvd::SubspaceIterationConditioner Conditioner>
class BenchRunnerRandomizedSvd : public BenchRunnerBase<MatrixType> {
  using ScalarType = typename MatrixType::Scalar;
  using RealType = typename Eigen::NumTraits<ScalarType>::Real;

public:
  BenchRunnerRandomizedSvd(const BenchConfig &benchConf, const RandomizedSvdConfig &rsvdConf)
      : BenchRunnerBase<MatrixType>(benchConf), m_rsvdConfig(rsvdConf) {}

  void run() {
    std::mt19937_64 randomEngine;
    randomEngine.seed(m_rsvdConfig.prngSeed);

    const auto tic = std::chrono::steady_clock::now();
    Rsvd::RandomizedSvd<MatrixType, std::mt19937_64, Conditioner> rsvd(randomEngine);
    rsvd.compute(this->m_testMatrix, m_rsvdConfig.rank, m_rsvdConfig.oversampling,
                 m_rsvdConfig.numIter);
    const auto toc = std::chrono::steady_clock::now();

    this->m_stats.runtime = std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic);

    const MatrixType reconstructed =
        rsvd.matrixU() * rsvd.singularValues().asDiagonal() * rsvd.matrixV().adjoint();

    this->m_stats.relativeApproximationError =
        Rsvd::relativeFrobeniusNormError(this->m_testMatrix, reconstructed);
  }

  void pushAsCsv(std::stringstream &ss) const {
    this->m_benchConfig.pushAsCsv(ss);
    ss << ",";
    this->m_rsvdConfig.pushAsCsv(ss);
    ss << ",";
    this->m_stats.pushAsCsv(ss);
  }

private:
  const RandomizedSvdConfig &m_rsvdConfig;
};

} // namespace Benchmark

#endif


#pragma once
//===----------------------------------------------------------------------===//
#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <string>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Range list.
template<typename _position_t = $u32>
struct range_list {
  using position_t = _position_t;

  /// Represents an integer range.
  struct range {
  private:
    position_t begin_;
    position_t length_; // minus one

  public:
    inline
    range(std::size_t begin, std::size_t length)
        : begin_(static_cast<position_t>(begin)),
          length_(static_cast<position_t>(length - 1)) {
      assert(length > 0);
      assert(begin <= std::numeric_limits<position_t>::max());
      assert(length - 1 <= std::numeric_limits<position_t>::max());
    }

    /// Returns the begin of the range.
    inline std::size_t
    begin() const noexcept { return std::size_t(begin_); }

    /// Returns the length of the range.
    inline std::size_t
    length() const noexcept { return std::size_t(length_) + 1; }

    // For debugging purposes.
    void
    print(std::ostream& os) const noexcept {
      os << "[" << begin() << "," << length() << "]";
    }
  };

  /// The ordered list of non-overlapping ranges.
  std::vector<range> ranges_;

  /// The universe [0, n_).
  $u64 n_;

  // TODO make private
  range_list() = default;

  /// Constructs a range list from the given bitmap.
  explicit range_list(const boost::dynamic_bitset<$u32>& in)
      : ranges_(), n_(in.size()) {
    std::size_t current_begin = in.find_first();
    std::size_t current_length = 1;
    while (current_begin < n_) {
      while (current_begin + current_length < n_
          && in[current_begin + current_length]) {
        ++current_length;
      }
      ranges_.emplace_back(current_begin, current_length);
      current_begin = in.find_next(current_begin + current_length);
      current_length = 1;
    }
  }

  ~range_list() = default;
  range_list(const range_list& other) = default;
  range_list(range_list&& other) noexcept = default;

  __forceinline__ range_list&
  operator=(const range_list& other) = default;

  __forceinline__ range_list&
  operator=(range_list&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t
  size_in_bytes() const {
    return ranges_.size() * sizeof(range) /* ranges */
        + sizeof(std::size_t) /* number of ranges */
        + sizeof(n_) /* bit-length of the original bitmap */;
  }

  /// Return the length of the bitmap.
  std::size_t
  size() const {
    return n_;
  }

  /// Conversion to an boost::dynamic_bitset.
  boost::dynamic_bitset<$u32>
  to_bitset() {
    boost::dynamic_bitset<$u32> ret(n_);
    for (range& r : ranges_) {
      for (std::size_t j = r.begin; j < (r.begin + r.length); ++j) {
        ret[j] = true;
      }
    }
    return ret;
  }

  /// Bitwise AND
  range_list __forceinline__
  operator&(const range_list& other) const {
    range_list ret;
    ret.n_ = n_;
    auto this_it = ranges_.begin();
    const auto this_it_end = ranges_.end();
    auto other_it = other.ranges_.begin();
    const auto other_it_end = other.ranges_.end();
    while (!(this_it == this_it_end || other_it == other_it_end)) {
      if (*this_it == *other_it) {
        ret.ranges_.push_back(*this_it);
        ++this_it;
        ++other_it;
      }
      else {
        if (*this_it < *other_it) {
          ++this_it;
        }
        else {
          ++other_it;
        }
      }
    }
    return ret;
  }

  /// Bitwise AND (range encoding)
  range_list __forceinline__
  and_re(const range_list& other) const {
    return *this & other; // nothing special here. fall back to AND
  }

  /// Bitwise XOR
  range_list __forceinline__
  operator^(const range_list& other) const {
    range_list ret;
    ret.n_ = n_;

    auto this_it = ranges_.begin();
    const auto this_it_end = ranges_.end();
    auto other_it = other.ranges_.begin();
    const auto other_it_end = other.ranges_.end();
    while (!(this_it == this_it_end || other_it == other_it_end)) {
      if (*this_it < *other_it) {
        ret.ranges_.push_back(*this_it);
        ++this_it;
      }
      else if (*other_it < *this_it) {
        ret.ranges_.push_back(*other_it);
        ++other_it;
      }
      else {
        ++this_it;
        ++other_it;
      }
    }
    if (this_it != this_it_end) {
      while (this_it != this_it_end) {
        ret.ranges_.push_back(*this_it);
        ++this_it;
      }
    }
    if (other_it != other_it_end) {
      while (other_it != other_it_end) {
        ret.ranges_.push_back(*other_it);
        ++other_it;
      }
    }
    return ret;
  }

  /// Bitwise XOR (range encoding)
  range_list __forceinline__
  xor_re(const range_list& other) const {
    return *this ^ other; // nothing special here. fall back to XOR
  }

  /// Computes (a XOR b) & this
  /// Note: this, a and b must be different instances. Otherwise, the behavior
  /// is undefined.
  __forceinline__ range_list&
  fused_xor_and(const range_list& a, const range_list& b) {
    const auto x = a ^ b;
    auto y = *this & x;
    std::swap(ranges_, y.ranges_);
    return *this;
  }

  static std::string
  name() {
    return "range_list_"
        + std::to_string(sizeof(position_t) * 8);
  }

  /// Returns the value of the bit at the position pos.
  u1 test(const std::size_t pos) const {
    auto it = std::lower_bound(ranges_.begin(), ranges_.end(), pos,
        [](const range& lhs, std::size_t rhs) -> u1 {
          return (lhs.begin() + lhs.length()) < rhs;
        });
    if (it == ranges_.end()) {
      return false;
    }
    const auto rb = (*it).begin();
    const auto re = rb + (*it).length();
    return pos >= rb && pos < re;
  }

  //===--------------------------------------------------------------------===//
  /// Iterator, with skip support.
  class iter {
    const range_list& outer_;

    //===------------------------------------------------------------------===//
    // Iterator state
    //===------------------------------------------------------------------===//
    /// Read position within the list.
    $u64 read_pos_;
    /// Points to the beginning of the current range.
    $u64 range_begin_;
    /// The length of the current range.
    $u64 range_length_;
    //===------------------------------------------------------------------===//

  public:
    explicit __forceinline__
    iter(const range_list& outer)
        : outer_(outer),
          read_pos_(0),
          range_begin_(read_pos_ < outer.ranges_.size() // FIXME oO ???
                  ? outer_.ranges_[0].begin()
                  : outer_.n_),
          range_length_(read_pos_ < outer.ranges_.size()
                  ? outer_.ranges_[0].length()
                  : 0) {
    }

    iter(iter&&) noexcept = default;
    iter&
    operator=(iter&& other) noexcept = default;

    void __forceinline__
    next() {
      ++read_pos_;
      if (read_pos_ < outer_.ranges_.size()) {
        range_begin_ = outer_.ranges_[read_pos_].begin();
        range_length_ = outer_.ranges_[read_pos_].length();
      }
      else {
        range_begin_ = outer_.n_;
        range_length_ = 0;
        read_pos_ = outer_.ranges_.size();
      }
    }

    void __forceinline__
    skip_to(const std::size_t to_pos) {
      if (to_pos < (range_begin_ + range_length_)) {
        range_length_ -= to_pos - range_begin_;
        range_begin_ = to_pos;
        return;
      }
      auto search = std::lower_bound(
          outer_.ranges_.begin(), outer_.ranges_.end(), to_pos,
          [](const range& lhs, const std::size_t rhs) -> bool {
            return (lhs.begin() + lhs.length()) < rhs + 1;
          });
      if (search != outer_.ranges_.end()) {
        read_pos_ = std::distance(outer_.ranges_.begin(), search);
        if (to_pos < (*search).begin()) {
          range_begin_ = (*search).begin();
          range_length_ = (*search).length();
        }
        else {
          range_begin_ = to_pos;
          range_length_ = (*search).length() - (to_pos - (*search).begin());
        }
      }
      else {
        range_begin_ = outer_.n_;
        range_length_ = 0;
      }
    }

    u1 __forceinline__
    end() const noexcept {
      return range_length_ == 0;
    }

    u64 __forceinline__
    pos() const noexcept {
      return range_begin_;
    }

    u64 __forceinline__
    length() const noexcept {
      return range_length_;
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(n_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + ",\"ranges\":" + std::to_string(ranges_.size())
        + "}";
  }

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "[";
    if (!ranges_.empty()) {
      os << ranges_[0];
      for (std::size_t i = 1; i < ranges_.size(); ++i) {
        os << "," << ranges_[i];
      }
    }
    os << "]";
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


#include <boost/accumulators/statistics/weighted_kurtosis.hpp>


/* Author: Wolfgang Bangerth, University of Heidelberg, 1999 */

/*    $Id: step-5.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 1999-2004, 2006-2008, 2010-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// Again, the first few include files are already known, so we won't comment
// on them:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.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/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.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/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// This one is new. We want to read a triangulation from disk, and the class
// which does this is declared in the following file:
#include <deal.II/grid/grid_in.h>

// We will use a circular domain, and the object describing the boundary of it
// comes from this file:
#include <deal.II/grid/tria_boundary_lib.h>

// This is C++ ...
#include <fstream>
// ... and this is too: We will convert integers to strings using the C++
// stringstream class ostringstream:
#include <sstream>

// Finally, this has been discussed in previous tutorial programs before:
using namespace dealii;

// @sect3{The Step5 class template}

// The main class is mostly as in the previous example. The most visible
// change is that the function make_grid_and_dofs has been
// removed, since creating the grid is now done in the run
// function and the rest of its functionality is now in
// setup_system. Apart from this, everything is as before.
template <int dim>
class Step5
{
public:
  Step5 ();
  void run ();

private:
  void setup_system ();
  void assemble_system ();
  void solve ();
  void output_results (const unsigned int cycle) const;

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

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double>       solution;
  Vector<double>       system_rhs;
};

// @sect3{Nonconstant coefficients, using Assert}

// In step-4, we showed how to use non-constant boundary values and right hand
// side.  In this example, we want to use a variable coefficient in the
// elliptic operator instead. Of course, the suitable object is a
// Function, as we have used for the right hand side and boundary
// values in the last example. We will use it again, but we implement another
// function value_list which takes a list of points and returns
// the values of the function at these points as a list. The reason why such a
// function is reasonable although we can get all the information from the
// value function as well will be explained below when assembling
// the matrix.
//
// The need to declare a seemingly useless default constructor exists here
// just as in the previous example.
template <int dim>
class Coefficient : public Function<dim>
{
public:
  Coefficient ()  : Function<dim>() {}

  virtual double value (const Point<dim>   &p,
                        const unsigned int  component = 0) const;

  virtual void value_list (const std::vector<Point<dim> > &points,
                           std::vector<double>            &values,
                           const unsigned int              component = 0) const;
};

// This is the implementation of the coefficient function for a single
// point. We let it return 20 if the distance to the origin is less than 0.5,
// and 1 otherwise. As in the previous example, we simply ignore the second
// parameter of the function that is used to denote different components of
// vector-valued functions (we deal only with a scalar function here, after
// all):
template <int dim>
double Coefficient<dim>::value (const Point<dim> &p,
                                const unsigned int /*component*/) const
{
  if (p.square() < 0.5*0.5)
    return 20;
  else
    return 1;
}

// And this is the function that returns the value of the coefficient at a
// whole list of points at once. Of course, we need to make sure that the
// values are the same as if we would ask the value function for
// each point individually.
//
// This method takes three parameters: a list of points at which to evaluate
// the function, a list that will hold the values at these points, and the
// vector component that should be zero here since we only have a single
// scalar function.  Now, of course the size of the output array
// (values) must be the same as that of the input array
// (points), and we could simply assume that. However, in
// practice, it turns out that more than 90 per cent of programming errors are
// invalid function parameters such as invalid array sizes, etc, so we should
// try to make sure that the parameters are valid. For this, the
// Assert macro is a good means, since it makes sure that the
// condition which is given as first argument is valid, and if not throws an
// exception (its second argument) which will usually terminate the program
// giving information where the error occurred and what the reason was. This
// generally reduces the time to find programming errors dramatically and we
// have found assertions an invaluable means to program fast.
//
// On the other hand, all these checks (there are more than 4200 of them in
// the library at present) should not slow down the program too much if you
// want to do large computations. To this end, the Assert macro
// is only used in debug mode and expands to nothing if in optimized
// mode. Therefore, while you test your program on small problems and debug
// it, the assertions will tell you where the problems are.  Once your program
// is stable, you can switch off debugging and the program will run your real
// computations without the assertions and at maximum speed. (In fact, it
// turns out the switching off all the checks in the library that prevent you
// from calling functions with the wrong arguments by switching to optimized
// mode, makes most programs run faster by about a factor of four. This
// should, however, not try to induce you to always run in optimized mode:
// Most people who have tried that soon realize that they introduce lots of
// errors that would have easily been caught had they run the program in debug
// mode while developing.) For those who want to try: The way to switch from
// debug mode to optimized mode is to go edit the Makefile in this
// directory. It should have a line debug-mode = on; simply
// replace it by debug-mode = off and recompile your program. The
// output of the make program should already indicate to you that
// the program is now compiled in optimized mode, and it will later also be
// linked to libraries that have been compiled for optimized mode.
//
// Here, as has been said above, we would like to make sure that the size of
// the two arrays is equal, and if not throw an exception. Comparing the sizes
// of two arrays is one of the most frequent checks, which is why there is
// already an exception class ExcDimensionMismatch that takes the
// sizes of two vectors and prints some output in case the condition is
// violated:

template <int dim>
void Coefficient<dim>::value_list (const std::vector<Point<dim> > &points,
                                   std::vector<double>            &values,
                                   const unsigned int              component) const
{
  Assert (values.size() == points.size(),
          ExcDimensionMismatch (values.size(), points.size()));
  // Since examples are not very good if they do not demonstrate their point,
  // we will show how to trigger this exception at the end of the main
  // program, and what output results from this (see the Results
  // section of this example program). You will certainly notice that the
  // output is quite well suited to quickly find what the problem is and what
  // parameters are expected. An additional plus is that if the program is run
  // inside a debugger, it will stop at the point where the exception is
  // triggered, so you can go up the call stack to immediately find the place
  // where the the array with the wrong size was set up.

  // While we're at it, we can do another check: the coefficient is a scalar,
  // but the Function class also represents vector-valued
  // function. A scalar function must therefore be considered as a
  // vector-valued function with only one component, so the only valid
  // component for which a user might ask is zero (we always count from
  // zero). The following assertion checks this. If the condition in the
  // Assert call is violated, an exception of type
  // ExcRange will be triggered; that class takes the violating
  // index as first argument, and the second and third arguments denote a
  // range that includes the left point but is open at the right, i.e. here
  // the interval [0,1). For integer arguments, this means that the only value
  // in the range is the zero, of course. (The interval is half open since we
  // also want to write exceptions like ExcRange(i,0,v.size()),
  // where an index must be between zero but less than the size of an
  // array. To save us the effort of writing v.size()-1 in many
  // places, the range is defined as half-open.)
  Assert (component == 0,
          ExcIndexRange (component, 0, 1));

  // The rest of the function is uneventful: we define n_q_points
  // as an abbreviation for the number of points for which function values are
  // requested, and then simply fill the output value:
  const unsigned int n_points = points.size();

  for (unsigned int i=0; i<n_points; ++i)
    {
      if (points[i].square() < 0.5*0.5)
        values[i] = 20;
      else
        values[i] = 1;
    }
}

// @sect3{The Step5 class implementation}

// @sect4{Step5::Step5}

// This function is as before.
template <int dim>
Step5<dim>::Step5 () :
  fe (1),
  dof_handler (triangulation)
{}

// @sect4{Step5::setup_system}

// This is the function make_grid_and_dofs from the previous
// example, minus the generation of the grid. Everything else is unchanged:
template <int dim>
void Step5<dim>::setup_system ()
{
  dof_handler.distribute_dofs (fe);

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

  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, c_sparsity);
  sparsity_pattern.copy_from(c_sparsity);

  system_matrix.reinit (sparsity_pattern);

  solution.reinit (dof_handler.n_dofs());
  system_rhs.reinit (dof_handler.n_dofs());
}

// @sect4{Step5::assemble_system}

// As in the previous examples, this function is not changed much with regard
// to its functionality, but there are still some optimizations which we will
// show. For this, it is important to note that if efficient solvers are used
// (such as the preconditions CG method), assembling the matrix and right hand
// side can take a comparable time, and you should think about using one or
// two optimizations at some places.
//
// What we will show here is how we can avoid calls to the shape_value,
// shape_grad, and quadrature_point functions of the FEValues object, and in
// particular optimize away most of the virtual function calls of the Function
// object. The way to do so will be explained in the following, while those
// parts of this function that are not changed with respect to the previous
// example are not commented on.
//
// The first parts of the function are completely unchanged from before:
template <int dim>
void Step5<dim>::assemble_system ()
{
  QGauss<dim>  quadrature_formula(2);

  FEValues<dim> fe_values (fe, quadrature_formula,
                           update_values    |  update_gradients |
                           update_quadrature_points  |  update_JxW_values);

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

  FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
  Vector<double>       cell_rhs (dofs_per_cell);

  std::vector<unsigned int> local_dof_indices (dofs_per_cell);

  // Here is one difference: for this program, we will again use a constant
  // right hand side function and zero boundary values, but a variable
  // coefficient. We have already declared the class that represents this
  // coefficient above, so we only have to declare a corresponding object
  // here.
  //
  // Then, below, we will ask the coefficient function object to
  // compute the values of the coefficient at all quadrature points on one
  // cell at once. The reason for this is that, if you look back at how we did
  // this in step-4, you will realize that we called the function computing
  // the right hand side value inside nested loops over all degrees of freedom
  // and over all quadrature points, i.e. dofs_per_cell*n_q_points times. For
  // the coefficient that is used inside the matrix, this would actually be
  // dofs_per_cell*dofs_per_cell*n_q_points. On the other hand, the function
  // will of course return the same value every time it is called with the
  // same quadrature point, independently of what shape function we presently
  // treat; secondly, these are virtual function calls, so are rather
  // expensive. Obviously, there are only n_q_point different values, and we
  // shouldn't call the function more often than that. Or, even better than
  // this, compute all of these values at once, and get away with a single
  // function call per cell.
  //
  // This is exactly what we are going to do. For this, we need some space to
  // store the values in. We therefore also have to declare an array to hold
  // these values:
  const Coefficient<dim> coefficient;
  std::vector<double>    coefficient_values (n_q_points);

  // Next is the typical loop over all cells to compute local contributions
  // and then to transfer them into the global matrix and vector.
  //
  // The only two things in which this loop differs from step-4 is that we
  // want to compute the value of the coefficient in all quadrature points on
  // the present cell at the beginning, and then use it in the computation of
  // the local contributions. This is what we do in the call to
  // coefficient.value_list in the fourth line of the loop.
  //
  // The second change is how we make use of this coefficient in computing the
  // cell matrix contributions. This is in the obvious way, and not worth more
  // comments. For the right hand side, we use a constant value again.
  typename DoFHandler<dim>::active_cell_iterator
  cell = dof_handler.begin_active(),
  endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      cell_matrix = 0;
      cell_rhs = 0;

      fe_values.reinit (cell);

      coefficient.value_list (fe_values.get_quadrature_points(),
                              coefficient_values);

      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) += (coefficient_values[q_point] *
                                   fe_values.shape_grad(i,q_point) *
                                   fe_values.shape_grad(j,q_point) *
                                   fe_values.JxW(q_point));

            cell_rhs(i) += (fe_values.shape_value(i,q_point) *
                            1.0 *
                            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)
            system_matrix.add (local_dof_indices[i],
                               local_dof_indices[j],
                               cell_matrix(i,j));

          system_rhs(local_dof_indices[i]) += cell_rhs(i);
        }
    }

  // With the matrix so built, we use zero boundary values again:
  std::map<unsigned int,double> boundary_values;
  VectorTools::interpolate_boundary_values (dof_handler,
                                            0,
                                            ZeroFunction<dim>(),
                                            boundary_values);
  MatrixTools::apply_boundary_values (boundary_values,
                                      system_matrix,
                                      solution,
                                      system_rhs);
}

// @sect4{Step5::solve}

// The solution process again looks mostly like in the previous
// examples. However, we will now use a preconditioned conjugate gradient
// algorithm. It is not very difficult to make this change. In fact, the only
// thing we have to alter is that we need an object which will act as a
// preconditioner. We will use SSOR (symmetric successive overrelaxation),
// with a relaxation factor of 1.2. For this purpose, the
// SparseMatrix class has a function which does one SSOR step,
// and we need to package the address of this function together with the
// matrix on which it should act (which is the matrix to be inverted) and the
// relaxation factor into one object. The PreconditionSSOR class
// does this for us. (PreconditionSSOR class takes a template
// argument denoting the matrix type it is supposed to work on. The default
// value is SparseMatrix@<double@>, which is exactly what we need
// here, so we simply stick with the default and do not specify anything in
// the angle brackets.)
//
// Note that for the present case, SSOR doesn't really perform much better
// than most other preconditioners (though better than no preconditioning at
// all). A brief comparison of different preconditioners is presented in the
// Results section of the next tutorial program, step-6.
//
// With this, the rest of the function is trivial: instead of the
// PreconditionIdentity object we have created before, we now use
// the preconditioner we have declared, and the CG solver will do the rest for
// us:
template <int dim>
void Step5<dim>::solve ()
{
  SolverControl           solver_control (1000, 1e-12);
  SolverCG<>              solver (solver_control);

  PreconditionSSOR<> preconditioner;
  preconditioner.initialize(system_matrix, 1.2);

  solver.solve (system_matrix, solution, system_rhs,
                preconditioner);

  std::cout << "   " << solver_control.last_step()
            << " CG iterations needed to obtain convergence."
            << std::endl;
}

// @sect4{Step5::output_results and setting output flags}

// Writing output to a file is mostly the same as for the previous example,
// but here we will show how to modify some output options and how to
// construct a different filename for each refinement cycle.
template <int dim>
void Step5<dim>::output_results (const unsigned int cycle) const
{
  DataOut<dim> data_out;

  data_out.attach_dof_handler (dof_handler);
  data_out.add_data_vector (solution, "solution");

  data_out.build_patches ();

  // For this example, we would like to write the output directly to a file in
  // Encapsulated Postscript (EPS) format. The library supports this, but
  // things may be a bit more difficult sometimes, since EPS is a printing
  // format, unlike most other supported formats which serve as input for
  // graphical tools. Therefore, you can't scale or rotate the image after it
  // has been written to disk, and you have to decide about the viewpoint or
  // the scaling in advance.
  //
  // The defaults in the library are usually quite reasonable, and regarding
  // viewpoint and scaling they coincide with the defaults of
  // Gnuplot. However, since this is a tutorial, we will demonstrate how to
  // change them. For this, we first have to generate an object describing the
  // flags for EPS output (similar flag classes exist for all supported output
  // formats):
  DataOutBase::EpsFlags eps_flags;
  // They are initialized with the default values, so we only have to change
  // those that we don't like. For example, we would like to scale the z-axis
  // differently (stretch each data point in z-direction by a factor of four):
  eps_flags.z_scaling = 4;
  // Then we would also like to alter the viewpoint from which we look at the
  // solution surface. The default is at an angle of 60 degrees down from the
  // vertical axis, and 30 degrees rotated against it in mathematical positive
  // sense. We raise our viewpoint a bit and look more along the y-axis:
  eps_flags.azimut_angle = 40;
  eps_flags.turn_angle   = 10;
  // That shall suffice. There are more flags, for example whether to draw the
  // mesh lines, which data vectors to use for colorization of the interior of
  // the cells, and so on. You may want to take a look at the documentation of
  // the EpsFlags structure to get an overview of what is possible.
  //
  // The only thing still to be done, is to tell the output object to use
  // these flags:
  data_out.set_flags (eps_flags);
  // The above way to modify flags requires recompilation each time we would
  // like to use different flags. This is inconvenient, and we will see more
  // advanced ways in step-19 where the output flags are determined at run
  // time using an input file (step-19 doesn't show many other things; you
  // should feel free to read over it even if you haven't done step-6 to
  // step-18 yet).

  // Finally, we need the filename to which the results are to be written. We
  // would like to have it of the form solution-N.eps, where N is
  // the number of the refinement cycle. Thus, we have to convert an integer
  // to a part of a string; this can be done using the sprintf
  // function, but in C++ there is a more elegant way: write everything into a
  // special stream (just like writing into a file or to the screen) and
  // retrieve what you wrote as a string. This applies the usual conversions
  // from integer to strings, and one could as well use stream modifiers such
  // as setw, setprecision, and so on. In C++, you
  // can do this by using the so-called stringstream classes:
  std::ostringstream filename;

  // In order to now actually generate a filename, we fill the stringstream
  // variable with the base of the filename, then the number part, and finally
  // the suffix indicating the file type:
  filename << "solution-"
           << cycle
           << ".eps";

  // We can get whatever we wrote to the stream using the str()
  // function. The result is a string which we have to convert to a char*
  // using the c_str() function. Use that as filename for the
  // output stream and then write the data to the file:
  std::ofstream output (filename.str().c_str());

  data_out.write_eps (output);
}

// @sect4{Step5::run}

// The second to last thing in this program is the definition of the
// run() function. In contrast to the previous programs, we will
// compute on a sequence of meshes that after each iteration is globall
// refined. The function therefore consists of a loop over 6 cycles. In each
// cycle, we first print the cycle number, and then have to decide what to do
// with the mesh. If this is not the first cycle, we simply refine the
// existing mesh once globally. Before running through these cycles, however,
// we have to generate a mesh:

// In previous examples, we have already used some of the functions from the
// GridGenerator class. Here we would like to read a grid from a
// file where the cells are stored and which may originate from someone else,
// or may be the product of a mesh generator tool.
//
// In order to read a grid from a file, we generate an object of data type
// GridIn and associate the triangulation to it (i.e. we tell it to fill our
// triangulation object when we ask it to read the file). Then we open the
// respective file and initialize the triangulation with the data in the file:
template <int dim>
void Step5<dim>::run ()
{
  GridIn<dim> grid_in;
  grid_in.attach_triangulation (triangulation);
  std::ifstream input_file("circle-grid.inp");
  // We would now like to read the file. However, the input file is only for a
  // two-dimensional triangulation, while this function is a template for
  // arbitrary dimension. Since this is only a demonstration program, we will
  // not use different input files for the different dimensions, but rather
  // kill the whole program if we are not in 2D:
  Assert (dim==2, ExcInternalError());
  // ExcInternalError is a globally defined exception, which may be thrown
  // whenever something is terribly wrong. Usually, one would like to use more
  // specific exceptions, and particular in this case one would of course try
  // to do something else if dim is not equal to two, e.g. create
  // a grid using library functions. Aborting a program is usually not a good
  // idea and assertions should really only be used for exceptional cases
  // which should not occur, but might due to stupidity of the programmer,
  // user, or someone else. The situation above is not a very clever use of
  // Assert, but again: this is a tutorial and it might be worth to show what
  // not to do, after all.

  // So if we got past the assertion, we know that dim==2, and we can now
  // actually read the grid. It is in UCD (unstructured cell data) format (but
  // the ending of the UCD-file is inp), as
  // supported as input format by the AVS Explorer (a visualization program),
  // for example:
  grid_in.read_ucd (input_file);
  // If you like to use another input format, you have to use an other
  // grid_in.read_xxx function. (See the documentation of the
  // GridIn class to find out what input formats are presently
  // supported.)

  // The grid in the file describes a circle. Therefore we have to use a
  // boundary object which tells the triangulation where to put new points on
  // the boundary when the grid is refined. This works in the same way as in
  // the first example. Note that the HyperBallBoundary constructor takes two
  // parameters, the center of the ball and the radius, but that their default
  // (the origin and 1.0) are the ones which we would like to use here.
  static const HyperBallBoundary<dim> boundary;
  triangulation.set_boundary (0, boundary);

  for (unsigned int cycle=0; cycle<6; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle != 0)
        triangulation.refine_global (1);

      // Now that we have a mesh for sure, we write some output and do all the
      // things that we have already seen in the previous examples.
      std::cout << "   Number of active cells: "
                << triangulation.n_active_cells()
                << std::endl
                << "   Total number of cells: "
                << triangulation.n_cells()
                << std::endl;

      setup_system ();
      assemble_system ();
      solve ();
      output_results (cycle);
    }
}

// @sect3{The main function}

// The main function looks mostly like the one in the previous example, so we
// won't comment on it further:
int main ()
{
  deallog.depth_console (0);

  Step5<2> laplace_problem_2d;
  laplace_problem_2d.run ();

  // Finally, we have promised to trigger an exception in the
  // Coefficient class through the Assert macro we
  // have introduced there. For this, we have to call its
  // value_list function with two arrays of different size (the
  // number in parentheses behind the declaration of the object). We have
  // commented out these lines in order to allow the program to exit
  // gracefully in normal situations (we use the program in day-to-day testing
  // of changes to the library as well), so you will only get the exception by
  // un-commenting the following lines. Take a look at the Results section of
  // the program to see what happens when the code is actually run:
  /*
    Coefficient<2>    coefficient;
    std::vector<Point<2> > points (2);
    std::vector<double>    coefficient_values (1);
    coefficient.value_list (points, coefficient_values);
  */

  return 0;
}


#define BOOST_TEST_MODULE pcraster dal step_mapper
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#define private public
#include "dal_StepMapper.h"

BOOST_AUTO_TEST_CASE(step_mapper)
{
  using namespace dal;

  {
    StepMapper mapper(1.0, 2.0, 100.0, 200.0);
    BOOST_CHECK(comparable(mapper.destination(-1.0), -100.0));
    BOOST_CHECK(comparable(mapper.destination( 0.0),    0.0));
    BOOST_CHECK(comparable(mapper.destination( 1.0),  100.0));
    BOOST_CHECK(comparable(mapper.destination( 2.0),  200.0));
    BOOST_CHECK(comparable(mapper.destination( 3.0),  300.0));
  }

  {
    StepMapper mapper(1.0, 250.0, 1.0, 250.0);
    BOOST_CHECK_EQUAL(mapper.d_conversionFactor, 1.0);
    BOOST_CHECK(comparable(mapper.destination(167.0), 167.0));
    BOOST_CHECK(comparable(std::fmod(mapper.destination(167.0), 1.0), 0.0));
  }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "collapse"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <gudhi/Flag_complex_edge_collapser.h>
#include <gudhi/distance_functions.h>
#include <gudhi/graph_simplicial_complex.h>

#include <iostream>
#include <tuple>
#include <vector>
#include <array>
#include <cmath>

struct Simplicial_complex {
  using Vertex_handle = short;
  using Filtration_value = float;
};

using Vertex_handle = Simplicial_complex::Vertex_handle;
using Filtration_value = Simplicial_complex::Filtration_value;
using Filtered_edge = std::tuple<Vertex_handle, Vertex_handle, Filtration_value>;
using Filtered_edge_list = std::vector<Filtered_edge>;

template<typename Filtered_edge_range>
bool find_edge_in_list(const Filtered_edge& edge, const Filtered_edge_range& edge_list) {
  for (auto edge_from_list : edge_list) {
    if (edge_from_list == edge)
      return true;
  }
  return false;
}

template<typename Filtered_edge_range>
void trace_and_check_collapse(const Filtered_edge_range& filtered_edges, const Filtered_edge_list& removed_edges) {
  std::cout << "BEFORE COLLAPSE - Total number of edges: " << filtered_edges.size() << std::endl;
  BOOST_CHECK(filtered_edges.size() > 0);
  for (auto filtered_edge : filtered_edges) {
    std::cout << "f[" << std::get<0>(filtered_edge) << ", " << std::get<1>(filtered_edge) << "] = "
              << std::get<2>(filtered_edge) << std::endl;
  }

  std::cout << "COLLAPSE - keep edges: " << std::endl;
  auto remaining_edges = Gudhi::collapse::flag_complex_collapse_edges(filtered_edges);

  std::cout << "AFTER COLLAPSE - Total number of edges: " << remaining_edges.size() << std::endl;
  BOOST_CHECK(remaining_edges.size() <= filtered_edges.size());
  for (auto filtered_edge_from_collapse : remaining_edges) {
    std::cout << "f[" << std::get<0>(filtered_edge_from_collapse) << ", " << std::get<1>(filtered_edge_from_collapse)
              << "] = " << std::get<2>(filtered_edge_from_collapse) << std::endl;
    // Check each edge from collapse is in the input
    BOOST_CHECK(find_edge_in_list(filtered_edge_from_collapse, filtered_edges));
  }

  std::cout << "CHECK COLLAPSE - Total number of removed edges: " << removed_edges.size() << std::endl;
  for (auto removed_filtered_edge : removed_edges) {
    std::cout << "f[" << std::get<0>(removed_filtered_edge) << ", " << std::get<1>(removed_filtered_edge) << "] = "
              << std::get<2>(removed_filtered_edge) << std::endl;
    // Check each removed edge from collapse is in the input
    BOOST_CHECK(!find_edge_in_list(removed_filtered_edge, remaining_edges));
  }

}

BOOST_AUTO_TEST_CASE(collapse) {
  std::cout << "***** COLLAPSE *****" << std::endl;
  //  1   2
  //  o---o
  //  |   |
  //  |   |
  //  |   |
  //  o---o
  //  0   3
  Filtered_edge_list edges {{0, 1, 1.},
                            {1, 2, 1.},
                            {2, 3, 1.},
                            {3, 0, 1.}};
  trace_and_check_collapse(edges, {});

  //  1   2
  //  o---o
  //  |\ /|
  //  | x |
  //  |/ \|
  //  o---o
  //  0   3
  edges.emplace_back(0, 2, 2.);
  edges.emplace_back(1, 3, 2.);
  trace_and_check_collapse(edges, {{1, 3, 2.}});

  //  1   2   4
  //  o---o---o
  //  |\ /|   |
  //  | x |   |
  //  |/ \|   |
  //  o---o---o
  //  0   3   5
  edges.emplace_back(2, 4, 3.);
  edges.emplace_back(4, 5, 3.);
  edges.emplace_back(5, 3, 3.);
  trace_and_check_collapse(edges, {{1, 3, 2.}});

  //  1   2   4
  //  o---o---o
  //  |\ /|\ /|
  //  | x | x |
  //  |/ \|/ \|
  //  o---o---o
  //  0   3   5
  edges.emplace_back(2, 5, 4.);
  edges.emplace_back(4, 3, 4.);
  trace_and_check_collapse(edges, {{1, 3, 2.}, {4, 3, 4.}});

  //  1   2   4
  //  o---o---o
  //  |\ /|\ /|
  //  | x | x |  + [0,4] and [1,5]
  //  |/ \|/ \|
  //  o---o---o
  //  0   3   5
  edges.emplace_back(1, 5, 5.);
  edges.emplace_back(0, 4, 5.);
  trace_and_check_collapse(edges, {{1, 3, 2.}, {4, 3, 4.}, {0, 4, 5.}});
}

BOOST_AUTO_TEST_CASE(collapse_from_array) {
  std::cout << "***** COLLAPSE FROM ARRAY *****" << std::endl;
  //  1   2
  //  o---o
  //  |\ /|
  //  | x |
  //  |/ \|
  //  o---o
  //  0   3
  std::array<Filtered_edge, 6> f_edge_array = {{{0, 1, 1.},
                                                {1, 2, 1.},
                                                {2, 3, 1.},
                                                {3, 0, 1.},
                                                {0, 2, 2.},
                                                {1, 3, 2.}}};
  trace_and_check_collapse(f_edge_array, {{1, 3, 2.}});
}

BOOST_AUTO_TEST_CASE(collapse_from_proximity_graph) {
  std::cout << "***** COLLAPSE FROM PROXIMITY GRAPH *****" << std::endl;
  //  1   2
  //  o---o
  //  |\ /|
  //  | x |
  //  |/ \|
  //  o---o
  //  0   3
  std::vector<std::vector<Filtration_value>> point_cloud = {{0., 0.},
                                                            {0., 1.},
                                                            {1., 0.},
                                                            {1., 1.} };

  Filtration_value threshold = std::numeric_limits<Filtration_value>::infinity();
  using Proximity_graph = Gudhi::Proximity_graph<Simplicial_complex>;
  Proximity_graph proximity_graph = Gudhi::compute_proximity_graph<Simplicial_complex>(point_cloud,
                                                                                threshold,
                                                                                Gudhi::Euclidean_distance());

  auto remaining_edges = Gudhi::collapse::flag_complex_collapse_edges(
      boost::adaptors::transform(edges(proximity_graph), [&](auto&&edge){
        return std::make_tuple(static_cast<Vertex_handle>(source(edge, proximity_graph)),
                               static_cast<Vertex_handle>(target(edge, proximity_graph)),
                               get(Gudhi::edge_filtration_t(), proximity_graph, edge));
      })
    );

  BOOST_CHECK(remaining_edges.size() == 5);

  std::size_t filtration_is_edge_length_nb = 0;
  std::size_t filtration_is_diagonal_length_nb = 0;
  float epsilon = std::numeric_limits<Filtration_value>::epsilon();
  for (auto filtered_edge : remaining_edges) {
    if (std::get<2>(filtered_edge) == 1.)
      filtration_is_edge_length_nb++;
    if (std::fabs(std::get<2>(filtered_edge) - std::sqrt(2.)) <= epsilon)
      filtration_is_diagonal_length_nb++;
  }
  BOOST_CHECK(filtration_is_edge_length_nb == 4);
  BOOST_CHECK(filtration_is_diagonal_length_nb == 1);
}


//
// boost-geometry-render
//
// Copyright (c) 2011
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <cstdlib>
#include <gl/glut.h>
#include <gl/graphics.hpp>
#include <gl/geometry_render.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <iostream>
#include <boost/thread.hpp>
#include <boost/assign/list_of.hpp>

namespace bg = boost::geometry;

template<typename Geometry>
struct geometry_renderer{
	geometry_renderer(Geometry& geometry) : geometry_(geometry){};
	void operator ()(){
		glColor3f(1.0f, 0.0f, 0.0f);
		gl::render(geometry_);
	}
private:
	Geometry& geometry_;
};

template<typename Geometry>
geometry_renderer<Geometry>
make_geometry_renderer(Geometry& geometry){
	return geometry_renderer<Geometry>(geometry);
}

void
idle(){
	boost::this_thread::sleep(boost::posix_time::milliseconds(16));
	glutPostRedisplay();
}

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

	typedef bg::model::d2::point_xy<float> point_type;

	bg::model::linestring<point_type> linestring;
	linestring = boost::assign::list_of<point_type>
		( 0.2f,  0.8f)
		( 0.7f,  0.6f)
		( 0.7f, -0.3f)
		( 0.4f, -0.5f)
		(-0.4f, -0.6f)
		(-0.8f, -0.3f)
		(-0.6f,  0.2f)
		(-0.4f,  0.4f)
		(-0.2f, -0.3f)
		( 0.1f, -0.1f)
		( 0.2f, 0.8f);

	gl::graphics g(argc, argv, 500, 500);
	gl::displayfunc(g, make_geometry_renderer(linestring));
	gl::idlefunc(g, &idle);

	g.run();
	return 0;
}


#include <gtest/gtest.h>
#include <stdexcept>
#include <Eigen/Dense>

// testing following API
#include "estimation/IEstimator.h"
#include "estimation/ParticleFilterSIR.h"
#include "estimation/Input.h"
#include "estimation/InputValue.h"
#include "estimation/Output.h"
#include "estimation/OutputValue.h"
#include "probability/sampling.h"

using namespace std;
using namespace estimation;
using namespace probability;

namespace ParticleFilterSIRTest
{
  void f(VectorXd& x, const VectorXd& u)
  {
    x[0] = x[0] + u[0];
  }
  void h(VectorXd& z, const VectorXd& x)
  {
    z[0] = x[0];
  }

  void f2(VectorXd& x, const VectorXd& u)
  {
    VectorXd x_apriori(x.size());

    x_apriori[0] = x[0] + 0.1*x[1];
    x_apriori[1] = x[1];

    x = x_apriori;
  }
  void h2(VectorXd& z, const VectorXd& x)
  {
    z[0] = x[1];
  }

  // -----------------------------------------
  // tests
  // -----------------------------------------
  TEST(ParticleFilterSIRTest, initialization)
  {
    ParticleFilterSIR pf(100);

    // check setting required params ----------------------------
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "STM missing"
    pf.setStateTransitionModel(f);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "OM missing"
    pf.setObservationModel(h);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "PNC missing"
    MatrixXd Q(1,1); Q << 0.1;
    pf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "MNC missing"
    MatrixXd R(1,1); R << 10;
    pf.setMeasurementNoiseCovariance(R);

    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "particles not initialized"

    // check particle initializing ------------------------------
    ParticleFilterSIR pf_unititialized = pf;

    // init with known state
    VectorXd x0(1); x0 << -1;			// current state, i.e. mean
    pf.setInitialState(x0);			// init particles with mean
    EXPECT_NO_THROW(pf.validate());

    // init randomly
    pf = pf_unititialized;
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "particles not initialized"
    VectorXd a(1); a << -1;			// lower bound
    VectorXd b(1); b << 1;			// upper bound
    pf.initializeParticles(a, b);		// uniformly random
    EXPECT_NO_THROW(pf.validate());

    // check initialization of output ---------------------------
    // default output
    Output out = pf.getLastEstimate();
    OutputValue defaultOutVal;
    EXPECT_GT(out.size(), 0);
    EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());

    // check setting the control input --------------------------
    InputValue ctrl(0);
    Input in_ctrl(ctrl);
    // when using control input, the length must be set before
    EXPECT_THROW(pf.setControlInput(in_ctrl), length_error);
    pf.setControlInputSize(1);
    EXPECT_NO_THROW(pf.setControlInput(in_ctrl));
  }

  TEST(ParticleFilterSIRTest, validation)
  {
    ParticleFilterSIR pf(100);

    // check validation itself (e.g. invalid sizes of vectors/matrices,
    // empty callbacks, etc.)
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "STM missing"
    pf.setStateTransitionModel(0);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "STM missing"
    pf.setStateTransitionModel(f);
    pf.setObservationModel(0);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "OM missing"
    pf.setObservationModel(h);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "PNC missing"
    MatrixXd Q_f(2,2); Q_f << 1, 0.1, 0.1, 1;
    pf.setProcessNoiseCovariance(Q_f);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "MNC missing"
    MatrixXd R_f(2,1); R_f << 10, 1;
    pf.setMeasurementNoiseCovariance(R_f);

    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "particles not initialized"
    VectorXd x0(1); x0 << -1;
    pf.setInitialState(x0);

    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "Q invalid size"
    MatrixXd Q(1,1); Q << 0.1;
    pf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(pf.validate(), IEstimator::estimator_error);	// "R not square"
    MatrixXd R(1,1); R << 10;
    pf.setMeasurementNoiseCovariance(R);

    EXPECT_NO_THROW(pf.validate());				// all required params given

    // size of R cannot be checked, must match number of formulas in h
    // -> check during compile time
  }

  TEST(ParticleFilterSIRTest, validationEffect)
  {
    ParticleFilterSIR pf(100);

    // a correct initialization
    pf.setStateTransitionModel(f);
    pf.setObservationModel(h);
    MatrixXd Q(1,1); Q << 0.1;
    pf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    pf.setMeasurementNoiseCovariance(R);
    VectorXd x0(1); x0 << -3;
    pf.setInitialState(x0);
    pf.setControlInputSize(1);		// initializes u, so it can be used in f

    // validate has an effect? ----------------------------------
    InputValue measurement(-3);
    Input in(measurement);
    Output out;

    EXPECT_THROW(out = pf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
    EXPECT_NO_THROW(pf.validate());
    EXPECT_NO_THROW(out = pf.estimate(in));

    // changing a parameter -> PF must be re-validated
    pf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(out = pf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
    EXPECT_NO_THROW(pf.validate());

    // PF should now be released and return an estimate
    EXPECT_NO_THROW(out = pf.estimate(in));
    EXPECT_GT(out.size(), 0);
    OutputValue defaultOutVal;
    // estimated value, not the default
    EXPECT_NE(out.getValue(), defaultOutVal.getValue());
  }

  TEST(ParticleFilterSIRTest, functionality)
  {
    ParticleFilterSIR pf(1000);

    pf.setStateTransitionModel(f);
    pf.setObservationModel(h);
    MatrixXd Q(1,1); Q << 0.1;
    pf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    pf.setMeasurementNoiseCovariance(R);
    VectorXd x0(1); x0 << -3;
    pf.setInitialState(x0);

    pf.validate();

    // check if the calculation of an estimate is correct
    // first measurement z1:
    InputValue measurement(-3);		// should result in the same value with higher variance
    Input in(measurement);
    Output out;

    // u contained in a state transition formula, so an error will be
    // thrown when u is not initialized with the correct size
    pf.setControlInputSize(1);			// sets default control input = 0
    pf.validate();				// must be validated
    EXPECT_NO_THROW(out = pf.estimate(in));	// estimation should work

    EXPECT_EQ(out.size(), 1);
    EXPECT_NEAR(out[0].getValue(), -3, 0.5);
    EXPECT_NE(out[0].getVariance(), 0);		// variance should have changed
    EXPECT_NEAR(out[0].getVariance(), 0, 0.5);

    // control input = 0 should not change values
    InputValue ctrlVal(0);
    Input ctrl(ctrlVal);
    pf.setControlInput(ctrl);
    out = pf.estimate(in);
    EXPECT_NEAR(out[0].getValue(), -3, 0.5);

    double var;
    var = out[0].getVariance();

    // missing measurement
    InputValue missingValue;
    Input inMissing(missingValue);
    EXPECT_NO_THROW(out = pf.estimate(inMissing));

    EXPECT_NEAR(out[0].getValue(), -3, 0.5);	// only time update
    EXPECT_GT(out[0].getVariance(), var);	// variance increased

    // multi dimensional example -------------------------------------
    ParticleFilterSIR pf2(1000);

    pf.setStateTransitionModel(f2);
    pf.setObservationModel(h2);
    MatrixXd Q2(2,2); Q2 << 0.1, 0.01, 0.01, 0.1;
    pf.setProcessNoiseCovariance(Q2);
    MatrixXd R2(1,1); R2 << 10;
    pf.setMeasurementNoiseCovariance(R2);
    VectorXd x02(2); x02 << -3, 3;
    pf.setInitialState(x02);

    pf.validate();
    EXPECT_NO_THROW(out = pf.estimate(in));	// estimation should work

    ASSERT_EQ(out.size(), 2);
    EXPECT_NEAR(out[0].getValue(), -3, 0.5);
    EXPECT_NEAR(out[1].getValue(), 3, 1);
  }
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, logical_and) {
  using stan::math::logical_and;
  EXPECT_TRUE(logical_and(1, 1));
  EXPECT_TRUE(logical_and(5.7, -1.9));

  EXPECT_FALSE(logical_and(0, 0));
  EXPECT_FALSE(logical_and(0, 1));
  EXPECT_FALSE(logical_and(1, 0));
  EXPECT_FALSE(logical_and(0.0, 0.0));
  EXPECT_FALSE(logical_and(0.0, 1.0));
  EXPECT_FALSE(logical_and(1, 0.0));
}

TEST(MathFunctions, logical_and_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(stan::math::logical_and(1.0, nan));
  EXPECT_TRUE(stan::math::logical_and(nan, 2.0));
  EXPECT_TRUE(stan::math::logical_and(nan, nan));
}


#ifdef COMPILATION// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4-*-
$CXX $0 -o $0x -lfftw3 -lboost_unit_test_framework -lboost_timer&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2020

#define BOOST_TEST_MODULE "C++ Unit Tests for Multi FFTW adaptor (cpu) with thrust complex"
#define BOOST_TEST_DYN_LINK
#include<boost/test/unit_test.hpp>

#include <boost/timer/timer.hpp>

#include "../../fftw.hpp"

#include<complex>
#include<chrono>

#include<thrust/complex.h>

namespace multi = boost::multi;

namespace utf = boost::unit_test::framework;

struct watch : private std::chrono::high_resolution_clock{
	std::string label_; time_point  start_;
	watch(std::string label ="") : label_{label}, start_{now()}{}
	~watch(){
		std::cerr<< label_<<": "<< std::chrono::duration<double>(now() - start_).count() <<" sec"<<std::endl;
	}
};

BOOST_AUTO_TEST_CASE(fft_combinations, *boost::unit_test::tolerance(0.00001)){

	using complex = std::complex<double>;

	auto const in = []{
		multi::array<complex, 4> ret({32, 90, 98, 96});
		std::generate(ret.data_elements(), ret.data_elements() + ret.num_elements(),
			[](){return complex{std::rand()*1./RAND_MAX, std::rand()*1./RAND_MAX};}
		);
		return ret;
	}();
	std::cout<<"memory size "<< in.num_elements()*sizeof(complex)/1e6 <<" MB\n";

	std::vector<std::array<bool, 4>> cases = {
		{false, true , true , true },
		{false, true , true , false},
		{true , false, false, false},
		{true , true , false, false},
		{false, false, true , false},
		{false, false, false, false},
	};

	using std::cout;
	for(auto c : cases){
		cout<<"case "; copy(begin(c), end(c), std::ostream_iterator<bool>{cout,", "}); cout<<"\n";
		multi::array<complex, 4> out = in;
		{
			boost::timer::auto_cpu_timer t{"cpu_oplac %ws wall, CPU (%p%)\n"};
			multi::fftw::dft_forward(c, in, out);
		}
		{
			multi::fftw::plan p(c, in, out, multi::fftw::forward);
			boost::timer::auto_cpu_timer t{"cpu_oplac planned %ws wall, CPU (%p%)\n"};
			p();
		}
		{
			auto in_rw = in;
			boost::timer::auto_cpu_timer t{"cpu_iplac %ws wall, CPU (%p%)\n"};
			multi::fftw::dft_forward(c, in_rw);
		//	BOOST_TEST( abs( in_rw[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
		{
			auto in_rw = in;
			multi::fftw::plan p(c, in_rw, in_rw, multi::fftw::forward);
			boost::timer::auto_cpu_timer t{"cpu_iplac planned %ws wall, CPU (%p%)\n"};
			p();
		//	BOOST_TEST( abs( in_rw[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
		{
			auto in_rw = in;
			multi::fftw::plan p(c, in_rw, in_rw, multi::fftw::forward);// | FFTW_MEASURE);
			boost::timer::auto_cpu_timer t{"cpu_iplac planned measured %ws wall, CPU (%p%)\n"};
			p();
		//	BOOST_TEST( abs( in_rw[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
		{
			boost::timer::auto_cpu_timer t{"cpu_alloc %ws wall, CPU (%p%)\n"};
			auto out_cpy = multi::fftw::dft_forward(c, in);
			BOOST_TEST( abs( out_cpy[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
		{
			auto in_rw = in;
			boost::timer::auto_cpu_timer t{"cpu_move %ws wall, CPU (%p%)\n"};
			auto out_cpy = multi::fftw::dft_forward(c, std::move(in_rw));
			BOOST_REQUIRE( in_rw.empty() );
			BOOST_TEST( abs( out_cpy[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
	}
}

BOOST_AUTO_TEST_CASE(fftw_4D_power_benchmark, *boost::unit_test::disabled() ){
	using complex = std::complex<double>;
	namespace fftw = multi::fftw;

	auto x = multi::array<complex, 4>::extensions_type({64, 128, 128, 128});
	multi::array<complex, 4> in(x);
	std::iota(in.data_elements(), in.data_elements() + in.num_elements(), 1.2);

	BOOST_REQUIRE( in[0][0][0][0] == 1.2 );
	std::array<bool, 4> c = {false, true, true, true};
	[&, _ = watch{utf::current_test_case().full_name()+" inplace FTTT"}]{
		fftw::dft(c, in, fftw::forward);
	}();
	[&, _ = watch{utf::current_test_case().full_name()+" inplace FTTT"}]{
		fftw::dft(c, in, fftw::forward);
	}();
	auto in0000 = in[0][0][0][0];
	BOOST_REQUIRE( in0000 != 1.2 );

	multi::array<complex, 4> out(x);
	[&, _ = watch{utf::current_test_case().full_name()+" outofplace FTTT"}]{
		fftw::dft(c, in, out, fftw::forward);
	}();
	[&, _ = watch{utf::current_test_case().full_name()+" outofplace FTTT"}]{
		fftw::dft(c, in, out, fftw::forward);
	}();
	[&, _ = watch{utf::current_test_case().full_name()+" outofplace FTTT"}]{
		fftw::dft(c, in, out, fftw::forward);
	}();
	[&, _ = watch{utf::current_test_case().full_name()+" outofplace+alloc FTTT"}]{
		multi::array<complex, 4> out2(x);
		fftw::dft(c, in, out2, fftw::forward);
	}();
	[&, _ = watch{utf::current_test_case().full_name()+" outofplace+alloc FTTT"}]{
		multi::array<complex, 4> out2(x);
		fftw::dft(c, in, out2, fftw::forward);
	}();
	BOOST_REQUIRE( in0000 == in[0][0][0][0] );

}


//
//  BIE_correct.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/6/18.
//
//

#ifndef BIE_correct_hpp
#define BIE_correct_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "get_disp_from_BIE.hpp"
using namespace Eigen;

void BIE_correct(const Array<int,Dynamic,1> &BIE_top_surf_index, const Array<int,Dynamic,1> &BIE_bot_surf_index, const VectorXd &fe_global, int Ndofn, int nx, const double dx,InfiniteBoundary &BIE_inf_top, InfiniteBoundary &BIE_inf_bot, VectorXd &u_n, VectorXd &v_n);

#endif /* BIE_correct_hpp */


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

template <typename DerivedF, typename DerivedFF, typename DerivedC>
IGL_INLINE void igl::bfs_orient(
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedC> & C)
{
  using namespace Eigen;
  using namespace std;
  SparseMatrix<int> A;
  orientable_patches(F,C,A);

  // number of faces
  const int m = F.rows();
  // number of patches
  const int num_cc = C.maxCoeff()+1;
  VectorXi seen = VectorXi::Zero(m);

  // Edge sets
  const int ES[3][2] = {{1,2},{2,0},{0,1}};

  if(&FF != &F)
  {
    FF = F;
  }
  // loop over patches
#pragma omp parallel for
  for(int c = 0;c<num_cc;c++)
  {
    queue<int> Q;
    // find first member of patch c
    for(int f = 0;f<FF.rows();f++)
    {
      if(C(f) == c)
      {
        Q.push(f);
        break;
      }
    }
    assert(!Q.empty());
    while(!Q.empty())
    {
      const int f = Q.front();
      Q.pop();
      if(seen(f) > 0)
      {
        continue;
      }
      seen(f)++;
      // loop over neighbors of f
      for(typename SparseMatrix<int>::InnerIterator it (A,f); it; ++it)
      {
        // might be some lingering zeros, and skip self-adjacency
        if(it.value() != 0 && it.row() != f)
        {
          const int n = it.row();
          assert(n != f);
          // loop over edges of f
          for(int efi = 0;efi<3;efi++)
          {
            // efi'th edge of face f
            Vector2i ef(FF(f,ES[efi][0]),FF(f,ES[efi][1]));
            // loop over edges of n
            for(int eni = 0;eni<3;eni++)
            {
              // eni'th edge of face n
              Vector2i en(FF(n,ES[eni][0]),FF(n,ES[eni][1]));
              // Match (half-edges go same direction)
              if(ef(0) == en(0) && ef(1) == en(1))
              {
                // flip face n
                FF.row(n) = FF.row(n).reverse().eval();
              }
            }
          }
          // add neighbor to queue
          Q.push(n);
        }
      }
    }
  }

  // make sure flip is OK if &FF = &F
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::bfs_orient<Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


#define BOOST_TEST_MODULE RankFilterModule
#include <boost/test/included/unit_test.hpp>

#include <boost/array.hpp>

#include "rank_filter.hxx"

struct RankFilterFixture
{
    public:

        static const unsigned long size = 10;

        boost::array<double, size> array_1;
        boost::array<double, size> reverse_array_1;

        boost::array<double, size> expected_result_1;
        boost::array<double, size> result_1;

        boost::array<double, size>::iterator array_1_begin;
        boost::array<double, size>::iterator array_1_end;

        boost::array<double, size>::iterator reverse_array_1_begin;
        boost::array<double, size>::iterator reverse_array_1_end;

        boost::array<double, size>::iterator result_1_begin;
        boost::array<double, size>::iterator result_1_end;

    public:

        RankFilterFixture()
        {
            for (int i = 0; i < size; i++)
            {
                array_1[i] = i;
            }

            for (int i = 0; i < size; i++)
            {
                reverse_array_1[i] = size - i - 1;
            }

            expected_result_1.fill(0);
            result_1.fill(0);

            array_1_begin = array_1.begin();
            array_1_end = array_1.end();
            reverse_array_1_begin = reverse_array_1.begin();
            reverse_array_1_end = reverse_array_1.end();

            result_1_begin = result_1.begin();
            result_1_end = result_1.end();
        };

        ~RankFilterFixture()
        {
        };
};

BOOST_AUTO_TEST_SUITE( RankFilterSuite )

BOOST_FIXTURE_TEST_CASE(test_rank_filter_1, RankFilterFixture)
{
    expected_result_1 = array_1;

    rank_filter::lineRankOrderFilter1D(array_1_begin, array_1_end, result_1_begin, result_1_end, 0, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_2, RankFilterFixture)
{
    expected_result_1 = reverse_array_1;

    rank_filter::lineRankOrderFilter1D(reverse_array_1_begin, reverse_array_1_end, result_1_begin, result_1_end, 0, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_3, RankFilterFixture)
{
    expected_result_1 = array_1;
    expected_result_1[0] = expected_result_1[1];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2];

    rank_filter::lineRankOrderFilter1D(array_1_begin, array_1_end, result_1_begin, result_1_end, 1, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_4, RankFilterFixture)
{
    expected_result_1 = reverse_array_1;
    expected_result_1[0] = expected_result_1[1];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2];

    rank_filter::lineRankOrderFilter1D(reverse_array_1_begin, reverse_array_1_end, result_1_begin, result_1_end, 1, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_5, RankFilterFixture)
{
    expected_result_1 = array_1;
    expected_result_1[0] = expected_result_1[1];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2];

    rank_filter::lineRankOrderFilter1D(array_1_begin, array_1_end, result_1_begin, result_1_end, 2, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_6, RankFilterFixture)
{
    expected_result_1 = reverse_array_1;
    expected_result_1[0] = expected_result_1[1];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2];

    rank_filter::lineRankOrderFilter1D(reverse_array_1_begin, reverse_array_1_end, result_1_begin, result_1_end, 2, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_7, RankFilterFixture)
{
    expected_result_1 = array_1;
    expected_result_1[0] = expected_result_1[1] = expected_result_1[2];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2] = expected_result_1[expected_result_1.size() - 3];

    rank_filter::lineRankOrderFilter1D(array_1_begin, array_1_end, result_1_begin, result_1_end, 3, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_8, RankFilterFixture)
{
    expected_result_1 = reverse_array_1;
    expected_result_1[0] = expected_result_1[1] = expected_result_1[2];
    expected_result_1[expected_result_1.size()  - 1] = expected_result_1[expected_result_1.size() - 2] = expected_result_1[expected_result_1.size() - 3];

    rank_filter::lineRankOrderFilter1D(reverse_array_1_begin, reverse_array_1_end, result_1_begin, result_1_end, 3, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_FIXTURE_TEST_CASE(test_rank_filter_9, RankFilterFixture)
{
    for (size_t i = 0; i < array_1.size(); i++)
    {
        expected_result_1[i] = (i < 5) ? array_1[5] : array_1[4];
    }

    rank_filter::lineRankOrderFilter1D(array_1_begin, array_1_end, result_1_begin, result_1_end, size - 1, 0.5);

    BOOST_CHECK_EQUAL(expected_result_1, result_1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>
#include <stdio.h>    // printf()
#include <string>
#include <boost/asio.hpp>
#include <pigpio.h>
#include "encoder_interface.h"
#include "motor_interface.h"
#include "serial_interface.h"

using namespace std::chrono_literals;

void print_header();
encoder::Encoder setup_encoder();
long read_encoder_value(serial::Serial& serial_port);
motor::Motor setup_motor();
void run_peaks_test(encoder::Encoder& encoder, motor::Motor& motor, serial::Serial& serial_port, int const test_number);
void run_drive_test(int const speed, encoder::Encoder& encoder, motor::Motor& motor, serial::Serial& serial_port, int const test_number);
int calculate_peaks_expected(double const test_duration, double const signal_freq);
int test_signal(double const a, double const f, double const s, double const t);
bool check_direction_change(long const val_k0, long const val_k1);

int main()
{
  print_header();

  encoder::Encoder encoder = setup_encoder();

  motor::Motor motor = setup_motor();
  motor.init();
  motor.activate();

  serial::Serial serial_port{"/dev/ttyACM0"};
  serial_port.open();

  run_peaks_test(encoder, motor, serial_port, 1);
  printf("\n");
  std::this_thread::sleep_for(1s);

  run_drive_test(40, encoder, motor, serial_port, 2);
  printf("\n");
  std::this_thread::sleep_for(1s);

  run_drive_test(-40, encoder, motor, serial_port, 3);
  printf("\n");
  std::this_thread::sleep_for(1s);

  motor.deactivate();
  serial_port.close();
  std::this_thread::sleep_for(1s);

  gpioTerminate();

  printf("Tests complete.\n");
  printf("\n");

  return 0;
}

void print_header()
{
  printf("\n");
  printf("==============================================\n");
  printf("==============================================\n");
  printf("           Functional Encoder Test:           \n");
  printf("              RPi 4 to TB6612FNG              \n");
  printf("                                              \n");
  printf("             Author: Jack Center              \n");
  printf("==============================================\n");
  printf("==============================================\n");
  printf("\n");
}

void run_peaks_test(encoder::Encoder& encoder, motor::Motor& motor, serial::Serial& serial_port, int const test_number)
{
  printf("-- Test %i: count encoder peaks --\n", test_number);
  auto time_loop = 2ms;
  auto time_test = 2250ms;

  double const amp = 40;
  double const freq = 2;
  double const shift = 0;

  int peaks_expected = calculate_peaks_expected(time_test.count() / 1000, freq);

  int count = 0;
  int dir_current = 1;
  int peaks = 0;

  std::this_thread::sleep_for(1s);

  auto time_start = std::chrono::system_clock::now();
  auto time_loop_start = time_start;

  while (std::chrono::system_clock::now() - time_start < time_test)
  {
    if (time_loop < std::chrono::system_clock::now() - time_loop_start)
    {
      auto time_test_elapsed = std::chrono::duration<double>(std::chrono::system_clock::now() - time_start);
      int signal_value = test_signal(amp, freq, shift, time_test_elapsed.count());
      motor.drive(signal_value);

      long enc_val = read_encoder_value(serial_port);
      encoder.update(enc_val);

      long const dir_encoder = encoder.get_direction();
      if (check_direction_change(dir_current, dir_encoder))
      {
        ++peaks;
        dir_current = dir_encoder;
      }

      time_loop_start += time_loop;
    }
  }

  motor.stop();

  // Result
  if (peaks == peaks_expected)
  {
    printf("\tTest Passed: %i of %i expected peaks measured.\n", peaks, peaks_expected);
  } else
  {
    printf("\tTest Failed: %i of %i expected peaks measured.\n", peaks, peaks_expected);
  }

  motor.brake();
}

void run_drive_test(int const speed, encoder::Encoder& encoder, motor::Motor& motor, serial::Serial& serial_port, int const test_number)
{
  int const direction = speed / abs(speed);

  std::string title;
  if (direction == 1)
  {
    title = "drive forward";
  } else
  {
    title = "drive backward";
  }

  std::cout << "-- Test " << test_number << ": " << title << " --\n";
  auto time_control = 2ms;
  auto time_measurement = 50ms;
  auto time_test = 1000ms;

  motor.drive(speed);
  std::this_thread::sleep_for(1s);

  int fails_position = 0;
  int fails_velocity = 0;
  int count_measurements = 0;

  auto time_test_start = std::chrono::system_clock::now();
  auto time_control_start = time_test_start;
  auto time_measurement_start = time_test_start;

  while (std::chrono::system_clock::now() - time_test_start < time_test)
  {
    // Control Loop
    if (time_control < std::chrono::system_clock::now() - time_control_start)
    {
      long enc_val = read_encoder_value(serial_port);
      encoder.update(enc_val);

      time_control_start += time_control;
    }

    // Test Loop
    if (time_measurement < std::chrono::system_clock::now() - time_measurement_start)
    {
      if (direction * (encoder.get_current_value() - encoder.get_previous_value()) <= 0)
      {
        ++fails_position;
      }

      if (direction * encoder.get_velocity() <= 0)
      {
        ++fails_velocity;
      }

      ++count_measurements;
      time_measurement_start += time_measurement;
    }
  }

  motor.stop();

  if (fails_position || fails_velocity)
  {
    printf("\tTests Failed. Number of failed measurements:\n");
    printf("\t  Position:\t%i of %i.\n", fails_position, count_measurements);
    printf("\t  Velocity:\t%i of %i.\n", fails_velocity, count_measurements);
  } else
  {
    printf("\tTest Passed:\t%i measurements taken.\n", count_measurements);
  }

  motor.brake();
}

int calculate_peaks_expected(double const test_duration, double const signal_freq)
{
    return (int) 2 * test_duration * signal_freq;
}

encoder::Encoder setup_encoder()
{
  int cpr = 8400;
  int rate = 500;

  encoder::Encoder encoder{cpr, rate};

  serial::Serial serial_port{"/dev/ttyACM0"};
  serial_port.open();
  serial_port.write("a");
  std::string response_string1 = serial_port.read_line();   // enc value
  std::string response_string2 = serial_port.read_line();   // time
  serial_port.close();

  long enc_val = std::stol(response_string1);

  encoder.set_initial_value(enc_val);

  return encoder;
}
motor::Motor setup_motor()
{
  int const in1_pin = 27;
  int const in2_pin = 22;
  int const pwm_pin = 18;
  int const stby_pin = 17;
  int const diretion = 1;

  motor::Motor motor{ in1_pin, in2_pin, pwm_pin, stby_pin, diretion };

  return motor;
}

int test_signal(double const a, double const f, double const s, double const t)
{
  double val = a*sin(2 * M_PI * f * t - s);
  return (int) std::round(val);
}

long read_encoder_value(serial::Serial& serial_port)
{
  serial_port.write("a");
  std::string response_string1 = serial_port.read_line();   // enc value
  std::string response_string2 = serial_port.read_line();   // time

  return std::stol(response_string1);
}

bool check_direction_change(long const val_k0, long const val_k1)
{
  bool result = false;

  if (val_k1 && val_k1 != val_k0)
  {
    result = true;
  }

  return result;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <boostnumpy@martin-wolf.org>
 *
 * @file    boost/numpy/utilities.hpp
 * @version $Revision$
 * @date    $Date$
 * @author  Martin Wolf <boostnumpy@martin-wolf.org>
 *
 * @brief This file defines utility functions similar to those universal
 *        functions available in the numpy module.
 */
#ifndef BOOST_NUMPY_UTILITIES_HPP_INCLUDED
#define BOOST_NUMPY_UTILITIES_HPP_INCLUDED

#include <boost/numpy/dtype.hpp>
#include <boost/numpy/ndarray.hpp>

namespace boost {
namespace numpy {

/**
 * @brief Equivalent to the ``numpy.all`` function.
 *        Checks if all array elements along the given axis evaluate to
 *        ``true``, by calling the PyArray_All function. The returned
 *        boost::python::object object is either a numpy scalar or a numpy
 *        ndarray.
 */
python::object
all(ndarray const & a, int axis);

void
all(ndarray const & a, int axis, ndarray & out);

/**
 * @brief Equivalent to the ``numpy.any`` function.
 *        Checks if any array elements along the given axis evalutes to
 *        ``true``, by calling the PyArray_Any function. The returned
 *        boost::python::object object is either a numpy scalar or a numpy
 *        ndarray.
 */
python::object
any(ndarray const & a, int axis);

void
any(ndarray const & a, int axis, ndarray & out);

/**
 * @brief Compares element-wise the content the ndarray a0 and a1 if they are
 *        equal. The resulting ndarray is a boolean array with a shape that
 *        fits the broadcasted shape of the two i
 */
ndarray
equal(ndarray const & x1, ndarray const & x2);

void
equal(ndarray const & x1, ndarray const & x2, ndarray & out);

/**
 * @brief Creates an empty ndarray of the same shape and data type as the given
 *     ndarray.
 */
ndarray
empty_like(ndarray const & a);

/**
 * @brief Creates an empty ndarray of the same shape as the given ndarray but
 *    with the given data type.
 */
ndarray
empty_like(ndarray const & a, dtype const & dt);

}// namespace numpy
}// namespace boost

#endif // !BOOST_NUMPY_UTILITIES_HPP_INCLUDED


#include "quadrotor_msgs/decode_msgs.h"
#include <quadrotor_msgs/comm_types.h>
#include <Eigen/Geometry>

namespace quadrotor_msgs
{

bool decodeOutputData(const std::vector<uint8_t> &data,
                      quadrotor_msgs::OutputData &output)
{
  struct OUTPUT_DATA output_data;
  if(data.size() != sizeof(output_data))
    return false;

  memcpy(&output_data, &data[0], sizeof(output_data));
  output.loop_rate = output_data.loop_rate;
  output.voltage = output_data.voltage/1e3;

  const double roll = output_data.roll/1e2 * M_PI/180;
  const double pitch = output_data.pitch/1e2 * M_PI/180;
  const double yaw = output_data.yaw/1e2 * M_PI/180;
  // Asctec (2012 firmware) uses  Z-Y-X convention
  Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
  output.orientation.w = q.w();
  output.orientation.x = q.x();
  output.orientation.y = q.y();
  output.orientation.z = q.z();

  output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180;
  output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180;
  output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180;

  output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81;
  output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81;
  output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81;

  output.pressure_dheight = output_data.dheight/1e3;
  output.pressure_height = output_data.height/1e3;

  output.magnetic_field.x = output_data.mag[0]/2500.0;
  output.magnetic_field.y = output_data.mag[1]/2500.0;
  output.magnetic_field.z = output_data.mag[2]/2500.0;

  for(int i = 0; i < 8; i++)
  {
    output.radio_channel[i] = output_data.radio[i];
  }
  //for(int i = 0; i < 4; i++)
  //  output.motor_rpm[i] = output_data.rpm[i];

  output.seq = output_data.seq;

  return true;
}

bool decodeStatusData(const std::vector<uint8_t> &data,
                      quadrotor_msgs::StatusData &status)
{
  struct STATUS_DATA status_data;
  if(data.size() != sizeof(status_data))
    return false;
  memcpy(&status_data, &data[0], sizeof(status_data));

  status.loop_rate = status_data.loop_rate;
  status.voltage = status_data.voltage/1e3;
  status.seq = status_data.seq;

  return true;
}

bool decodePPROutputData(const std::vector<uint8_t> &data,
                         quadrotor_msgs::PPROutputData &output)
{
  struct PPR_OUTPUT_DATA output_data;
  if(data.size() != sizeof(output_data))
    return false;
  memcpy(&output_data, &data[0], sizeof(output_data));

  output.quad_time = output_data.time;
  output.des_thrust = output_data.des_thrust*1e-4;
  output.des_roll = output_data.des_roll*1e-4;
  output.des_pitch = output_data.des_pitch*1e-4;
  output.des_yaw = output_data.des_yaw*1e-4;
  output.est_roll = output_data.est_roll*1e-4;
  output.est_pitch = output_data.est_pitch*1e-4;
  output.est_yaw = output_data.est_yaw*1e-4;
  output.est_angvel_x = output_data.est_angvel_x*1e-3;
  output.est_angvel_y = output_data.est_angvel_y*1e-3;
  output.est_angvel_z = output_data.est_angvel_z*1e-3;
  output.est_acc_x = output_data.est_acc_x*1e-4;
  output.est_acc_y = output_data.est_acc_y*1e-4;
  output.est_acc_z = output_data.est_acc_z*1e-4;
  output.pwm[0] = output_data.pwm1;
  output.pwm[1] = output_data.pwm2;
  output.pwm[2] = output_data.pwm3;
  output.pwm[3] = output_data.pwm4;

  return true;
}

}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check maxlog behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog;
  using boost::simd::Maxlog;
  T ref = T(0);
  STF_TYPE_IS(decltype(Maxlog<T>()), T);
  STF_EQUAL(Maxlog<T>(), ref);
  STF_EQUAL(maxlog( as(T{}) ),ref);
}

STF_CASE_TPL( "Check maxlog behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog;
  using boost::simd::Maxlog;
  T ref = T(709.78271289338400);

  STF_TYPE_IS(decltype(Maxlog<T>()), T);
  STF_IEEE_EQUAL(Maxlog<T>(), ref);
  STF_IEEE_EQUAL(maxlog( as(T{}) ), ref);
}

STF_CASE_TPL( "Check maxlog behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog;
  using boost::simd::Maxlog;
  T ref = T(88.3762626647949);
  STF_TYPE_IS(decltype(Maxlog<T>()), T);
  STF_IEEE_EQUAL(Maxlog<T>(), ref);
  STF_IEEE_EQUAL(maxlog( as(T{}) ), ref);
}


/* Copyright 2015 see AUTHORS
 *
 *
 */

#ifndef FRAMESEGMENT_HPP
#define FRAMESEGMENT_HPP

#include <vector>
#include <memory>

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

#include "Frame.hpp"
#include "AtomGroup.hpp"
#include "ProteinSegment.hpp"

/**
 * @class FrameSegment
 * @brief a frame segment is a list of atoms from a frame
 */
class FrameSegment : public AtomGroup {
public:
    /**
     * @param protein_segment
     */
    FrameSegment(const std::shared_ptr<ProteinSegment> & protein_segment);

    /**
     * @param frame
     */
    void set_frame(const Frame & frame);

    /**
     * @return displacement vector
     */
    Eigen::VectorXd fit_to_reference();

private:
    std::shared_ptr<ProteinSegment> protein_segment;
};

#endif