text:
// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcLineClusterAlgorithm.hh
    \brief File containing the declaration of the LpcLineClusterAlgorithm class
*/

/*! \class LpcLineClusterAlgorithm
    \brief Class that defines the (extended) line vertexing algorithm
*/

#ifndef LPC_LINE_CLUSTER_ALGORITHM_HH
#define LPC_LINE_CLUSTER_ALGORITHM_HH

#include "LACE/LpcAbsClusterAlgorithm.hh"

#include "LACE/LpcBinRange.hh"
#include "LACE/LpcClusterData.hh"
#include "LACE/LpcFunctions.hh"
#include "LACE/LpcLineFitter.hh"
#include "LACE/LpcLineSegment.hh"
#include "LACE/LpcPoint.hh"
#include "LACE/LpcResiduals.hh"

#include <Eigen/Dense>
#include <map>
#include <string>
#include <vector>

class LpcAbsCurve;
class LpcCurve;
class LpcCluster;
class LpcHitCollection;
class LpcParameters;
class LpcVertex;

class LpcLineClusterAlgorithm : public LpcAbsClusterAlgorithm {

public:

    //! Constructor
    /*!
      \param [in] thePars a pointer to the constant lpc parameters
    */
    LpcLineClusterAlgorithm(const LpcParameters* thePars);

    //! Destructor
    virtual ~LpcLineClusterAlgorithm();

    //! Process the main lpc curve (and its branches), finding vertices and clusters
    /*!
      \param [in] theCurve The pointer to the constant main lpc curve (read-only access)
      \returns an LpcClusterData object, which stores vectors of vertex and cluster pointers
    */
    virtual LpcClusterData findClusters(const LpcCurve* theCurve);

protected:

    //! Various functions and methods

    //! Typedef for a map of matched hits for each line segment
    typedef std::map<int, std::vector<int> > LpcLineIntMap;

    //! Process either the main curve or one of its branches
    /*!
      \param [in] theCurve The pointer to the (read-only) main lpc or one of its branches
      \returns the paired vector<LpcVertex*>, vector<LpcCluster*> for the curve
    */
    LpcClusterData processCurve(const LpcAbsCurve* theCurve);

    //! Find the lpc ranges that straddle the lpc feature points.
    //! This function fills the lpcRanges_ vector
    void findLpcRanges();

    //! Find the ranges of the line segments that are near the feature points.
    //! This function fills the lineRanges_ vector
    void findLineRanges();

    //! Find the line segments that are near the feature points.
    //! This function fills the lineSegments_ vector
    void findLineSegments();

    //! Find the vertex points for the given line segments
    /*!
      \param [in] lineSegments The vector of line segments
      \returns an Eigen MatrixXd of the vertices (row = vertex, col = co-ordinates)
    */
    Eigen::MatrixXd findVtxPoints(const std::vector<LpcLineSegment>& lineSegments) const;

    //! Find the extended line segments that are near the feature points.
    //! This function fills the extLineSegments_ vector
    /*!
      \param [in] initVtxPoints A MatrixXd of initial vertices (row = vtx, col = co-ords)
    */
    void findExtendedLines(const Eigen::MatrixXd& initVtxPoints);

    //! Find the hits associated to the initial line segments
    /*!
      \param [in] lineSegments Vector of LpcLineSegments that we want to find the associated hits for
      \param [in] uniqueHits Boolean to specify if any previously used hit is to be ignored
      \returns the LpcLineIntMap dictionary, key = line segment number, value = vector of hit ids
    */
    LpcLineIntMap findAssociatedHits(const std::vector<LpcLineSegment>& lineSegments,
				     bool uniqueHits = false);

    //! Create clusters using the associated hits, line segments and vertices
    /*!
      \param [in] vtxMatrix An Eigen MatrixXd of the vertices (row = vtx, col = coords)
      \returns a vector of LpcCluster pointers
    */
    std::vector<LpcCluster*> formClusters(const Eigen::MatrixXd& vtxMatrix);

    //! Create a complete list of cluster hits (indices) using extended lines, lpcRange and residual cut
    /*!
      \param [in] matchedHits The vector of hit indices to be considered for the cluster
      \param [in] lpcRange The range of the lpc points to be considered for the cluster
      \param [in] residualCut The maximum allowed residual for a hit to be part of the cluster
    */
    void formClusterHits(std::vector<int>& matchedHits,
			 const LpcBinRange& lpcRange,
			 double residualCut);

    //! Create a new LpcCluster pointer from the required hits
    /*!
      \param [in] index The index integer number of the cluster
      \param [in] matchedHits The vector of hit indices to be used for the cluster
      \param [in] lpcRange The range of lpc points used for the cluster
      \returns a new LpcCluster pointer
    */
    LpcCluster* createCluster(int index, const std::vector<int>& matchedHits,
			      const LpcBinRange& lpcRange) const;

    //! Create a new single LpcCluster pointer from all available hits
    /*!
      \param [in] index The index integer number of the cluster
      \returns a new LpcCluster pointer
    */
    LpcCluster* createSingleCluster(int index) const;

    //! Create a new single LpcVertex pointer as the position of the hit with the lowest 1st co-ord ("x")
    /*!
      \returns a new LpcVertex pointer
    */
    LpcVertex* createSingleVertex() const;

    //! Add any remaining hits to the clusters by finding the closest
    //! distance to the cluster principal axis
    /*!
      \param [in] lpcClusters The vector of LpcCluster pointers
    */
    void addRemainingHits(const std::vector<LpcCluster*>& lpcClusters);

    //! Create a vector of LpcVertex pointers containing the vertex information
    /*!
      \param [in] vtxPointMatrix The matrix of the vertex co-ordinates (row = vtx, col = x,y,z..)
      \return a vector of new LpcVertex pointers
    */
    std::vector<LpcVertex*> createVertices(const Eigen::MatrixXd& vtxPointMatrix) const;

    //! Merge clusters
    /*!
      \param [in] vtxPointMatrix The matrix of the vertex co-ordinates (row = vtx, col = x,y,z..)
      \param [in] lpcClusters The vector of original LpcCluster pointers
      \returns the pair of merged vertex and (a copy of the) cluster vectors
    */
    LpcClusterData mergeClusters(const Eigen::MatrixXd& vtxPointMatrix,
				 const std::vector<LpcCluster*>& lpcClusters);

    //! Check and potentially correct the allowed lpc index number
    /*!
      \param [in] lpcInt The initial lpc index point number
      \returns the index point number within the range [0, nLpcPoints1_]
    */
    int checkLpcInt(int lpcInt) const;

    //! Get the residual cut given the residual mean and rms
    /*!
      \param [in] residualMean The mean of the hit-to-lpc residuals
      \param [in] residualRms The rms of the hit-to-lpc residuals
      \returns the residual cut
    */
    double getResidualCut(double residualMean, double residualRms) const;

    //! Print out a vector of LpcBinRange objects
    /*!
      \param [in] preamble A starting string for the printout line
      \param [in] binRanges The given vector of LpcBinRange objects
    */
    void printBinRanges(const std::string& preamble,
			const std::vector<LpcBinRange>& binRanges) const;

private:

    //! Private default constructor
    LpcLineClusterAlgorithm();

    //! Copy constructor
    LpcLineClusterAlgorithm(const LpcLineClusterAlgorithm& other);

    //! Assignment operator
    LpcLineClusterAlgorithm& operator=(const LpcLineClusterAlgorithm& other);

    //! The pointer to the constant hit collection
    const LpcHitCollection* theHits_;

    //! The line fitter used to find the regression line for the set of hits
    LpcLineFitter lineFitter_;

    //! Number of lpc points to consider for each line segment
    int nLpcSegment_;

    //! Number of sigma for residual cut
    double nResSigma_;

    //! The minimum selection cut for lpc-to-hit residuals for the vertexing
    double minVtxResCut_;

    //! Maximum distance to associate any remaining hits with a cluster
    double maxDist_;

    //! Specify if we want to merge neighbouring clusters
    bool doClusterMerging_;

    //! Specify if we want to do vertexing and clustering for branches
    int doBranchVtx_;

    //! Specify the minimum angle for merging clusters
    double mergeDegAngle_;

    //! The cosine of the merging cluster angle
    double mergeCosAngle_;

    //! The LpcFunctions object
    LpcFunctions functions_;

    //! The curve index number
    int curveId_;

    //! The branch index number
    int branchId_;

    //! Hit and lpc point information

    //! The number of hit points
    int nHitPoints_;

    //! The number of co-ordinate dimensions
    int nDim_;

    //! The number of lpc points
    int nLpcPoints_;

    //! The number of lpc points - 1
    int nLpcPoints1_;

    //! The Eigen matrix of unscaled hit positions
    Eigen::MatrixXd hitPoints_;

    //! The vector of LpcPoints
    std::vector<LpcPoint> lpcPoints_;

    //! The lpc residual object
    LpcResiduals lpcResiduals_;

    //! The hit-to-lpc residuals
    Eigen::VectorXd hitResiduals_;

    //! The closest lpc point integer for each hit
    Eigen::VectorXi hitNearLpc_;

    //! An array to keep track of which hits have already been used for clusters
    Eigen::VectorXi usedHits_;

    //! Internal vectors of various lpc point bin ranges

    //! The number of cosine peaks
    int nPeaks_;

    //! The vector of lpc (min,max) point ranges for each cosine feature peak
    std::vector<LpcBinRange> cosPeakRanges_;

    //! The vector of lpc (min,max) point ranges that straddle the feature points
    std::vector<LpcBinRange> lpcRanges_;

    //! The vector of lpc (min,max) point ranges that are near the feature points
    std::vector<LpcBinRange> lineRanges_;

    //! The vector of line segments
    std::vector<LpcLineSegment> lineSegments_;

    //! The vector of extended line segments
    std::vector<LpcLineSegment> extLineSegments_;

    //! An internal boolean to specify if cluster merging was successful
    bool mergedOK_;

};

#endif

ext: hh

max_stars_repo_name: petrmanek/LACE

max_issues_repo_name: petrmanek/LACE

max_forks_repo_name: petrmanek/LACE


text:
// Copyright (C) 2015-2018 Internet Systems Consortium, Inc. ("ISC")
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <config.h>

#include <stats/observation.h>
#include <exceptions/exceptions.h>
#include <util/boost_time_utils.h>
#include <boost/shared_ptr.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <gtest/gtest.h>

#include <iostream>
#include <sstream>

#include <unistd.h>

using namespace isc;
using namespace isc::stats;
using namespace boost::posix_time;

namespace {

/// @brief Test class for Observation
///
/// This simple fixture class initializes four observations:
/// a (integer), b (float), c(time duration) and d (string).
class ObservationTest : public ::testing::Test {
public:

    /// @brief Constructor
    /// Initializes four observations.
    ObservationTest()
        :a("alpha", static_cast<int64_t>(1234)), // integer
         b("beta", 12.34), // float
         c("gamma", millisec::time_duration(1,2,3,4)), // duration
         d("delta", "1234") { // string
    }

    Observation a;
    Observation b;
    Observation c;
    Observation d;
};

// Basic tests for the Observation constructors. This test checks whether
// parameters passed to the constructor initialize the object properly.
TEST_F(ObservationTest, constructor) {

    EXPECT_EQ(Observation::STAT_INTEGER, a.getType());
    EXPECT_EQ(Observation::STAT_FLOAT, b.getType());
    EXPECT_EQ(Observation::STAT_DURATION, c.getType());
    EXPECT_EQ(Observation::STAT_STRING, d.getType());

    EXPECT_EQ(1234, a.getInteger().first);
    EXPECT_EQ(12.34, b.getFloat().first);
    EXPECT_EQ(millisec::time_duration(1,2,3,4),
              c.getDuration().first);
    EXPECT_EQ("1234", d.getString().first);

    // Let's check that attempting to get a different type
    // than used will cause an exception.
    EXPECT_THROW(a.getFloat(), InvalidStatType);
    EXPECT_THROW(a.getDuration(), InvalidStatType);
    EXPECT_THROW(a.getString(), InvalidStatType);

    EXPECT_THROW(b.getInteger(), InvalidStatType);
    EXPECT_THROW(b.getDuration(), InvalidStatType);
    EXPECT_THROW(b.getString(), InvalidStatType);

    EXPECT_THROW(c.getInteger(), InvalidStatType);
    EXPECT_THROW(c.getFloat(), InvalidStatType);
    EXPECT_THROW(c.getString(), InvalidStatType);

    EXPECT_THROW(d.getInteger(), InvalidStatType);
    EXPECT_THROW(d.getFloat(), InvalidStatType);
    EXPECT_THROW(d.getDuration(), InvalidStatType);
}

// This test checks whether it is possible to set to an absolute value for all
// given types.
TEST_F(ObservationTest, setValue) {

    EXPECT_NO_THROW(a.setValue(static_cast<int64_t>(5678)));
    EXPECT_NO_THROW(b.setValue(56e+78));
    EXPECT_NO_THROW(c.setValue(millisec::time_duration(5,6,7,8)));
    EXPECT_NO_THROW(d.setValue("fiveSixSevenEight"));

    EXPECT_EQ(5678, a.getInteger().first);
    EXPECT_EQ(56e+78, b.getFloat().first);
    EXPECT_EQ(millisec::time_duration(5,6,7,8),
              c.getDuration().first);
    EXPECT_EQ("fiveSixSevenEight", d.getString().first);

    // Now check whether setting value to a different type does
    // throw an exception
    EXPECT_THROW(a.setValue(56e+78), InvalidStatType);
    EXPECT_THROW(a.setValue(millisec::time_duration(5,6,7,8)), InvalidStatType);
    EXPECT_THROW(a.setValue("fiveSixSevenEight"), InvalidStatType);

    EXPECT_THROW(b.setValue(static_cast<int64_t>(5678)), InvalidStatType);
    EXPECT_THROW(b.setValue(millisec::time_duration(5,6,7,8)), InvalidStatType);
    EXPECT_THROW(b.setValue("fiveSixSevenEight"), InvalidStatType);

    EXPECT_THROW(c.setValue(static_cast<int64_t>(5678)), InvalidStatType);
    EXPECT_THROW(c.setValue(56e+78), InvalidStatType);
    EXPECT_THROW(c.setValue("fiveSixSevenEight"), InvalidStatType);

    EXPECT_THROW(d.setValue(static_cast<int64_t>(5678)), InvalidStatType);
    EXPECT_THROW(d.setValue(56e+78), InvalidStatType);
    EXPECT_THROW(d.setValue(millisec::time_duration(5,6,7,8)), InvalidStatType);
}

// This test checks whether it is possible to add value to existing
// counter.
TEST_F(ObservationTest, addValue) {

    // Note: all Observations were set to 1234,12.34 or similar in
    // ObservationTest constructor.

    EXPECT_NO_THROW(a.addValue(static_cast<int64_t>(5678)));
    EXPECT_NO_THROW(b.addValue(56.78));
    EXPECT_NO_THROW(c.addValue(millisec::time_duration(5,6,7,8)));
    EXPECT_NO_THROW(d.addValue("fiveSixSevenEight"));

    EXPECT_EQ(6912, a.getInteger().first);
    EXPECT_EQ(69.12, b.getFloat().first);

    EXPECT_EQ(millisec::time_duration(6,8,10,12), c.getDuration().first);
    EXPECT_EQ("1234fiveSixSevenEight", d.getString().first);
}

// Test checks whether timing is reported properly.
TEST_F(ObservationTest, timers) {
    ptime before = microsec_clock::local_time();
    b.setValue(123.0); // Set it to a random value and record the time.

    // Allow a bit of imprecision. This test allows 500ms. That should be ok,
    // when running on virtual machines.
    ptime after = before + milliseconds(500);

    // Now wait some time. We want to confirm that the timestamp recorded is the
    // time the observation took place, not current time.
    sleep(1);

    FloatSample sample = b.getFloat();

    // Let's check that the timestamp is within (before,after) range:
    // before < sample-time < after
    EXPECT_TRUE(before <= sample.second);
    EXPECT_TRUE(sample.second <= after);
}

// Checks whether an integer statistic can generate proper JSON structures.
// See http://kea.isc.org/wiki/StatsDesign for details.
TEST_F(ObservationTest, integerToJSON) {

    a.setValue(static_cast<int64_t>(1234));

    std::string exp = "[ [ 1234, \""
        + isc::util::ptimeToText(a.getInteger().second) + "\" ] ]";

    std::cout << a.getJSON()->str() << std::endl;
    EXPECT_EQ(exp, a.getJSON()->str());
}

// Checks whether a floating point statistic can generate proper JSON
// structures. See http://kea.isc.org/wiki/StatsDesign for details.
TEST_F(ObservationTest, floatToJSON) {

    // Let's use a value that converts easily to floating point.
    // No need to deal with infinite fractions in binary systems.
    b.setValue(1234.5);

    std::string exp = "[ [ 1234.5, \""
        + isc::util::ptimeToText(b.getFloat().second) + "\" ] ]";

    std::cout << b.getJSON()->str() << std::endl;
    EXPECT_EQ(exp, b.getJSON()->str());
}

// Checks whether a time duration statistic can generate proper JSON structures.
// See http://kea.isc.org/wiki/StatsDesign for details.
TEST_F(ObservationTest, durationToJSON) {

    // 1 hour 2 minutes 3 seconds and 4 milliseconds
    c.setValue(time_duration(1,2,3,4));

    std::string exp = "[ [ \"01:02:03.000004\", \""
        + isc::util::ptimeToText(c.getDuration().second) + "\" ] ]";

    std::cout << c.getJSON()->str() << std::endl;
    EXPECT_EQ(exp, c.getJSON()->str());
}

// Checks whether a string statistic can generate proper JSON structures.
// See http://kea.isc.org/wiki/StatsDesign for details.
TEST_F(ObservationTest, stringToJSON) {

    //
    d.setValue("Lorem ipsum dolor sit amet");

    std::string exp = "[ [ \"Lorem ipsum dolor sit amet\", \""
        + isc::util::ptimeToText(d.getString().second) + "\" ] ]";

    std::cout << d.getJSON()->str() << std::endl;
    EXPECT_EQ(exp, d.getJSON()->str());
}

// Checks whether reset() resets the statistics properly.
TEST_F(ObservationTest, reset) {
    a.reset(); // integer
    b.reset(); // float
    c.reset(); // duration
    d.reset(); // string

    EXPECT_EQ(0, a.getInteger().first);
    EXPECT_EQ(0.0, b.getFloat().first);
    EXPECT_EQ(time_duration(0,0,0,0), c.getDuration().first);
    EXPECT_EQ("", d.getString().first);
}

// Checks whether an observation can keep its name.
TEST_F(ObservationTest, names) {
    EXPECT_EQ("alpha", a.getName());
    EXPECT_EQ("beta", b.getName());
    EXPECT_EQ("gamma", c.getName());
    EXPECT_EQ("delta", d.getName());
}

};

ext: cc

max_stars_repo_name: mkLEDA/kea

max_issues_repo_name: mkLEDA/kea

max_forks_repo_name: mkLEDA/kea


text:
#include <mutex>
#include <memory>
#include <iostream>
#include <boost/circular_buffer.hpp>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <pcl/filters/voxel_grid.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>

#include <hdl_localization/pose_estimator.hpp>

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using namespace gtsam;

namespace hdl_localization {

class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;

  HdlLocalizationNodelet() {
  }
  virtual ~HdlLocalizationNodelet() {
  }

  void onInit() override {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    processing_time.resize(16);
    imuAccNoise = private_nh.param<double>("imuAccNoise", 0.01);
    imuGyrNoise = private_nh.param<double>("imuGyrNoise", 0.001);
    odomLinearNoise = private_nh.param<double>("odomLinearNoise", 0.01);
    odomAngularNoise = private_nh.param<double>("odomAngularNoise", 0.01);
    gravity= private_nh.param<double>("gravity", 10.04);
    opt_frames = private_nh.param<int>("opt_frames", 20);
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
    use_imu = private_nh.param<bool>("use_imu", true);
    invert_imu = private_nh.param<bool>("invert_imu", false);
    key_interval = private_nh.param<float>("key_interval", 0.1);
    imu_sub = mt_nh.subscribe("/imu/data_raw", 256, &HdlLocalizationNodelet::imu_callback, this);
    points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
    pose_incre_pub = nh.advertise<nav_msgs::Odometry>("/ndt/incre_odom", 30, false);
    pose_pub = nh.advertise<nav_msgs::Odometry>("/ndt/global_odom", 20, false);
    initialize_params();

  }

private:
  void initialize_params() {
    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;

    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
    pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());

    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if(ndt_neighbor_search_method == "DIRECT1") {
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      registration = ndt;
//        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
//      localregistration=ndt;
    } else if(ndt_neighbor_search_method == "DIRECT7") {
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      registration = ndt;
//      localregistration=ndt;

    } else if(ndt_neighbor_search_method == "GICP_OMP"){
      NODELET_INFO("search_method GICP_OMP is selected");
      registration = gicp;
    }
     else {
      if(ndt_neighbor_search_method == "KDTREE") {
        NODELET_INFO("search_method KDTREE is selected");
      } else {
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      registration = ndt;

    }

    // initialize pose estimator
    if(private_nh.param<bool>("specify_init_pose", true))
    {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3d(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaterniond(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
        private_nh.param<double>("cool_time_duration", 0.5)
      ));
    }
//    poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(1.0,0.0,0.0,0.0),gtsam::Point3(0.0,0.0,0.0));
      //*****************factor graph********************//
      boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(gravity);
      p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
      p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
      p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
      gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias
      priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
      priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s
      priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
      correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1); // meter
      noiseModelBetweenBias = (gtsam::Vector(6) << 3.99395e-03, 3.99395e-03, 3.99395e-03, 1.56363e-03, 1.56363e-03, 1.56363e-03).finished();
      imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias);

      key_count=0;
      systemInitialized= false;

//**************************************************************//
  }

private:
  /**
   * @brief callback for imu data
   * @param imu_msg
   */
  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      imu_data.push_back(imu_msg);
//      ROS_INFO("hahahahahahah");
  }
  /**
   * @brief callback for point cloud data
   * @param points_msg
   */
  void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {

      points_curr_time=points_msg->header.stamp;

      if(firstkey)
      {
          points_pre_time=points_msg->header.stamp;
          firstkey= false;
      }

      auto time_interval= points_curr_time-points_pre_time;

      if(time_interval.toSec()<key_interval) return;// select key frames according to time interval

      std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);

      if(!pose_estimator) {
          NODELET_ERROR("waiting for initial pose input!!");
          return;
      }

      if(!globalmap) {
          NODELET_ERROR("globalmap has not been received!!");
          return;
      }

      const auto& stamp = points_msg->header.stamp;
      pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
      pcl::fromROSMsg(*points_msg, *pcl_cloud);

      if(pcl_cloud->empty()) {
          NODELET_ERROR("cloud is empty!!");
          return;
      }

      // transform pointcloud into odom_child_frame_id
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
      if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
          NODELET_ERROR("point cloud cannot be transformed into target frame!!");
          return;
      }

      auto filtered = downsample(cloud);

      auto t1 = ros::WallTime::now();

//      cur_cloud=filtered;
//      localregistration->setInputTarget(pre_cloud);
//      localregistration->setInputSource(cur_cloud);
//      Eigen::Matrix4f localOdom_temp;
//      Eigen::Matrix4d localOdom;
//      localOdom_temp = localregistration->getFinalTransformation();
//      localOdom=localOdom_temp.cast<double>();

      if(!systemInitialized){

          resetOptimization();

          newgraph.add(PriorFactor<Pose3>(X(0), prevPose_, priorPoseNoise));
          std::cout<<"initial value:"<<prevPose_.x()<<" "<<prevPose_.y()<<" "<<prevPose_.z()<<" "<<prevPose_.rotation().roll()<<" "
          <<prevPose_.rotation().pitch()<<" "<<prevPose_.rotation().yaw()<<std::endl;

          //initial velocity
          prevVel_ = gtsam::Vector3(0, 0, 0);
          gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
          newgraph.add(priorVel);

          //initial bias
          prevBias_ = gtsam::imuBias::ConstantBias();
          gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
          newgraph.add(priorBias);

          //add values
          initialEstimate.insert(X(0), prevPose_);
          initialEstimate.insert(V(0), prevVel_);
          initialEstimate.insert(B(0), prevBias_);

          optimizer->update(newgraph, initialEstimate);

          newgraph.resize(0);
          initialEstimate.clear();
          imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
          systemInitialized=true;
          key_count=1;
          return;
      }
      else{

          if(key_count==opt_frames)
          {
              gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(X(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(V(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(B(key_count-1)));
              // reset graph
              resetOptimization();
              // add pose
              gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
              newgraph.add(priorPose);
              // add velocity
              gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
              newgraph.add(priorVel);
              // add bias
              gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
              newgraph.add(priorBias);
              // add values
              initialEstimate.insert(X(0), prevPose_);
              initialEstimate.insert(V(0), prevVel_);
              initialEstimate.insert(B(0), prevBias_);
              // optimize once
              optimizer->update(newgraph, initialEstimate);
              newgraph.resize(0);
              initialEstimate.clear();

              key_count=1;
          }
          gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                               gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
          std::cout<<"prior value:"<<poseFrom.x()<<" "<<poseFrom.y()<<" "<<poseFrom.z()<<" "<<poseFrom.rotation().roll()<<" "
                   <<poseFrom.rotation().pitch()<<" "<<poseFrom.rotation().yaw()<<std::endl;

          auto aligned = pose_estimator->correct(filtered);

          gtsam::Pose3 poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                             gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
          std::cout<<"update value:"<<poseTo.x()<<" "<<poseTo.y()<<" "<<poseTo.z()<<" "<<poseTo.rotation().roll()<<" "
                   <<poseTo.rotation().pitch()<<" "<<poseTo.rotation().yaw()<<std::endl;

           std::lock_guard<std::mutex> lock(imu_data_mutex);
           auto imu_iter = imu_data.begin();
           for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
                  if(stamp < (*imu_iter)->header.stamp) {
                      break;
                  }
                  const auto& acc = (*imu_iter)->linear_acceleration;
                  const auto& gyro = (*imu_iter)->angular_velocity;
                  imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z),gtsam::Vector3(gyro.x, gyro.y, gyro.z), 0.01);
              }
           imu_data.erase(imu_data.begin(),imu_iter);

          gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_count), poseTo, correctionNoise);
          newgraph.add(pose_factor);

          const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorImu_);
          gtsam::ImuFactor imu_factor(X(key_count - 1), V(key_count - 1), X(key_count), V(key_count), B(key_count - 1), preint_imu);

          newgraph.add(imu_factor);
          newgraph.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key_count - 1), B(key_count), gtsam::imuBias::ConstantBias(),
                                                                              gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorImu_->deltaTij()) * noiseModelBetweenBias)));

          gtsam::NavState propState_ = imuIntegratorImu_->predict(prevState_, prevBias_);

          std::cout<<" imu integre propState:"<<propState_.pose().x()<<" "<<propState_.pose().y()<<" "<<propState_.pose().z()<<" "<<
                   propState_.pose().rotation().roll()<<" "<<propState_.pose().rotation().pitch()<<" "<<propState_.pose().rotation().yaw()<<std::endl;

          if(propState_.pose().z()<-0.1)
          {
              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),-0.1));
          }
          else if (propState_.pose().z()>0.1)
          {

              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),0.10));
          }

          std::cout<<"propState_.pose().z():"<<propState_.pose().z()<<std::endl;

          noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << odomLinearNoise, odomLinearNoise, odomLinearNoise, odomAngularNoise, odomAngularNoise, odomAngularNoise).finished());

          newgraph.add(BetweenFactor<Pose3>(X(key_count-1), X(key_count), poseFrom.between(poseTo), odometryNoise));

          initialEstimate.insert(X(key_count), poseTo);
          initialEstimate.insert(V(key_count), propState_.v());
          initialEstimate.insert(B(key_count), prevBias_);
      }

      optimizer->update(newgraph, initialEstimate);
      newgraph.resize(0);
      initialEstimate.clear();
      gtsam::Pose3 latestEstimate;
      isamCurrentEstimate = optimizer->calculateEstimate();

      prevPose_ = isamCurrentEstimate.at<Pose3>(X(key_count));

      prevVel_ =isamCurrentEstimate.at<Vector3>(V(key_count));
      prevState_ = gtsam::NavState(prevPose_, prevVel_);
      prevBias_=isamCurrentEstimate.at<imuBias::ConstantBias>(B(key_count));
      imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);

      pose_estimator->update(prevState_,prevBias_);

      Eigen::Quaterniond quat(prevPose_.rotation().toQuaternion().w(),prevPose_.rotation().toQuaternion().x(),prevPose_.rotation().toQuaternion().y(),prevPose_.rotation().toQuaternion().z());
      quat.normalize();
      geometry_msgs::Quaternion odom_quat;
      odom_quat.w = quat.w();
      odom_quat.x = quat.x();
      odom_quat.y = quat.y();
      odom_quat.z = quat.z();

      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = stamp;
      odom_trans.header.frame_id = "map";
      odom_trans.child_frame_id = odom_child_frame_id;

      odom_trans.transform.translation.x = prevPose_.x();
      odom_trans.transform.translation.y = prevPose_.y();
      odom_trans.transform.translation.z = prevPose_.z();
      odom_trans.transform.rotation = odom_quat;

      pose_broadcaster.sendTransform(odom_trans);

      nav_msgs::Odometry laserOdometryROS;
      laserOdometryROS.header.stamp = points_msg->header.stamp;
      laserOdometryROS.header.frame_id = "map";
      laserOdometryROS.child_frame_id = odom_child_frame_id;
      laserOdometryROS.pose.pose.position.x = prevPose_.x();
      laserOdometryROS.pose.pose.position.y = prevPose_.y();
      laserOdometryROS.pose.pose.position.z = prevPose_.z();
      laserOdometryROS.pose.pose.orientation = odom_quat;
      pose_pub.publish(laserOdometryROS);

      points_pre_time = points_curr_time;

//      pre_cloud=filtered;
      ++key_count;

      auto t2 = ros::WallTime::now();
      NODELET_INFO_STREAM("processing_time: " <<(t2 - t1).toSec()* 1000.0 << "[msec]");

  }

  /**
   * @brief callback for globalmap input
   * @param points_msg
   */
  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;
    registration->setInputTarget(globalmap);
  }

  /**
   * @brief callback for initial pose input ("2D Pose Estimate" on rviz)
   * @param pose_msg
   */
  void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) {
    NODELET_INFO("initial pose received!!");
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    const auto& p = pose_msg->pose.pose.position;
    const auto& q = pose_msg->pose.pose.orientation;
    pose_estimator.reset(
          new hdl_localization::PoseEstimator(
            registration,
            ros::Time::now(),
            Eigen::Vector3d(p.x, p.y, p.z),
            Eigen::Quaterniond(q.w, q.x, q.y, q.z),
            private_nh.param<double>("cool_time_duration", 0.5))
    );
  }

  /**
   * @brief downsampling
   * @param cloud   input cloud
   * @return downsampled cloud
   */
  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter) {
      return cloud;
    }
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;
    return filtered;
  }

  /**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */

  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4d& pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

    void publish_incre_odometry(const ros::Time& stamp, const Eigen::Matrix4d& pose) {
        // broadcast the transform over tf
        geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_child_frame_id, "incre_baselink");
        pose_incre_broadcaster.sendTransform(odom_trans);

        // publish the transform
        nav_msgs::Odometry odom;
        odom.header.stamp = stamp;
        odom.header.frame_id = odom_child_frame_id;

        odom.pose.pose.position.x = pose(0, 3);
        odom.pose.pose.position.y = pose(1, 3);
        odom.pose.pose.position.z = pose(2, 3);
        odom.pose.pose.orientation = odom_trans.transform.rotation;

        odom.child_frame_id = "incre_baselink";
        odom.twist.twist.linear.x = 0.0;
        odom.twist.twist.linear.y = 0.0;
        odom.twist.twist.angular.z = 0.0;

        pose_incre_pub.publish(odom);
    }

  /**
   * @brief convert a Eigen::Matrix to TransformedStamped
   * @param stamp           timestamp
   * @param pose            pose matrix
   * @param frame_id        frame_id
   * @param child_frame_id  child_frame_id
   * @return transform
   */
  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4d& pose, const std::string& frame_id, const std::string& child_frame_id) {
    Eigen::Quaterniond quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

    void resetOptimization()
    {

        std::cout<<"resetOptimization"<<std::endl;
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        optimizer = new gtsam::ISAM2(optParameters);
        gtsam::NonlinearFactorGraph newGraphFactors;
        newgraph = newGraphFactors;
        gtsam::Values NewGraphValues;
        initialEstimate = NewGraphValues;

//        gtsam::LevenbergMarquardtParams optParameters;
//        optParameters.setVerbosity("ERROR");
//        optParameters.setOrderingType("METIS");
//        optParameters.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
//        gtsam::NonlinearFactorGraph newGraphFactors;
//        newgraph = newGraphFactors;
//        gtsam::Values NewGraphValues;
//        initialEstimate = NewGraphValues;
//        optimizer= new gtsam::LevenbergMarquardtOptimizer optimizer(newgraph,initialEstimate,optParameters);

    }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;
  ros::Publisher pose_incre_pub;
  ros::Publisher pose_pub;

  std::string odom_child_frame_id;

  bool use_imu;
  bool invert_imu;

  float key_interval;
  int opt_frames;
  ros::Subscriber imu_sub;
  ros::Subscriber points_sub;
  ros::Subscriber globalmap_sub;
  ros::Subscriber initialpose_sub;
  ros::Subscriber updateGlobalPose_sub;

  ros::Time points_pre_time,points_curr_time;

  ros::Publisher aligned_pub;
  tf::TransformBroadcaster pose_broadcaster;
  tf::TransformBroadcaster pose_incre_broadcaster;
  tf::TransformListener tf_listener;

  // imu input buffer
  std::mutex imu_data_mutex;
  std::vector<sensor_msgs::ImuConstPtr> imu_data;

  // globalmap and registration method
  pcl::PointCloud<PointT>::Ptr globalmap;
  pcl::Filter<PointT>::Ptr downsample_filter;
  pcl::Registration<PointT, PointT>::Ptr registration;
  pcl::Registration<PointT, PointT>::Ptr localregistration;

  // pose estimator
  std::mutex pose_estimator_mutex;
  std::unique_ptr<hdl_localization::PoseEstimator> pose_estimator;

  // processing time buffer
  boost::circular_buffer<double> processing_time;

  // ********************the followings are for graph optimization*********************
public:
  bool firstkey=true;
  NonlinearFactorGraph newgraph;
  gtsam::ISAM2 *optimizer;
//  gtsam::LevenbergMarquardtOptimizer *optimizer;
  gtsam::Values initialEstimate;
  gtsam::Values isamCurrentEstimate;
  gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
  bool systemInitialized;
  int key_count;
  double imuAccNoise,imuGyrNoise,gravity;
  double odomLinearNoise,odomAngularNoise;

  gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
  gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
  gtsam::Vector noiseModelBetweenBias;

//  pcl::PointCloud<PointT>::Ptr pre_cloud;  //registration for local odometry
//  pcl::PointCloud<PointT>::Ptr cur_cloud;  //registration for local odometry
  gtsam::Vector3 prevVel_;
  gtsam::imuBias::ConstantBias prevBias_;
  gtsam::NavState prevState_;
  gtsam::Pose3 prevPose_;
//  gtsam::Pose3 poseFrom;

};

}

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)

ext: cpp


text:
// 2019 Team AobaZero
// This is a work derived from Leela Zero (May 1, 2019).
/*
    This file is part of Leela Zero.
    Copyright (C) 2017-2019 Gian-Carlo Pascutto and contributors

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

    Leela Zero 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 Leela Zero.  If not, see <http://www.gnu.org/licenses/>.

    Additional permission under GNU GPL version 3 section 7

    If you modify this Program, or any covered work, by linking or
    combining it with NVIDIA Corporation's libraries from the
    NVIDIA CUDA Toolkit and/or the NVIDIA CUDA Deep Neural
    Network library and/or the NVIDIA TensorRT inference library
    (or a modified version of those libraries), containing parts covered
    by the terms of the respective license agreement, the licensors of
    this Program grant you additional permission to convey the resulting
    work.
*/

#include "config.h"

#include <algorithm>
#include <cctype>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <exception>
#include <fstream>
#include <limits>
#include <memory>
#include <random>
#include <string>
#include <vector>
#include <boost/algorithm/string.hpp>

#include "GTP.h"
#include "FastBoard.h"
#include "FullBoard.h"
#include "GameState.h"
#include "Network.h"
//#include "SGFTree.h"
#include "SMP.h"
//#include "Training.h"
#include "UCTSearch.h"
#include "Utils.h"

using namespace Utils;

// Configuration flags
bool cfg_gtp_mode;
bool cfg_allow_pondering;
unsigned int cfg_num_threads;
unsigned int cfg_batch_size;
int cfg_max_playouts;
int cfg_max_visits;
size_t cfg_max_memory;
size_t cfg_max_tree_size;
int cfg_max_cache_ratio_percent;
TimeManagement::enabled_t cfg_timemanage;
int cfg_lagbuffer_cs;
int cfg_resignpct;
int cfg_noise;
int cfg_random_cnt;
int cfg_random_min_visits;
float cfg_random_temp;
std::uint64_t cfg_rng_seed;
bool cfg_dumbpass;
#ifdef USE_OPENCL
std::vector<int> cfg_gpus;
bool cfg_sgemm_exhaustive;
bool cfg_tune_only;
#ifdef USE_HALF
precision_t cfg_precision;
#endif
#endif
float cfg_puct;
float cfg_logpuct;
float cfg_logconst;
float cfg_softmax_temp;
float cfg_fpu_reduction;
float cfg_fpu_root_reduction;
float cfg_ci_alpha;
float cfg_lcb_min_visit_ratio;
std::string cfg_weightsfile;
std::string cfg_logfile;
FILE* cfg_logfile_handle;
bool cfg_quiet;
std::string cfg_options_str;
bool cfg_benchmark;
bool cfg_cpu_only;
AnalyzeTags cfg_analyze_tags;

#if 0
/* Parses tags for the lz-analyze GTP command and friends */
AnalyzeTags::AnalyzeTags(std::istringstream& cmdstream, const GameState& game) {
    std::string tag;

    /* Default color is the current one */
    m_who = game.board.get_to_move();

    auto avoid_not_pass_resign_b = false, avoid_not_pass_resign_w = false;
    auto allow_b = false, allow_w = false;

    while (true) {
        cmdstream >> std::ws;
        if (isdigit(cmdstream.peek())) {
            tag = "interval";
        } else {
            cmdstream >> tag;
            if (cmdstream.fail() && cmdstream.eof()) {
                /* Parsing complete */
                m_invalid = false;
                return;
            }
        }

        if (tag == "avoid" || tag == "allow") {
            std::string textcolor, textmoves;
            size_t until_movenum;
            cmdstream >> textcolor;
            cmdstream >> textmoves;
            cmdstream >> until_movenum;
            if (cmdstream.fail()) {
                return;
            }

            std::vector<int> moves;
            std::istringstream movestream(textmoves);
            while (!movestream.eof()) {
                std::string textmove;
                getline(movestream, textmove, ',');
                auto sepidx = textmove.find_first_of(':');
                if (sepidx != std::string::npos) {
                    if (!(sepidx == 2 || sepidx == 3)) {
                        moves.clear();
                        break;
                    }
                    auto move1_compressed = game.board.text_to_move(
                        textmove.substr(0, sepidx)
                    );
                    auto move2_compressed = game.board.text_to_move(
                        textmove.substr(sepidx + 1)
                    );
                    if (move1_compressed == FastBoard::NO_VERTEX ||
                        move1_compressed == FastBoard::PASS ||
                        move1_compressed == FastBoard::RESIGN ||
                        move2_compressed == FastBoard::NO_VERTEX ||
                        move2_compressed == FastBoard::PASS ||
                        move2_compressed == FastBoard::RESIGN)
                    {
                        moves.clear();
                        break;
                    }
                    auto move1_xy = game.board.get_xy(move1_compressed);
                    auto move2_xy = game.board.get_xy(move2_compressed);
                    auto xmin = std::min(move1_xy.first, move2_xy.first);
                    auto xmax = std::max(move1_xy.first, move2_xy.first);
                    auto ymin = std::min(move1_xy.second, move2_xy.second);
                    auto ymax = std::max(move1_xy.second, move2_xy.second);
                    for (auto move_x = xmin; move_x <= xmax; move_x++) {
                        for (auto move_y = ymin; move_y <= ymax; move_y++) {
                            moves.push_back(game.board.get_vertex(move_x,move_y));
                        }
                    }
                } else {
                    auto move = game.board.text_to_move(textmove);
                    if (move == FastBoard::NO_VERTEX) {
                        moves.clear();
                        break;
                    }
                    moves.push_back(move);
                }
            }
            if (moves.empty()) {
                return;
            }

            int color;
            if (textcolor == "w" || textcolor == "white") {
                color = FastBoard::WHITE;
            } else if (textcolor == "b" || textcolor == "black") {
                color = FastBoard::BLACK;
            } else {
                return;
            }

            if (until_movenum < 1) {
                return;
            }
            until_movenum += game.get_movenum() - 1;

            for (const auto& move : moves) {
                if (tag == "avoid") {
                    add_move_to_avoid(color, move, until_movenum);
                    if (move != FastBoard::PASS && move != FastBoard::RESIGN) {
                        if (color == FastBoard::BLACK) {
                            avoid_not_pass_resign_b = true;
                        } else {
                            avoid_not_pass_resign_w = true;
                        }
                    }
                } else {
                    add_move_to_allow(color, move, until_movenum);
                    if (color == FastBoard::BLACK) {
                        allow_b = true;
                    } else {
                        allow_w = true;
                    }
                }
            }
            if ((allow_b && avoid_not_pass_resign_b) ||
                (allow_w && avoid_not_pass_resign_w)) {
                /* If "allow" is in use, it is illegal to use "avoid" with any
                 * move that is not "pass" or "resign". */
                return;
            }
        } else if (tag == "w" || tag == "white") {
            m_who = FastBoard::WHITE;
        } else if (tag == "b" || tag == "black") {
            m_who = FastBoard::BLACK;
        } else if (tag == "interval") {
            cmdstream >> m_interval_centis;
            if (cmdstream.fail()) {
                return;
            }
        } else if (tag == "minmoves") {
            cmdstream >> m_min_moves;
            if (cmdstream.fail()) {
                return;
            }
        } else {
            return;
        }
    }
}

void AnalyzeTags::add_move_to_avoid(int color, int vertex, size_t until_move) {
    m_moves_to_avoid.emplace_back(color, until_move, vertex);
}

void AnalyzeTags::add_move_to_allow(int color, int vertex, size_t until_move) {
    m_moves_to_allow.emplace_back(color, until_move, vertex);
}

int AnalyzeTags::interval_centis() const {
    return m_interval_centis;
}

int AnalyzeTags::invalid() const {
    return m_invalid;
}

int AnalyzeTags::who() const {
    return m_who;
}

size_t AnalyzeTags::post_move_count() const {
    return m_min_moves;
}

bool AnalyzeTags::is_to_avoid(int color, int vertex, size_t movenum) const {
    for (auto& move : m_moves_to_avoid) {
        if (color == move.color && vertex == move.vertex && movenum <= move.until_move) {
            return true;
        }
    }
    if (vertex != FastBoard::PASS && vertex != FastBoard::RESIGN) {
        auto active_allow = false;
        for (auto& move : m_moves_to_allow) {
            if (color == move.color && movenum <= move.until_move) {
                active_allow = true;
                if (vertex == move.vertex) {
                    return false;
                }
            }
        }
        if (active_allow) {
            return true;
        }
    }
    return false;
}

bool AnalyzeTags::has_move_restrictions() const {
    return !m_moves_to_avoid.empty() || !m_moves_to_allow.empty();
}
#endif

std::unique_ptr<Network> GTP::s_network;

void GTP::initialize(std::unique_ptr<Network>&& net) {
    s_network = std::move(net);
/*
    bool result;
    std::string message;
    std::tie(result, message) =
        set_max_memory(cfg_max_memory, cfg_max_cache_ratio_percent);
    if (!result) {
        // This should only ever happen with 60 block networks on 32bit machine.
        myprintf("LOW MEMORY SETTINGS! Couldn't set default memory limits.\n");
        myprintf("The network you are using might be too big\n");
        myprintf("for the default settings on your system.\n");
        throw std::runtime_error("Error setting memory requirements.");
    }
    myprintf("%s\n", message.c_str());
*/
}

void GTP::setup_default_parameters() {
    cfg_gtp_mode = false;
    cfg_allow_pondering = true;

    // we will re-calculate this on Leela.cpp
    cfg_num_threads = 0;
    // we will re-calculate this on Leela.cpp
    cfg_batch_size = 0;

    cfg_max_memory = UCTSearch::DEFAULT_MAX_MEMORY;
    cfg_max_playouts = UCTSearch::UNLIMITED_PLAYOUTS;
    cfg_max_visits = UCTSearch::UNLIMITED_PLAYOUTS;
    // This will be overwriiten in initialize() after network size is known.
    cfg_max_tree_size = UCTSearch::DEFAULT_MAX_MEMORY;
    cfg_max_cache_ratio_percent = 10;
    cfg_timemanage = TimeManagement::AUTO;
    cfg_lagbuffer_cs = 100;
//    cfg_weightsfile = leelaz_file("best-network");
#ifdef USE_OPENCL
    cfg_gpus = { };
    cfg_sgemm_exhaustive = false;
    cfg_tune_only = false;

#ifdef USE_HALF
    cfg_precision = precision_t::AUTO;
#endif
#endif
    cfg_puct = 0.5f;
    cfg_logpuct = 0.015f;
    cfg_logconst = 1.7f;
    cfg_softmax_temp = 1.0f;
    cfg_fpu_reduction = 0.25f;
    // see UCTSearch::should_resign
    cfg_resignpct = -1;
    cfg_noise = false;
    cfg_fpu_root_reduction = cfg_fpu_reduction;
    cfg_ci_alpha = 1e-5f;
    cfg_lcb_min_visit_ratio = 0.10f;
    cfg_random_cnt = 0;
    cfg_random_min_visits = 1;
    cfg_random_temp = 1.0f;
    cfg_dumbpass = false;
    cfg_logfile_handle = nullptr;
    cfg_quiet = false;
    cfg_benchmark = false;
#ifdef USE_CPU_ONLY
    cfg_cpu_only = true;
#else
    cfg_cpu_only = false;
#endif

    cfg_analyze_tags = AnalyzeTags{};

    // C++11 doesn't guarantee *anything* about how random this is,
    // and in MinGW it isn't random at all. But we can mix it in, which
    // helps when it *is* high quality (Linux, MSVC).
    std::random_device rd;
    std::ranlux48 gen(rd());
    std::uint64_t seed1 = (gen() << 16) ^ gen();
    // If the above fails, this is one of our best, portable, bets.
    std::uint64_t seed2 = std::chrono::high_resolution_clock::
        now().time_since_epoch().count();
    cfg_rng_seed = seed1 ^ seed2;
}
#if 0
const std::string GTP::s_commands[] = {
    "protocol_version",
    "name",
    "version",
    "quit",
    "known_command",
    "list_commands",
    "boardsize",
    "clear_board",
    "komi",
    "play",
    "genmove",
    "showboard",
    "undo",
    "final_score",
    "final_status_list",
    "time_settings",
    "time_left",
    "fixed_handicap",
    "last_move",
    "move_history",
    "place_free_handicap",
    "set_free_handicap",
    "loadsgf",
    "printsgf",
    "kgs-genmove_cleanup",
    "kgs-time_settings",
    "kgs-game_over",
    "heatmap",
    "lz-analyze",
    "lz-genmove_analyze",
    "lz-memory_report",
    "lz-setoption",
    "gomill-explain_last_move",
    ""
};

// Default/min/max could be moved into separate fields,
// but for now we assume that the GUI will not send us invalid info.
const std::string GTP::s_options[] = {
    "option name Maximum Memory Use (MiB) type spin default 2048 min 128 max 131072",
    "option name Percentage of memory for cache type spin default 10 min 1 max 99",
    "option name Visits type spin default 0 min 0 max 1000000000",
    "option name Playouts type spin default 0 min 0 max 1000000000",
    "option name Lagbuffer type spin default 0 min 0 max 3000",
    "option name Resign Percentage type spin default -1 min -1 max 30",
    "option name Pondering type check default true",
    ""
};

std::string GTP::get_life_list(const GameState & game, bool live) {
    std::vector<std::string> stringlist;
    std::string result;
    const auto& board = game.board;

    if (live) {
        for (int i = 0; i < board.get_boardsize(); i++) {
            for (int j = 0; j < board.get_boardsize(); j++) {
                int vertex = board.get_vertex(i, j);

                if (board.get_state(vertex) != FastBoard::EMPTY) {
                    stringlist.push_back(board.get_string(vertex));
                }
            }
        }
    }

    // remove multiple mentions of the same string
    // unique reorders and returns new iterator, erase actually deletes
    std::sort(begin(stringlist), end(stringlist));
    stringlist.erase(std::unique(begin(stringlist), end(stringlist)),
                     end(stringlist));

    for (size_t i = 0; i < stringlist.size(); i++) {
        result += (i == 0 ? "" : "\n") + stringlist[i];
    }

    return result;
}

void GTP::execute(GameState & game, const std::string& xinput) {
    std::string input;
    static auto search = std::make_unique<UCTSearch>(game, *s_network);

    bool transform_lowercase = true;

    // Required on Unixy systems
    if (xinput.find("loadsgf") != std::string::npos) {
        transform_lowercase = false;
    }

    /* eat empty lines, simple preprocessing, lower case */
    for (unsigned int tmp = 0; tmp < xinput.size(); tmp++) {
        if (xinput[tmp] == 9) {
            input += " ";
        } else if ((xinput[tmp] > 0 && xinput[tmp] <= 9)
                || (xinput[tmp] >= 11 && xinput[tmp] <= 31)
                || xinput[tmp] == 127) {
               continue;
        } else {
            if (transform_lowercase) {
                input += std::tolower(xinput[tmp]);
            } else {
                input += xinput[tmp];
            }
        }

        // eat multi whitespace
        if (input.size() > 1) {
            if (std::isspace(input[input.size() - 2]) &&
                std::isspace(input[input.size() - 1])) {
                input.resize(input.size() - 1);
            }
        }
    }

    std::string command;
    int id = -1;

    if (input == "") {
        return;
    } else if (input == "exit") {
        exit(EXIT_SUCCESS);
    } else if (input.find("#") == 0) {
        return;
    } else if (std::isdigit(input[0])) {
        std::istringstream strm(input);
        char spacer;
        strm >> id;
        strm >> std::noskipws >> spacer;
        std::getline(strm, command);
    } else {
        command = input;
    }

    /* process commands */
    if (command == "protocol_version") {
        gtp_printf(id, "%d", GTP_VERSION);
        return;
    } else if (command == "name") {
        gtp_printf(id, PROGRAM_NAME);
        return;
    } else if (command == "version") {
        gtp_printf(id, PROGRAM_VERSION);
        return;
    } else if (command == "quit") {
        gtp_printf(id, "");
        exit(EXIT_SUCCESS);
    } else if (command.find("known_command") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;

        cmdstream >> tmp;     /* remove known_command */
        cmdstream >> tmp;

        for (int i = 0; s_commands[i].size() > 0; i++) {
            if (tmp == s_commands[i]) {
                gtp_printf(id, "true");
                return;
            }
        }

        gtp_printf(id, "false");
        return;
    } else if (command.find("list_commands") == 0) {
        std::string outtmp(s_commands[0]);
        for (int i = 1; s_commands[i].size() > 0; i++) {
            outtmp = outtmp + "\n" + s_commands[i];
        }
        gtp_printf(id, outtmp.c_str());
        return;
    } else if (command.find("boardsize") == 0) {
        std::istringstream cmdstream(command);
        std::string stmp;
        int tmp;

        cmdstream >> stmp;  // eat boardsize
        cmdstream >> tmp;

        if (!cmdstream.fail()) {
            if (tmp != BOARD_SIZE) {
                gtp_fail_printf(id, "unacceptable size");
            } else {
                float old_komi = game.get_komi();
                Training::clear_training();
                game.init_game(tmp, old_komi);
                gtp_printf(id, "");
            }
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("clear_board") == 0) {
        Training::clear_training();
        game.reset_game();
        search = std::make_unique<UCTSearch>(game, *s_network);
        assert(UCTNodePointer::get_tree_size() == 0);
        gtp_printf(id, "");
        return;
    } else if (command.find("komi") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        float komi = KOMI;
        float old_komi = game.get_komi();

        cmdstream >> tmp;  // eat komi
        cmdstream >> komi;

        if (!cmdstream.fail()) {
            if (komi != old_komi) {
                game.set_komi(komi);
            }
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("play") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        std::string color, vertex;

        cmdstream >> tmp;   //eat play
        cmdstream >> color;
        cmdstream >> vertex;

        if (!cmdstream.fail()) {
            if (!game.play_textmove(color, vertex)) {
                gtp_fail_printf(id, "illegal move");
            } else {
                gtp_printf(id, "");
            }
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("genmove") == 0
               || command.find("lz-genmove_analyze") == 0) {
        auto analysis_output = command.find("lz-genmove_analyze") == 0;

        std::istringstream cmdstream(command);
        std::string tmp;
        cmdstream >> tmp;  // eat genmove

        int who;
        AnalyzeTags tags;

        if (analysis_output) {
            tags = AnalyzeTags{cmdstream, game};
            if (tags.invalid()) {
                gtp_fail_printf(id, "cannot parse analyze tags");
                return;
            }
            who = tags.who();
        } else {
            /* genmove command */
            cmdstream >> tmp;
            if (tmp == "w" || tmp == "white") {
                who = FastBoard::WHITE;
            } else if (tmp == "b" || tmp == "black") {
                who = FastBoard::BLACK;
            } else {
                gtp_fail_printf(id, "syntax error");
                return;
            }
        }

        if (analysis_output) {
            // Start of multi-line response
            cfg_analyze_tags = tags;
            if (id != -1) gtp_printf_raw("=%d\n", id);
            else gtp_printf_raw("=\n");
        }
        // start thinking
        {
            game.set_to_move(who);
            // Outputs winrate and pvs for lz-genmove_analyze
            int move = search->think(who);
            game.play_move(move);

            std::string vertex = game.move_to_text(move);
            if (!analysis_output) {
                gtp_printf(id, "%s", vertex.c_str());
            } else {
                gtp_printf_raw("play %s\n", vertex.c_str());
            }
        }

        if (cfg_allow_pondering) {
            // now start pondering
            if (!game.has_resigned()) {
                // Outputs winrate and pvs through gtp for lz-genmove_analyze
                search->ponder();
            }
        }
        if (analysis_output) {
            // Terminate multi-line response
            gtp_printf_raw("\n");
        }
        cfg_analyze_tags = {};
        return;
    } else if (command.find("lz-analyze") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;

        cmdstream >> tmp; // eat lz-analyze
        AnalyzeTags tags{cmdstream, game};
        if (tags.invalid()) {
            gtp_fail_printf(id, "cannot parse analyze tags");
            return;
        }
        // Start multi-line response.
        if (id != -1) gtp_printf_raw("=%d\n", id);
        else gtp_printf_raw("=\n");
        // Now start pondering.
        if (!game.has_resigned()) {
            cfg_analyze_tags = tags;
            // Outputs winrate and pvs through gtp
            game.set_to_move(tags.who());
            search->ponder();
        }
        cfg_analyze_tags = {};
        // Terminate multi-line response
        gtp_printf_raw("\n");
        return;
    } else if (command.find("kgs-genmove_cleanup") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;

        cmdstream >> tmp;  // eat kgs-genmove
        cmdstream >> tmp;

        if (!cmdstream.fail()) {
            int who;
            if (tmp == "w" || tmp == "white") {
                who = FastBoard::WHITE;
            } else if (tmp == "b" || tmp == "black") {
                who = FastBoard::BLACK;
            } else {
                gtp_fail_printf(id, "syntax error");
                return;
            }
            game.set_passes(0);
            {
                game.set_to_move(who);
                int move = search->think(who, UCTSearch::NOPASS);
                game.play_move(move);

                std::string vertex = game.move_to_text(move);
                gtp_printf(id, "%s", vertex.c_str());
            }
            if (cfg_allow_pondering) {
                // now start pondering
                if (!game.has_resigned()) {
                    search->ponder();
                }
            }
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("undo") == 0) {
        if (game.undo_move()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "cannot undo");
        }
        return;
    } else if (command.find("showboard") == 0) {
        gtp_printf(id, "");
        game.display_state();
        return;
    } else if (command.find("final_score") == 0) {
        float ftmp = game.final_score();
        /* white wins */
        if (ftmp < -0.1) {
            gtp_printf(id, "W+%3.1f", float(fabs(ftmp)));
        } else if (ftmp > 0.1) {
            gtp_printf(id, "B+%3.1f", ftmp);
        } else {
            gtp_printf(id, "0");
        }
        return;
    } else if (command.find("final_status_list") == 0) {
        if (command.find("alive") != std::string::npos) {
            std::string livelist = get_life_list(game, true);
            gtp_printf(id, livelist.c_str());
        } else if (command.find("dead") != std::string::npos) {
            std::string deadlist = get_life_list(game, false);
            gtp_printf(id, deadlist.c_str());
        } else {
            gtp_printf(id, "");
        }
        return;
    } else if (command.find("time_settings") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        int maintime, byotime, byostones;

        cmdstream >> tmp >> maintime >> byotime >> byostones;

        if (!cmdstream.fail()) {
            // convert to centiseconds and set
            game.set_timecontrol(maintime * 100, byotime * 100, byostones, 0);

            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("time_left") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, color;
        int time, stones;

        cmdstream >> tmp >> color >> time >> stones;

        if (!cmdstream.fail()) {
            int icolor;

            if (color == "w" || color == "white") {
                icolor = FastBoard::WHITE;
            } else if (color == "b" || color == "black") {
                icolor = FastBoard::BLACK;
            } else {
                gtp_fail_printf(id, "Color in time adjust not understood.\n");
                return;
            }

            game.adjust_time(icolor, time * 100, stones);

            gtp_printf(id, "");

            if (cfg_allow_pondering) {
                // KGS sends this after our move
                // now start pondering
                if (!game.has_resigned()) {
                    search->ponder();
                }
            }
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("auto") == 0) {
        do {
            int move = search->think(game.get_to_move(), UCTSearch::NORMAL);
            game.play_move(move);
            game.display_state();

        } while (game.get_passes() < 2 && !game.has_resigned());

        return;
    } else if (command.find("go") == 0 && command.size() < 6) {
        int move = search->think(game.get_to_move());
        game.play_move(move);

        std::string vertex = game.move_to_text(move);
        myprintf("%s\n", vertex.c_str());
        return;
    } else if (command.find("heatmap") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        std::string symmetry;

        cmdstream >> tmp;   // eat heatmap
        cmdstream >> symmetry;
/*
        Network::Netresult vec;
        if (cmdstream.fail()) {
            // Default = DIRECT with no symmetric change
            vec = s_network->get_output(
                &game, Network::Ensemble::DIRECT,
                Network::IDENTITY_SYMMETRY, false);
        } else if (symmetry == "all") {
            for (auto s = 0; s < Network::NUM_SYMMETRIES; ++s) {
                vec = s_network->get_output(
                    &game, Network::Ensemble::DIRECT, s, false);
                Network::show_heatmap(&game, vec, false);
            }
        } else if (symmetry == "average" || symmetry == "avg") {
            vec = s_network->get_output(
                &game, Network::Ensemble::AVERAGE, -1, false);
        } else {
            vec = s_network->get_output(
                &game, Network::Ensemble::DIRECT, std::stoi(symmetry), false);
        }

        if (symmetry != "all") {
            Network::show_heatmap(&game, vec, false);
        }
*/
        gtp_printf(id, "");
        return;
    } else if (command.find("fixed_handicap") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        int stones;

        cmdstream >> tmp;   // eat fixed_handicap
        cmdstream >> stones;

        if (!cmdstream.fail() && game.set_fixed_handicap(stones)) {
            auto stonestring = game.board.get_stone_list();
            gtp_printf(id, "%s", stonestring.c_str());
        } else {
            gtp_fail_printf(id, "Not a valid number of handicap stones");
        }
        return;
    } else if (command.find("last_move") == 0) {
        auto last_move = game.get_last_move();
        if (last_move == FastBoard::NO_VERTEX) {
            gtp_fail_printf(id, "no previous move known");
            return;
        }
        auto coordinate = game.move_to_text(last_move);
        auto color = game.get_to_move() == FastBoard::WHITE ? "black" : "white";
        gtp_printf(id, "%s %s", color, coordinate.c_str());
        return;
    } else if (command.find("move_history") == 0) {
        if (game.get_movenum() == 0) {
            gtp_printf_raw("= \n");
        } else {
            gtp_printf_raw("= ");
        }
        auto game_history = game.get_game_history();
        // undone moves may still be present, so reverse the portion of the
        // array we need and resize to trim it down for iteration.
        std::reverse(begin(game_history),
                     begin(game_history) + game.get_movenum() + 1);
        game_history.resize(game.get_movenum());
        for (const auto &state : game_history) {
            auto coordinate = game.move_to_text(state->get_last_move());
            auto color = state->get_to_move() == FastBoard::WHITE ? "black" : "white";
            gtp_printf_raw("%s %s\n", color, coordinate.c_str());
        }
        gtp_printf_raw("\n");
        return;
    } else if (command.find("place_free_handicap") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        int stones;

        cmdstream >> tmp;   // eat place_free_handicap
        cmdstream >> stones;

        if (!cmdstream.fail()) {
            game.place_free_handicap(stones, *s_network);
            auto stonestring = game.board.get_stone_list();
            gtp_printf(id, "%s", stonestring.c_str());
        } else {
            gtp_fail_printf(id, "Not a valid number of handicap stones");
        }

        return;
    } else if (command.find("set_free_handicap") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;

        cmdstream >> tmp;   // eat set_free_handicap

        do {
            std::string vertex;

            cmdstream >> vertex;

            if (!cmdstream.fail()) {
                if (!game.play_textmove("black", vertex)) {
                    gtp_fail_printf(id, "illegal move");
                } else {
                    game.set_handicap(game.get_handicap() + 1);
                }
            }
        } while (!cmdstream.fail());

        std::string stonestring = game.board.get_stone_list();
        gtp_printf(id, "%s", stonestring.c_str());

        return;
    } else if (command.find("loadsgf") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, filename;
        int movenum;

        cmdstream >> tmp;   // eat loadsgf
        cmdstream >> filename;

        if (!cmdstream.fail()) {
            cmdstream >> movenum;

            if (cmdstream.fail()) {
                movenum = 999;
            }
        } else {
            gtp_fail_printf(id, "Missing filename.");
            return;
        }

        auto sgftree = std::make_unique<SGFTree>();

        try {
            sgftree->load_from_file(filename);
            game = sgftree->follow_mainline_state(movenum - 1);
            gtp_printf(id, "");
        } catch (const std::exception&) {
            gtp_fail_printf(id, "cannot load file");
        }
        return;
    } else if (command.find("kgs-chat") == 0) {
        // kgs-chat (game|private) Name Message
        std::istringstream cmdstream(command);
        std::string tmp;

        cmdstream >> tmp; // eat kgs-chat
        cmdstream >> tmp; // eat game|private
        cmdstream >> tmp; // eat player name
        do {
            cmdstream >> tmp; // eat message
        } while (!cmdstream.fail());

        gtp_fail_printf(id, "I'm a go bot, not a chat bot.");
        return;
    } else if (command.find("kgs-game_over") == 0) {
        // Do nothing. Particularly, don't ponder.
        gtp_printf(id, "");
        return;
    } else if (command.find("kgs-time_settings") == 0) {
        // none, absolute, byoyomi, or canadian
        std::istringstream cmdstream(command);
        std::string tmp;
        std::string tc_type;
        int maintime, byotime, byostones, byoperiods;

        cmdstream >> tmp >> tc_type;

        if (tc_type.find("none") != std::string::npos) {
            // 30 mins
            game.set_timecontrol(30 * 60 * 100, 0, 0, 0);
        } else if (tc_type.find("absolute") != std::string::npos) {
            cmdstream >> maintime;
            game.set_timecontrol(maintime * 100, 0, 0, 0);
        } else if (tc_type.find("canadian") != std::string::npos) {
            cmdstream >> maintime >> byotime >> byostones;
            // convert to centiseconds and set
            game.set_timecontrol(maintime * 100, byotime * 100, byostones, 0);
        } else if (tc_type.find("byoyomi") != std::string::npos) {
            // KGS style Fischer clock
            cmdstream >> maintime >> byotime >> byoperiods;
            game.set_timecontrol(maintime * 100, byotime * 100, 0, byoperiods);
        } else {
            gtp_fail_printf(id, "syntax not understood");
            return;
        }

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("netbench") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp;
        int iterations;

        cmdstream >> tmp;  // eat netbench
        cmdstream >> iterations;

        if (!cmdstream.fail()) {
            s_network->benchmark(&game, iterations);
        } else {
            s_network->benchmark(&game);
        }
        gtp_printf(id, "");
        return;

    } else if (command.find("printsgf") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, filename;

        cmdstream >> tmp;   // eat printsgf
        cmdstream >> filename;

        auto sgf_text = SGFTree::state_to_string(game, 0);
        // GTP says consecutive newlines terminate the output,
        // so we must filter those.
        boost::replace_all(sgf_text, "\n\n", "\n");

        if (cmdstream.fail()) {
            gtp_printf(id, "%s\n", sgf_text.c_str());
        } else {
            std::ofstream out(filename);
            out << sgf_text;
            out.close();
            gtp_printf(id, "");
        }

        return;
    } else if (command.find("load_training") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, filename;

        // tmp will eat "load_training"
        cmdstream >> tmp >> filename;

        Training::load_training(filename);

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("save_training") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, filename;

        // tmp will eat "save_training"
        cmdstream >> tmp >>  filename;

        Training::save_training(filename);

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("dump_training") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, winner_color, filename;
        int who_won;

        // tmp will eat "dump_training"
        cmdstream >> tmp >> winner_color >> filename;

        if (winner_color == "w" || winner_color == "white") {
            who_won = FullBoard::WHITE;
        } else if (winner_color == "b" || winner_color == "black") {
            who_won = FullBoard::BLACK;
        } else {
            gtp_fail_printf(id, "syntax not understood");
            return;
        }

        Training::dump_training(who_won, filename);

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("dump_debug") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, filename;

        // tmp will eat "dump_debug"
        cmdstream >> tmp >> filename;

        Training::dump_debug(filename);

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }

        return;
    } else if (command.find("dump_supervised") == 0) {
        std::istringstream cmdstream(command);
        std::string tmp, sgfname, outname;

        // tmp will eat dump_supervised
        cmdstream >> tmp >> sgfname >> outname;

        Training::dump_supervised(sgfname, outname);

        if (!cmdstream.fail()) {
            gtp_printf(id, "");
        } else {
            gtp_fail_printf(id, "syntax not understood");
        }
        return;
    } else if (command.find("lz-memory_report") == 0) {
        auto base_memory = get_base_memory();
        auto tree_size = add_overhead(UCTNodePointer::get_tree_size());
        auto cache_size = add_overhead(s_network->get_estimated_cache_size());

        auto total = base_memory + tree_size + cache_size;
        gtp_printf(id,
            "Estimated total memory consumption: %d MiB.\n"
            "Network with overhead: %d MiB / Search tree: %d MiB / Network cache: %d\n",
            total / MiB, base_memory / MiB, tree_size / MiB, cache_size / MiB);
        return;
    } else if (command.find("lz-setoption") == 0) {
        return execute_setoption(*search.get(), id, command);
    } else if (command.find("gomill-explain_last_move") == 0) {
        gtp_printf(id, "%s\n", search->explain_last_think().c_str());
        return;
    }
    gtp_fail_printf(id, "unknown command");
    return;
}

std::pair<std::string, std::string> GTP::parse_option(std::istringstream& is) {
    std::string token, name, value;

    // Read option name (can contain spaces)
    while (is >> token && token != "value")
        name += std::string(" ", name.empty() ? 0 : 1) + token;

    // Read option value (can contain spaces)
    while (is >> token)
        value += std::string(" ", value.empty() ? 0 : 1) + token;

    return std::make_pair(name, value);
}

size_t GTP::get_base_memory() {
    // At the moment of writing the memory consumption is
    // roughly network size + 85 for one GPU and + 160 for two GPUs.
#ifdef USE_OPENCL
    auto gpus = std::max(cfg_gpus.size(), size_t{1});
    return s_network->get_estimated_size() + 85 * MiB * gpus;
#else
    return s_network->get_estimated_size();
#endif
}

std::pair<bool, std::string> GTP::set_max_memory(size_t max_memory,
    int cache_size_ratio_percent) {
    if (max_memory == 0) {
        max_memory = UCTSearch::DEFAULT_MAX_MEMORY;
    }

    // Calculate amount of memory available for the search tree +
    // NNCache by estimating a constant memory overhead first.
    auto base_memory = get_base_memory();

    if (max_memory < base_memory) {
        return std::make_pair(false, "Not enough memory for network. " +
            std::to_string(base_memory / MiB) + " MiB required.");
    }

    auto max_memory_for_search = max_memory - base_memory;

    assert(cache_size_ratio_percent >= 1);
    assert(cache_size_ratio_percent <= 99);
    auto max_cache_size = max_memory_for_search *
        cache_size_ratio_percent / 100;

    auto max_cache_count =
        (int)(remove_overhead(max_cache_size) / NNCache::ENTRY_SIZE);

    // Verify if the setting would not result in too little cache.
    if (max_cache_count < NNCache::MIN_CACHE_COUNT) {
        return std::make_pair(false, "Not enough memory for cache.");
    }
    auto max_tree_size = max_memory_for_search - max_cache_size;

    if (max_tree_size < UCTSearch::MIN_TREE_SPACE) {
        return std::make_pair(false, "Not enough memory for search tree.");
    }

    // Only if settings are ok we store the values in config.
    cfg_max_memory = max_memory;
    cfg_max_cache_ratio_percent = cache_size_ratio_percent;
    // Set max_tree_size.
    cfg_max_tree_size = remove_overhead(max_tree_size);
    // Resize cache.
    s_network->nncache_resize(max_cache_count);

    return std::make_pair(true, "Setting max tree size to " +
        std::to_string(max_tree_size / MiB) + " MiB and cache size to " +
        std::to_string(max_cache_size / MiB) +
        " MiB.");
}

void GTP::execute_setoption(UCTSearch & search,
                            int id, const std::string &command) {
    std::istringstream cmdstream(command);
    std::string tmp, name_token;

    // Consume lz_setoption, name.
    cmdstream >> tmp >> name_token;

    // Print available options if called without an argument.
    if (cmdstream.fail()) {
        std::string options_out_tmp("");
        for (int i = 0; s_options[i].size() > 0; i++) {
            options_out_tmp = options_out_tmp + "\n" + s_options[i];
        }
        gtp_printf(id, options_out_tmp.c_str());
        return;
    }

    if (name_token.find("name") != 0) {
        gtp_fail_printf(id, "incorrect syntax for lz-setoption");
        return;
    }

    std::string name, value;
    std::tie(name, value) = parse_option(cmdstream);

    if (name == "maximum memory use (mib)") {
        std::istringstream valuestream(value);
        int max_memory_in_mib;
        valuestream >> max_memory_in_mib;
        if (!valuestream.fail()) {
            if (max_memory_in_mib < 128 || max_memory_in_mib > 131072) {
                gtp_fail_printf(id, "incorrect value");
                return;
            }
            bool result;
            std::string reason;
            std::tie(result, reason) = set_max_memory(max_memory_in_mib * MiB,
                cfg_max_cache_ratio_percent);
            if (result) {
                gtp_printf(id, reason.c_str());
            } else {
                gtp_fail_printf(id, reason.c_str());
            }
            return;
        } else {
            gtp_fail_printf(id, "incorrect value");
            return;
        }
    } else if (name == "percentage of memory for cache") {
        std::istringstream valuestream(value);
        int cache_size_ratio_percent;
        valuestream >> cache_size_ratio_percent;
        if (cache_size_ratio_percent < 1 || cache_size_ratio_percent > 99) {
            gtp_fail_printf(id, "incorrect value");
            return;
        }
        bool result;
        std::string reason;
        std::tie(result, reason) = set_max_memory(cfg_max_memory,
            cache_size_ratio_percent);
        if (result) {
            gtp_printf(id, reason.c_str());
        } else {
            gtp_fail_printf(id, reason.c_str());
        }
        return;
    } else if (name == "visits") {
        std::istringstream valuestream(value);
        int visits;
        valuestream >> visits;
        cfg_max_visits = visits;

        // 0 may be specified to mean "no limit"
        if (cfg_max_visits == 0) {
            cfg_max_visits = UCTSearch::UNLIMITED_PLAYOUTS;
        }
        // Note that if the visits are changed but no
        // explicit command to set memory usage is given,
        // we will stick with the initial guess we made on startup.
        search.set_visit_limit(cfg_max_visits);

        gtp_printf(id, "");
    } else if (name == "playouts") {
        std::istringstream valuestream(value);
        int playouts;
        valuestream >> playouts;
        cfg_max_playouts = playouts;

        // 0 may be specified to mean "no limit"
        if (cfg_max_playouts == 0) {
            cfg_max_playouts = UCTSearch::UNLIMITED_PLAYOUTS;
        } else if (cfg_allow_pondering) {
            // Limiting playouts while pondering is still enabled
            // makes no sense.
            gtp_fail_printf(id, "incorrect value");
            return;
        }

        // Note that if the playouts are changed but no
        // explicit command to set memory usage is given,
        // we will stick with the initial guess we made on startup.
        search.set_playout_limit(cfg_max_playouts);

        gtp_printf(id, "");
    } else if (name == "lagbuffer") {
        std::istringstream valuestream(value);
        int lagbuffer;
        valuestream >> lagbuffer;
        cfg_lagbuffer_cs = lagbuffer;
        gtp_printf(id, "");
    } else if (name == "pondering") {
        std::istringstream valuestream(value);
        std::string toggle;
        valuestream >> toggle;
        if (toggle == "true") {
            if (cfg_max_playouts != UCTSearch::UNLIMITED_PLAYOUTS) {
                gtp_fail_printf(id, "incorrect value");
                return;
            }
            cfg_allow_pondering = true;
        } else if (toggle == "false") {
            cfg_allow_pondering = false;
        } else {
            gtp_fail_printf(id, "incorrect value");
            return;
        }
        gtp_printf(id, "");
    } else if (name == "resign percentage") {
        std::istringstream valuestream(value);
        int resignpct;
        valuestream >> resignpct;
        cfg_resignpct = resignpct;
        gtp_printf(id, "");
    } else {
        gtp_fail_printf(id, "Unknown option");
    }
    return;
}
#endif

max_issues_repo_name: yssaya/komaochi

max_forks_repo_name: kobanium/aoba-zero


text:
#pragma once
#include <unordered_map>
#include <string>
#include <memory>
#include <boost/thread/thread.hpp>
#include <stdint.h>
#include <atomic>
#include <chrono>

class Bucket {
private:
    std::atomic<double> _zero_time;
public:
    Bucket(double zero_time = 0.0) :
        _zero_time(zero_time) {};
    bool aquire(double now, double rate, double burst_size, double consume = 1.0) {
        auto zero_time_old = _zero_time.load();
        double zero_time_new;
        do {
            double tokens = std::min((now - zero_time_old) * rate, burst_size);
            if (tokens < consume) {
                return false;
            } else {
                tokens -= consume;
            }
            zero_time_new = now - tokens / rate;
        } while(!_zero_time.compare_exchange_weak(zero_time_old, zero_time_new));

        return true;
    }
};

class RateLimiter {
private:
    double _rate;
    double _burst_size;

    std::unordered_map<std::string, std::unique_ptr<Bucket> > _key_to_bucket;
    boost::shared_mutex _key_to_bucket_mu;

    static double now() {
        using dur = std::chrono::duration<double>;
        auto const now = std::chrono::steady_clock::now().time_since_epoch();
        return std::chrono::duration_cast<dur>(now).count();
    }
public:
    RateLimiter(double rate, double burst_size) :
        _rate(rate),
        _burst_size(burst_size) {};
    bool aquire(std::string &key) {
        boost::upgrade_lock<boost::shared_mutex> lock(_key_to_bucket_mu);

        auto bucket_it = _key_to_bucket.find(key);
        if (bucket_it == _key_to_bucket.end()) {
            boost::upgrade_to_unique_lock<boost::shared_mutex> unique_lock(lock);
            std::unique_ptr<Bucket> bucket(new Bucket());
            _key_to_bucket[key] = std::move(bucket);
            return true;
        } else {
            return bucket_it->second->aquire(now(), _rate, _burst_size);
        }
    };
};

ext: hpp

max_stars_repo_name: spebern/cpp-burst-pool-node

max_issues_repo_name: spebern/cpp-burst-pool-node

max_forks_repo_name: spebern/cpp-burst-pool-node


text:
#ifndef TRUNCATEDSVD_HPP
#define TRUNCATEDSVD_HPP

#include <armadillo>
#include <vector>

template <class Real>
void TruncatedSvd(long m, long n, std::vector<Real> &A, Real tolerance,
                  bool is_relative, long &r, std::vector<Real> &U,
                  std::vector<Real> &s, std::vector<Real> &Vt);

template <class Real>
void TruncatedSvd(const arma::Mat<Real> &A, Real tolerance, bool is_relative,
                  arma::Mat<Real> &U, arma::Col<Real> &s, arma::Mat<Real> &V,
                  long &r);

#endif

max_stars_repo_name: saibalde/tensortrain

max_issues_repo_name: saibalde/tensortrain

max_forks_repo_name: saibalde/tensortrain


text:
#include <boost/python.hpp>
#include <boost/test/unit_test.hpp>

#include <ackward/datetime/DateTime.hpp>
#include <ackward/datetime/Module.hpp>
#include <ackward/time/Module.hpp>

using namespace ackward::core;
using namespace ackward::datetime;
using namespace boost::python;

BOOST_AUTO_TEST_SUITE( datetime )
BOOST_AUTO_TEST_SUITE( datetime )

BOOST_AUTO_TEST_CASE( objectConstructor)
{
    object obj = ackward::datetime::module().attr("datetime")(1,1,1);
    DateTime d(obj);
}

BOOST_AUTO_TEST_CASE( today )
{
    DateTime d = DateTime::today();
}

BOOST_AUTO_TEST_CASE( now )
{
    DateTime d = DateTime::now();
}

BOOST_AUTO_TEST_CASE( utcnow )
{
    DateTime d = DateTime::utcnow();
}

BOOST_AUTO_TEST_CASE( fromtimestamp )
{
    DateTime d = DateTime::fromtimestamp(ackward::time::time());
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: abingham/ackward

max_issues_repo_name: abingham/ackward

max_forks_repo_name: abingham/ackward


text:
/*
 * main.cpp
 *
 *  Created on: 21/mag/2012
 *      Author: Fabrizio
 */

#include <boost/iterator/filter_iterator.hpp>
#include <iostream>

int main(int argc, char *argv[]) {
	int numbers[] = {1, 2, 4, 5, 7, 87, 0, 9, 10};
    const int N = sizeof(numbers)/sizeof(int);

	typedef boost::filter_iterator<bool (*)(int), int*> filter_iterator;

	auto is_even = [](int value) -> bool{ return (value % 2) == 0; };
	filter_iterator begin(is_even, numbers);
	filter_iterator end(is_even, numbers + N);

	std::for_each(begin, end, [](int value){ std::cout << "Number " << value << " is even." << std::endl;});

	return EXIT_SUCCESS;
}

max_stars_repo_name: dexpota/boost-api-examples

max_issues_repo_name: dexpota/boost-api-examples

max_forks_repo_name: dexpota/boost-api-examples


text:
#include <exception>
#include <functional>
#include <memory>
#include <stdexcept>
#include <boost/scope_exit.hpp>

#include <Windows.h>

#include "Window.h"
#include "ToastDemo.h"

WORD const IDM_BTN_CREATE_SHORTCUT = 1;
WORD const IDM_BTN_REMOVE_SHORTCUT = 2;
WORD const IDM_BTN_DISPLAY_TOAST = 3;

int APIENTRY wWinMain(
    _In_ HINSTANCE hInstance,
    _In_opt_ HINSTANCE hPrevInstance,
    _In_ LPTSTR lpCmdLine,
    _In_ int nCmdShow)
{
    try {
        if (FAILED(CoInitialize(nullptr)))
            throw std::runtime_error("cannot initialize COM");

        BOOST_SCOPE_EXIT(void) {
            CoUninitialize();
        } BOOST_SCOPE_EXIT_END;

        // create 3 buttons, one for creating shortcut, one for removing shortcut,
        // and one for display toast
        auto createHandler = [](Window *const window, std::unique_ptr<ToastDemo>& toastDemo) -> LRESULT {
            try {
                auto CreateButton = [](HWND const hWnd, WORD const IDM, std::wstring const& text, int x, int y, int width, int height) -> HWND {
                    return CreateWindow(
                        L"BUTTON", text.c_str(),
                        WS_TABSTOP | WS_VISIBLE | WS_CHILD | BS_DEFPUSHBUTTON,
                        x, y, width, height,
                        hWnd,
                        reinterpret_cast<HMENU>(IDM),
                        reinterpret_cast<HINSTANCE>(GetWindowLongPtr(hWnd, GWLP_HINSTANCE)),
                        nullptr
                    );
                };

                CreateButton(window->GetHWND(), IDM_BTN_CREATE_SHORTCUT, L"Create shortcut", 10, 10, 150, 30);
                CreateButton(window->GetHWND(), IDM_BTN_DISPLAY_TOAST, L"Display toast", 170, 10, 150, 30);
                CreateButton(window->GetHWND(), IDM_BTN_REMOVE_SHORTCUT, L"Remove shortcut", 340, 10, 150, 30);

                toastDemo = std::make_unique<ToastDemo>(window->GetHWND());
            }
            catch (std::exception const& e) {
                MessageBoxA(window->GetHWND(), e.what(), "Error", MB_ICONERROR);
            }

            return 0;
        };

        auto commandHandler = [](Window *const window, WPARAM wParam, std::unique_ptr<ToastDemo>& toastDemo) -> LRESULT {
            try {
                switch (LOWORD(wParam)) {
                case IDM_BTN_CREATE_SHORTCUT:
                    toastDemo->CreateShortcut();
                    MessageBox(window->GetHWND(), L"Shortcut is created.", L"Information", MB_ICONINFORMATION);
                    break;

                case IDM_BTN_REMOVE_SHORTCUT:
                    toastDemo->RemoveShortcut();
                    MessageBox(window->GetHWND(), L"Shortcut is removed.", L"Information", MB_ICONINFORMATION);
                    break;

                case IDM_BTN_DISPLAY_TOAST:
                    toastDemo->Display();
                    break;

                default:
                    break;
                }
            }
            catch (std::exception const& e) {
                MessageBoxA(window->GetHWND(), e.what(), "Error", MB_ICONERROR);
            }

            return 0;
        };

        auto destroyHandler = [](std::unique_ptr<ToastDemo>& toastDemo) -> LRESULT {
            toastDemo.release();
            PostQuitMessage(0);
            return 0;
        };

        Window window;
        std::unique_ptr<ToastDemo> toastDemo;

        // add message handlers
        window.AddMessageHandler(
            WM_CREATE,
            std::bind(createHandler,
                std::placeholders::_1,
                std::ref(toastDemo)
            )
        );
        window.AddMessageHandler(
            WM_COMMAND,
            std::bind(commandHandler,
                std::placeholders::_1,
                std::placeholders::_3,
                std::ref(toastDemo)
            )
        );
        window.AddMessageHandler(
            WM_DESTROY,
            std::bind(destroyHandler,
                std::ref(toastDemo)
            )
        );

        // now create window
        window.Create(L"WindowClass", L"Title");
        window.Show(nCmdShow);

        // and enter message loop
        return (int)window.DoMessageLoop();
    }
    catch (const std::exception& e) {
        MessageBoxA(nullptr, e.what(), "Error", MB_ICONERROR);
    }
    catch (...) {
        MessageBox(nullptr, L"Unknown exception.", L"Error", MB_ICONERROR);
    }
    return -1;
}