//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_CUBEHASH_HPP
#define BOOST_HASH_CUBEHASH_HPP

#include <boost/hash/detail/cubehash_policy.hpp>
#include <boost/hash/merkle_damgard_block_hash.hpp>
#include <boost/hash/stream_preprocessor.hpp>

// As of 2009-07-15, the submission to NIST for SHA-3 is CubeHash16/32
// http://cubehash.cr.yp.to/submission/tweak.pdf
#ifndef BOOST_HASH_CUBEHASH_DEFAULT_R
#define BOOST_HASH_CUBEHASH_DEFAULT_R 16
#endif
#ifndef BOOST_HASH_CUBEHASH_DEFAULT_B
#define BOOST_HASH_CUBEHASH_DEFAULT_B 32
#endif

namespace boost {
namespace hashes {

template <unsigned r, unsigned b, unsigned h>
struct cubehash_compressor {
  public:
    typedef detail::cubehash_policy<r, b, h> policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef typename policy_type::word_type word_type;

    static unsigned const state_bits = policy_type::state_bits;
    static unsigned const state_words = policy_type::state_words;
    typedef typename policy_type::state_type state_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef typename policy_type::block_type block_type;

  public:
    void
    operator()(state_type &state,
               block_type const &block) {
        process_block(state, block);
    }

  private:
    static void
    process_block(state_type &state,
                  block_type const &block) {
#ifdef BOOST_HASH_SHOW_PROGRESS
        printf("Xoring the following block to the state:\n");
        for (unsigned i = 0; i < block.size(); ++i) {
            printf("%.8x%c", block[i], (i+1) != block.size() ? ' ' : '\n');
        }
#endif
        for (unsigned i = 0; i < block_words; ++i) {
            state[i] ^= block[i];
        }
        policy_type::transform_r(state);
    }

};

template <unsigned r, unsigned b, unsigned h>
struct cubehash_finalizer {
    typedef detail::cubehash_policy<r, b, h> policy_type;
    typedef typename policy_type::state_type state_type;
    void operator()(state_type &state) const {
        state[31] ^= 1;
        policy_type::transform_10r(state);
    }
};

//
// If the second and third parameters are unspecified (or left 0), then
// the first parameter is the number of bits in the digest, and
// r and b will be set to defaults.
//
// Otherwise the three parameters are r, b, and h respectively.
//

template <unsigned, unsigned = 0, unsigned = 0>
struct cubehash;

template <unsigned r, unsigned b, unsigned h>
struct cubehash {
  private:
    typedef detail::cubehash_policy<r, b, h> policy_type;
  public:
    typedef merkle_damgard_block_hash<
                stream_endian::little_octet_big_bit,
                policy_type::digest_bits,
                typename policy_type::iv_generator,
                cubehash_compressor<r, b, h>,
                cubehash_finalizer<r, b, h>
            > block_hash_type_;
#ifdef BOOST_HASH_NO_HIDE_INTERNAL_TYPES
    typedef block_hash_type_ block_hash_type;
#else
    struct block_hash_type : block_hash_type_ {};
#endif
    template <unsigned value_bits>
    struct stream_hash {
        typedef stream_preprocessor<
                    stream_endian::little_octet_big_bit,
                    value_bits,
                    0, // No length padding!
                    block_hash_type
                > type_;
#ifdef BOOST_HASH_NO_HIDE_INTERNAL_TYPES
        typedef type_ type;
#else
        struct type : type_ {};
#endif
    };
    typedef typename block_hash_type::digest_type digest_type;
};

template <unsigned h>
struct cubehash<h, 0, 0>
 : cubehash<BOOST_HASH_CUBEHASH_DEFAULT_R,
            BOOST_HASH_CUBEHASH_DEFAULT_B,
            h> {};

} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_CUBEHASH_HPP


#include <boost/graph/transpose_graph.hpp>


#include "NDB_KHiddenGenerator.h"
#include <boost/algorithm/string.hpp>
#include <utility>

NDB_KHiddenGenerator::NDB_KHiddenGenerator(const DBRecord& record, int length, std::vector<double> probabilityRatios, double recordCountRatio, int definedPositionCount)
        : _probabilityRatios(std::move(probabilityRatios)), _recordCountRatio(recordCountRatio), _definedPositionCount(definedPositionCount)
{
    assert(std::abs(std::accumulate(_probabilityRatios.begin(), _probabilityRatios.end(), 0.0) -1) < 0.01);
    assert(_definedPositionCount == _probabilityRatios.size());
    double ratio = 0;
    for (int i = 0; i < _definedPositionCount - 1; ++i)
    {
        ratio += _probabilityRatios[i];
        _accumulatedProbabilityRatios.push_back(ratio);
    }
    _db.push_back(record);
    _length = length;
}

size_t NDB_KHiddenGenerator::Generate(Stream &output)
{
    assert(!_db.empty());
    std::size_t count = 0, n = ceil(_length * _recordCountRatio);
    const auto& dbRecord = _db[0];
    while (count < n)
    {
        int specifiedBitsCount = GetSpecifiedBitsCount(RandomValuesGenerator::GetRandomDouble(0, 1));
        auto indices = RandomValuesGenerator::GetRandomIndices(_length, _definedPositionCount);
        NDBRecord record{ std::vector<NDBChar>(_length, NDBChar::Wildcard) };
        for (auto index : indices)
        {
            record.Characters()[index] = dbRecord.Characters()[index] ? NDBChar::Bit1 : NDBChar::Bit0;
        }

        auto bits = RandomValuesGenerator::GetRandomChoice(indices, specifiedBitsCount);
        for (auto bit : bits)
        {
            record.Characters()[bit] = dbRecord.Characters()[bit] ? NDBChar::Bit0 : NDBChar::Bit1;
        }

        output << record.ToString() << "\n";
        ++count;
    }
    return count;
}

std::string NDB_KHiddenGenerator::GetName()
{
    return "K-Hidden";
}

int NDB_KHiddenGenerator::GetSpecifiedBitsCount(double rand)
{
    for (int i = 0; i < _accumulatedProbabilityRatios.size(); ++i)
    {
        if (_accumulatedProbabilityRatios[i] > rand)
            return i+1;
    }
    return static_cast<int>(_accumulatedProbabilityRatios.size()) + 1;
}

void NDB_KHiddenGenerator::PrintParameters() const
{
    std::cout << "Algorithm: " << GetName() << std::endl;
    std::cout << "l: " << _length << std::endl;
    std::vector<std::string> param;
    param.resize(_probabilityRatios.size());
    std::transform(_probabilityRatios.begin(), _probabilityRatios.end(), param.begin(), [](const auto& p){ return std::to_string(p);});
    std::cout << "p: " << boost::algorithm::join(param, " ") << std::endl;
    std::cout << "r: " << _recordCountRatio << std::endl;
    std::cout << "k: " << _definedPositionCount << std::endl;
}

FileUtils::CsvFileData NDB_KHiddenGenerator::GetCsvData() const
{
    return FileUtils::CsvFileData {.algorithm=GetName(), .length = _length, .probabilityRatios = _probabilityRatios, .recordCountRatio = _recordCountRatio, .specifiedBits=_definedPositionCount};
}


#include <boost/polygon/transform.hpp>


#include <boost/units/base_units/angle/gradian.hpp>


#include <boost/date_time/local_time/local_time.hpp>
#include <iostream>

using namespace boost::local_time;
using namespace boost::posix_time;
using namespace boost::gregorian;

int main()
{
  time_zone_ptr tz{new posix_time_zone{"CET+0"}};

  ptime pt1{date{2014, 12, 5}, time_duration{12, 0, 0}};
  local_date_time dt1{pt1, tz};

  ptime pt2{date{2014, 12, 5}, time_duration{18, 0, 0}};
  local_date_time dt2{pt2, tz};

  local_time_period tp{dt1, dt2};

  std::cout.setf(std::ios::boolalpha);
  std::cout << tp.contains(dt1) << '\n';
  std::cout << tp.contains(dt2) << '\n';
}


#include "QDate.h"

#include <boost/date_time/gregorian/gregorian_types.hpp>
#include <ctime>
#include <boost/date_time/posix_time/posix_time_io.hpp>
#include "boost/date_time/local_time/local_time.hpp"

namespace pt = boost::posix_time;
namespace g = boost::gregorian;

qtime::QDate::QDate(int day, int month, int year) {
	_d = new boost::gregorian::date(year, month, day);
	auto a = _d->day();
	auto b = _d->year();
	auto c = _d->month();
}

qtime::QDate::QDate(const QDate& that) {
	_d = new boost::gregorian::date(*that._d);
}

qtime::QDate::QDate(QDate&& that) noexcept
{
	if (this != &that)
	{
		_d = that._d;
		that._d = nullptr;
	}
}

qtime::QDate::QDate(const long &btime) {
	_d = new boost::gregorian::date(boost::posix_time::from_time_t(btime).date());
}

qtime::QDate::~QDate() {
	delete _d;
}

qtime::QDate& qtime::QDate::operator=(const qtime::QDate& other) {
	if (this != &other) {
		delete _d;
		_d = new boost::gregorian::date(*other._d);
	}
	return *this;
}

qtime::QDate& qtime::QDate::operator=(qtime::QDate&& other) noexcept
{
	if (this != &other) {
		delete _d;
		_d = std::exchange(other._d, nullptr);
	}
	return *this;
}

qtime::QDate qtime::QDate::operator+(int ndays) {
	auto date = *_d + boost::gregorian::days(ndays);
	QDate d(date.day(), date.month(), date.year());
	return d;
}
qtime::WEEKDAY qtime::QDate::DayOfTheWeek() const{
	auto iday = _d->day_of_week();
	auto wd = boost::lexical_cast<std::string>(_d->day_of_week());
	return qtime::WEEKDAY((int)iday);
}

int qtime::QDate::DayOfTheMonth() const
{
	return _d->day();
}

int qtime::QDate::DayOfYear() const
{
	return _d->day_of_year();
}

bool qtime::QDate::IsWeekend() const
{
	auto dw = DayOfTheWeek();
	return dw == WEEKDAY::SATURDAY || dw == WEEKDAY::SUNDAY;
}

bool qtime::QDate::IsMonthEnd() const
{
	auto de = boost::gregorian::gregorian_calendar::end_of_month_day(_d->year(), _d->month());
	auto td = _d->day();
	return td == de;
}

qtime::MONTH qtime::QDate::Month() const
{
	auto m = _d->month().as_enum();
	return MONTH(m);
}

int qtime::QDate::Year() const
{
	return _d->year();
}

qtime::QDate qtime::QDate::operator-(int ndays) const
{
	auto date = *_d - boost::gregorian::days(ndays);
	QDate d(date.day(), date.month(), date.year());
	return d;
}

int qtime::QDate::operator-(const QDate& that) const
{
	return (*_d - *(that._d)).days();
}

bool qtime::QDate::operator>(const QDate& that) const
{
	return (*_d) < *(that._d);
}

bool qtime::QDate::operator<(const QDate& that) const
{
	return !(this->operator<(that));
}

bool qtime::QDate::operator<=(const QDate& that) const
{
	return (this->operator<(that) || this->operator==(that));
}

bool qtime::QDate::operator>=(const QDate& that) const
{
	return (this->operator>(that) || this->operator==(that));
}

bool qtime::QDate::operator==(const QDate& that) const
{
	return *_d == *(that._d);
}

time_t qtime::QDate::toEpoch() const
{
	using namespace boost::posix_time;
	static ptime epoch(boost::gregorian::date(1970, 1, 1));
	time_duration::sec_type secs = (ptime(*_d, seconds(0)) - epoch).total_seconds();
	return time_t(secs);
}

bool qtime::QDate::isLeapYear(int year)
{
	bool cond1 = year % 4 == 0 && year % 100 == 0 && year % 400;
	bool cond2 = year % 4 == 0 && year % 100 != 0;

	return cond1 || cond2;
}

const std::locale fmt(std::locale::classic(),
	new boost::gregorian::date_facet("%m/%d/%Y"));

std::string dateAsMMDDYYYY(const boost::gregorian::date& date)
{
	std::ostringstream os;
	os.imbue(fmt);
	os << date;
	return os.str();
}
std::ostream&  qtime::operator<<(std::ostream& os, const qtime::QDate& dt) {
	os.imbue(fmt);
	os << *(dt._d);
	return os;
}

const qtime::QDate qtime::QDate::ZERO = QDate(01, 01, 1900);


#include <boost/functional/hash.hpp>
#include <iostream>
#include <ctime>
#include <unordered_map>

constexpr int N_INTS = 2;
constexpr int N_KEYS = 1e7;

int main() {
  std::cout << "Start C++ STL benchmark:\n";

  typedef std::array<int, N_INTS> KeyType;
  std::unordered_map<std::array<int, N_INTS>, double, boost::hash<KeyType>> h;

  h.reserve(N_KEYS * 2);
  const double t0 = std::clock();

  KeyType key;
  for (int i = 1; i <= N_KEYS; i++) {
    for (int j = 1; j <= N_INTS; j++) {
      key[j - 1] = i + j;
    }
    h[key] = i * 0.5;
  }
  const double t1 = std::clock();

  double val;
  for (int i = 1; i <= N_KEYS; i++) {
    for (int j = 1; j <= N_INTS; j++) {
      key[j - 1] = i + j;
    }
    val = h[key];
  }
  const double t2 = std::clock();

  h.clear();
  const double t3 = std::clock();

  std::cout << "Time to assemble / get / clear:"
      << "       " << (t1 - t0) / CLOCKS_PER_SEC
      << "       " << (t2 - t1) / CLOCKS_PER_SEC
      << "       " << (t3 - t2) / CLOCKS_PER_SEC
      << "\n";

  return 0;
}


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

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 "mitkGeometry3DToXML.h"

#include <boost/lexical_cast.hpp>

#include <tinyxml.h>

TiXmlElement* mitk::Geometry3DToXML::ToXML(const Geometry3D* geom3D)
{
  assert(geom3D);

  // really serialize
  const AffineTransform3D* transform = geom3D->GetIndexToWorldTransform();

  // get transform parameters that would need to be serialized
  AffineTransform3D::MatrixType matrix = transform->GetMatrix();
  AffineTransform3D::OffsetType offset = transform->GetOffset();

  bool isImageGeometry = geom3D->GetImageGeometry();
  BaseGeometry::BoundsArrayType bounds = geom3D->GetBounds();

  // create XML file
  // construct XML tree describing the geometry
  TiXmlElement* geomElem = new TiXmlElement("Geometry3D");
  geomElem->SetAttribute("ImageGeometry", isImageGeometry ? "true" : "false" );
  geomElem->SetAttribute("FrameOfReferenceID", geom3D->GetFrameOfReferenceID());

  // coefficients are matrix[row][column]!
  TiXmlElement* matrixElem = new TiXmlElement("IndexToWorld");
  matrixElem->SetAttribute("type", "Matrix3x3");
  matrixElem->SetAttribute("m_0_0", boost::lexical_cast<std::string>(matrix[0][0]));
  matrixElem->SetAttribute("m_0_1", boost::lexical_cast<std::string>(matrix[0][1]));
  matrixElem->SetAttribute("m_0_2", boost::lexical_cast<std::string>(matrix[0][2]));
  matrixElem->SetAttribute("m_1_0", boost::lexical_cast<std::string>(matrix[1][0]));
  matrixElem->SetAttribute("m_1_1", boost::lexical_cast<std::string>(matrix[1][1]));
  matrixElem->SetAttribute("m_1_2", boost::lexical_cast<std::string>(matrix[1][2]));
  matrixElem->SetAttribute("m_2_0", boost::lexical_cast<std::string>(matrix[2][0]));
  matrixElem->SetAttribute("m_2_1", boost::lexical_cast<std::string>(matrix[2][1]));
  matrixElem->SetAttribute("m_2_2", boost::lexical_cast<std::string>(matrix[2][2]));
  geomElem->LinkEndChild(matrixElem);

  TiXmlElement* offsetElem = new TiXmlElement("Offset");
  offsetElem->SetAttribute("type", "Vector3D");
  offsetElem->SetAttribute("x", boost::lexical_cast<std::string>(offset[0]));
  offsetElem->SetAttribute("y", boost::lexical_cast<std::string>(offset[1]));
  offsetElem->SetAttribute("z", boost::lexical_cast<std::string>(offset[2]));
  geomElem->LinkEndChild(offsetElem);

  TiXmlElement* boundsElem = new TiXmlElement("Bounds");
  TiXmlElement* boundsMinElem = new TiXmlElement("Min");
  boundsMinElem->SetAttribute("type", "Vector3D");
  boundsMinElem->SetAttribute("x", boost::lexical_cast<std::string>(bounds[0]));
  boundsMinElem->SetAttribute("y", boost::lexical_cast<std::string>(bounds[2]));
  boundsMinElem->SetAttribute("z", boost::lexical_cast<std::string>(bounds[4]));
  boundsElem->LinkEndChild(boundsMinElem);
  TiXmlElement* boundsMaxElem = new TiXmlElement("Max");
  boundsMaxElem->SetAttribute("type", "Vector3D");
  boundsMaxElem->SetAttribute("x", boost::lexical_cast<std::string>(bounds[1]));
  boundsMaxElem->SetAttribute("y", boost::lexical_cast<std::string>(bounds[3]));
  boundsMaxElem->SetAttribute("z", boost::lexical_cast<std::string>(bounds[5]));
  boundsElem->LinkEndChild(boundsMaxElem);
  geomElem->LinkEndChild(boundsElem);

  return geomElem;
}

mitk::Geometry3D::Pointer mitk::Geometry3DToXML::FromXML( TiXmlElement* geometryElement )
{
  if (!geometryElement)
  {
    MITK_ERROR << "Cannot deserialize Geometry3D from nullptr.";
    return nullptr;
  }

  AffineTransform3D::MatrixType matrix;
  AffineTransform3D::OffsetType offset;
  bool isImageGeometry(false);
  unsigned int frameOfReferenceID(0);
  BaseGeometry::BoundsArrayType bounds;

  if ( TIXML_SUCCESS != geometryElement->QueryUnsignedAttribute("FrameOfReferenceID", &frameOfReferenceID) )
  {
    MITK_WARN << "Missing FrameOfReference for Geometry3D.";
  }

  if ( TIXML_SUCCESS != geometryElement->QueryBoolAttribute("ImageGeometry", &isImageGeometry) )
  {
    MITK_WARN << "Missing bool ImageGeometry for Geometry3D.";
  }

  // matrix
  if ( TiXmlElement* matrixElem = geometryElement->FirstChildElement("IndexToWorld")->ToElement() )
  {
    bool matrixComplete = true;
    for ( unsigned int r = 0; r < 3; ++r )
    {
      for ( unsigned int c = 0; c < 3; ++c )
      {
        std::stringstream element_namer;
        element_namer << "m_" << r << "_" << c;

        std::string string_value;
        if (TIXML_SUCCESS == matrixElem->QueryStringAttribute(element_namer.str().c_str(),
                                                              &string_value))
        {
          try
          {
            matrix[r][c] = boost::lexical_cast<double>(string_value);
          }
          catch (boost::bad_lexical_cast& e)
          {
            MITK_ERROR << "Could not parse '" << string_value << "' as number: " << e.what();
            return nullptr;
          }
        }
        else
        {
          matrixComplete = false;
        }
      }
    }

    if ( !matrixComplete )
    {
      MITK_ERROR << "Could not parse all Geometry3D matrix coefficients!";
      return nullptr;
    }
  } else
  {
    MITK_ERROR << "Parse error: expected Matrix3x3 child below Geometry3D node";
    return nullptr;
  }

  // offset
  if ( TiXmlElement* offsetElem = geometryElement->FirstChildElement("Offset")->ToElement() )
  {
    bool vectorComplete = true;
    std::string offset_string[3];
    vectorComplete &= TIXML_SUCCESS == offsetElem->QueryStringAttribute("x", &offset_string[0]);
    vectorComplete &= TIXML_SUCCESS == offsetElem->QueryStringAttribute("y", &offset_string[1]);
    vectorComplete &= TIXML_SUCCESS == offsetElem->QueryStringAttribute("z", &offset_string[2]);

    if ( !vectorComplete )
    {
      MITK_ERROR << "Could not parse complete Geometry3D offset!";
      return nullptr;
    }

    for ( unsigned int d = 0; d < 3; ++d )
      try
      {
        offset[d] = boost::lexical_cast<double>(offset_string[d]);
      }
      catch ( boost::bad_lexical_cast& e )
      {
        MITK_ERROR << "Could not parse '" << offset_string[d] << "' as number: " << e.what();
        return nullptr;
      }
  }
  else
  {
    MITK_ERROR << "Parse error: expected Offset3D child below Geometry3D node";
    return nullptr;
  }

  // bounds
  if ( TiXmlElement* boundsElem = geometryElement->FirstChildElement("Bounds")->ToElement() )
  {
    bool vectorsComplete(true);
    std::string bounds_string[6];
    if ( TiXmlElement* minElem = boundsElem->FirstChildElement("Min")->ToElement() )
    {
      vectorsComplete &= TIXML_SUCCESS == minElem->QueryStringAttribute("x", &bounds_string[0]);
      vectorsComplete &= TIXML_SUCCESS == minElem->QueryStringAttribute("y", &bounds_string[2]);
      vectorsComplete &= TIXML_SUCCESS == minElem->QueryStringAttribute("z", &bounds_string[4]);
    } else
    {
      vectorsComplete = false;
    }

    if ( TiXmlElement* maxElem = boundsElem->FirstChildElement("Max")->ToElement() )
    {
      vectorsComplete &= TIXML_SUCCESS == maxElem->QueryStringAttribute("x", &bounds_string[1]);
      vectorsComplete &= TIXML_SUCCESS == maxElem->QueryStringAttribute("y", &bounds_string[3]);
      vectorsComplete &= TIXML_SUCCESS == maxElem->QueryStringAttribute("z", &bounds_string[5]);
    } else
    {
      vectorsComplete = false;
    }

    if ( !vectorsComplete )
    {
      MITK_ERROR << "Could not parse complete Geometry3D bounds!";
      return nullptr;
    }

    for (unsigned int d = 0; d < 6; ++d)
      try
      {
        bounds[d] = boost::lexical_cast<double>(bounds_string[d]);
      }
      catch (boost::bad_lexical_cast& e)
      {
        MITK_ERROR << "Could not parse '" << bounds_string[d] << "' as number: " << e.what();
        return nullptr;
      }
}

  // build GeometryData from matrix/offset
  AffineTransform3D::Pointer newTransform = AffineTransform3D::New();
  newTransform->SetMatrix(matrix);
  newTransform->SetOffset(offset);

  Geometry3D::Pointer newGeometry = Geometry3D::New();
  newGeometry->SetFrameOfReferenceID(frameOfReferenceID);
  newGeometry->SetImageGeometry(isImageGeometry);

  newGeometry->SetIndexToWorldTransform(newTransform);

  newGeometry->SetBounds(bounds);

  return newGeometry;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef GREENDATA_HPP
#define GREENDATA_HPP

#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

/*! @struct greenData
 *  @brief Contains all data defined from user input in the green section.
 */

struct greenData {
    /*! The way the derivatives of the Green's function are evaluated */
    int howDerivative;
    /*! Evaluation policy for the diagonal of the S and D operators */
    int howIntegrator;
    /*! Dielectric profile type */
    int howProfile;
    /*! The permittivity */
    double epsilon;
    /*! Scaling for the diagonal of the approximate collocation matrices */
    double scaling;
    /*! Inverse of the Debye length */
    double kappa;
    /*! Diagonal values of the permittivity tensor with respect to the lab frame */
    Eigen::Vector3d epsilonTensor;
    /*! Euler angles giving the rotation of the solvent orientation with respect to the lab frame */
    Eigen::Vector3d eulerAngles;
    /*! Real part of the permittivity of a metal sphere */
    double epsilonReal;
    /*! Imaginary part of the permittivity of a metal sphere */
    double epsilonImaginary;
    /*! Coordinates of the metal sphere center */
    std::vector<double> NPspheres;
    /*! Radius of the the metal sphere */
    double NPradii;
    /*! Permittivity inside the interface */
    double epsilon1;
    /*! Permittivity outside the interface */
    double epsilon2;
    /*! Center of the diffuse layer */
    double center;
    /*! Width of the diffuse layer */
    double width;
    /*! Origin of the dielectric layer */
    Eigen::Vector3d origin;
    /*! Maximum angular momentum */
    int maxL;
    /*! Whether the structure was initialized with user input or not */
    bool empty;

    greenData() { empty = true;}
    greenData(int how_d, int how_i, int how_p,
              double _epsilon = 1.0,
              double s = 1.07,
              double _kappa = 0.0,
              const Eigen::Vector3d & epstens = Eigen::Vector3d::Zero(),
              const Eigen::Vector3d & euler = Eigen::Vector3d::Zero(),
              double _epsReal = 0.0,
              double _epsImaginary = 0.0,
              const std::vector<double> & _sphere = std::vector<double>(),
              double _sphRadius = 0.0,
              double _e1 = 1.0,
              double _e2 = 1.0,
              double _c = 100.0,
              double _w = 5.0,
              const Eigen::Vector3d & _o = Eigen::Vector3d::Zero(),
              int l = 30) :
        howDerivative(how_d), howIntegrator(how_i), howProfile(how_p),
    epsilon(_epsilon), scaling(s), kappa(_kappa), epsilonTensor(epstens), eulerAngles(euler),
	epsilonReal(_epsReal), epsilonImaginary(_epsImaginary),
    NPspheres(_sphere), NPradii(_sphRadius),
    epsilon1(_e1), epsilon2(_e2), center(_c), width(_w), origin(_o), maxL(l) { empty = false; }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
};

#endif // GREENDATA_HPP


#define BOOST_TEST_MODULE Dynamic-typed OXM field operations testcases

#include <boost/test/unit_test.hpp>
#include <boost/concept/assert.hpp>
#include <boost/concept_check.hpp>

#include "types/bits.hh"
#include "oxm/field.hh"
#include "oxm/openflow_basic.hh"
#include "oxm/errors.hh"

using namespace runos;

struct Fixture {
    oxm::in_port  in_port;
    oxm::eth_src  eth_src;
    oxm::eth_dst  eth_dst;
    oxm::eth_type eth_type;
};

BOOST_FIXTURE_TEST_SUITE( runos_oxm_tests , Fixture )

BOOST_AUTO_TEST_CASE( binop_type_test ) {
    oxm::field<oxm::in_port> f1;
    oxm::field<oxm::eth_src> f2;
    oxm::field<> f3 = oxm::field<>(in_port);
    oxm::value<oxm::in_port> v1(10);
    oxm::value<oxm::eth_src> v2("aa:bb:cc:dd:ee:ff");
    oxm::value<> v3 = oxm::value<oxm::in_port>(20);
    oxm::mask<oxm::in_port> m1;
    oxm::mask<oxm::eth_src> m2;
    oxm::mask<> m3 = oxm::mask<oxm::in_port>(0);

    static_assert(
        std::is_same<
            decltype(oxm::detail::binop_type(f1, f1)),
            oxm::in_port
        >(), "test failed"
    );

    static_assert(
        std::is_same<
            decltype(oxm::detail::binop_type(f1, v1)),
            oxm::in_port
        >(), "test failed"
    );

    static_assert(
        std::is_same<
            decltype(oxm::detail::binop_type(v1, m3)),
            oxm::in_port
        >(), "test failed"
    );

    static_assert(
        std::is_same<
            decltype(oxm::detail::binop_type(f3, v1)),
            oxm::in_port
        >(), "test failed"
    );

    static_assert(
        not std::is_same<
            decltype(oxm::detail::binop_type(f3, m2)),
            oxm::in_port
        >(), "test failed"
    );

    static_assert(
        std::is_same<
            decltype(oxm::detail::binop_type(f3, v3)),
            oxm::type
        >(), "test failed"
    );
}

BOOST_AUTO_TEST_CASE( meta_test ) {
    BOOST_CHECK_EQUAL(in_port.nbits(), 32);
    BOOST_CHECK_EQUAL(in_port.nbytes(), 4);
    BOOST_CHECK(not in_port.maskable());

    BOOST_CHECK_EQUAL(eth_src.nbits(), 48);
    BOOST_CHECK_EQUAL(eth_src.nbytes(), 6);
    BOOST_CHECK(eth_src.maskable());

    BOOST_CHECK_EQUAL(eth_dst.nbits(), 48);
    BOOST_CHECK_EQUAL(eth_dst.nbytes(), 6);
    BOOST_CHECK(eth_dst.maskable());

    BOOST_CHECK_EQUAL(eth_type.nbits(), 16);
    BOOST_CHECK_EQUAL(eth_type.nbytes(), 2);
    BOOST_CHECK(not eth_type.maskable());
}

BOOST_AUTO_TEST_CASE( field_ctor_test ) {
    ////
    BOOST_TEST_MESSAGE( "Constructing some oxm::value`s" );
    ////
    oxm::value<oxm::in_port> v_in_port_10 { 10 };
    oxm::value<oxm::eth_src> v_eth_src_aabb { "aabb.ccdd.eeff" };
    oxm::value<oxm::in_port> v_copy_of_in_port_10 ( v_in_port_10 );
    oxm::value<oxm::eth_src> v_eth_src_aabb_ ( 0xaabbccddeeff );
    oxm::value<oxm::eth_dst> v_eth_dst_aabb =
        eth_dst == 0xaabbccddeeff;
    oxm::value<oxm::eth_src> v_eth_src_ddee =
        eth_src == "dd:ee:11:22:33:44";

    BOOST_CHECK_EQUAL(v_in_port_10, v_copy_of_in_port_10);
    BOOST_CHECK_EQUAL(v_eth_src_aabb, v_eth_src_aabb_);
    BOOST_CHECK_NE(v_eth_src_aabb, v_eth_dst_aabb);
    BOOST_CHECK_NE(v_eth_src_aabb, v_eth_src_ddee);

    BOOST_CHECK_THROW( oxm::value<>(in_port, bits<4>("1100")),
            oxm::bad_bit_length);

    ////
    BOOST_TEST_MESSAGE( "Constructing some oxm::mask`s" );
    ////
    oxm::mask<> m_in_port_wildcard ( in_port,
            bits<>(std::string("00000000000000000000000000000000")) );
    BOOST_CHECK( m_in_port_wildcard.wildcard() );
    BOOST_CHECK_EQUAL( m_in_port_wildcard, ~oxm::mask<oxm::in_port>() );

    oxm::mask<> m_in_port_exact ( in_port,
            bits<>(std::string("11111111111111111111111111111111")) );
    BOOST_CHECK( m_in_port_exact.exact() );

    BOOST_CHECK_THROW( oxm::mask<>( in_port, bits<>(std::string("00")) ),
            oxm::bad_bit_length );
    BOOST_CHECK_THROW( oxm::mask<>( in_port, bits<>(std::string("00000001")) ),
            oxm::bad_bit_length );

    oxm::mask<oxm::eth_src> m_eth_src_1 (eth_src, ethaddr("ff:00:00:00:00:00"));
    oxm::mask<oxm::eth_src> m_eth_src_12 {ethaddr("0f:f0:00:00:00:00")};
    oxm::mask<oxm::eth_src> m_eth_src_2 = eth_src & ethaddr("00:ff:00:00:00:00");

    oxm::mask<> m_eth_dst_3
        = oxm::mask<oxm::eth_dst>(ethaddr("00:00:ff:00:00:00"));
    oxm::mask<oxm::eth_dst> m_eth_dst_34(ethaddr("00:00:0f:f0:00:00"));
    oxm::mask<oxm::eth_dst> m_eth_dst_4(ethaddr("00:00:00:ff:00:00"));

    oxm::mask<> m_copy_of_eth_src_1 ( m_eth_src_1 );

    BOOST_CHECK( m_eth_dst_3.fuzzy() );
    BOOST_CHECK( m_eth_dst_34.fuzzy() );
    BOOST_CHECK( m_eth_dst_4.fuzzy() );
    BOOST_CHECK_EQUAL( m_eth_src_1, m_copy_of_eth_src_1 );

    ////
    BOOST_TEST_MESSAGE( "Constructing some oxm::field`s" );
    ////
    oxm::field<oxm::eth_src> f_eth_src_1 (ethaddr("aa:bb:cc:dd:ee:ff"),
                                          ethaddr("00:ff:00:ff:00:ff"));
    oxm::field<> f_eth_dst_1 (eth_dst, ethaddr("aa:bb:cc:dd:ee:ff").to_bits(),
                                       ethaddr("00:ff:00:ff:00:ff").to_bits());
    oxm::field<> f_copy_of_eth_src_1 (f_eth_src_1);
    oxm::field<oxm::eth_src> f_exact_1 = eth_src == ethaddr("aa:bb:cc:dd:ee:ff");
    oxm::field<oxm::eth_src> f_exact_2
        = (eth_src & ethaddr("ff:ff:ff:ff:ff:ff")) == ethaddr("aa:bb:cc:dd:ee:ff");

    BOOST_CHECK_EQUAL( oxm::value<oxm::eth_src>(f_eth_src_1),
                       oxm::value<oxm::eth_src>(ethaddr("00:bb:00:dd:00:ff")));
    BOOST_CHECK_EQUAL( oxm::mask<oxm::eth_src>(f_eth_src_1),
                       oxm::mask<oxm::eth_src>(ethaddr("00:ff:00:ff:00:ff")));
    BOOST_CHECK_EQUAL( f_eth_src_1.value_bits(), f_eth_dst_1.value_bits() );
    BOOST_CHECK_EQUAL( f_eth_src_1.mask_bits(), f_eth_dst_1.mask_bits() );
    BOOST_CHECK_NE( f_eth_src_1, f_eth_dst_1 );
    BOOST_CHECK_EQUAL( f_eth_src_1, f_copy_of_eth_src_1 );
    BOOST_CHECK_THROW( oxm::field<oxm::eth_type>(0x88cc, 3), oxm::bad_mask );
    BOOST_CHECK_EQUAL( f_exact_1, f_exact_2 );
    BOOST_CHECK( f_exact_1.exact() );

    ////
    BOOST_TEST_MESSAGE( "Casting oxm fields" );
    ////
    BOOST_CHECK_EQUAL( oxm::mask<>(f_eth_src_1),
                       oxm::mask<oxm::eth_src>( ethaddr("00:ff:00:ff:00:ff")) );
    BOOST_CHECK_EQUAL( oxm::value<>(f_eth_src_1),
                       oxm::value<oxm::eth_src>( ethaddr("00:bb:00:dd:00:ff")) );
    BOOST_CHECK_EQUAL( oxm::mask<>(oxm::field<>(
                    oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")) )),
            oxm::mask<oxm::eth_src>(ethaddr("ff:ff:ff:ff:ff:ff")) );
    BOOST_CHECK_EQUAL( oxm::mask<>(oxm::field<oxm::eth_type>(10)),
                       oxm::mask<oxm::eth_type>(0xffff) );
    BOOST_CHECK_EQUAL( oxm::mask<>(oxm::field<oxm::in_port>(10)),
                       oxm::mask<oxm::in_port>(0xffffffff) );
    BOOST_CHECK( oxm::field<>(eth_src).wildcard());
    BOOST_CHECK( oxm::field<oxm::eth_src>().wildcard());

    ////
    BOOST_TEST_MESSAGE( "Assignments" );
    ////

    oxm::field<> f_copied = f_eth_src_1 = f_exact_1;
    BOOST_CHECK_EQUAL( f_copied, f_exact_1 );
    BOOST_CHECK_EQUAL( f_eth_src_1, f_exact_1 );

    oxm::mask<> m_copied = m_eth_src_1 = m_eth_src_2;
    BOOST_CHECK_EQUAL( m_copied, m_eth_src_2 );
    BOOST_CHECK_EQUAL( m_copied, m_eth_src_2 );

    oxm::value<> v_copied = v_eth_src_aabb = v_eth_src_ddee;
    BOOST_CHECK_EQUAL( v_copied, v_eth_src_ddee );
    BOOST_CHECK_EQUAL( v_copied, v_eth_src_ddee );
}

BOOST_AUTO_TEST_CASE( value_and_mask_test ) {
    BOOST_CHECK_EQUAL(
            oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")) &
            oxm::mask<oxm::eth_src>(ethaddr("ff:00:ff:ff:00:00")),
            oxm::field<oxm::eth_src>(ethaddr("aa:00:cc:dd:00:00"),
                                     ethaddr("ff:00:ff:ff:00:00")) );

    BOOST_CHECK_EQUAL(
            oxm::mask<oxm::eth_src>(ethaddr("ff:00:ff:ff:00:00")) &
            oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")),
            oxm::field<oxm::eth_src>(ethaddr("aa:00:cc:dd:00:00"),
                                     ethaddr("ff:00:ff:ff:00:00")) );
    BOOST_CHECK_EQUAL(
            oxm::value<oxm::eth_type>(0x88cc) & oxm::mask<oxm::eth_type>(0xffff),
            oxm::field<oxm::eth_type>(0x88cc) );

    BOOST_CHECK_EQUAL(
            oxm::value<oxm::eth_type>(0x88cc) & oxm::mask<oxm::eth_type>(0),
            oxm::field<oxm::eth_type>(0, 0) );

    BOOST_CHECK_THROW( oxm::value<>(eth_src, ethaddr().to_bits()) &
                       oxm::mask<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands);

    BOOST_CHECK_THROW( oxm::mask<>(eth_src, ethaddr().to_bits()) &
                       oxm::value<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands);
}

BOOST_AUTO_TEST_CASE( match_value_value_test ) {
    BOOST_CHECK( oxm::value<oxm::in_port>(10) & oxm::value<oxm::in_port>(10) );
    BOOST_CHECK( not (oxm::value<oxm::in_port>(10) & oxm::value<oxm::in_port>(5)) );
    BOOST_CHECK_THROW( oxm::value<>(eth_src, ethaddr().to_bits()) &
                       oxm::value<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands );
}

BOOST_AUTO_TEST_CASE( match_value_field_test ) {
    BOOST_CHECK( oxm::value<oxm::in_port>(10) & oxm::field<oxm::in_port>(10) );
    BOOST_CHECK( not (oxm::value<oxm::in_port>(10) & oxm::field<oxm::in_port>(5)) );
    BOOST_CHECK( oxm::value<oxm::in_port>(10) & oxm::field<oxm::in_port>(0, 0) );

    BOOST_CHECK( oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")) &
                 oxm::field<oxm::eth_src>(ethaddr("aa:11:cc:dd:11:11"),
                                          ethaddr("ff:00:ff:ff:00:00")) );

    BOOST_CHECK( oxm::field<oxm::eth_src>(ethaddr("aa:11:cc:dd:11:11"),
                                          ethaddr("ff:00:ff:ff:00:00")) &
                 oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")) );

    BOOST_CHECK( not (oxm::field<oxm::eth_src>(ethaddr("a1:11:cc:dd:11:11"),
                                               ethaddr("ff:00:ff:ff:00:00")) &
                      oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff"))) );

    BOOST_CHECK_THROW( oxm::field<>(eth_src, ethaddr().to_bits()) &
                       oxm::value<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands );

    BOOST_CHECK_THROW( oxm::value<>(eth_src, ethaddr().to_bits()) &
                       oxm::field<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands );
}

BOOST_AUTO_TEST_CASE( match_field_field_test ) {
    BOOST_CHECK( oxm::field<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff"),
                                          ethaddr("ff:00:ff:00:ff:00")) &
                 oxm::field<oxm::eth_src>(ethaddr("11:22:33:44:55:66"),
                                          ethaddr("00:ff:00:ff:00:ff")) );

    BOOST_CHECK( oxm::field<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff"),
                                          ethaddr("ff:00:ff:00:ff:00")) &
                 oxm::field<oxm::eth_src>(ethaddr("11:22:3c:44:55:66"),
                                          ethaddr("00:ff:0f:ff:00:ff")) );

    BOOST_CHECK( oxm::field<oxm::eth_src>(ethaddr("aa:bb:cc:d4:ee:ff"),
                                          ethaddr("ff:00:ff:0f:ff:00")) &
                 oxm::field<oxm::eth_src>(ethaddr("11:22:33:44:55:66"),
                                          ethaddr("00:ff:00:ff:00:ff")) );

    BOOST_CHECK( not(oxm::field<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff"),
                                              ethaddr("ff:00:ff:00:ff:00")) &
                     oxm::field<oxm::eth_src>(ethaddr("11:22:33:44:55:66"),
                                              ethaddr("00:ff:0f:ff:00:ff"))) );

    BOOST_CHECK_THROW( oxm::field<>(eth_src, ethaddr().to_bits()) &
                       oxm::field<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands );
}

BOOST_AUTO_TEST_CASE( rewrite_value_value_test ) {
    BOOST_CHECK_EQUAL( oxm::value<oxm::eth_type>(10) >> oxm::value<oxm::eth_type>(20)
                     , oxm::value<oxm::eth_type>(20) );
    BOOST_CHECK_THROW( oxm::value<>(eth_src, ethaddr().to_bits()) >>
                       oxm::value<>(eth_dst, ethaddr().to_bits()),
                       oxm::bad_operands );
}

BOOST_AUTO_TEST_CASE( rewrite_value_field_test ) {
    BOOST_CHECK_EQUAL( oxm::value<oxm::eth_src>(ethaddr("aa:bb:cc:dd:ee:ff")) >>
                       oxm::field<oxm::eth_src>(ethaddr("11:22:33:44:55:66"),
                                                ethaddr("ff:00:ff:00:ff:00")),
                       oxm::value<oxm::eth_src>(ethaddr("11:bb:33:dd:55:ff")) );
}

BOOST_AUTO_TEST_CASE( rewrite_field_field_test ) {
    BOOST_CHECK_EQUAL( oxm::field<oxm::eth_src>(ethaddr("aa:bb:00:00:00:00"),
                                                ethaddr("ff:ff:00:00:00:00")) >>
                       oxm::field<oxm::eth_src>(ethaddr("00:11:22:33:00:00"),
                                                ethaddr("00:ff:ff:ff:00:00"))
                     , oxm::field<oxm::eth_src>(ethaddr("aa:11:22:33:00:00"),
                                                ethaddr("ff:ff:ff:ff:00:00")) );
}

BOOST_AUTO_TEST_SUITE_END( )


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_MATRIX_FREE_MESH_EVALUATOR_HPP
#define MFMG_DEALII_MATRIX_FREE_MESH_EVALUATOR_HPP

#include <mfmg/dealii/dealii_mesh_evaluator.hpp>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/diagonal_matrix.h>
#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>
#include <deal.II/lac/trilinos_sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <type_traits>

namespace mfmg
{

/**
 * An interface class the user should derive from when defining an operator to
 * be used in a matrix-free context.
 */
template <int dim>
class DealIIMatrixFreeMeshEvaluator : public DealIIMeshEvaluator<dim>
{
public:
  using size_type = dealii::types::global_dof_index;

  /**
   * The dimension of the underlying mesh.
   */
  static int constexpr _dim = dim;

  /**
   * The constructor is supposed to initialize the data required for performing
   * operator evaluations on the global finite element space given by @p
   * dof_handler and @p constraints.
   */
  DealIIMatrixFreeMeshEvaluator(dealii::DoFHandler<dim> &dof_handler,
                                dealii::AffineConstraints<double> &constraints);

  /**
   * Destructor.
   */
  virtual ~DealIIMatrixFreeMeshEvaluator() override = default;

  /**
   * Create a deep copy of this class such that initializing on another
   * agglomerate works. This was introduced because calls to member functions
   * matrix_free_initialize_agglomerate() and matrix_free_evaluate_agglomerate()
   * (implemented in user provided classes deriving from
   * DealIIMatrixFreeMeshEvaluator) are not thread-safe. There might be other
   * options to solve this problem.
   */
  virtual std::unique_ptr<DealIIMatrixFreeMeshEvaluator> clone() const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();
    return std::make_unique<DealIIMatrixFreeMeshEvaluator>(*this);
  }

  /**
   * Return the class name as std::string.
   */
  std::string get_mesh_evaluator_type() const override final;

  std::shared_ptr<dealii::LinearAlgebra::distributed::Vector<double>>
  build_range_vector() const;

  /**
   * Return the dimension of the finite element space.
   */
  dealii::types::global_dof_index m() const;

  /**
   * Initialize the operator for a given agglomerate described by @p
   * dof_handler.
   * @p dof_handler is expected to be initialized with a dealii::FiniteElement
   * object in this call.
   */
  virtual void matrix_free_initialize_agglomerate(
      dealii::DoFHandler<dim> & /*dof_handler*/) const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();
  }

  /**
   * Evaluate the operator on the agglomerate this object was initialized on in
   * matrix_free_initialize_agglomerate().
   */
  virtual void
  matrix_free_evaluate_agglomerate(dealii::Vector<double> const & /*src*/,
                                   dealii::Vector<double> & /*dst*/) const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();
  }

  /**
   * Return the diagonal of the matrix the agglomerate operator conrresponds to.
   */
  virtual std::vector<double> matrix_free_get_agglomerate_diagonal(
      dealii::AffineConstraints<double> & /*constraints*/) const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();

    return std::vector<double>();
  }

  /**
   *  Evaluate the operator on the global mesh.
   */
  virtual void matrix_free_evaluate_global(
      dealii::LinearAlgebra::distributed::Vector<double> const & /*src*/,
      dealii::LinearAlgebra::distributed::Vector<double> & /*dst*/) const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();
  }

  /**
   * Return the inverse of the diagonal of the matrix the global operator
   * corresponds to. Constrained degrees of freedom are set to zero.
   */
  virtual std::shared_ptr<dealii::DiagonalMatrix<
      dealii::LinearAlgebra::distributed::Vector<double>>>
  matrix_free_get_diagonal_inverse() const
  {
    ASSERT_THROW_NOT_IMPLEMENTED();

    return nullptr;
  }

  /**
   * Return the diagonal of the matrix the global operator corresponds to.
   * Constrained degrees of freedom are set to zero.
   */
  virtual dealii::LinearAlgebra::distributed::Vector<double>
  get_diagonal() override
  {
    ASSERT_THROW_NOT_IMPLEMENTED();

    return dealii::LinearAlgebra::distributed::Vector<double>();
  }
};

template <int dim>
struct is_matrix_free<DealIIMatrixFreeMeshEvaluator<dim>> : std::true_type
{
};

} // namespace mfmg

#endif


/*
 * MatrixStack.hpp
 *
 *  Created on: 21.03.2016
 *      Author: Ulrich Schwesinger
 */

#ifndef INCLUDE_ASLAM_BACKEND_MATRIXSTACK_HPP_
#define INCLUDE_ASLAM_BACKEND_MATRIXSTACK_HPP_

// standard includes
#include <cstdint>
#include <vector>
#include <utility> // std::move

// Eigen includes
#include <Eigen/Dense>

// Schweizer-Messer
#include <sm/logging.hpp>

// self includes
#include <aslam/Exceptions.hpp>
#include <aslam/backend/util/CommonDefinitions.hpp>

namespace aslam {
namespace backend {

  /**
   * \class MatrixStack
   * \brief Stores a chain rule of matrices in a stack-like data structure
   */
  class MatrixStack
  {
   public:
    static constexpr size_t DataAlignment =
#ifdef EIGEN_DEFAULT_ALIGN_BYTES
        EIGEN_DEFAULT_ALIGN_BYTES;
#elif defined EIGEN_MAX_ALIGN_BYTES
        EIGEN_MAX_ALIGN_BYTES; // for older Eigen versions
#else
        16; // for even older Eigen versions
#endif

    typedef double Scalar;
    template <int Rows, int Cols>
    using Map = Eigen::Map<Eigen::Matrix<Scalar, Rows, Cols>, Eigen::Aligned>;
    template <int Rows, int Cols>
    using ConstMap = Eigen::Map<const Eigen::Matrix<Scalar, Rows, Cols>, Eigen::Aligned>;

    /**
     * \class Header
     * \brief Metadata for stack elements
     */
    struct Header
    {
      Header(const int cols_, const std::size_t dataIndex_)
          : cols(cols_), dataIndex(dataIndex_)
      {

      }
      uint16_t cols; /// \brief Number of columns of the data
      uint32_t dataIndex;  /// \brief Index to the beginning of the data of this element
    };

    /**
     * \class PopGuard
     * \brief Ensures that \p pop() is called on the stack when the guard goes
     *        out of scope
     */
    struct PopGuard
    {
      PopGuard(MatrixStack& stack)
          : _stack(&stack)
      {

      }

      PopGuard(const PopGuard&) = delete;
      void operator=(const PopGuard&) = delete;

      PopGuard(PopGuard&& pg)
      {
        *this = std::move(pg);
      }
      void operator=(PopGuard&& pg)
      {
        this->_stack = pg._stack;
        pg._stack = nullptr;
      }

      void pop()
      {
        if (_stack != nullptr) {
          _stack->pop();
          _stack = nullptr;
        }
      }
      ~PopGuard() { this->pop(); }

      MatrixStack& stack()
      {
        SM_ASSERT_NOTNULL_DBG(Exception, _stack, "");
        return *_stack;
      }

     private:
      MatrixStack* _stack;
    };

   public:

    /// \brief Constructs a stack
    MatrixStack(const uint16_t numRows, const std::size_t maxNumMatrices, const std::size_t estimatedNumElementsPerMatrix)
        : _data(maxNumMatrices*estimatedNumElementsPerMatrix), _numRows(numRows)
    {
      _headers.reserve(maxNumMatrices);
    }

    /// \brief Is the stack empty?
    bool empty() const { return _headers.empty(); }

    /// \brief Number of matrices stored
    std::size_t numMatrices() const { return _headers.size(); }

    /// \brief Number of matrix elements stored
    std::size_t numElements() const { return _dataSize; }

    /// \brief Push a matrix \p mat to the top of the stack
    template <typename DERIVED>
    EIGEN_ALWAYS_INLINE void push(const Eigen::MatrixBase<DERIVED>& mat)
    {
      if (LIKELY(!this->empty()))
      {
        SM_ASSERT_EQ_DBG(Exception, this->numTopCols(), mat.rows(), "Incompatible matrix sizes");
        this->allocate(mat.cols()); // We allocate space for 1 more matrix. Stack wasn't empty before, so now we have at least 2.
        this->top().noalias() = this->matrix(this->numMatrices()-2)*mat;
      }
      else
      {
        SM_ASSERT_EQ_DBG(Exception, this->numRows(), mat.rows(), "Incompatible matrix sizes");
        this->allocate(mat.cols());
        this->top() = mat;
      }
    }

    /// \brief Push a scalar factor \p scalar to the top of the stack
    EIGEN_ALWAYS_INLINE void push(const double scalar)
    {
      if (LIKELY(!this->empty()))
      {
        this->allocate(this->numTopCols()); // We allocate space for 1 more matrix. Stack wasn't empty before, so now we have at least 2.
        this->top() = this->matrix(this->numMatrices()-2)*scalar;
      }
      else
      {
        this->allocate(this->numRows());
        this->top() = Eigen::MatrixXd::Identity(this->numRows(), this->numRows())*scalar;
      }
    }

    /// \brief Push a matrix \p mat to the top of the stack and return a pop guard to ensure the matrix is popped again
    template <typename DERIVED>
    EIGEN_ALWAYS_INLINE PopGuard pushWithGuard(const Eigen::MatrixBase<DERIVED>& mat)
    {
      this->push(mat);
      return PopGuard(*this);
    }

    /// \brief Push a scalar \p scalar to the top of the stack and return a pop guard to ensure the matrix is popped again
    EIGEN_ALWAYS_INLINE PopGuard pushWithGuard(const double scalar)
    {
      this->push(scalar);
      return PopGuard(*this);
    }

    /// \brief Pop the top element from the stack
    void pop()
    {
      SM_ASSERT_FALSE(Exception, this->empty(), "pop() on an empty stack is forbidden!");
      _headers.pop_back();
      _dataSize = this->numMatrices() == 0 ? 0 : _headers.back().dataIndex + this->numRows()*_headers.back().cols; // Be careful with manual memory alignment in allocate()
    }

    /// \brief Const getter for the top matrix in the stack
    template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
    EIGEN_ALWAYS_INLINE ConstMap<Rows, Cols> top() const
    {
      SM_ASSERT_FALSE(Exception, this->empty(), "");
      return ConstMap<Rows, Cols>( &(_data[_headers.back().dataIndex]), this->numRows(), _headers.back().cols );
    }

    /// \brief Mutable getter for the top matrix in the stack
    template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
    EIGEN_ALWAYS_INLINE Map<Rows, Cols> top()
    {
      SM_ASSERT_FALSE(Exception, this->empty(), "");
      return Map<Rows, Cols>( &(_data[_headers.back().dataIndex]), this->numRows(), _headers.back().cols );
    }

   protected:

    /// \brief Allocates memory and metadata for an element of size \p _numRows x \p cols.
    void allocate(const int cols)
    {
      static constexpr int align = DataAlignment/sizeof(Scalar);
      std::size_t start = this->numElements();
      if (start % align != 0) // manual memory alignment
        start += align - start % align;

      const std::size_t newSize = start + this->numRows()*cols;
      if (UNLIKELY(newSize > _data.size())) {
        SM_WARN_STREAM_NAMED("optimization", "Matrix stack has to reallocate memory (" <<
            newSize << " > " <<  _data.size() << "). Consider constructing the matrix stack "
            "with more memory reserves for faster execution.");
        _data.resize(std::max(2*_data.size(), newSize));
      }

      _dataSize = newSize;
      _headers.emplace_back(cols, start);

      SM_ASSERT_TRUE_DBG(Exception, (uintptr_t)(&(_data[_headers.back().dataIndex])) % DataAlignment == 0, "Memory is not properly aligned");
    }

    /// \brief Const getter for the \p i-th matrix in the stack
    template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
    EIGEN_ALWAYS_INLINE ConstMap<Rows, Cols> matrix(const std::size_t i) const
    {
      SM_ASSERT_LT(IndexOutOfBoundsException, i, this->numMatrices(), "");
      return ConstMap<Rows, Cols>( &(_data[_headers[i].dataIndex]), this->numRows(), _headers[i].cols );
    }

   protected:

    /// \brief Number of columns of the matrix on top of the stack
    int numTopCols() const
    {
      return _headers.back().cols;
    }

   private:
    /// \brief Number of rows determined by the first matrix that was pushed
    int numRows() const
    {
      return _numRows;
    }

   private:
    std::vector<Scalar, Eigen::aligned_allocator<Scalar>> _data; /// \brief The data of the matrices
    std::vector<Header> _headers; /// \brief Metadata for the matrix entries

    uint16_t _numRows; /// \brief Number of rows
    std::size_t _dataSize = 0; /// \brief Number of elements in \p _data. Note
  };

} /* namespace aslam */
} /* namespace backend */

#endif /* INCLUDE_ASLAM_BACKEND_MATRIXSTACK_HPP_ */


/*
 * Sha256CredentialsMatcher.cpp
 *
 *  Created on: 2012-8-17
 *      Author: chenhl
 */
#include <cetty/shiro/authc/HashedCredentialsMatcher.h>

#include <boost/assert.hpp>

#include <cetty/shiro/crypt/DigestEngine.h>
#include <cetty/shiro/authc/AuthenticationInfo.h>
#include <cetty/shiro/authc/AuthenticationToken.h>
#include <cetty/shiro/crypt/DigestEngine.h>
#include <cetty/shiro/crypt/MD5Engine.h>
#include <cetty/shiro/crypt/SHA1Engine.h>

#include <cetty/logging/LoggerHelper.h>
#include <cetty/config/ConfigCenter.h>

namespace cetty {
namespace shiro {
namespace authc {

using namespace cetty::shiro::crypt;

const std::string HashedCredentialsMatcher::MD5ENGINE = "md5_engine";
const std::string HashedCredentialsMatcher::SHA1ENGINE = "sha1_engine";

HashedCredentialsMatcher::HashedCredentialsMatcher()
    : digestEngine(NULL){
    ConfigCenter::instance().configure(&config);
    init();
}

void HashedCredentialsMatcher::init(){
    if (config.hashIterations < 1) {
        config.hashIterations = 1;
    }

    if(config.hashAlgorithm == MD5ENGINE){
        digestEngine = new MD5Engine();
   } else if(config.hashAlgorithm == SHA1ENGINE){
       digestEngine = new SHA1Engine();
   } else {
       digestEngine = new SHA1Engine();
   }
}

bool HashedCredentialsMatcher::match(const AuthenticationToken& token,
                                     const AuthenticationInfo& info) {
    std::string tokenCredentials;
    std::string accountCredentials;

    getCredentials(token, &tokenCredentials);
    hashProvidedCredentials(token, info, &accountCredentials);

    return tokenCredentials == accountCredentials;
}

void HashedCredentialsMatcher::getCredentials(const AuthenticationInfo& info,
                                              std::string* credentials) {
    assert(credentials != NULL);

    const std::string& infoCredentials = info.getCredentials();

    if (isStoredCredentialsHexEncoded()) {
        DigestEngine::digestFromHex(infoCredentials, credentials);
    } else {
        DigestEngine::digestFromBase64(infoCredentials, credentials);
    }
}

void HashedCredentialsMatcher::getCredentials(const AuthenticationToken &token,
                                              std::string* credentials) {
    if (credentials != NULL) {
        if (isStoredCredentialsHexEncoded()){
            DigestEngine::digestFromHex(token.getCredentials(), credentials);
        } else {
            DigestEngine::digestFromBase64(token.getCredentials(), credentials);
        }
    }
}

void HashedCredentialsMatcher::hashProvidedCredentials(const AuthenticationToken& token,
                                                       const AuthenticationInfo& info,
                                                       std::string* credentials) {
    if (!credentials) {
        return;
    }

    std::string salt = info.getCredentialsSalt();
    if (salt.empty() && isHashSalted()) {
        salt = token.getSalt();
    }

    std::string c;
    getCredentials(info, &c);
    c.append(salt);

    DigestEngine* digestEngine = getDigestEngine();
    if (!digestEngine) {
        BOOST_ASSERT(false);
        LOG_ERROR << "has no proper digest engine.";
        return;
    }

    digestEngine->reset();
    digestEngine->update(c);
    DigestEngine::Digest digest = digestEngine->digest();

    int iterations = config.hashIterations - 1; //already hashed once above
    //iterate remaining number:
    for (int i = 0; i < iterations; i++) {
        digestEngine->reset();
        digestEngine->update((const void*)digest.data(), (int)digest.size());
        digest = digestEngine->digest();
    }

    credentials->assign(digest.begin(), digest.end());
}

DigestEngine* HashedCredentialsMatcher::getDigestEngine() {
    return digestEngine;
}

HashedCredentialsMatcher::~HashedCredentialsMatcher(){
    if(digestEngine){
        delete digestEngine;
        digestEngine = NULL;
    }
}

}
}
}


/****************************************************************
 **                                                            **
 **  Copyright(C) 2020 Quanergy Systems. All Rights Reserved.  **
 **  Contact: http://www.quanergy.com                          **
 **                                                            **
 ****************************************************************/

#include <algorithm>
#include <map>
#include <cmath>
#include <chrono>
#include <fstream>
#include <csignal>

#include <quanergy/modules/encoder_angle_calibration.h>

#include <Eigen/Dense>

namespace quanergy
{
  namespace calibration
  {
    /** Laser firing rate, in Hz */
    const double EncoderAngleCalibration::FIRING_RATE = 53828.;

    // I choose this value by looking at multiple segments of revolutions from
    // -pi to pi and found all endpoints were within this value of -pi and pi.
    const double EncoderAngleCalibration::PI_TOLERANCE = 0.01;

    EncoderAngleCalibration::EncoderAngleCalibration()
    {
      reset();
    }

    void EncoderAngleCalibration::reset()
    {
      // mark complete and clean up threads
      setParams(0., 0.);

      nonempty_condition_.notify_all();

      for (auto& future : futures_)
      {
        future.get();
      }
      futures_.clear();

      // reset variables
      num_valid_samples_ = 0;
      amplitude_accumulator_ = AccumulatorSet();
      phase_averager_.clear();
      last_phase_ = 0.;
      started_calibration_ = false;
      first_run_ = true;
      calibration_count_ = 0;
      stats_ = StatsType();
      calibration_complete_ = false;
    }

    EncoderAngleCalibration::~EncoderAngleCalibration()
    {
      calibration_complete_ = true;
      nonempty_condition_.notify_all();

      for (auto& future : futures_)
      {
        future.get();
      }
    }

    void EncoderAngleCalibration::calibrateOnly()
    {
      run_forever_ = true;
    }

    void EncoderAngleCalibration::setFrameRate(double frame_rate)
    {
      frame_rate_ = frame_rate;
    }

    boost::signals2::connection EncoderAngleCalibration::connect(
        const typename Signal::slot_type& subscriber)
    {
      return signal_.connect(subscriber);
    }

    void EncoderAngleCalibration::setRequiredNumSamples(double num_samples)
    {
      required_samples_ = num_samples;
    }

    void EncoderAngleCalibration::slot(PointCloudHVDIRPtr const & cloud_ptr)
    {
      if (!cloud_ptr)
        return;

      if (calibration_complete_)
      {
        applyCalibration(cloud_ptr);
        return;
      }

      // if we haven't fired up the threads to calculate the encoder error, fire
      // them up now
      if (futures_.empty())
      {
        unsigned int num_threads = std::thread::hardware_concurrency();
        for (int i = 0; i < num_threads; i++)
          futures_.push_back(std::async(
              std::launch::async, &EncoderAngleCalibration::processAngles, this));
      }

      // if this is the first time to slot is called, we want to record the time
      // we started calibrating. If the timeout has elapsed, we'll want to throw
      // an exception.
      if (!run_forever_)
      {
        if (!started_calibration_)
        {
          std::cout << "QuanergyClient: Starting encoder calibration. This may take up to "
                    << std::chrono::duration_cast<std::chrono::seconds>(timeout_).count()
                    << " seconds to complete..." << std::endl;
          started_calibration_ = true;
          time_started_ = std::chrono::system_clock::now();
        }
        else if (std::chrono::system_clock::now() - time_started_ > timeout_)
        {
          // if we've timed out and the phase hasn't converged, it could be
          // because there isn't a lot of error. If this is the case, the
          // average amplitude will be below a threshold (~0.05 rads)
          // Check the average amplitude, if below the threshold report that
          // no calibration is necessary and do not apply a calibration to
          // future point clouds

          namespace ba = boost::accumulators;

          std::lock_guard<decltype(container_mutex_)> lock(container_mutex_);
          if (ba::mean(amplitude_accumulator_) < amplitude_threshold_)
          {
            std::stringstream msg;
            msg << "QuanergyClient: Encoder calibration not required for this sensor.\n"
              "Average amplitude calculated: " << ba::mean(amplitude_accumulator_);
            std::cout << msg.str() << std::endl;

            setParams(0., 0.);
            applyCalibration(cloud_ptr);
            return;
          }

          std::stringstream msg;
          msg << "QuanergyClient: Phase values did not converge for encoder calibration before timeout"
                  "\nNumber of consecutive valid frames: " << num_valid_samples_ << " / " << required_samples_ <<
                  "\nNumber of incomplete frames: " << stats_.num_incomplete_frames <<
                  "\nNumber of phase values outside of convergence: " << stats_.num_divergent_phase_values;

          if (num_valid_samples_ == 0)
          {
            msg << "\nEncoder calibration only works for M-series sensors configured with a scan field width of "
              "360 degrees and it is configured for a frame rate of " << frame_rate_ << " Hz. Since there were no "
              "valid samples, it's likely the sensor is not configured to match these conditions.";
          }

          throw std::runtime_error(msg.str());
        }
      }

      // Add the angles to the container while checking for completion
      // only iterate through the width to avoid repeated angles
      encoder_angles_.reserve(cloud_ptr->width);
      for (auto it = cloud_ptr->begin(); it != cloud_ptr->begin() + cloud_ptr->width; ++it)
      {
        const auto& pt = *it;
        if (!encoder_angles_.empty() && std::abs(encoder_angles_.back() - pt.h) > M_PI)
        {
          // we're at a discontinuity
          // check that the existing hvdir_pts are complete and push them to
          // queue
          if (checkComplete())
          {
            std::lock_guard<decltype(queue_mutex_)> lock(queue_mutex_);
            period_queue_.push(std::move(encoder_angles_));
            nonempty_condition_.notify_one();
          }
          else
          {
            // record statistics in case of timeout
            stats_.num_incomplete_frames++;
          }

          // if encoder_angles_ are not complete, discard period
          // we just moved encoder_angles_. Create a new object
          encoder_angles_ = AngleContainer();
          encoder_angles_.reserve(cloud_ptr->size());
        }

        encoder_angles_.push_back(pt.h);
      }
    }

    void EncoderAngleCalibration::setParams(double amplitude, double phase)
    {
      // In reality, amplitude should be much much less than this but this
      // protects the integrity of the math
      if (amplitude < -2 * M_PI || amplitude > 2 * M_PI
          || phase < -2 * M_PI || phase > 2 * M_PI)
      {
        throw std::invalid_argument("EncoderAngleCalibration amplitude or phase out of range [-2PI, 2PI]");
      }

      amplitude_ = amplitude;
      phase_ = phase;
      zero_offset_ = getOffset(0.);

      calibration_complete_ = true;
    }

    void EncoderAngleCalibration::applyCalibration(PointCloudHVDIRPtr const & cloud_ptr) const
    {
      if (!cloud_ptr)
        return;

      // return immediately if there are no slots
      if (signal_.num_slots() == 0)
        return;

      PointCloudHVDIR & cloud = *cloud_ptr;

      for (auto& point : cloud)
      {
        // corrects in place, saves copying other values
        point.h += zero_offset_ - getOffset(point.h);
        if (point.h < -M_PI)
        {
          point.h += 2 * M_PI;
        }
        else if (point.h > M_PI)
        {
          point.h -= 2 * M_PI;
        }
      }

      signal_(cloud_ptr);
    }

    void EncoderAngleCalibration::processAngles()
    {

      using namespace quanergy::common;

      while (!calibration_complete_ && num_valid_samples_ < required_samples_)
      {
        AngleContainer encoder_angles;
        // pop a vector of encoder angles off the queue
        {
          std::unique_lock<decltype(queue_mutex_)> lock(queue_mutex_);
          if (period_queue_.empty())
          {
            nonempty_condition_.wait(
                lock, [this]()
                {
                  return (!period_queue_.empty() || calibration_complete_);
                });
          }

          if (calibration_complete_)
            return;

          encoder_angles.swap(period_queue_.front());
          period_queue_.pop();
        }

        // calculate the amplitude and phase and display to user
        auto sine_parameters = calculate(encoder_angles);

        // display all results if we're running forever so we can analyze the
        // results. We won't be averaging in this mode so return immediately
        if (run_forever_)
        {
          if (first_run_)
          {
            std::cout << "QuanergyClient: AMPLITUDE(rads), PHASE(rads)" << std::endl;
            first_run_ = false;
          }

          std::stringstream output;
          output << sine_parameters.first << "," << sine_parameters.second
                 << std::endl;

          std::cout << output.str();
          continue;
        }

        // At this point we're not running forever. We will collect valid
        // calibrations until enough have been collected and then average the
        // results and use those values as the calibration to apply to outgoing
        // points.

        // this will lock the container mutex until the end of the while loop
        // we want exclusive access for the rest of the loop
        std::lock_guard<decltype(container_mutex_)> lock(container_mutex_);

        // we're in the critical section. Check to see if the calibration is
        // complete. If it is, there's no need to push to the back of the
        // container and find the average
        if (calibration_complete_)
          return;

        namespace ba = boost::accumulators;

        if (ba::count(amplitude_accumulator_) == 0 && phase_averager_.empty())
        {
          amplitude_accumulator_(sine_parameters.first);
          phase_averager_.accumulate(sine_parameters.second);
        }
        else if (angleDiff(sine_parameters.second, last_phase_) < phase_convergence_threshold_)
        {
          amplitude_accumulator_(sine_parameters.first);
          phase_averager_.accumulate(sine_parameters.second);

          num_valid_samples_++;

          if (num_valid_samples_ > required_samples_)
          {
            setParams(ba::mean(amplitude_accumulator_), phase_averager_.avg());

            std::cout << "QuanergyClient: Calibration complete." << std::endl
              << "  amplitude : " << amplitude_ << std::endl
              << "  phase     : " << phase_ << std::endl;

            // notify all threads waiting on period_queue_ so they can wake up,
            // check calibration_complete_ and return
            nonempty_condition_.notify_all();
          }
        }
        else
        {
          // we've calculated a phase value which is outside the window of
          // convergence. Clear the amplitude and phase containers and start
          // again.
          amplitude_accumulator_ = AccumulatorSet(); // resets accumulator
          phase_averager_.clear();
          num_valid_samples_ = 0;

          // add the current values to the containers
          amplitude_accumulator_(sine_parameters.first);
          phase_averager_.accumulate(sine_parameters.second);
          stats_.num_divergent_phase_values++;
        }

        last_phase_ = sine_parameters.second;

      } // end of while(num_valid_samples < required_samples_)

    }

    bool EncoderAngleCalibration::checkComplete() const
    {
      auto min = std::min(encoder_angles_.front(), encoder_angles_.back());
      auto max = std::max(encoder_angles_.front(), encoder_angles_.back());

      // check to make sure the front and back horizontal angle elements are
      // near the expected endpoints (-pi and pi)
      if (max < (M_PI - PI_TOLERANCE) || min > (-M_PI + PI_TOLERANCE))
      {
        return (false);
      }

      // check that the encoder period is within the steady-state range
      if ( encoder_angles_.size() > ((FIRING_RATE / frame_rate_) + encoder_count_tolerance_) ||
          encoder_angles_.size() < ((FIRING_RATE / frame_rate_) - encoder_count_tolerance_ ))
      {
        return (false);
      }

      // iterate over encoder_angles_ and check to make sure each point is within
      // if you divide the firing rate by the frame rate you get the number of
      // points per rev. Convert to radians
      auto rads_per_count = 2 * M_PI / (FIRING_RATE / frame_rate_);

      for (int i = 1; i < encoder_angles_.size(); i++)
      {
        // TCP packets are in points chunks of 50. If we see a delta angle of
        // more than 5 encoder counts, discard as dropped packet.
        if (std::abs(encoder_angles_[i] - encoder_angles_[i-1]) > 5 * rads_per_count)
        {
          return (false);
        }
      }

      return (true);
    }

    EncoderAngleCalibration::SineParameters EncoderAngleCalibration::calculate(
        const AngleContainer& encoder_angles)
    {
      if (encoder_angles.empty())
      {
        throw std::runtime_error(
            "QuanergyClient: Cannot calculate sine parameters of empty angle set");
      }

      auto slope = fitLine(encoder_angles);

      // calculate an initial error sinusoid from the line. This will tell us
      // how much our line needs to be shifted such that the sinusoid should be
      // vertically shifted. We will shift the line by this value and
      // recalculate our sinusoid
      std::vector<double> model;
      std::vector<double> sinusoid;

      auto calc_sinusoid =
          [&sinusoid, &encoder_angles, &model](double slope, double y_intercept)
      {
        model.clear();
        sinusoid.clear();
        for (int count = 0; count < encoder_angles.size(); count++)
        {
          model.push_back(slope * count + y_intercept);
          // no y-intercept this time
          sinusoid.push_back(encoder_angles[count] - slope * count - y_intercept);
        }
      };

      calc_sinusoid(slope, 0.);

      // determine the sinusoid vertical translation from the x-axis
      auto vertical_offset = (*std::max_element(sinusoid.begin(), sinusoid.end()) +
                              *std::min_element(sinusoid.begin(), sinusoid.end())) /
                             2;

      calc_sinusoid(slope, vertical_offset);

      auto smoothed_sinusoid = movingAvgFilter(sinusoid, moving_average_period_counts_);

      if (run_forever_)
      {
        std::stringstream encoder_filename;
        std::stringstream model_filename;
        std::stringstream error_filename;
        std::stringstream smoothed_filename;

        {
          std::lock_guard<decltype(file_mutex_)> lock(file_mutex_);

          encoder_filename << "encoder" << calibration_count_ << ".csv";
          model_filename << "model" << calibration_count_ << ".csv";
          error_filename << "error" << calibration_count_ << ".csv";
          smoothed_filename << "smoothed" << calibration_count_ << ".csv";

          calibration_count_++;
        }

        std::ofstream encoder_output{encoder_filename.str()};
        std::ofstream model_output{model_filename.str()};
        std::ofstream error_output{error_filename.str()};
        std::ofstream smoothed_output{smoothed_filename.str()};

        for (int i = 0; i < encoder_angles.size(); i++)
        {
          encoder_output << encoder_angles[i] << std::endl;
          model_output << model[i] << std::endl;
          error_output << sinusoid[i] << std::endl;
          smoothed_output << smoothed_sinusoid[i] << std::endl;
        }

        encoder_output.close();
        model_output.close();
        error_output.close();
        smoothed_output.close();
      }

      bool clockwise = (slope < 0) ? true : false;

      return (findSinusoidParameters(smoothed_sinusoid, clockwise));
    }

      EncoderAngleCalibration::AngleContainer
      EncoderAngleCalibration::unwrapEncoderAngles(
          const AngleContainer& encoder_angles)
    {
      // create a new vector with the new contents
      AngleContainer unwrapped_encoder_values;

      // we set a threshold to identify the 0 encoder value when we're iterating
      // over the vector. We want to identify the zero-crossing but not identify
      // a
      // discontinuity. If the difference in angle is greater than this
      // threshold,
      // we're at a discontinuity
      const double discontinuity_threshold = M_PI;

      double last_encoder_angle = encoder_angles[0];
      double unwrap_addition = 0;

      // we want to catch the second zero crossing to stop
      int zero_crossing_count = 0;
      for (const auto& angle : encoder_angles)
      {
        // if we're going to positive to negative or negative to positive (zero
        // crossing) AND we're below the discontinuity threshold, it's a zero
        // crossing
        // We only care about the zero-crossing for the first value
        if (((last_encoder_angle < 0 && angle >= 0) ||
             (last_encoder_angle > 0 && angle <= 0)) &&
            std::abs(angle - last_encoder_angle) < discontinuity_threshold)
        {
          if (unwrapped_encoder_values.empty())
          {
            // start recording unwrapped angles
            unwrapped_encoder_values.push_back(angle);
            last_encoder_angle = angle;
            zero_crossing_count++;
            continue;
          }
          else
          {
            zero_crossing_count++;
          }
        }

        if (zero_crossing_count > 1)
          break;

        if (unwrapped_encoder_values.empty())
        {
          last_encoder_angle = angle;
          continue;
        }

        if (std::abs(angle - last_encoder_angle) > discontinuity_threshold)
        {
          // we're at a discontinuity
          if (angle > last_encoder_angle)
          {
            // we went from -Pi to Pi, so we have a negative slope. Subtract
            // 2*Pi
            // from unwrap addition
            unwrap_addition -= 2 * M_PI;
          }
          else
          {
            // the opposite of above
            unwrap_addition += 2 * M_PI;
          }
        }

        unwrapped_encoder_values.push_back(angle + unwrap_addition);
        last_encoder_angle = angle;
      }

      return (unwrapped_encoder_values);
    }

    double EncoderAngleCalibration::fitLine(const AngleContainer& encoder_angles)
    {
      return (encoder_angles.back() - encoder_angles.front()) / encoder_angles.size();
    }

    EncoderAngleCalibration::AngleContainer
    EncoderAngleCalibration::movingAvgFilter(
        const AngleContainer& encoder_angles, int period)
    {
      namespace ba = boost::accumulators;

      auto half_period = static_cast<int>(period / 2);

      // assert size of container is greater than moving average period

      AngleContainer filtered_angles;
      for (auto signal_iter = encoder_angles.begin();
           signal_iter != encoder_angles.end(); ++signal_iter)
      {
        // make sure we don't iterate over the endpoints. If we're at an
        // endpoint we'll just take an average up until the endpoint. This means
        // our filtered signal will be the same size as our input.
        auto beg_iter = std::distance(encoder_angles.begin(), signal_iter) < half_period
                            ? encoder_angles.begin()
                            : signal_iter - half_period;

        auto end_iter = std::distance(signal_iter, encoder_angles.end()) < half_period
                            ? encoder_angles.end()
                            : signal_iter + half_period;

        ba::accumulator_set<double, ba::stats<ba::tag::mean>> avg_acc;
        for (auto& avg_iter = beg_iter; avg_iter < end_iter; ++avg_iter)
          avg_acc(*avg_iter);

        filtered_angles.push_back(ba::mean(avg_acc));
      }

      return filtered_angles;
    }

    EncoderAngleCalibration::SineParameters
    EncoderAngleCalibration::findSinusoidParameters(
        const AngleContainer& sine_signal,
        bool clockwise)
    {
      // First, compute the amplitude. It will be the maximum value of the
      // signal:
      auto amplitude =
          *std::max_element(sine_signal.begin(), sine_signal.end());

      auto it_max = std::max_element(sine_signal.begin(), sine_signal.end());
      auto it_min = std::min_element(sine_signal.begin(), sine_signal.end());

      auto max_index = std::distance(sine_signal.begin(),it_max);
      auto min_index = std::distance(sine_signal.begin(),it_min);

      if (min_index == max_index)
        throw std::runtime_error("QuanergyClient: Peak detection found min and max peaks to be same value");

      // these indices are in encoder counts where the first angle starts at
      // either -pi or pi since the motor direction is arbitrary. Determine
      // which endpoint the revolution started from
      double starting_angle = (clockwise) ? M_PI : -M_PI;
      double direction = (clockwise) ? -1. : 1.;

      // convert min/max index to angles depending on motor direction
      double max_error_angle = starting_angle + (direction) * (2 * M_PI * max_index / sine_signal.size());
      double min_error_angle = starting_angle + (direction) * (2 * M_PI * min_index / sine_signal.size());

      // the angle value where the sinusoid crosses from negative to positive is
      // the negative phase
      double negative_phase = (min_error_angle + max_error_angle) / 2.;

      // we need to apply a shift of pi to our calcluation above if the max
      // error angle is below the min error angle
      if (max_error_angle < min_error_angle)
      {
        // keep the negative phase in (-pi,pi]. This is the range of
        // std::atan2()
        if (negative_phase <= 0)
          negative_phase += M_PI;
        else
          negative_phase -= M_PI;
      }

      return std::make_pair(amplitude, -negative_phase);
    }

  } /* calibration */
} /* quanergy */


#include <osmium/io/reader.hpp>
#include <osmium/io/pbf_input.hpp>
#include <osmium/osm/types.hpp>
#include <osmium/visitor.hpp>
#include <osmium/index/map/sparse_mem_array.hpp>
#include <osmium/handler/node_locations_for_ways.hpp>

#include <boost/geometry.hpp>

#include <cstring>
#include <cstdlib>

#include <ios>
#include <locale>
#include <string>
#include <vector>
#include <ostream>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <stdexcept>

// No need for associative container or binary search; class is index [0,n-1]
// http://wiki.openstreetmap.org/wiki/Simple_3D_buildings#Roof
// http://taginfo.openstreetmap.org/keys/roof%3Ashape#values
static const std::vector<std::string> roofClasses{
    {"flat"},
    {"skillion"},
    {"gabled"},
    {"half-hipped"},
    {"hipped"},
    {"pyramidal"},
    {"gambrel"},
    {"mansard"},
    {"dome"},
    {"onion"},
    {"round"},
    {"saltbox"},
};

// Detects roofs and extracts (lat,lon,class) centroid tuples, piping them into the stream
struct BuildingHandler final : osmium::handler::Handler {
  explicit BuildingHandler(std::ostream& out_) : out{out_} {}

  // Handle ways
  void way(const osmium::Way& way) {
    const auto* roof = way.get_value_by_key("roof:shape");
    if (not roof)
      return;

    const auto it = std::find(std::begin(roofClasses), std::end(roofClasses), roof);
    if (it == std::end(roofClasses))
      return;

    const auto roofClass = std::distance(std::begin(roofClasses), it);

    // Report simple centroid of building way nodes
    using Point = boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;
    using Line = boost::geometry::model::linestring<Point>;

    Line line;
    for (const auto& node : way.nodes())
      boost::geometry::append(line, Point{node.location().lon(), node.location().lat()});

    Point center;
    boost::geometry::centroid(line, center);

    const auto longitude = boost::geometry::get<0>(center);
    const auto latitude = boost::geometry::get<1>(center);

    out << std::fixed << latitude << ',' << longitude << ',' << roofClass << '\n';
  }

  // Handle areas
  void area(const osmium::Area& /*area*/) {
    // TODO; ways should give us most of the roof shapes
  }

  std::ostream& out;
};

int main(int argc, char** argv) try {
  std::locale::global(std::locale(""));
  std::ios_base::sync_with_stdio(false);

  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " in.osm.pbf" << std::endl;
    return EXIT_FAILURE;
  }

  // Buildings can be mapped based on areas or ways
  osmium::io::Reader extractReader(argv[1]);

  // Locations need to be cached otherwise we get empty refs
  using PositiveIndex = osmium::index::map::SparseMemArray<osmium::unsigned_object_id_type, osmium::Location>;
  using LocationHandler = osmium::handler::NodeLocationsForWays<PositiveIndex>;

  PositiveIndex positiveIndex;
  LocationHandler locationHandler{positiveIndex};

  // Extracts buildings
  BuildingHandler buildingHandler{std::cout};

  // Run pipeline
  osmium::apply(extractReader, locationHandler, buildingHandler);

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


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

/*
* Author: Luca Marchionni
* Author: Bence Magyar
* Author: Enrique Fernández
* Author: Paul Mathieu
*
* Author: Braden McDorman (Semio AI, Inc.)
*/

#include <quori_holonomic_drive_controller/odometry.hpp>

#include <boost/bind.hpp>

using namespace quori_holonomic_drive_controller;

namespace bacc = boost::accumulators;

Odometry::Odometry(size_t velocity_rolling_window_size)
: timestamp_(0.0)
, x_(0.0)
, y_(0.0)
, heading_(0.0)
, linear_(0.0)
, angular_(0.0)
, wheel_separation_(0.0)
, left_wheel_radius_(0.0)
, right_wheel_radius_(0.0)
, left_wheel_old_pos_(0.0)
, right_wheel_old_pos_(0.0)
, velocity_rolling_window_size_(velocity_rolling_window_size)
, linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
{
}

void Odometry::init(const ros::Time& time)
{
  // Reset accumulators and timestamp:
  resetAccumulators();
  timestamp_ = time;
}

bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
{
  /// Get current wheel joint positions:
  const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
  const double right_wheel_cur_pos = right_pos * right_wheel_radius_;

  /// Estimate velocity of wheels using old and current position:
  const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
  const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

  /// Update old position with current:
  left_wheel_old_pos_  = left_wheel_cur_pos;
  right_wheel_old_pos_ = right_wheel_cur_pos;

  /// Compute linear and angular diff:
  const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
  const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;

  /// Integrate odometry:
  integrate_fun_(linear, angular);

  /// We cannot estimate the speed with very small time intervals:
  const double dt = (time - timestamp_).toSec();
  if (dt < 0.0001)
    return false; // Interval too small to integrate with

  timestamp_ = time;

  /// Estimate speeds using a rolling mean to filter them out:
  linear_acc_(linear/dt);
  angular_acc_(angular/dt);

  linear_ = bacc::rolling_mean(linear_acc_);
  angular_ = bacc::rolling_mean(angular_acc_);

  return true;
}

void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
{
  /// Save last linear and angular velocity:
  linear_ = linear;
  angular_ = angular;

  /// Integrate odometry:
  const double dt = (time - timestamp_).toSec();
  timestamp_ = time;
  integrate_fun_(linear * dt, angular * dt);
}

void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
{
  wheel_separation_   = wheel_separation;
  left_wheel_radius_  = left_wheel_radius;
  right_wheel_radius_ = right_wheel_radius;
}

void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
{
  velocity_rolling_window_size_ = velocity_rolling_window_size;

  resetAccumulators();
}

void Odometry::integrateRungeKutta2(double linear, double angular)
{
  const double direction = heading_ + angular * 0.5;

  /// Runge-Kutta 2nd order integration:
  x_       += linear * cos(direction);
  y_       += linear * sin(direction);
  heading_ += angular;
}

/**
 * \brief Other possible integration method provided by the class
 * \param linear
 * \param angular
 */
void Odometry::integrateExact(double linear, double angular)
{
  if (fabs(angular) < 1e-6)
    integrateRungeKutta2(linear, angular);
  else
  {
    /// Exact integration (should solve problems when angular is zero):
    const double heading_old = heading_;
    const double r = linear/angular;
    heading_ += angular;
    x_       +=  r * (sin(heading_) - sin(heading_old));
    y_       += -r * (cos(heading_) - cos(heading_old));
  }
}

void Odometry::resetAccumulators()
{
  linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
}