/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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.
 *
 *  Created on: Apr 22, 2012
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

/**
 * @file Parameters.hpp
 * @brief This file contains struct definitions that encapsulate parameters and settings.
 * @author Stefan Leutenegger
 * @author Andreas Forster
 */

#ifndef INCLUDE_OKVIS_PARAMETERS_HPP_
#define INCLUDE_OKVIS_PARAMETERS_HPP_

#include <deque>
#include <vector>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Woverloaded-virtual"
#include <opencv2/opencv.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>
#include <okvis/Time.hpp>
#include <okvis/MultiFrame.hpp>
#include <okvis/cameras/NCameraSystem.hpp>
#include <okvis/kinematics/Transformation.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {

/// \brief Struct to define the behavior of the camera extrinsics.
struct ExtrinsicsEstimationParameters
{
  // set to 0 in order to turn off
  /// \brief Default Constructor -- fixed camera extrinsics.
  ExtrinsicsEstimationParameters()
      : sigma_absolute_translation(0.0),
        sigma_absolute_orientation(0.0),
        sigma_c_relative_translation(0.0),
        sigma_c_relative_orientation(0.0)
  {
  }

  /**
   * @brief Constructor.
   * @param sigma_absolute_translation Absolute translation stdev. [m]
   * @param sigma_absolute_orientation Absolute orientation stdev. [rad]
   * @param sigma_c_relative_translation Relative translation noise density. [m/sqrt(Hz)]
   * @param sigma_c_relative_orientation Relative orientation noise density. [rad/sqrt(Hz)]
   */
  ExtrinsicsEstimationParameters(double sigma_absolute_translation,
                                 double sigma_absolute_orientation,
                                 double sigma_c_relative_translation,
                                 double sigma_c_relative_orientation)
      : sigma_absolute_translation(sigma_absolute_translation),
        sigma_absolute_orientation(sigma_absolute_orientation),
        sigma_c_relative_translation(sigma_c_relative_translation),
        sigma_c_relative_orientation(sigma_c_relative_orientation)
  {
  }

  // absolute (prior) w.r.t frame S
  double sigma_absolute_translation; ///< Absolute translation stdev. [m]
  double sigma_absolute_orientation; ///< Absolute orientation stdev. [rad]

  // relative (temporal)
  double sigma_c_relative_translation; ///< Relative translation noise density. [m/sqrt(Hz)]
  double sigma_c_relative_orientation; ///< Relative orientation noise density. [rad/sqrt(Hz)]
};

/*!
 * \brief IMU parameters.
 *
 * A simple struct to specify properties of an IMU.
 *
 */
struct ImuParameters{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  okvis::kinematics::Transformation T_BS; ///< Transformation from Body frame to IMU (sensor frame S).
  double a_max;  ///< Accelerometer saturation. [m/s^2]
  double g_max;  ///< Gyroscope saturation. [rad/s]
  double sigma_g_c;  ///< Gyroscope noise density.
  double sigma_bg;  ///< Initial gyroscope bias.
  double sigma_a_c;  ///< Accelerometer noise density.
  double sigma_ba;  ///< Initial accelerometer bias
  double sigma_gw_c; ///< Gyroscope drift noise density.
  double sigma_aw_c; ///< Accelerometer drift noise density.
  double tau;  ///< Reversion time constant of accerometer bias. [s]
  double g;  ///< Earth acceleration.
  Eigen::Vector3d a0;  ///< Mean of the prior accelerometer bias.
  int rate;  ///< IMU rate in Hz.
};

/*!
 * \brief Magnetometer parameters.
 *
 * A simple struct to specify properties of a magnetometer.
 *
 */
struct MagnetometerParameters{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double stdev; ///< Measurement (white noise part) standard deviation. [uT]
  double priorStdev; ///< Prior. [uT]
  double tau;        ///< Reversion time constant of bias [s]
  double sigma_c;    ///< Bias noise density [uT/sqrt(Hz)]
  double updateFrequency; ///< Related state estimates are inserted at this frequency. [Hz]
};

/*!
 * \brief GPS parameters
 *
 * A simple struct to specify properties of a GPS receiver.
 *
 */
struct GpsParameters{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Vector3d antennaOffset; ///< The position offset of the antenna in body (B) coordinates.
};

/*!
 * \brief Position sensor parameters.
 *
 * A simple struct to specify properties of a position sensor.
 *
 */
struct PositionSensorParameters{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Vector3d positionSensorOffset; ///< The position offset of the position sensor in body (B) coordinates.
  bool isLeveled; ///< If true, the position sensor measurements are assumed to be world z up (exactly, i.e. only yaw gets estimated).
};

/*!
 * \brief Magnetic ENU z bias
 *
 * A simple struct to specify the dynamics of magnetic ENU z component variation.
 *
 */
struct MagneticEnuZParameters{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double priorStdev;  ///< ENU z-component bias prior. [uT]
  double sigma_c;     ///< ENU z-component noise density. [uT/s/sqrt(Hz)]
  double tau;         ///< Reversion time constant of ENU z-component. [s]
  double updateFrequency; ///< Related state estimates are inserted at this frequency. [Hz]
};

/*!
 * \brief Barometer parameters.
 *
 * A simple struct to specify properties of a barometer.
 *
 */
struct BarometerParameters{
  double staticPressureStdev; ///< Measurement (white noise part) standard deviation. [kPa]
  double temperatureStdev; ///< Measurement (white noise part) standard deviation. [kPa]
};

/*!
 * \brief QFF parameters.
 *
 * A simple struct to specify the QFF state dynamics.
 *
 */
struct QffParameters{
  double priorStdev;  ///< Prior of QFF [kPa]
  double sigma_c;     ///< Drift noise density. [kPa/sqrt(s)]
  double tau;         ///< Reversion time constant. [s]
  double updateFrequency; ///< Related state estimates are inserted at this frequency. [Hz]
};

/*!
 * \brief Differential pressure sensor parameters.
 *
 * A simple struct to specify properties of a differential pressure sensor.
 *
 */
struct DifferentialPressureSensorParameters{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double differentialPressureStdev; ///< Measurement (white noise part) standard deviation. [Pa]
  /// Measurement (white noise part) standard deviation [m/s^2]
  /// Be conservative, this has to account for model uncertainties, too...
  double accelerationStdev;
  /// Polynomial for -z direction (dimensionless) force, alpha deg.
  /// polynomial vector of lenght n as (Matlab polyval style):
  /// c[0]*x^(n-1)+c[1]*x^(n-2)+...+c[n-2]*x+c[n-1] .
  Eigen::VectorXd c_minus_z_of_alpha;
  double c_y_of_beta; ///< Polynomial (only proportionality) for y direction (dimensionless) force, beta deg.
  double m; ///< Mass. [kg]
  double A; ///< Reference wing area. [m^2]
};

/*!
 * \brief Wind parameters.
 *
 * A simple struct to specify the wind state dynamics.
 *
 */
struct WindParameters{
  double priorStdev;  ///< Prior of wind. [m/s]
  double sigma_c;     ///< Drift noise density. [m/s/sqrt(s)]
  double tau;         ///< Reversion time constant. [s];
  double updateFrequency; ///< Related state estimates are inserted at this frequency. [Hz]
};

/**
 * @brief Parameters for optimization and related things (detection).
 */
struct Optimization{
  int max_iterations; ///< Maximum iterations the optimization should perform.
  int min_iterations; ///< Minimum iterations the optimization should perform.
  double timeLimitForMatchingAndOptimization; ///< The time limit for both matching and optimization. [s]
  okvis::Duration timeReserve; ///< Store a little more on the beginning and end of the IMU buffer. [s]
  double detectionThreshold;  ///< Keypoint detection threshold.
  bool useMedianFilter;     ///< Use a Median filter over captured image?
  int detectionOctaves;     ///< Number of keypoint detection octaves.
  int maxNoKeypoints;       ///< Restrict to a maximum of this many keypoints per image (strongest ones).
  int numKeyframes; ///< Number of keyframes.
  int numImuFrames; ///< Number of IMU frames.
};

/**
 * @brief Information on camera and IMU setup.
 */
struct SensorsInformation {
  int cameraRate;     ///< Camera rate in Hz.
  double imageDelay;  ///< Camera image delay. [s]
  int imuIdx;         ///< IMU index. Anything other than 0 will probably not work.
  double frameTimestampTolerance; ///< Time tolerance between frames to accept them as stereo frames. [s]
};

/// @brief Some visualization settings.
struct Visualization {
  bool displayImages; ///< Display images?
};

enum class FrameName { B, S, W, Wc };

/// @brief Some publishing parameters.
struct PublishingParameters {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  int publishRate = 200;  ///< Maximum publishing rate. [Hz]
  bool publishLandmarks = true; ///< Select, if you want to publish landmarks at all.
  float landmarkQualityThreshold = 1.0e-5; ///< Quality threshold under which landmarks are not published. Between 0 and 1.
  float maxLandmarkQuality = 0.05; ///< Quality above which landmarks are assumed to be of the best quality. Between 0 and 1.
  size_t maxPathLength = 100 ; ///< Maximum length of ros::nav_mgsgs::Path to be published.
  bool publishImuPropagatedState = true; ///< Should the state that is propagated with IMU messages be published? Or just the optimized ones?
  okvis::kinematics::Transformation T_Wc_W = okvis::kinematics::Transformation::Identity(); ///< Provide custom World frame Wc
  FrameName trackedBodyFrame = FrameName::B; ///< B or S, the frame of reference that will be expressed relative to the selected worldFrame Wc
  FrameName velocitiesFrame = FrameName::B; ///< B or S,  the frames in which the velocities of the selected trackedBodyFrame will be expressed in
};

/// @brief Struct to combine all parameters and settings.
struct VioParameters {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Optimization optimization;    ///< Optimization parameters.
  Visualization visualization;  ///< Visualization parameters.
  SensorsInformation sensors_information; ///< Information on camera and IMU setup.
  ExtrinsicsEstimationParameters camera_extrinsics; ///< Camera extrinsic estimation parameters.
  okvis::cameras::NCameraSystem nCameraSystem;  ///< Camera configuration.
  ImuParameters imu;  ///< IMU parameters
  MagnetometerParameters magnetometer;  ///< Magnetometer parameters.
  PositionSensorParameters position;  ///< Position sensor parameters.
  GpsParameters gps; ///< GPS parameters
  MagneticEnuZParameters magnetic_enu_z;  ///< Dynamics of magnetic ENU z component variation.
  BarometerParameters barometer;  ///< Barometer parameters.
  QffParameters qff;  ///< QFF parameters.
  DifferentialPressureSensorParameters differential; ///< Differential pressure sensor parameters.
  WindParameters wind;  ///< Wind parameters.
  PublishingParameters publishing; ///< Publishing parameters.
};

} // namespace okvis

#endif // INCLUDE_OKVIS_PARAMETERS_HPP_


/*!
 * @file swp_surfvolhydrogen_primary.cpp
 * @author Robert I A Patterson Robert.Patterson@wias-berlin.de
 *

  Copyright (C) 2012 Robert I A Patterson

  File purpose:
    Implementation of the SurfVolHydrogenPrimary class declared in the
    swp_surfvol_primary.h header file.
    @brief Implementation of particle described by its volume and surface area.

  Licence:
    This file is part of "sweepc".

    sweepc is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

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

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_primary.h"
#include "swp_surfvolhydrogen_primary.h"
#include "swp_aggmodel_type.h"
#include "swp_model_factory.h"

#include <stdexcept>
#include <boost/random/poisson_distribution.hpp>

using namespace Sweep;
using namespace Sweep::AggModels;

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (protected).
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(void)
: iH(1u)
{
}

// Initialising constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(double time, const Sweep::ParticleModel &model)
: Primary(time, model)
, iH(1u)
{
}

// Copy constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(const SurfVolHydrogenPrimary &copy)
: iH(1u)
{
    *this = copy;
}

// Stream-reading constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(std::istream &in, const Sweep::ParticleModel &model)
: iH(1u)
{
    Deserialize(in, model);
}

// Default destructor.
SurfVolHydrogenPrimary::~SurfVolHydrogenPrimary()
{
    releaseMem();
}

// OPERATOR OVERLOADS.

// Assignment operator (Primary RHS).
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::operator=(const Primary &rhs)
{
    // Attempt to cast the primary to a SurfVolHydrogenPrimary.  This will
    // throw an exception if the cast fails.
    operator=(dynamic_cast<const SurfVolHydrogenPrimary&>(rhs));

    return *this;
}

// Assignment operator (SurfVolHydrogenPrimary RHS).
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::operator=(const SurfVolHydrogenPrimary &rhs)
{
    // First copy everything for spherical primaries.
    Primary::operator=(rhs);

    return *this;
}

// AGGREGATION MODEL.

// Returns the aggregation model which this primary describes.
AggModels::AggModelType SurfVolHydrogenPrimary::AggID(void) const {return AggModels::SurfVolHydrogen_ID;}

// BASIC DERIVED PARTICLE PROPERTIES.

// Calculates the derived properties from the unique properties.  This
// function is broadly similar to the version in the spherical Primary
// class except that it uses the surface-volume model.  Therefore the
// surface area is not altered and the collision diameter is calculated
// using the arithmetic mean function.
void SurfVolHydrogenPrimary::UpdateCache(void)
{
    // Store the correct surface area.
    const double s = m_surf;

    // Pretend that the primary is spherical and set the cache
    // accordingly.  This will set m_surf the the surface are
    // of an equivalent volume sphere.
    Primary::UpdateCache();

    // The surface area is now set to that of a sphere. Set
    // correct surface area (minimum is spherical surface area).
    m_surf = std::max(s, m_surf);

    // Calculate diameters.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol; // TODO:  Correct expression for Dmob.
}

// Returns the equivalent spherical particle surface area.
double SurfVolHydrogenPrimary::SphSurfaceArea(void) const {return PI * std::pow(6.0 * m_vol / PI, TWO_THIRDS);}

// Returns the number of primary particles if the aggregate is assumed
// to consist of mono-sized primaries.
unsigned int SurfVolHydrogenPrimary::PP_Count(void) const
{
    // Note the minimum number of primary particles must be 1.
    return std::max(1u, (unsigned int)((m_surf * m_surf * m_surf) /
                                       (36.0 * PI * m_vol * m_vol)));
}

// Returns the primary particle diameter if the aggregate is assumed
// to consist of mono-sized primaries.
double SurfVolHydrogenPrimary::PP_Diameter(void) const
{
    // This should always be <= equiv. sphere diameter.
    return 6.0 * m_vol / m_surf;
}

// OPERATIONS.

// Adjusts the primary with the given composition and
// tracker values changes n times.  If the particle cannot be adjust
// n times, then this function returns the number of times
// it was adjusted.
unsigned int SurfVolHydrogenPrimary::Adjust(const fvector &dcomp, const fvector &dvalues, rng_type &rng,
                                    unsigned int n)
{
    // Calculate change in volume.
    double dvol = 0.0;
    for (unsigned int i=0; i!=dcomp.size(); ++i) {
        dvol += dcomp[i] * m_pmodel->Components(i)->MolWt() /
                m_pmodel->Components(i)->Density();
    }
    dvol *= (double)n / NA;

    // Calculate change in surface area.
    double invRadius = 0.0;
    if (dvol > 0.0) {
        // Inverse growth radius.
        invRadius = sqrt(4.0 * PI / m_surf);
    } else {
        // Inverse oxidation radius.
        invRadius = m_surf / (3.0 * m_vol);
    }

    // Save new and old surface areas
    const double oldS = m_surf;
    const double s = m_surf + (2.0 * dvol * invRadius);

    // Adjust the particle assuming that it is spherical.
    Primary::Adjust(dcomp, dvalues, rng, n);

    // Maintain the concentration of H per unit surface area during oxidation
    if(dvol < 0.0) {
        fvector comp = Composition();
        //comp[iH] = static_cast<int>(s * comp[iH] / oldS + 0.5);
        comp[iH] = s * comp[iH] / oldS;
        SetComposition(comp);
    }

    // Set correct surface area, which was incorrectly set by
    // Primary::Adjust.
    m_surf    = std::max(s, m_surf);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return n;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::Coagulate(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    const double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Coagulate(rhs, rng);

    // The spherical particle Coagulate() routine has set the
    // surface area incorrectly.  We now replace the surface area
    // to the correct point-contact value.
    m_surf    = std::max(m_surf, s);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::Fragment(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    const double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Fragment(rhs, rng);

    // The spherical particle Coagulate() routine has set the
    // surface area incorrectly.  We now replace the surface area
    // to the correct point-contact value.
    m_surf    = std::max(m_surf, s);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

// This routine sinters the Primary for the given length of
// time using the provided sintering model.
void SurfVolHydrogenPrimary::Sinter(double dt, Cell &sys,
                            const Processes::SinteringModel &model,
                            rng_type &rng,
                            double wt)
{
  throw std::runtime_error("sintering not implemented at present for SurfVolHydrogenPrimary");
}

// READ/WRITE/COPY.

// Returns a copy of the model data.
SurfVolHydrogenPrimary *const SurfVolHydrogenPrimary::Clone(void) const
{
    return new SurfVolHydrogenPrimary(*this);
}

/*!
 * \return      Number of surface Hydrogens (active sites)
 */
double SurfVolHydrogenPrimary::GetSites() const
{
    return Composition()[iH];
}

// Writes the object to a binary stream.
void SurfVolHydrogenPrimary::Serialize(std::ostream &out) const
{
    if (out.good()) {
        // Output the version ID (=0 at the moment).
        const unsigned int version = 0;
        out.write((char*)&version, sizeof(version));

        // Output base class.
        Primary::Serialize(out);
    } else {
        throw std::invalid_argument("Output stream not ready (Sweep, SurfVolHydrogenPrimary::Serialize).");
    }
}

// Reads the object from a binary stream.
void SurfVolHydrogenPrimary::Deserialize(std::istream &in, const Sweep::ParticleModel &model)
{
    if (in.good()) {
        // Read the output version.  Currently there is only one
        // output version, so we don't do anything with this variable.
        // Still needs to be read though.
        unsigned int version = 0;
        in.read(reinterpret_cast<char*>(&version), sizeof(version));

        switch (version) {
            case 0:
                // Read base class.
                Primary::Deserialize(in, model);
                break;
            default:
                throw std::runtime_error("Serialized version number is invalid (Sweep, SurfVolHydrogenPrimary::Deserialize).");
        }
    } else {
        throw std::invalid_argument("Input stream not ready (Sweep, SurfVolHydrogenPrimary::Deserialize).");
    }
}


/**
 * @file test_finalSelectionKernelRegressionLCB.cpp
 * @brief This file defines the test cases for the final selection kernel regression LCB policy.
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <cstddef>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "proseco_planning/action/action.h"
#include "proseco_planning/action/actionClass.h"
#include "proseco_planning/agent/agent.h"
#include "proseco_planning/config/configuration.h"
#include "proseco_planning/config/defaultConfiguration.h"
#include "proseco_planning/config/scenarioOptions.h"
#include "proseco_planning/node.h"
#include "proseco_planning/policies/final_selection/finalSelectionKernelRegressionLCB.h"
#include "proseco_planning/util/alias.h"

namespace utf = boost::unit_test;
using namespace proseco_planning;

struct ConfigFixture {
  ConfigFixture() { Config::create(config::scenarioSimple, config::optionsSimple); }
  ~ConfigFixture() { Config::get()->reset(); }
};

struct FinalSelectionKernelRegressionLCBFixture : ConfigFixture, FinalSelectionKernelRegressionLCB {
  FinalSelectionKernelRegressionLCBFixture() : FinalSelectionKernelRegressionLCB("") {}
};

BOOST_FIXTURE_TEST_SUITE(finalSelectionKernelRegressionLCB,
                         FinalSelectionKernelRegressionLCBFixture)

// ----------------------------------------------------------------
// test cases for kernels
// ----------------------------------------------------------------

BOOST_AUTO_TEST_CASE(euclidean_kernel_for_actions, *utf::tolerance(0.00001f)) {
  m_gammaAction = 1.f;

  ActionPtr action_0_0     = std::make_shared<Action>(0.f, 0.f);
  ActionPtr action_1_0     = std::make_shared<Action>(1.f, 0.f);
  ActionPtr action_0_1     = std::make_shared<Action>(0.f, 1.f);
  ActionPtr action_1_1     = std::make_shared<Action>(1.f, 1.f);
  ActionPtr action_m1_0    = std::make_shared<Action>(-1.f, 0.f);
  ActionPtr action_0_m1    = std::make_shared<Action>(0.f, -1.f);
  ActionPtr action_m1_m1   = std::make_shared<Action>(-1.f, -1.f);
  ActionPtr action_0p5_0p5 = std::make_shared<Action>(0.5f, 0.5f);

  // identity
  BOOST_TEST(Action::getSimilarity(action_0_0, action_0_0, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_1_0, action_1_0, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_0_1, action_0_1, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_1_1, action_1_1, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_m1_0, action_m1_0, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_0_m1, action_0_m1, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_m1_m1, action_m1_m1, m_gammaAction) == 1.f);
  BOOST_TEST(Action::getSimilarity(action_0p5_0p5, action_0p5_0p5, m_gammaAction) == 1.f);

  BOOST_TEST(Action::getSimilarity(action_0_0, action_1_1, m_gammaAction) == 0.1353352f);
  BOOST_TEST(Action::getSimilarity(action_1_1, action_0p5_0p5, m_gammaAction) == 0.6065306f);
  BOOST_TEST(Action::getSimilarity(action_m1_m1, action_0p5_0p5, m_gammaAction) == 0.0111089f);

  // symmetry
  BOOST_TEST(Action::getSimilarity(action_0_0, action_1_0, m_gammaAction) == 0.3678794f);
  BOOST_TEST(Action::getSimilarity(action_1_0, action_0_0, m_gammaAction) == 0.3678794f);
}

BOOST_AUTO_TEST_CASE(manhattan_kernel_for_action_classes, *utf::tolerance(0.00001f)) {
  m_gammaActionClass = 1.f;

  using ac = ActionClass;

  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::DO_NOTHING) == 1.f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::ACCELERATE) == 0.3678794f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::DECELERATE) == 0.3678794f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_LEFT) == 0.3678794f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_RIGHT) == 0.3678794f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_LEFT_FAST) == 0.1353352f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_LEFT_SLOW) == 0.1353352f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_RIGHT_FAST) == 0.1353352f);
  BOOST_TEST(useManhattanKernel(ac::DO_NOTHING, ac::CHANGE_RIGHT_SLOW) == 0.1353352f);

  BOOST_TEST(useManhattanKernel(ac::CHANGE_LEFT_SLOW, ac::CHANGE_LEFT_FAST) == 0.1353352f);
  BOOST_TEST(useManhattanKernel(ac::CHANGE_LEFT_SLOW, ac::CHANGE_RIGHT_SLOW) == 0.1353352f);
  BOOST_TEST(useManhattanKernel(ac::CHANGE_LEFT_SLOW, ac::ACCELERATE) == 0.0497870f);
  BOOST_TEST(useManhattanKernel(ac::CHANGE_RIGHT_FAST, ac::DECELERATE) == 0.0497870f);

  BOOST_TEST(useManhattanKernel(ac::CHANGE_LEFT_SLOW, ac::CHANGE_RIGHT_FAST) == 0.0183156f);
  BOOST_TEST(useManhattanKernel(ac::CHANGE_RIGHT_FAST, ac::CHANGE_LEFT_SLOW) == 0.0183156f);
  BOOST_TEST(useManhattanKernel(ac::CHANGE_RIGHT_SLOW, ac::CHANGE_LEFT_FAST) == 0.0183156f);
}

// ----------------------------------------------------------------
// test cases for actions
// ----------------------------------------------------------------

BOOST_AUTO_TEST_CASE(best_action_set) {
  m_moveGrouping = false;
  m_gammaAction  = 0.2f;
  m_cpAction     = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  ActionPtr accelerate      = std::make_shared<Action>(5.f, 2.f);
  ActionPtr changeLeftFast  = std::make_shared<Action>(5.f, 4.f);
  ActionPtr decelerate      = std::make_shared<Action>(-5.f, -2.f);
  ActionPtr changeRightSlow = std::make_shared<Action>(-5.f, -4.f);

  for (auto& agent : node->m_agents) {
    agent.addAvailableAction(accelerate);
    agent.addAvailableAction(changeLeftFast);
    agent.addAvailableAction(decelerate);
    agent.addAvailableAction(changeRightSlow);

    agent.m_actionValues.at(accelerate)      = 950.f;
    agent.m_actionValues.at(changeLeftFast)  = 900.f;
    agent.m_actionValues.at(decelerate)      = 100.f;
    agent.m_actionValues.at(changeRightSlow) = 1000.f;

    agent.m_actionVisits.at(accelerate)      = 20.f;
    agent.m_actionVisits.at(changeLeftFast)  = 20.f;
    agent.m_actionVisits.at(decelerate)      = 20.f;
    agent.m_actionVisits.at(changeRightSlow) = 5.f;
  }
  setBestActionSet(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST(m_bestActionSet.at(idx) == accelerate);
  }
}

BOOST_AUTO_TEST_CASE(best_action_set_random) {
  m_moveGrouping = false;
  m_gammaAction  = 0.2f;
  m_cpAction     = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  ActionPtr accelerate     = std::make_shared<Action>(5.f, 2.f);
  ActionPtr changeLeftFast = std::make_shared<Action>(5.f, 4.f);
  ActionPtr decelerate     = std::make_shared<Action>(-5.f, -2.f);

  for (auto& agent : node->m_agents) {
    agent.addAvailableAction(accelerate);
    agent.addAvailableAction(changeLeftFast);
    agent.addAvailableAction(decelerate);
  }
  setBestActionSet(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST(m_bestActionSet.at(idx) != nullptr);
  }
}

BOOST_AUTO_TEST_CASE(best_action_set_all_values_equal) {
  m_moveGrouping = false;
  m_gammaAction  = 0.2f;
  m_cpAction     = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  ActionPtr accelerate      = std::make_shared<Action>(5.f, 0.5f);
  ActionPtr changeLeftFast  = std::make_shared<Action>(5.f, 1.0f);
  ActionPtr changeRightFast = std::make_shared<Action>(5.f, -1.0f);

  for (auto& agent : node->m_agents) {
    agent.m_actionValues.insert({accelerate, 1000.f});
    agent.m_actionValues.insert({changeLeftFast, 1000.f});
    agent.m_actionValues.insert({changeRightFast, 1000.f});

    agent.m_actionVisits.insert({accelerate, 20.f});
    agent.m_actionVisits.insert({changeLeftFast, 20.f});
    agent.m_actionVisits.insert({changeRightFast, 20.f});
  }
  setBestActionSet(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST(m_bestActionSet.at(idx) == accelerate);
  }
}

BOOST_AUTO_TEST_CASE(best_action_set_with_move_grouping) {
  m_moveGrouping = true;
  m_gammaAction  = 0.2f;
  m_cpAction     = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  ActionPtr accelerate     = std::make_shared<Action>(ActionClass::ACCELERATE, 5.f, 2.f);
  ActionPtr changeLeftFast = std::make_shared<Action>(ActionClass::CHANGE_LEFT_FAST, 5.f, 4.f);
  ActionPtr doNothing      = std::make_shared<Action>(ActionClass::DO_NOTHING, 0.f, 0.f);

  m_bestActionClassSet.clear();

  for (auto& agent : node->m_agents) {
    m_bestActionClassSet.push_back(ActionClass::DO_NOTHING);

    agent.m_actionValues.insert({accelerate, 1000.f});
    agent.m_actionValues.insert({changeLeftFast, 1000.f});
    agent.m_actionValues.insert({doNothing, 500.f});

    agent.m_actionVisits.insert({accelerate, 20.f});
    agent.m_actionVisits.insert({changeLeftFast, 20.f});
    agent.m_actionVisits.insert({doNothing, 5.f});
  }
  setBestActionSet(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST(m_bestActionSet.at(idx) == doNothing);
  }
}

// ----------------------------------------------------------------
// test cases for action classes
// ----------------------------------------------------------------

BOOST_AUTO_TEST_CASE(best_action_class_set) {
  m_gammaActionClass = 1.0f;
  m_cpActionClass    = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  for (auto& agent : node->m_agents) {
    agent.m_actionClassValues.insert({ActionClass::ACCELERATE, 950.f});
    agent.m_actionClassValues.insert({ActionClass::CHANGE_LEFT_FAST, 800.f});
    agent.m_actionClassValues.insert({ActionClass::DECELERATE, 100.f});
    agent.m_actionClassValues.insert({ActionClass::CHANGE_RIGHT_SLOW, 1000.f});

    agent.m_actionClassVisits.insert({ActionClass::ACCELERATE, 20.f});
    agent.m_actionClassVisits.insert({ActionClass::CHANGE_LEFT_FAST, 20.f});
    agent.m_actionClassVisits.insert({ActionClass::DECELERATE, 20.f});
    agent.m_actionClassVisits.insert({ActionClass::CHANGE_RIGHT_SLOW, 5.f});
  }
  setBestActionClass(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST((m_bestActionClassSet.at(idx) == ActionClass::ACCELERATE));
  }
}

BOOST_AUTO_TEST_CASE(best_action_class_set_random) {
  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  ActionPtr accelerate     = std::make_shared<Action>(ActionClass::ACCELERATE);
  ActionPtr changeLeftFast = std::make_shared<Action>(ActionClass::CHANGE_LEFT_FAST);
  ActionPtr decelerate     = std::make_shared<Action>(ActionClass::DECELERATE);

  for (auto& agent : node->m_agents) {
    agent.addAvailableAction(accelerate);
    agent.addAvailableAction(changeLeftFast);
    agent.addAvailableAction(decelerate);
  }
  BOOST_CHECK_NO_THROW(setBestActionClass(node));
}

BOOST_AUTO_TEST_CASE(best_action_class_set_all_values_equal) {
  m_gammaActionClass = 1.0f;
  m_cpActionClass    = 0.5f;

  auto root = std::make_unique<Node>(sOpt().agents);
  auto node = root.get();

  for (auto& agent : node->m_agents) {
    agent.m_actionClassValues.insert({ActionClass::ACCELERATE, 1000.f});
    agent.m_actionClassValues.insert({ActionClass::CHANGE_LEFT_FAST, 1000.f});
    agent.m_actionClassValues.insert({ActionClass::CHANGE_RIGHT, 1000.f});

    agent.m_actionClassVisits.insert({ActionClass::ACCELERATE, 20.f});
    agent.m_actionClassVisits.insert({ActionClass::CHANGE_LEFT_FAST, 20.f});
    agent.m_actionClassVisits.insert({ActionClass::CHANGE_RIGHT, 20.f});
  }
  setBestActionClass(node);

  for (std::size_t idx = 0; idx < node->m_agents.size(); ++idx) {
    BOOST_TEST((m_bestActionClassSet.at(idx) == ActionClass::ACCELERATE));
  }
}

BOOST_AUTO_TEST_SUITE_END()


//
// file radical_atoms.cc
// David Cosgrove
// AstraZeneca
// 15th February 2013
//
// This function takes a molecule and builds a list of those atoms that
// are free radicals, taken from a selected list of elements.

#include <iostream>
#include <map>
#include <vector>

#include <oechem.h>
#include <oedepict.h>

#include <boost/tuple/tuple.hpp>

#include "DACOEMolAtomIndex.H"

using namespace OEChem;
using namespace OEDepict;
using namespace OESystem;

namespace DACLIB {

// *****************************************************************************
void radical_atoms( OEMolBase &mol ,
                    std::vector<OEAtomBase *> &rad_atoms ) {

  static std::map<unsigned int,boost::tuple<int,int,int> > ELECTRONS;
  if( ELECTRONS.empty() ) {
    ELECTRONS[OEElemNo::C] = boost::tuple<int,int,int>( 4 , -1 , -1 );
    ELECTRONS[OEElemNo::N] = boost::tuple<int,int,int>( 5 , 3 , -1 );
    ELECTRONS[OEElemNo::O] = boost::tuple<int,int,int>( 6 , -1 , -1 );
    ELECTRONS[OEElemNo::Si] = boost::tuple<int,int,int>( 4 , -1 , -1 );
    ELECTRONS[OEElemNo::P] = boost::tuple<int,int,int>( 5 , 3 , -1 );
    ELECTRONS[OEElemNo::S] = boost::tuple<int,int,int>( 6 , 4 , 2 );
  }

  for( OEIter<OEAtomBase> atom = mol.GetAtoms() ; atom ; ++atom ) {
    std::map<unsigned int,boost::tuple<int,int,int> >::iterator p = ELECTRONS.find( atom->GetAtomicNum() );
    if( p != ELECTRONS.end() ) {
      bool rad = true;
      int n = p->second.get<0>() + atom->GetValence() - atom->GetFormalCharge();
      if( !(n - 8) ) {
        rad = false;
      }
      if( -1 != p->second.get<1>() ) {
        n = p->second.get<1>() + atom->GetValence() - atom->GetFormalCharge();
        if( !(n - 8) ) {
          rad = false;
        }
      }
      if( -1 != p->second.get<2>() ) {
        n = p->second.get<2>() + atom->GetValence() - atom->GetFormalCharge();
        if( !(n - 8) ) {
          rad = false;
        }
      }
      if( rad ) {
        rad_atoms.push_back( atom );
      }
    }
  }

}

#ifdef NOTYET
// *****************************************************************************
// this one is just for checking the previous one, really.  It is almost
// certainly slower as it has to do a full depiction calculation.
// It's not compiled in by default as it requires an extra library (oedepict)
// that isn't always linked to non-graphical programs.
bool radical_atoms( OEMolBase &mol ) {

  OEPrepareDepiction(mol);

  unsigned int width  = 200;
  unsigned int height = 200;
  OE2DMolDisplayOptions opts(width, height, OEScale::AutoScale);

  OE2DMolDisplay disp(mol, opts);

  for (OEIter<OE2DAtomDisplay> adisp = disp.GetAtomDisplays(); adisp; ++adisp) {
    if( adisp->GetRadical() ) {
      return true;
    }
  }

  return false;

}
#endif

} // EO namespace DACLIB


#include <burst/algorithm/searching/detail/element_position_bitmask_table.hpp>

#include <doctest/doctest.h>

#include <boost/range/numeric.hpp>

#include <array>
#include <bitset>
#include <climits>
#include <iostream>
#include <map>
#include <set>
#include <string>

using char_masks = burst::algorithm::detail::element_position_bitmask_table<std::array<std::bitset<32>, (1 << sizeof(char) * CHAR_BIT)>>;
using integral_masks = burst::algorithm::detail::element_position_bitmask_table<std::map<int, std::bitset<64>>>;

TEST_SUITE("element_position_bitmask_table")
{
    TEST_CASE("Размер таблицы масок для пустого образца равен нулю")
    {
        const auto pattern_masks = char_masks(std::string(""));
        CHECK(pattern_masks.length() == 0ul);
    }

    TEST_CASE("Размер таблицы масок для непустого образца равен длине образца")
    {
        const auto pattern_masks = char_masks(std::string("abcabc"));
        CHECK(pattern_masks.length() == 6);
    }

    TEST_CASE("Биты взведены на тех позициях, на которых элемент встречается в образце")
    {
        const auto pattern_masks = integral_masks{1, 2, 1, 3};
        CHECK(pattern_masks[1] == 0b0101);
        CHECK(pattern_masks[2] == 0b0010);
        CHECK(pattern_masks[3] == 0b1000);
    }

    TEST_CASE("Биты, более старшие, чем длина образца, не взведены")
    {
        const auto pattern = std::string("abc");
        const auto pattern_masks = char_masks(pattern);
        const auto unique_pattern_elements = std::set<char>(pattern.begin(), pattern.end());
        const auto all_ones = ~char_masks::bitmask_type{0b0};

        for (auto element: unique_pattern_elements)
        {
            const auto mask = pattern_masks[element];
            CHECK((mask & all_ones) == mask);
        }
    }

    TEST_CASE("Маска несуществующего в образце элемента — нулевая")
    {
        const auto pattern_masks = char_masks(std::string("qwertyqwerty"));

        CHECK(pattern_masks['a'] == 0b0);
        CHECK(pattern_masks['b'] == 0b0);
    }

    TEST_CASE("Объединение масок для всех элементов образца даёт маску, у которой взведены "
        "все биты, соответствующие образцу")
    {
        const auto pattern = std::string("abac");
        const auto pattern_masks = char_masks(pattern);

        const auto unique_pattern_elements = std::set<char>(pattern.begin(), pattern.end());
        const auto disjunction =
            boost::accumulate(unique_pattern_elements, char_masks::bitmask_type{0b0},
                [& pattern_masks] (auto mask, auto element)
                {
                    return mask | pattern_masks[element];
                });

        CHECK(disjunction == (0b1 << pattern.length()) - 1);
    }
}


#include "extconfig.h"
#include <utility>
#include <Eigen/Cholesky>
#include <cmath>

/*
 * Member function definitions of EncoderHots template class.
 * See tsencoder.h for template class declaration.
 */

template <size_t M, size_t N, size_t O>
EncoderHots<M, N, O>::EncoderHots(const EncoderHotsConfig& config) :
m_skip_order_counter(0),
m_A(decltype(m_A)::Ones()), /* elements in last column of time stamp matrix are always 1 */
m_P(decltype(m_P)::Zero()),
m_B(decltype(m_B)::Zero()),
m_T(decltype(m_T)::Ones()), /* last element of time vector is always 1 */
m_t0(0),
m_alpha(0.0),
m_config(config),
m_count(0),
m_tc(0.0),
m_state(state_t::STOP),
m_index(index_t::NONE),
m_iqhandler(nullptr) {
    /*
     * EXT interrupts will trigger too often for high resolution encoders.
     * TODO: consider using ICU to record timestamps
     * TODO: determine  better count/rev limit
     */
    if (config.counts_per_rev) {
        chSysHalt("Encoder counts per revolution too high");
    }
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::start() {
    chSysLock();
    osalDbgAssert((extp->state == EXT_STOP) || (extp->state == EXT_ACTIVE),
            "invalid state");
    if (extp->state == EXT_STOP) {
        extStartI(extp);
    }
    extChannelEnableSetModeI(extp, m_config.a, EXT_CH_MODE_BOTH_EDGES,
                             ab_callback, this);
    extChannelEnableSetModeI(extp, m_config.b, EXT_CH_MODE_BOTH_EDGES,
                             ab_callback, this);
    if (m_config.z == PAL_NOLINE) {
        m_index = index_t::NONE;
    } else {
        extChannelEnableSetModeI(extp, m_config.z, EXT_CH_MODE_RISING_EDGE,
                                 index_callback, this);
        m_index = index_t::NOTFOUND;
    }
    m_state = state_t::READY;
    chSysUnlock();
    m_iqhandler.start();
    chVTSet(&m_event_deadline_timer, m_event_deadline,
            add_event_callback, static_cast<void*>(this));
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::stop() {
    chVTReset(&m_event_deadline_timer);
    m_iqhandler.stop();

    chSysLock();
    m_state = state_t::STOP;
    m_index = index_t::NONE;

    extChannelDisableClearModeI(extp, PAL_PAD(m_config.a));
    extChannelDisableClearModeI(extp, PAL_PAD(m_config.b));
    if (m_config.z != PAL_NOLINE) {
        extChannelDisableClearModeI(extp, PAL_PAD(m_config.z));
    }
    extStopIfChannelsDisabledI(extp);
    chSysUnlock();
}

template <size_t M, size_t N, size_t O>
typename EncoderHots<M, N, O>::state_t EncoderHots<M, N, O>::state() const {
    return m_state;
}

template <size_t M, size_t N, size_t O>
typename EncoderHots<M, N, O>::index_t EncoderHots<M, N, O>::index() const volatile {
    return m_index;
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::update_polynomial_fit() {
    /*
     * Redefine oldest event to be at time zero. Scale time so that difference
     * in time from oldest to newest event is equal to 1.
     *
     * If the difference A(N - 1, 1) - A(0, 1) is too large, the value will be
     * nonsensical due to uint rolling back to zero. The difference must also
     * be less than int32_t_max as these values will be cast from uint32_t to
     * int32_t.
     */
    m_iqhandler.wait();
    auto events = m_iqhandler.circular_buffer();
    size_t event_index = m_iqhandler.index();
    size_t newest_index = event_index - 1;
    if (event_index == 0) {
        newest_index = N - 1;
    }
    m_t0 = events[event_index].first;
    m_alpha = 1.0f / (events[newest_index].first - m_t0);

    for (unsigned int i = 0; i < N; ++i) {
        m_A(i, M - 1) = m_alpha * (events[(event_index + i) % N].first - m_t0);
        m_B(i) = events[(event_index + i) % N].second;
    }
    m_iqhandler.signal();

    for (unsigned int i = 0; i < M - 1; ++i) {
        m_A.col(M - 2 - i) = m_A.col(M - 1 - i).cwiseProduct(m_A.col(M - 1));
    }
    m_P.noalias() = (m_A.transpose() * m_A).ldlt().solve(
            (m_A.transpose() * m_B.template cast<polycoeff_t>()));
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::update_estimate_time(rtcnt_t tc) {
    m_tc = tc;
    polycoeff_t tcf = m_alpha * static_cast<polycoeff_t>(tc - m_t0);
    chDbgAssert(tcf > 1.0f, "tcf should always be ahead of last prediction");
    m_T(M - 1) = tcf;
    for (unsigned int i = 0; i < M - 1; ++i) {
        m_T(M - 2 - i) = m_T(M - 1 - i) * tcf;
    }
}

template <size_t M, size_t N, size_t O>
polycoeff_t EncoderHots<M, N, O>::position() const {
    polycoeff_t x = m_P.transpose() * m_T;
    polycoeff_t count = static_cast<polycoeff_t>(m_count);
    if (std::abs(x - count) > 1.0f) {
        return count;
    }
    return x;
}

// TODO: Validity of velocity and acceleration for large error in estimated
// position (i.e. position is adjusted if a large amount of time passes without
// an encoder event).
template <size_t M, size_t N, size_t O>
polycoeff_t EncoderHots<M, N, O>::velocity() const {
    Eigen::Matrix<hotscnt_t, M, 1> velocity_coefficients;
    for (unsigned int i = 0; i < M; ++i) {
        velocity_coefficients(i) = M - i;
    }
    return velocity_coefficients.template cast<polycoeff_t>().cwiseProduct(
            m_P.template head<M>()).transpose() * m_T.template tail<M>();
}

template <size_t M, size_t N, size_t O>
polycoeff_t EncoderHots<M, N, O>::acceleration() const {
    Eigen::Matrix<hotscnt_t, M - 1 , 1> acceleration_coefficients;
    for (unsigned int i = 0; i < M - 1; ++i) {
        acceleration_coefficients(i) = (M - i) * (M - i - 1);
    }
    return acceleration_coefficients.template cast<polycoeff_t>().cwiseProduct(
            m_P.template head<M - 1>()).transpose() * m_T.template tail<M - 1>();
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::ab_callback(EXTDriver* extp, expchannel_t channel) {
    (void)extp;
    chSysLockFromISR();
    auto enc = static_cast<EncoderHots<M, N, O>*>(extGetChannelCallbackObject(channel));
    if (((channel == PAL_PAD(enc->m_config.a)) &&
                (palReadLine(enc->m_config.a) != palReadLine(enc->m_config.b))) ||
        ((channel == PAL_PAD(enc->m_config.b)) &&
                (palReadLine(enc->m_config.a) == palReadLine(enc->m_config.b)))) {
        ++enc->m_count;
    } else {
        --enc->m_count;
    }
    chVTSetI(&enc->m_event_deadline_timer, enc->m_event_deadline,
            add_event_callback, static_cast<void*>(enc));
    event_t event = std::make_pair(chSysGetRealtimeCounterX(), enc->m_count);
    auto iqh = enc->m_iqhandler;
    if (enc->m_skip_order_counter++ != 0) {
        /* need to overwrite previous sample */
        if (iqh.m_buffer_index-- == 0) {
            iqh.m_buffer_index = iqh.m_circular_buffer.size() - 1;
        }
    }
    iqh.insertI(&event);
    if (enc->m_skip_order_counter > O) {
        enc->m_skip_order_counter = 0;
    }
    chSysUnlockFromISR();
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::index_callback(EXTDriver* extp, expchannel_t channel) {
    chSysLockFromISR();
    auto enc = static_cast<EncoderHots<M, N, O>*>(extGetChannelCallbackObject(channel));
    enc->m_count = enc->m_config.z_count;
    enc->m_index = index_t::FOUND;
    extChannelDisableClearModeI(extp, PAL_PAD(enc->m_config.z));
    chSysUnlockFromISR();
}

template <size_t M, size_t N, size_t O>
void EncoderHots<M, N, O>::add_event_callback(void* p) {
    chSysLockFromISR();
    auto enc = static_cast<EncoderHots<M, N, O>*>(p);
    chVTSetI(&enc->m_event_deadline_timer, enc->m_event_deadline,
            add_event_callback, p);
    event_t event = std::make_pair(chSysGetRealtimeCounterX(), enc->m_count);
    enc->m_iqhandler.insertI(&event);
    enc->m_skip_order_counter = 0; /* don't overwrite on next edge */
    chSysUnlockFromISR();
}


// This is a demo code for using a SSD model to do detection.
// The code is modified from examples/cpp_classification/classification.cpp.
// Usage:
//    ssd_detect [FLAGS] model_file weights_file list_file
//
// where model_file is the .prototxt file defining the network architecture, and
// weights_file is the .caffemodel file containing the network parameters, and
// list_file contains a list of image files with the format as follows:
//    folder/img1.JPEG
//    folder/img2.JPEG
// list_file can also contain a list of video files with the format as follows:
//    folder/video1.mp4
//    folder/video2.mp4
//
#include <caffe/caffe.hpp>
#include <caffe/util/bbox_util.hpp>
#ifdef USE_OPENCV
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#endif  // USE_OPENCV
#include <algorithm>
#include <iomanip>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <sstream>
#include <limits>
#include <cmath>

#include <regex>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

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

namespace fs = boost::filesystem;

#ifdef USE_OPENCV
using namespace caffe;  // NOLINT(build/namespaces)

#ifdef XOPENME
#include <xopenme.h>
#endif

class Detector {
 public:
  Detector(const string& model_file,
           const string& weights_file,
           const string& mean_file,
           const string& mean_value);

  std::vector<vector<float> > Detect(const cv::Mat& img);

 private:
  void SetMean(const string& mean_file, const string& mean_value);

  void WrapInputLayer(std::vector<cv::Mat>* input_channels);

  void Preprocess(const cv::Mat& img,
                  std::vector<cv::Mat>* input_channels);

 private:
  shared_ptr<Net<float> > net_;
  cv::Size input_geometry_;
  int num_channels_;
  cv::Mat mean_;
};

Detector::Detector(const string& model_file,
                   const string& weights_file,
                   const string& mean_file,
                   const string& mean_value) {
#ifdef CPU_ONLY
  Caffe::set_mode(Caffe::CPU);
#else
  Caffe::set_mode(Caffe::GPU);
#endif

  /* Load the network. */
  net_.reset(new Net<float>(model_file, TEST));
  net_->CopyTrainedLayersFrom(weights_file);

  CHECK_EQ(net_->num_inputs(), 1) << "Network should have exactly one input.";
  CHECK_EQ(net_->num_outputs(), 1) << "Network should have exactly one output.";

  Blob<float>* input_layer = net_->input_blobs()[0];
  num_channels_ = input_layer->channels();
  CHECK(num_channels_ == 3 || num_channels_ == 1)
    << "Input layer should have 1 or 3 channels.";
  input_geometry_ = cv::Size(input_layer->width(), input_layer->height());

  /* Load the binaryproto mean file. */
  SetMean(mean_file, mean_value);
}

std::vector<vector<float> > Detector::Detect(const cv::Mat& img) {
  Blob<float>* input_layer = net_->input_blobs()[0];
  input_layer->Reshape(1, num_channels_,
                       input_geometry_.height, input_geometry_.width);
  /* Forward dimension change to all layers. */
  net_->Reshape();

  std::vector<cv::Mat> input_channels;
  WrapInputLayer(&input_channels);

  Preprocess(img, &input_channels);

  net_->Forward();

  /* Copy the output layer to a std::vector */
  Blob<float>* result_blob = net_->output_blobs()[0];
  const float* result = result_blob->cpu_data();
  const int num_det = result_blob->height();
  vector<vector<float> > detections;
  for (int k = 0; k < num_det; ++k) {
    if (result[0] == -1) {
      // Skip invalid detection.
      result += 7;
      continue;
    }
    vector<float> detection(result, result + 7);
    detections.push_back(detection);
    result += 7;
  }
  return detections;
}

/* Load the mean file in binaryproto format. */
void Detector::SetMean(const string& mean_file, const string& mean_value) {
  cv::Scalar channel_mean;
  if (!mean_file.empty()) {
    CHECK(mean_value.empty()) <<
      "Cannot specify mean_file and mean_value at the same time";
    BlobProto blob_proto;
    ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);

    /* Convert from BlobProto to Blob<float> */
    Blob<float> mean_blob;
    mean_blob.FromProto(blob_proto);
    CHECK_EQ(mean_blob.channels(), num_channels_)
      << "Number of channels of mean file doesn't match input layer.";

    /* The format of the mean file is planar 32-bit float BGR or grayscale. */
    std::vector<cv::Mat> channels;
    float* data = mean_blob.mutable_cpu_data();
    for (int i = 0; i < num_channels_; ++i) {
      /* Extract an individual channel. */
      cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
      channels.push_back(channel);
      data += mean_blob.height() * mean_blob.width();
    }

    /* Merge the separate channels into a single image. */
    cv::Mat mean;
    cv::merge(channels, mean);

    /* Compute the global mean pixel value and create a mean image
     * filled with this value. */
    channel_mean = cv::mean(mean);
    mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
  }
  if (!mean_value.empty()) {
    CHECK(mean_file.empty()) <<
      "Cannot specify mean_file and mean_value at the same time";
    stringstream ss(mean_value);
    vector<float> values;
    string item;
    while (getline(ss, item, ',')) {
      float value = std::atof(item.c_str());
      values.push_back(value);
    }
    CHECK(values.size() == 1 || values.size() == num_channels_) <<
      "Specify either 1 mean_value or as many as channels: " << num_channels_;

    std::vector<cv::Mat> channels;
    for (int i = 0; i < num_channels_; ++i) {
      /* Extract an individual channel. */
      cv::Mat channel(input_geometry_.height, input_geometry_.width, CV_32FC1,
          cv::Scalar(values[i]));
      channels.push_back(channel);
    }
    cv::merge(channels, mean_);
  }
}

/* Wrap the input layer of the network in separate cv::Mat objects
 * (one per channel). This way we save one memcpy operation and we
 * don't need to rely on cudaMemcpy2D. The last preprocessing
 * operation will write the separate channels directly to the input
 * layer. */
void Detector::WrapInputLayer(std::vector<cv::Mat>* input_channels) {
  Blob<float>* input_layer = net_->input_blobs()[0];

  int width = input_layer->width();
  int height = input_layer->height();
  float* input_data = input_layer->mutable_cpu_data();
  for (int i = 0; i < input_layer->channels(); ++i) {
    cv::Mat channel(height, width, CV_32FC1, input_data);
    input_channels->push_back(channel);
    input_data += width * height;
  }
}

void Detector::Preprocess(const cv::Mat& img,
                            std::vector<cv::Mat>* input_channels) {
  /* Convert the input image to the input image format of the network. */
  cv::Mat sample;
  if (img.channels() == 3 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
  else if (img.channels() == 4 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
  else if (img.channels() == 4 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
  else if (img.channels() == 1 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
  else
    sample = img;

  cv::Mat sample_resized;
  if (sample.size() != input_geometry_)
    cv::resize(sample, sample_resized, input_geometry_);
  else
    sample_resized = sample;

  cv::Mat sample_float;
  if (num_channels_ == 3)
    sample_resized.convertTo(sample_float, CV_32FC3);
  else
    sample_resized.convertTo(sample_float, CV_32FC1);

  cv::Mat sample_normalized;
  cv::subtract(sample_float, mean_, sample_normalized);

  /* This operation will write the separate BGR planes directly to the
   * input layer of the network because it is wrapped by the cv::Mat
   * objects in input_channels. */
  cv::split(sample_normalized, *input_channels);

  CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
        == net_->input_blobs()[0]->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";
}

void x_clock_start(int timer) {
#ifdef XOPENME
  xopenme_clock_start(timer);
#endif
}

void x_clock_end(int timer) {
#ifdef XOPENME
  xopenme_clock_end(timer);
#endif
}

double x_get_time(int timer) {
#ifdef XOPENME
  return xopenme_get_timer(timer);
#else
  return 0;
#endif
}

DEFINE_string(mean_file, "",
    "The mean file used to subtract from the input image.");
DEFINE_string(mean_value, "104,117,123",
    "If specified, can be one value or can be same as image channels"
    " - would subtract from the corresponding channel). Separated by ','."
    "Either mean_file or mean_value should be provided, not both.");
DEFINE_string(out_file, "",
    "If provided, store the detection results in the out_file.");
DEFINE_double(confidence_threshold, 0.5,
    "Only store detections with score higher than the threshold.");
DEFINE_double(iou_threshold, 0.7,
    "Threshold for IoU metric to determine false positives.");
DEFINE_bool(continuous, false,
    "Run continuously for each image from the dataset");
DEFINE_bool(webcam, false,
    "Run continuously for webcam snapshots");
DEFINE_string(labelmap_file, "",
    "If provided, should point to the file with labels.");
DEFINE_string(label_dir, "",
    "Directory with image labels (the ground truth data).");
DEFINE_string(out_images_dir, "out",
    "In continuous mode, puts processed images into this directory (recreates the directory first).");
DEFINE_int32(webcam_max_image_count, 5000,
    "Maximum image count generated in the webcam mode.");

enum Category {
  UNASSIGNED = -2,
  UNKNOWN = -1,
  EASY = 0,
  MODERATE = 1,
  HARD = 2
};

static const float MIN_HEIGHT[] = { 40, 25, 25 };
static const float MAX_OCCLUSION[] = {0, 1, 2};
static const float MAX_TRUNCATION[] = { 0.15, 0.3, 0.5 };

int float_precision() {
  const char* s = getenv("FLOAT_PRECISION");
  if (NULL == s || strcmp(s, "") == 0) {
    return 3;
  }
  return atoi(s);
}

void detect_single_image(Detector& detector, const string& file, std::ostream& out) {
  long ct_repeat=0;
  long ct_repeat_max=1;
  int ct_return=0;

  if (getenv("CT_REPEAT_MAIN")!=NULL) ct_repeat_max=atol(getenv("CT_REPEAT_MAIN"));

  x_clock_start(1);
  cv::Mat img = cv::imread(file, -1);
  x_clock_end(1);
  CHECK(!img.empty()) << "Unable to decode image " << file;

  x_clock_start(2);
  std::vector<vector<float> > detections = detector.Detect(img);
  x_clock_end(2);

  /* Print the detection results. */
  for (int i = 0; i < detections.size(); ++i) {
    const vector<float>& d = detections[i];
    // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
    CHECK_EQ(d.size(), 7);
    const float score = d[2];
    if (score >= FLAGS_confidence_threshold) {
      out << file << " ";
      out << static_cast<int>(d[1]) << " ";
      out << score << " ";
      out << static_cast<int>(d[3] * img.cols) << " ";
      out << static_cast<int>(d[4] * img.rows) << " ";
      out << static_cast<int>(d[5] * img.cols) << " ";
      out << static_cast<int>(d[6] * img.rows) << std::endl;
    }
  }
}

std::map<int, std::string> read_labelmap(const std::string& file) {
  static const std::regex label_regex("^\\s*label: (\\d+)\\s*$");
  static const std::regex display_name_regex("^\\s*display_name: \"([^\"]+)\"\\s*$");

  std::map<int, std::string> labelmap;

  std::ifstream f(file);
  std::string line;
  int label = -1;
  std::string display_name = "";
  while (std::getline(f, line)) {
    std::smatch match;
    if (std::regex_search(line, match, label_regex)) {
      label = std::stoi(match[1]);
    } else if (std::regex_search(line, match, display_name_regex)) {
      display_name = match[1];
    }
    if (0 <= label && !display_name.empty()) {
      labelmap[label] = display_name;
      label = -1;
      display_name = "";
    }
  }

  return labelmap;
}

std::map<std::string, int> flip_labelmap(const std::map<int, std::string>& labelmap) {
  std::map<std::string, int> ret;
  for (auto it = labelmap.begin(); it != labelmap.end(); ++it) {
    ret[it->second] = it->first;
  }
  return ret;
}

cv::Scalar get_color(const std::string& label) {
  static const std::map<std::string, cv::Scalar> colors = {
    {"background", CV_RGB(0, 0, 0)},
    {"aeroplane", CV_RGB(128, 0, 0)},
    {"bicycle", CV_RGB(0, 191, 255)},
    {"bird", CV_RGB(128, 128, 0)},
    {"boat", CV_RGB(0, 0, 128)},
    {"bottle", CV_RGB(128, 0, 128)},
    {"bus", CV_RGB(0, 128, 128)},
    {"car", CV_RGB(255, 191, 0)},
    {"cat", CV_RGB(64, 0, 0)},
    {"chair", CV_RGB(192, 0, 0)},
    {"cow", CV_RGB(64, 128, 0)},
    {"diningtable", CV_RGB(192, 128, 0)},
    {"dog", CV_RGB(64, 0, 128)},
    {"horse", CV_RGB(192, 0, 128)},
    {"motorbike", CV_RGB(64, 128, 128)},
    {"person", CV_RGB(255, 0, 191)},
    {"pottedplant", CV_RGB(0, 64, 0)},
    {"sheep", CV_RGB(128, 64, 0)},
    {"sofa", CV_RGB(0, 192, 0)},
    {"train", CV_RGB(128, 192, 0)},
    {"tvmonitor", CV_RGB(0, 64, 128)}
  };
  static const cv::Scalar default_color = CV_RGB(255, 255, 255);

  auto it = colors.find(label);
  return it == colors.end() ? default_color : it->second;
}

fs::path label_file(const fs::path& label_dir, const std::string& image_file_name) {
  if (label_dir.empty()) {
    return fs::path("");
  }
  fs::path ret = label_dir / image_file_name;
  ret.replace_extension(".txt");
  return ret;
}

static bool eq(float a, float b) {
  return std::fabs(a - b) < std::numeric_limits<float>::epsilon();
}

static bool gr(float a, float b) {
  return a - b > (std::fabs(a) < std::fabs(b) ? std::fabs(b) : std::fabs(a)) * std::numeric_limits<float>::epsilon();
}

static bool grEq(float a, float b) {
  return eq(a, b) || gr(a, b);
}

static const int DONTCARE_LABEL_ID = -2;
static const std::string DONTCARE_LABEL = "dontcare";

struct object {
  std::string label;
  int label_id = -1;
  float score = 1;
  float xmin = 0;
  float ymin = 0;
  float xmax = 0;
  float ymax = 0;
  bool assigned = false;
  float occlusion = 0;
  float truncation = 0;
  int assigned_difficulty = UNASSIGNED;

  bool empty() const {
    return label.empty() || -1 == label_id;
  }

  bool dontcare() const {
    return label_id == DONTCARE_LABEL_ID || label == DONTCARE_LABEL;
  }

  NormalizedBBox to_bbox() const {
    NormalizedBBox ret;
    ret.set_xmin(xmin);
    ret.set_ymin(ymin);
    ret.set_xmax(xmax);
    ret.set_ymax(ymax);
    ret.set_label(label_id);
    return ret;
  }

  float height() const {
    return ymax - ymin;
  }

  bool should_ignore(int difficulty) const {
    return grEq(occlusion, MAX_OCCLUSION[difficulty]) || grEq(truncation, MAX_TRUNCATION[difficulty]) || grEq(MIN_HEIGHT[difficulty], height());
  }

  int difficulty() const {
    if (!should_ignore(EASY)) {
      return EASY;
    }
    if (!should_ignore(MODERATE)) {
      return MODERATE;
    }
    if (!should_ignore(HARD)) {
      return HARD;
    }
    return UNKNOWN;
  }
};

float iou(const object& o1, const object& o2) {
  return JaccardOverlap(o1.to_bbox(), o2.to_bbox(), false);
}

bool care(const object& o, const std::vector<object>& dontcare) {
  const float threshold = "car" == o.label ? 0.7 : 0.5;
  for (auto dc : dontcare) {
    if (grEq(iou(o, dc), threshold)) {
      return false;
    }
  }
  return true;
}

struct Eval {
  int tp = 0;
  int all_gt = 0;
};

Eval eval_boxes(std::vector<object>& expected, std::vector<object>& recognized, const std::string& label, const int difficulty) {
  std::vector<object*> gt;
  for (object& o : expected) {
    if (o.label == label && o.difficulty() == difficulty) {
      gt.push_back(&o);
    }
  }
  std::vector<object*> rec;
  for (object& o : recognized) {
    if ((UNKNOWN == difficulty && o.label == label) ||
        (UNKNOWN != difficulty && o.label == label && UNASSIGNED == o.assigned_difficulty && grEq(o.height(), MIN_HEIGHT[difficulty]))
    ) {
      rec.push_back(&o);
    }
  }
  //std::cout << "Objects with difficulty " << difficulty << ": rec=" << rec.size() << ", gt=" << gt.size() << std::endl;
  std::vector<bool> assigned_rec(rec.size());
  std::vector<bool> assigned_gt(gt.size());
  Eval ret;
  ret.all_gt = gt.size();
  for (int r_index = 0; r_index < rec.size(); ++r_index) {
    object* r_box = rec[r_index];
    float threshold = "car" == r_box->label ? 0.7 : 0.5;
    for (int gt_index = 0; gt_index < gt.size(); ++gt_index) {
      if (assigned_gt[gt_index]) {
        continue;
      }
      object* gt_box = gt[gt_index];
      if (grEq(iou(*r_box, *gt_box), threshold)) {
        assigned_rec[r_index] = true;
        assigned_gt[gt_index] = true;
        r_box->assigned_difficulty = difficulty;
        break;
      }
    }
    if (assigned_rec[r_index]) {
      ret.tp++;
    }
  }
  return ret;
}

struct Stat {
  float avg = 0;
  float min = 0;
  float max = 0;
  int count = 0;

  void clear() {
    avg = 0;
    min = 0;
    max = 0;
    count = 0;
  }

  void add(float v) {
    ++count;
    avg = (avg*(count - 1) + v)/count;
    min = 0 == min || gr(min, v) ? v : min;
    max = 0 == max || gr(v, max) ? v : max;
  }

  void addIf(float v, bool condition) {
    if (!eq(v, 0) || condition) {
      add(v);
    }
  }
};

float calc_mAP(const std::map<std::string, std::vector<Stat>>& avg_precision) {
  float s = 0;
  int count = 0;
  for (const auto& e : avg_precision) {
    for (const auto& stat: e.second) {
      if (0 < stat.count) {
        s += stat.avg;
        ++count;
      }
    }
  }
  return 0 == count ? 0 : s / static_cast<float>(count);
}

float safe_div(int all_rec, int tp) {
  return 0 == all_rec ? 0 : static_cast<float>(tp) / static_cast<float>(all_rec);
}

object parse_ground_truth(const std::string& line, const std::map<std::string, int>& reverse_labelmap) {
  static const std::map<std::string, std::string> label_aliases = {
    {"pedestrian", "person"},
    {"cyclist", "bicycle"},
    {"motorcycle", "motorbike"}
  };
  object ret;
  std::string& label = ret.label;
  std::istringstream iss(line);
  float _;
  iss >> label >> _ >> _ >> _ >> ret.xmin >> ret.ymin >> ret.xmax >> ret.ymax;
  if (iss) {
    boost::algorithm::to_lower(label);
    if (DONTCARE_LABEL == label) {
      ret.label_id = DONTCARE_LABEL_ID;
    } else {
      auto it = label_aliases.find(label);
      if (it != label_aliases.end()) {
        label = it->second;
      }
      auto it2 = reverse_labelmap.find(label);
      ret.label_id = it2 == reverse_labelmap.end() ? -1 : it2->second;
    }
  }
  return ret;
}

object parse_detections(const cv::Mat& img, const std::vector<float>& d, const std::map<int, std::string>& labelmap) {
  // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
  CHECK_EQ(d.size(), 7);
  object ret;
  ret.score = d[2];
  if (ret.score < FLAGS_confidence_threshold) {
    // don't bother anymore
    return ret;
  }
  ret.label_id = static_cast<int>(d[1]);
  auto label_it = labelmap.find(ret.label_id);
  ret.label = label_it == labelmap.end() ? "" : label_it->second;
  boost::algorithm::to_lower(ret.label);
  ret.xmin = static_cast<int>(d[3] * img.cols);
  ret.ymin = static_cast<int>(d[4] * img.rows);
  ret.xmax = static_cast<int>(d[5] * img.cols);
  ret.ymax = static_cast<int>(d[6] * img.rows);
  return ret;
}

void read_label_file(const fs::path& file, const std::map<std::string, int>& reverse_labelmap, std::vector<object>& gt, std::vector<object>& dontcare) {
  std::ifstream f(file.string());
  std::string line;
  while (std::getline(f, line)) {
    object o = parse_ground_truth(line, reverse_labelmap);
    if (o.dontcare()) {
      dontcare.push_back(o);
    } else if (!o.empty()) {
      gt.push_back(o);
    }
  }
}

void draw_rect(cv::Mat& img, const object& o, const cv::Scalar& color, bool bottom_label = true) {
  cv::rectangle(img, cv::Point(o.xmin, o.ymin), cv::Point(o.xmax, o.ymax), color, 1);

  const auto font = cv::FONT_HERSHEY_SIMPLEX;
  const float scale = 0.3;
  const int thickness = 1;
  int baseline = 0;
  cv::Size text_size = cv::getTextSize(o.label, font, scale, thickness, &baseline);
  const cv::Point text_point(o.xmin, bottom_label ? o.ymax : o.ymin);
  cv::rectangle(img, text_point, text_point + cv::Point(text_size.width + 2, -text_size.height - 4), color, CV_FILLED);
  cv::putText(img, o.label, text_point + cv::Point(1, -2), font, scale, CV_RGB(255, 255, 255), thickness);
}

void draw_rect(cv::Mat& img, const object& o) {
  draw_rect(img, o, get_color(o.label));
}

void count_object(const object& o, std::map<std::string, int>& counter_map) {
  auto it = counter_map.find(o.label);
  if (it == counter_map.end()) {
    counter_map[o.label] = 1;
  } else {
    it->second++;
  }
}

void print_counter_map(std::ostream& out, std::map<std::string, int>& counter_map, const std::string& prefix) {
  for (auto it = counter_map.begin(); it != counter_map.end(); ++it) {
    out << prefix << " " << it->first << ": " << it->second << std::endl;
  }
}

void print_objects(std::ostream& out, const std::vector<object>& objects, const std::string& prefix) {
  for (auto o : objects) {
    out << prefix << " " << o.label << ": "
      << o.xmin << " " << o.ymin << " " << o.xmax << " " << o.ymax << " " << o.score << std::endl;
  }
}

bool interrupt_requested() {
  const char* finisher = getenv("FINISHER_FILE");
  if (NULL == finisher || strcmp(finisher, "") == 0) {
    return false;
  }
  return fs::exists(finisher);
}

std::string skip_files_including() {
  const char* f = getenv("SKIP_FILES_INCLUDING");
  return NULL == f || strcmp(f, "") == 0 ? "" : f;
}

struct detect_context {
  Detector* detector;
  std::ostream* out;
  fs::path out_dir;
  fs::path label_dir;
  std::map<int, std::string> labelmap;
  std::map<std::string, int> reverse_labelmap;
  std::map<std::string, std::vector<Stat>> avg_precision;
};

static int count_label(const std::vector<object>& objects, const std::string& label) {
  int ret = 0;
  for (const auto& o : objects) {
    if (o.label == label) {
      ++ret;
    }
  }
  return ret;
}

void detect_img(detect_context& ctx, cv::Mat& orig_img, const std::string& filename, const fs::path& original_path = "") {
  static const cv::Scalar ground_truth_color = CV_RGB(200, 200, 200);

  const int timer = 2;
  x_clock_start(timer);
  std::vector<vector<float> > detections = ctx.detector->Detect(orig_img);
  x_clock_end(timer);

  std::string out_file = fs::absolute((ctx.out_dir / filename)).string();
  std::string boxed_out_file = fs::absolute((ctx.out_dir / ("boxed_" + filename))).string();
  std::map<std::string, int> recognized;
  std::map<std::string, int> expected;

  // read expected results
  std::vector<object> ground_truth;
  std::vector<object> dontcare;
  read_label_file(label_file(ctx.label_dir, filename), ctx.reverse_labelmap, ground_truth, dontcare);
  cv::Mat boxed_img = orig_img.clone();
  for (auto const& o: ground_truth) {
    draw_rect(boxed_img, o, ground_truth_color, false);
    count_object(o, expected);
  }

  std::vector<object> recognized_objects;
  for (int i = 0; i < detections.size(); ++i) {
    object o = parse_detections(orig_img, detections[i], ctx.labelmap);
    if (o.score >= FLAGS_confidence_threshold) {
      count_object(o, recognized);
      recognized_objects.push_back(o);
      draw_rect(boxed_img, o);
    }
  }
  CHECK(cv::imwrite(out_file, orig_img)) << "Failed to write file " << out_file;
  CHECK(cv::imwrite(boxed_out_file, boxed_img)) << "Failed to write file " << boxed_out_file;
  *ctx.out << "File: " << out_file << std::endl;
  if (!original_path.empty()) {
    *ctx.out << "Original file: " << original_path.string() << std::endl;
  }
  *ctx.out << "Duration: " << x_get_time(timer) << " sec" << std::endl;
  print_counter_map(*ctx.out, recognized, "Recognized");
  print_counter_map(*ctx.out, expected, "Expected");
  print_objects(*ctx.out, recognized_objects, "Detection");
  print_objects(*ctx.out, ground_truth, "Ground truth");

  std::vector<object> filtered_recognized;
  std::copy_if(recognized_objects.begin(), recognized_objects.end(), std::back_inserter(filtered_recognized), [&](const object& o){ return care(o, dontcare); });
  std::vector<object> filtered_expected;
  std::copy_if(ground_truth.begin(), ground_truth.end(), std::back_inserter(filtered_expected), [&](const object& o){ return care(o, dontcare); });

  for (const auto& e : ctx.labelmap) {
    const std::string& k = e.second;
    int all_rec = count_label(filtered_recognized, k);
    int all_gt = count_label(filtered_expected, k);

    bool report = 0 != all_rec || 0 != all_gt;  // don't report not found and actually unexpected labels, but still count them for mAP

    eval_boxes(filtered_expected, filtered_recognized, k, UNKNOWN);
    auto easy = eval_boxes(filtered_expected, filtered_recognized, k, EASY);
    auto mod = eval_boxes(filtered_expected, filtered_recognized, k, MODERATE);
    auto hard = eval_boxes(filtered_expected, filtered_recognized, k, HARD);
    int tp = easy.tp + mod.tp + hard.tp;
    int fp = all_rec - tp;
    if (report) {
      *ctx.out << "True positive " << k << ": " << easy.tp << " easy, " << mod.tp << " moderate, " << hard.tp << " hard" << std::endl;
      *ctx.out << "False positive " << k << ": " << fp << std::endl;
    }

    std::vector<float> precision = {
      safe_div(easy.tp + fp, easy.tp),
      safe_div(mod.tp + fp, mod.tp),
      safe_div(hard.tp + fp, hard.tp)
    };

    float recall = 0;
    if (0 == all_gt) {
      recall = 0 == all_rec ? 1 : 0;
    } else {
      recall = static_cast<float>(tp) / static_cast<float>(all_gt);
    }

    if (report) {
      *ctx.out << "Precision " << k << ": " << precision[EASY] << " easy, " << precision[MODERATE] << " moderate, " << precision[HARD] << " hard" << std::endl;
      *ctx.out << "Recall " << k << ": " << recall << std::endl;
    }

    auto& ap = ctx.avg_precision[k];
    ap[EASY].addIf(precision[EASY], 0 < easy.all_gt);
    ap[MODERATE].addIf(precision[MODERATE], 0 < mod.all_gt);
    ap[HARD].addIf(precision[HARD], 0 < hard.all_gt);
    if (report) {
      *ctx.out << "Rolling AP " << k << ": " << ap[EASY].avg << " easy, " << ap[MODERATE].avg << " moderate, " << ap[HARD].avg << " hard" << std::endl;
    }
  }
  *ctx.out << "Rolling mAP: " << calc_mAP(ctx.avg_precision) << std::endl;
  *ctx.out << std::endl;
  ctx.out->flush();
}

void detect_continuously(detect_context& ctx, const string& dir) {
  fs::directory_iterator end_iter;
  std::vector<fs::path> paths;
  for (fs::directory_iterator dir_iter(dir) ; dir_iter != end_iter ; ++dir_iter) {
    if (!fs::is_regular_file(dir_iter->status())) {
      // skip non-images
      continue;
    }
    paths.push_back(dir_iter->path());
  }
  std::sort(paths.begin(), paths.end());

  std::string start = skip_files_including();
  if (!start.empty()) {
    fs::path start_path(start);
    auto begin = paths.begin();
    auto end = paths.end();
    auto it = std::find(begin, end, start_path);
    if (it != begin && it != end) {
      paths.erase(begin, ++it);
    }
  }

  for (const auto& p : paths) {
    if (interrupt_requested()) {
      return;
    }
    std::string file = p.string();
    cv::Mat img = cv::imread(file, -1);
    CHECK(!img.empty()) << "Unable to decode image " << file;
    detect_img(ctx, img, p.filename().string(), p);
  }
}

static boost::atomic<cv::Mat*> camera_frame(NULL);
static boost::atomic<bool> camera_ok(true);
static boost::atomic<bool> stop_camera(false);

void camera_reader(int input_device) {
  cv::VideoCapture cap(input_device);
  while (!stop_camera) {
    cv::Mat* img = new cv::Mat;
    if (!cap.read(*img)) {
      camera_ok = false;
      break;
    }
    cv::Mat* old = camera_frame.exchange(img);
    if (old) {
      delete old;
    }
  }
  cap.release();
}

void detect_webcam(detect_context& ctx, int input_device) {
  boost::thread camera_thread(camera_reader, input_device);
  for (int i = 0; camera_ok && !interrupt_requested(); i = (i + 1) % FLAGS_webcam_max_image_count) {
    cv::Mat* frame = camera_frame.exchange(NULL);
    if (frame) {
      std::ostringstream ss;
      ss << std::setw(6) << std::setfill('0') << i;
      detect_img(ctx, *frame, "webcam_" + ss.str() + ".jpg");
      delete frame;
    }
  }
  stop_camera = true;
  camera_thread.join();
}

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

#ifdef XOPENME
  xopenme_init(4,0);
#endif

  ::google::InitGoogleLogging(argv[0]);
  // Print output to stderr (while still logging)
  FLAGS_alsologtostderr = 1;

#ifndef GFLAGS_GFLAGS_H_
  namespace gflags = google;
#endif

  gflags::SetUsageMessage("Do detection using SSD mode.\n"
        "Usage:\n"
        "    ssd_detect [FLAGS] model_file weights_file [list_file]\n");
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  if (argc < 3) {
    gflags::ShowUsageWithFlagsRestrict(argv[0], "ck run");
    return 1;
  }

  const string& model_file = argv[1];
  const string& weights_file = argv[2];
  const string& mean_file = FLAGS_mean_file;
  const string& mean_value = FLAGS_mean_value;
  const string& out_file = FLAGS_out_file;

  // Initialize the network.
  x_clock_start(0);
  Detector detector(model_file, weights_file, mean_file, mean_value);
  x_clock_end(0);

  // Set the output mode.
  std::streambuf* buf = std::cout.rdbuf();
  std::ofstream outfile;
  if (!out_file.empty()) {
    outfile.open(out_file.c_str());
    if (outfile.good()) {
      buf = outfile.rdbuf();
    }
  }
  std::ostream out(buf);

  if (FLAGS_continuous || FLAGS_webcam) {
    detect_context ctx;
    ctx.detector = &detector;
    ctx.out = &out;
    ctx.out_dir = fs::path(FLAGS_out_images_dir);

    if (skip_files_including().empty()) {
      fs::remove_all(ctx.out_dir);
    }
    fs::create_directory(ctx.out_dir);

    ctx.label_dir = fs::path(FLAGS_label_dir);
    ctx.labelmap = read_labelmap(FLAGS_labelmap_file);
    ctx.reverse_labelmap = flip_labelmap(ctx.labelmap);
    for (const auto& e : ctx.labelmap) {
      ctx.avg_precision[e.second] = {Stat(), Stat(), Stat()};
    }
    *ctx.out << std::setprecision(float_precision());

    if (FLAGS_continuous) {
      detect_continuously(ctx, argv[3]);
    } else {
      const char* source_str = getenv("IMAGE_SOURCE_DEVICE");
      detect_webcam(ctx, NULL == source_str ? 0 : atoi(source_str));
    }
  } else {
    detect_single_image(detector, argv[3], out);
  }

#ifdef XOPENME
  xopenme_dump_state();
  xopenme_finish();
#endif

  return 0;
}
#else
int main(int argc, char** argv) {
  LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV.";
}
#endif  // USE_OPENCV


#include "pch.h"
#include <stdlib.h>
#include <string.h>
#include <algorithm>
#include <boost/foreach.hpp>
#include "IdString.h"

namespace glscene
{
	IdString::IdString( boost::string_ref value )
	{
		ConstIdString(value);
	}

	IdString::IdString( const std::string &value )
	{
		ConstIdString(boost::string_ref(value));
	}

	void IdString::ConstIdString( boost::string_ref value )
	{
		//Use the djb2 hash algorithm.
		boost::uint32_t currHash = 5381;
		BOOST_FOREACH(char c, value)
		{
			currHash = ((currHash << 5) + currHash) + c;
		}

		stringData.theHash = currHash;

		//Copy the string.
		if(value.size() > SIZE_ACTUAL_STRING - 1)
		{
			const size_t firstChars = (SIZE_ACTUAL_STRING / 2);
			const size_t lastChars = ((SIZE_ACTUAL_STRING + 1) / 2);
			std::copy(value.begin(), value.begin() + firstChars, &stringData.strString[0]);
			std::copy(value.end() - lastChars, value.end(), &stringData.strString[firstChars]);
			stringData.strString[SIZE_ACTUAL_STRING - 1] = '\0';
		}
		else
		{
			std::copy(value.begin(), value.end(), &stringData.strString[0]);
			std::fill(&stringData.strString[value.size()], &stringData.strString[SIZE_ACTUAL_STRING], '\0');
		}
	}

	bool IdString::operator<( const IdString &rhs ) const
	{
		for(int iLoop = 0; iLoop < NUM_INTEGERS; iLoop++)
		{
			if(iIntegers[iLoop] < rhs.iIntegers[iLoop])
				return true;
			if(iIntegers[iLoop] != rhs.iIntegers[iLoop])
				return false;
		}

		return false;
	}

	bool IdString::operator==( const IdString &rhs ) const
	{
		for(int iLoop = 0; iLoop < NUM_INTEGERS; iLoop++)
		{
			if(iIntegers[iLoop] != rhs.iIntegers[iLoop])
				return false;
		}

		return true;
	}
}


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * (C) 2009-2018, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "vmd_molfile_plugin_wrapper.h"
#include "pteros/core/pteros_error.h"
#include "../molfile_plugins/periodic_table.h"
#include <Eigen/Core>
#include <cmath>
// General molfile_plugin includes
#include "molfile_plugin.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif

using namespace std;
using namespace pteros;
using namespace Eigen;

void box_from_vmd_rep(float fa, float fb, float fc,
                              float alpha, float beta, float gamma, Eigen::Matrix3f& box){
#define XX 0
#define YY 1
#define ZZ 2
#define DEG2RAD (M_PI/180.0)
#define RAD2DEG (180.0/M_PI)

    double cosa,cosb,cosg,sing;
    box.fill(0.0);
    box(XX,XX) = fa;

    if ((alpha!=90.0) || (beta!=90.0) || (gamma!=90.0)) {
      if (alpha != 90.0) {
    cosa = cos(alpha*DEG2RAD);
      } else {
    cosa = 0;
      }
      if (beta != 90.0) {
    cosb = cos(beta*DEG2RAD);
      } else {
    cosb = 0;
      }
      if (gamma != 90.0) {
    cosg = cos(gamma*DEG2RAD);
    sing = sin(gamma*DEG2RAD);
      } else {
    cosg = 0;
    sing = 1;
      }
      box(YY,XX) = fb*cosg;
      box(YY,YY) = fb*sing;
      box(ZZ,XX) = fc*cosb;
      box(ZZ,YY) = fc*(cosa - cosb*cosg)/sing;
      box(ZZ,ZZ) = sqrt(fc*fc
             - box(ZZ,XX)*box(ZZ,XX) - box(ZZ,YY)*box(ZZ,YY));
    } else {
      box(YY,YY) = fb;
      box(ZZ,ZZ) = fc;
    }

    // We obtained box as a set of row-vectors. Transform it to column vectors
    box.transposeInPlace();
    box /= 10.0; // Convert to nm
}

VMD_molfile_plugin_wrapper::VMD_molfile_plugin_wrapper(string& fname): Mol_file(fname),
    handle(nullptr), w_handle(nullptr)
{

}

VMD_molfile_plugin_wrapper::~VMD_molfile_plugin_wrapper(){
    if(mode=='r'){
        if(handle){
            plugin->close_file_read(handle);
            handle = NULL;
        }

    } else {
        if(w_handle){
            plugin->close_file_write(w_handle);
            w_handle = NULL;
        }
    }
}

void VMD_molfile_plugin_wrapper::open(char open_mode){
    mode = open_mode;

    if(mode=='r'){
        if(handle) throw Pteros_error("Can't open file for reading twice - handle busy!");
        handle = NULL;
        handle = plugin->open_file_read(fname.c_str(), &open_mode, &natoms);
        if(!handle) throw Pteros_error("Can't open file '{}'!",fname);
    } else {
        if(w_handle) throw Pteros_error("Can't open file for writing twice - handle busy!");
        w_handle = NULL;
    }

}

bool VMD_molfile_plugin_wrapper::do_read(System *sys, Frame *frame, const Mol_file_content &what){

    if(what.atoms()){
        // READ STRUCTURE:

        if(sys->num_atoms()>0)
            throw Pteros_error("Can't read structure to the system, which is not empty!");

        int flags;
        vector<molfile_atom_t> atoms(natoms);
        plugin->read_structure(handle,&flags,(molfile_atom_t*)&atoms.front());
        // Allocate atoms in the system
        allocate_atoms_in_system(*sys,natoms);
        // Copy atoms to the system
        Atom at;
        int pos,idx;
        for(int i=0; i<natoms; ++i){
            at.name = atoms[i].name;
            at.resname = atoms[i].resname;
            at.resid = atoms[i].resid;
            at.chain = atoms[i].chain[0];
            at.occupancy = atoms[i].occupancy;
            at.beta = atoms[i].bfactor;
            at.charge = atoms[i].charge;
            at.element_number = atoms[i].atomicnumber;

            // pdb_plugin guesses mass based on element record, which is
            // often absent. So it is likely that mass will be zero here!
            // Check and guess ourself if there is no mass!
            // We can't use functions from VMD here since atom CA will be
            // treated as calcium for example. Our technique is more primitive
            // and only recognizes few most common elements
            // In this case we also have to guess element itself
            if(atoms[i].mass>0){
                at.mass = atoms[i].mass;
            } else {
                // Guess atomfrom atom name
                get_element_from_atom_name(at.name, at.element_number, at.mass);
            }

            set_atom_in_system(*sys,i,at);
        }
        sys->assign_resindex();

    }

    if(what.coord() || what.traj()){
        // READ FRAME:
        molfile_timestep_t ts;
        // Set zeros to box variables
        ts.A = ts.B = ts.C = ts.alpha = ts.beta = ts.gamma = 0.0;

        frame->coord.resize(natoms);
        ts.coords = (float*)&frame->coord.front();

        int ret = plugin->read_next_timestep(handle,natoms,&ts);

        if(ret!=MOLFILE_SUCCESS){
            return false;
        }

        for(int i=0; i<natoms; ++i) frame->coord[i].array() /= 10.0;

        // Convert box to our format
        Matrix3f b;
        b.fill(0.0);
        if(ts.A*ts.B*ts.C){
            // Only convert if all three vectors are non-zero
            box_from_vmd_rep(ts.A,ts.B,ts.C,ts.alpha,ts.beta,ts.gamma,b);
        }
        frame->box.set_matrix(b);

        frame->time = ts.physical_time;

        return true;
    }

    // If we are here than something is wrong
    return false;
}

void VMD_molfile_plugin_wrapper::do_write(const Selection &sel, const Mol_file_content &what) {

    if(what.atoms()){
        // WRITE STRUCTURE:
        if(!w_handle)
            w_handle = plugin->open_file_write(fname.c_str(), plugin->name, sel.size());

        vector<molfile_atom_t> atoms(sel.size());
        for(int i=0; i<sel.size(); ++i){
            strcpy( atoms[i].name, sel.name(i).c_str() );
            strcpy( atoms[i].resname, sel.resname(i).c_str() );
            atoms[i].resid = sel.resid(i);
            stringstream ss;
            ss << sel.chain(i);
            strcpy( atoms[i].chain, ss.str().c_str() );
            atoms[i].occupancy = sel.occupancy(i);
            atoms[i].bfactor = sel.beta(i);
            atoms[i].mass = sel.mass(i);
            atoms[i].charge = sel.charge(i);
            atoms[i].atomicnumber = sel.element_number(i);

            // For MOL2 we also need to set atom type as a string
            strcpy( atoms[i].type, sel.element_name(i).c_str() );
        }
        int flags = MOLFILE_OCCUPANCY | MOLFILE_BFACTOR | MOLFILE_ATOMICNUMBER
                    | MOLFILE_CHARGE | MOLFILE_MASS;
        plugin->write_structure(w_handle,flags,&atoms.front());
    }

    if(what.coord() || what.traj()){
        // WRITE COORDINATES:
        if(!w_handle)
            w_handle = plugin->open_file_write(fname.c_str(), plugin->name, sel.size());

        molfile_timestep_t ts;
        int n = sel.size();
        vector<float> buffer(n*3);
        int k = 0;
        for(int i=0; i<n; ++i){
            buffer[k] = sel.x(i)*10.0;
            buffer[k+1] = sel.y(i)*10.0;
            buffer[k+2] = sel.z(i)*10.0;
            k+=3;
        }
        ts.coords = &buffer.front();
        ts.velocities = NULL; // No velocities currently supported
        // Only convert periodic box if it is present
        if(sel.box().is_periodic()){
            Eigen::Vector3f v,a;
            sel.box().to_vectors_angles(v,a);
            ts.A = v(0)*10.0;
            ts.B = v(1)*10.0;
            ts.C = v(2)*10.0;
            ts.alpha = a(0);
            ts.beta = a(1);
            ts.gamma = a(2);
        } else {
            ts.A = 0.0;
            ts.B = 0.0;
            ts.C = 0.0;
            ts.alpha = 0;
            ts.beta = 0;
            ts.gamma = 0;
        }

        ts.physical_time = sel.get_system()->time(sel.get_frame());

        plugin->write_timestep(w_handle, &ts);
    }
}

//--------------------------------------------------------
// molfile plugins registration bootstrap
//--------------------------------------------------------

#define IMPORT_PLUGIN(name) \
    VMDPLUGIN_EXTERN int name##plugin_init(); \
    VMDPLUGIN_EXTERN int name##plugin_register(void *v, vmdplugin_register_cb cb); \
    VMDPLUGIN_EXTERN int name##plugin_fini();

#define REGISTER_PLUGIN(name,ret) \
    name##plugin_init(); \
    name##plugin_register(nullptr, &register_cb); \
    ret[cur_name] = cur_plugin;

IMPORT_PLUGIN(pdb)
IMPORT_PLUGIN(dcd)
IMPORT_PLUGIN(tng)
IMPORT_PLUGIN(xyz)
IMPORT_PLUGIN(mol2)

molfile_plugin_t *cur_plugin;
string cur_name;

static int register_cb(void *v, vmdplugin_t *p) {
  cur_name = string(p->name);
  cur_plugin = (molfile_plugin_t *)p;
  return VMDPLUGIN_SUCCESS;
}

std::map<string,molfile_plugin_t*> register_all_plugins(){
    std::map<string,molfile_plugin_t*> ret;

    REGISTER_PLUGIN(pdb,ret)
    REGISTER_PLUGIN(dcd,ret)
    REGISTER_PLUGIN(tng,ret)
    REGISTER_PLUGIN(xyz,ret)
    REGISTER_PLUGIN(mol2,ret)

    /*
    LOG()->debug("Registered VMD molfile plugins:");
    for(auto& item: ret){
         LOG()->debug("{}", item.first);
    }
    */

    /*
    setvbuf(stdout,NULL,_IOLBF,0);
    */

    return ret;
}

std::map<string,molfile_plugin_t*> VMD_molfile_plugin_wrapper::molfile_plugins = register_all_plugins();


#ifndef BOOST_THREAD_PTHREAD_TIMESPEC_HPP
#define BOOST_THREAD_PTHREAD_TIMESPEC_HPP
//  (C) Copyright 2007-8 Anthony Williams
//  (C) Copyright 2012 Vicente J. Botet Escriba
//
//  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)

#include <boost/thread/detail/config.hpp>
#include <boost/thread/thread_time.hpp>
#if defined BOOST_THREAD_USES_DATETIME
#include <boost/date_time/posix_time/conversion.hpp>
#endif
#include <pthread.h>
#ifndef _WIN32
#include <unistd.h>
#endif
#ifdef BOOST_THREAD_USES_CHRONO
#include <boost/chrono/duration.hpp>
#endif

#if defined(macintosh) || defined(__APPLE__) || defined(__APPLE_CC__)
#     define BOOST_THREAD_TIMESPEC_MAC_API
#include <sys/time.h> //for gettimeofday and timeval
#else
#include <time.h>  // for clock_gettime
#endif

#include <boost/config/abi_prefix.hpp>

namespace boost
{
  namespace detail
  {
#if defined BOOST_THREAD_USES_DATETIME
    inline struct timespec to_timespec(boost::system_time const& abs_time)
    {
      struct timespec timeout = { 0,0};
      boost::posix_time::time_duration const time_since_epoch=abs_time-boost::posix_time::from_time_t(0);

      timeout.tv_sec=time_since_epoch.total_seconds();
      timeout.tv_nsec=(long)(time_since_epoch.fractional_seconds()*(1000000000l/time_since_epoch.ticks_per_second()));
      return timeout;
    }
#endif
#if defined BOOST_THREAD_USES_CHRONO
    inline timespec to_timespec(chrono::nanoseconds const& ns)
    {
      struct timespec ts;
      ts.tv_sec = static_cast<long>(chrono::duration_cast<chrono::seconds>(ns).count());
      ts.tv_nsec = static_cast<long>((ns - chrono::duration_cast<chrono::seconds>(ns)).count());
      return ts;
    }

#endif

    inline timespec to_timespec(boost::intmax_t const& ns)
    {
      boost::intmax_t s = ns / 1000000000l;
      struct timespec ts;
      ts.tv_sec = static_cast<long> (s);
      ts.tv_nsec = static_cast<long> (ns - s * 1000000000l);
      return ts;
    }
    inline boost::intmax_t to_nanoseconds_int_max(timespec const& ts)
    {
      return static_cast<boost::intmax_t>(ts.tv_sec) * 1000000000l + ts.tv_nsec;
    }
    inline bool timespec_ge_zero(timespec const& ts)
    {
      return (ts.tv_sec >= 0) || (ts.tv_nsec >= 0);
    }
    inline timespec timespec_now()
    {
      timespec ts;

#if defined CLOCK_MONOTONIC && defined BOOST_THREAD_USEFIXES_TIMESPEC
      if ( ::clock_gettime( CLOCK_MONOTONIC, &ts ) )
      {
        ts.tv_sec = 0;
        ts.tv_nsec = 0;
        BOOST_ASSERT(0 && "Boost::Thread - Internal Error");
      }
#elif defined(BOOST_THREAD_TIMESPEC_MAC_API)
      timeval tv;
      ::gettimeofday(&tv, 0);
      ts.tv_sec = tv.tv_sec;
      ts.tv_nsec = tv.tv_usec * 1000;
#else
      if ( ::clock_gettime( CLOCK_REALTIME, &ts ) )
      {
        ts.tv_sec = 0;
        ts.tv_nsec = 0;
        BOOST_ASSERT(0 && "Boost::Thread - Internal Error");
      }
#endif
      return ts;
    }

    inline timespec timespec_now_realtime()
    {
      timespec ts;

#if defined(BOOST_THREAD_TIMESPEC_MAC_API)
      timeval tv;
      ::gettimeofday(&tv, 0);
      ts.tv_sec = tv.tv_sec;
      ts.tv_nsec = tv.tv_usec * 1000;
#else
      if ( ::clock_gettime( CLOCK_REALTIME, &ts ) )
      {
        ts.tv_sec = 0;
        ts.tv_nsec = 0;
        BOOST_ASSERT(0 && "Boost::Thread - Internal Error");
      }
#endif
      return ts;
    }
    inline timespec timespec_zero()
    {
      timespec ts;
      ts.tv_sec = 0;
      ts.tv_nsec = 0;
      return ts;
    }
    inline timespec timespec_plus(timespec const& lhs, timespec const& rhs)
    {
      return to_timespec(to_nanoseconds_int_max(lhs) + to_nanoseconds_int_max(rhs));
    }
    inline timespec timespec_minus(timespec const& lhs, timespec const& rhs)
    {
      return to_timespec(to_nanoseconds_int_max(lhs) - to_nanoseconds_int_max(rhs));
    }
    inline bool timespec_gt(timespec const& lhs, timespec const& rhs)
    {
      return to_nanoseconds_int_max(lhs) > to_nanoseconds_int_max(rhs);
    }
    inline bool timespec_ge(timespec const& lhs, timespec const& rhs)
    {
      return to_nanoseconds_int_max(lhs) >= to_nanoseconds_int_max(rhs);
    }

  }
}

#include <boost/config/abi_suffix.hpp>

#endif


#define BOOST_TEST_MODULE "test_hard_core_excluded_volume_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>
#include <mjolnir/forcefield/global/HardCoreExcludedVolumePotential.hpp>

BOOST_AUTO_TEST_CASE(HCEXV_double)
{
    using real_type = double;
    using potential_type = mjolnir::HardCoreExcludedVolumePotential<real_type>;
    using parameter_type = potential_type::parameter_type;

    constexpr static std::size_t N = 10000;
    constexpr static real_type   h = 1e-6;
    constexpr static real_type tol = 1e-6;

    const real_type epsilon = 1.0;
    const real_type hard_core_radius    = 10.0;
    const real_type soft_shell_thickness = 3.0;

    potential_type potential(2.0, epsilon);

    const parameter_type params{soft_shell_thickness, hard_core_radius};

    const real_type x_min = 0.8 * soft_shell_thickness + hard_core_radius;
    const real_type x_max = potential.absolute_cutoff(params);

    mjolnir::test::check_potential(potential, params,
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(HCEXV_float)
{
    using real_type = float;
    using potential_type = mjolnir::HardCoreExcludedVolumePotential<real_type>;
    using parameter_type = potential_type::parameter_type;

    constexpr static std::size_t N = 1000;
    constexpr static real_type   h = 0.001f;
    constexpr static real_type tol = 0.002f;

    const real_type epsilon = 1.0f;
    const real_type hard_core_radius    = 10.0f;
    const real_type soft_shell_thickness = 3.0f;

    potential_type potential(2.0f, epsilon);

    const parameter_type params{soft_shell_thickness, hard_core_radius};

    const real_type x_min = 0.8f * soft_shell_thickness + hard_core_radius;
    const real_type x_max = potential.absolute_cutoff(params);

    mjolnir::test::check_potential(potential, params,
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(HCEXV_cutoff_length)
{
    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using potential_type = mjolnir::HardCoreExcludedVolumePotential<real_type>;

    using parameter_list_type  = mjolnir::HardCoreExcludedVolumeParameterList<traits_type>;
    using parameter_type       = parameter_list_type::parameter_type;
    using ignore_molecule_type = parameter_list_type::ignore_molecule_type;
    using ignore_group_type    = parameter_list_type::ignore_group_type;

    const real_type epsilon = 1.0;
    potential_type potential(/*cutoff = */ 2.0, epsilon);

    // generate a parameter
    std::vector<std::pair<std::size_t, parameter_type>> parameters(10);
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        parameters.at(i).first         = i;
        parameters.at(i).second.sigma  = static_cast<real_type>(i) + 1.0;
        parameters.at(i).second.radius = static_cast<real_type>(i) + 1.0;
    }
    parameter_list_type parameter_list(parameters, {}, ignore_molecule_type{"Nothing"}, ignore_group_type({}));

    // estimated maximum cutoff (from per-particle parameter)
    const auto max_cutoff = potential.max_cutoff(parameter_list.parameters().begin(), parameter_list.parameters().end());

    // secure maximum cutoff calculated from per-pair parameter
    // This requires O(N^2), so it will not calculated in usual cases
    real_type max_abs_cutoff = 0.0;
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            max_abs_cutoff = std::max(max_abs_cutoff, potential.absolute_cutoff(parameter_list.prepare_params(i, j)));
        }
    }

    // max cutoff estimated from per-particle parameter should be the same or larger than the secure max cutoff
    BOOST_TEST(max_abs_cutoff <= max_cutoff);

    const real_type safety = 1.0 + std::numeric_limits<real_type>::epsilon();
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            BOOST_TEST(potential.potential(max_abs_cutoff * safety, parameter_list.prepare_params(i, j)) == 0.0);
        }
    }
}