#include "onlinenodehyperplane.h"
#include "nodeinfogain.h"
#include "utilities.h"
#include <boost/foreach.hpp>

OnlineNodeHyperplane::OnlineNodeHyperplane(const HyperParameters &hp, int depth, int numFeatures,
        std::vector<int>& labelStatistics) : OnlineNode(hp, depth, numFeatures, labelStatistics), m_bestFeature( -1 ),
        m_bestThreshold( 0.0 ) {
    m_features.reserve(hp.numRandomFeatures);
    m_nodeConf.resize(m_hp->numClasses, 0.0);
    // select random features and thresholds
    for ( int feat = 0; feat < hp.numRandomFeatures; feat++) {
        Feature f;
        f.indeces = randPerm(numFeatures, m_hp->numProjFeatures );
        f.weights = std::vector<float>(m_hp->numProjFeatures, 0.0 );
        fillWithRandomNumbers(f.weights);
        f.bestStatistics = 0;
        f.bestGini = 1e10;
        // We assume having the same number of random thresholds as random features
        std::vector<float> thresholds(/*hp.numRandomFeatures*/ 1,0.0);
        fillWithRandomNumbers(thresholds);
        std::vector<float>::iterator thrIt(thresholds.begin());
        std::vector<float>::iterator thrEnd(thresholds.end());
        while (thrIt != thrEnd) {
            *thrIt = *thrIt; // * (float)hp.numRandomFeatures;
            ++thrIt;
        }
        BOOST_FOREACH(float thr,thresholds) {
            Statistics stat;
            stat.lTotal = 0;
            stat.rTotal = 0;
            stat.lCount.resize(m_hp->numLabeled,0);
            stat.rCount.resize(m_hp->numLabeled,0);
            stat.threshold = thr;
            f.statistics.push_back(stat);
        }
        m_features.push_back(f);
    }
}

OnlineNodeHyperplane::OnlineNodeHyperplane(const HyperParameters &hp, int depth, int numFeatures) :
        OnlineNode(hp, depth, numFeatures), m_bestFeature( -1 ),
        m_bestThreshold( 0.0 ) {
    m_features.reserve(hp.numRandomFeatures);
    m_nodeConf.resize(m_hp->numClasses, 0.0);
    // select random features and thresholds
    for ( int feat = 0; feat < hp.numRandomFeatures; feat++) {
        Feature f;
        f.indeces = randPerm(numFeatures, m_hp->numProjFeatures );
        f.weights = std::vector<float>(m_hp->numProjFeatures, 0.0 );
        fillWithRandomNumbers(f.weights);
        f.bestStatistics = 0;
        f.bestGini = 1e10;
        // We assume having the same number of random thresholds as random features
        std::vector<float> thresholds(1/*hp.numRandomFeatures*/,0.0);
        fillWithRandomNumbers(thresholds);
        std::vector<float>::iterator thrIt(thresholds.begin());
        std::vector<float>::iterator thrEnd(thresholds.end());
        while (thrIt != thrEnd) {
            *thrIt = *thrIt;// * (float)hp.numRandomFeatures;
            ++thrIt;
        }

        BOOST_FOREACH(float thr,thresholds) {
            Statistics stat;
            stat.lTotal = 0;
            stat.rTotal = 0;
            stat.lCount.resize(m_hp->numLabeled,0);
            stat.rCount.resize(m_hp->numLabeled,0);
            stat.threshold = thr;
            f.statistics.push_back(stat);
        }
        m_features.push_back(f);
    }
}

void OnlineNodeHyperplane::updateHypotheses(const std::vector<float>& sample, const int label) {
    // Go over each feature and each threshold and update the statistics
    m_numSamples++;
    double bestGini = 1e10;

    std::vector<Feature>::iterator featureIt(m_features.begin());
    std::vector<Feature>::iterator featureEnd(m_features.end());
    double tmpGini = 1e10;
    int currentFeature = 0;

    while ( featureIt != featureEnd ) {
        std::vector<Statistics>::iterator statIt(featureIt->statistics.begin());
        std::vector<Statistics>::iterator statEnd(featureIt->statistics.end());
        float val =  0.0; //sample[featureIt->index];
        int counter = 0;
        BOOST_FOREACH(int feat, featureIt->indeces) {
            val += sample[feat] * featureIt->weights[counter];
            counter++;
        }

        int currentStatistics = 0;

        while ( statIt != statEnd ) {
            if ( val < statIt->threshold ) {
                statIt->lTotal++;
                statIt->lCount[label]++;
            } else {
                statIt->rTotal++;
                statIt->rCount[label]++;
            }
            double lTotal = static_cast<double>(statIt->lTotal) + 1e-10;
            double rTotal = static_cast<double>(statIt->rTotal) + 1e-10;
            double lGini = 0.0, rGini = 0.0;
            for ( int c = 0; c < m_hp->numClasses; c++ ) {
                lGini += static_cast<double>(statIt->lCount[c])*(1.0 - static_cast<double>(statIt->lCount[c]/lTotal));
                rGini += static_cast<double>(statIt->rCount[c])*(1.0 - static_cast<double>(statIt->rCount[c]/rTotal));
            }
            tmpGini = (lGini + rGini)/(lTotal + rTotal);

            if (tmpGini < bestGini) {
                if (tmpGini < m_bestGini) {
                    m_secondBestGini = m_bestGini;
                    m_bestGini = tmpGini;
                }
                bestGini = tmpGini;
                featureIt->bestStatistics = currentStatistics;
                m_bestThreshold = statIt->threshold;
                m_bestFeature = currentFeature;
            }
            ++statIt;
            ++currentStatistics;
        }

        ++featureIt;
        ++currentFeature;
    }

}

void OnlineNodeHyperplane::evalNode(const matrix<float>& data, const std::vector<int>& inBagSamples,
                                    std::vector<int>& leftNodeSamples, std::vector<int>& rightNodeSamples) {
    BOOST_FOREACH(int n, inBagSamples) {
        if (data(n, m_bestFeature) > m_bestThreshold) {
            rightNodeSamples.push_back(n);
        } else {
            leftNodeSamples.push_back(n);
        }
    }
}

double OnlineNodeHyperplane::eval(const std::vector<float>& sample, const int label) {
    if (m_isLeaf) {
        return (double)m_nodeLabel;
    } else {
        float val =  0.0;
        int counter = 0;
        BOOST_FOREACH(int feat, m_bestIndeces) {
            val += sample[feat] * m_bestWeights[counter];
            counter++;
        }

        return (val < m_bestThreshold) ? m_leftChildNode->eval(sample,label) :
               m_rightChildNode->eval(sample,label);
    }
}

void OnlineNodeHyperplane::cleanUpAndFreeze() {
    // Do some cleaning here
    m_bestIndeces = m_features[m_bestFeature].indeces;
    m_bestWeights = m_features[m_bestFeature].weights;
    m_features.clear();
}

void OnlineNodeHyperplane::train(const std::vector<float>& sample, const int label) {
    if (m_isLeaf) {
        updateHypotheses(sample, label);

        bool doSplit = shouldISplit();

        if ( doSplit ) {
            /*
            cout << "Split occured! numSamples " << m_numSamples << endl;
            dispVector(m_epsilons);
            cout << endl;
            dispVector(m_deltaGinis);
            exit(0);
            */
            m_isLeaf = false;

            if (m_hp->verbose) {
                cout << "Node #: " << m_nodeIndex << " selected feature #: " << m_bestFeature;
                cout << " and the threshold is: " << m_bestThreshold << " at depth " << m_depth << endl;
            }

            // split the data

            // forward statistics to child nodes
            std::vector<int> lLabelStatistics(m_hp->numClasses,0), rLabelStatistics(m_hp->numClasses,0);
            Statistics bestStatistics = m_features[ m_bestFeature ].statistics[ m_features[m_bestFeature].bestStatistics ];
            std::vector<int>::const_iterator it(bestStatistics.lCount.begin());
            std::vector<int>::const_iterator end(bestStatistics.lCount.end());
            for (int c = 0; c < m_hp->numClasses && it != end; c++ ) {
                lLabelStatistics[c] = *it;
                ++it;
            }
            it = bestStatistics.rCount.begin();
            end = bestStatistics.rCount.end();
            for (int c = 0; c < m_hp->numClasses && it != end; c++ ) {
                rLabelStatistics[c] = *it;
                ++it;
            }

            // clean up the current node and freeze its state
            cleanUpAndFreeze();

            // pass them to the left and right child, respectively
            m_leftChildNode = Ptr(new OnlineNodeHyperplane(*m_hp,m_depth + 1,m_numFeatures,lLabelStatistics));
            m_rightChildNode = Ptr(new OnlineNodeHyperplane(*m_hp,m_depth + 1,m_numFeatures,rLabelStatistics));

            m_leftChildNode->train(sample,label);
            m_rightChildNode->train(sample,label);

        } else { // Update Statistics
            if (m_hp->verbose) {
                cout << "Node #: " << m_nodeIndex << " is terminal, at depth " << m_depth << endl;
            }

            // calc confidence, labels, etc
            m_isLeaf = true;

            m_labelStatistics[label]++;

            int bestClass = 0;
            float bestLabel = 0;
            std::vector<int>::iterator labelIt = m_labelStatistics.begin(), labelEnd = m_labelStatistics.end();

            int tmpN = 0;
            while ( labelIt != labelEnd) {
                if (*labelIt > bestLabel) {
                    bestLabel = *labelIt;
                    bestClass = tmpN;
                }
                ++labelIt;
                ++tmpN;
            }
            //cout << "NodeLabel: " << m_nodeLabel << " Real Label: " << label << " Best Class: " << bestClass << endl;
            m_nodeLabel = bestClass;
        }
    } else {
        float val =  0.0;
        int counter = 0;
        BOOST_FOREACH(int feat, m_bestIndeces) {
            val += sample[feat] * m_bestWeights[counter];
            counter++;
        }
        if (val < m_bestThreshold) {
            m_leftChildNode->train(sample,label);
        } else {
            m_rightChildNode->train(sample,label);
        }
    }
}

void OnlineNodeHyperplane::eval(const matrix<float>& data, const std::vector<int>& sampleIndeces,
                                matrix<float>& confidences, std::vector<int>& predictions) {
    if (m_isLeaf) {
        // Make predictions and confidences
        int tmpN;
        BOOST_FOREACH( int n, sampleIndeces) {
            predictions[n] = m_nodeLabel;
            tmpN = 0;
            BOOST_FOREACH(float conf, m_nodeConf) {
                confidences(n, tmpN) = conf;
                tmpN++;
            }
        }
    } else {
        // split the data
        std::vector<int> leftNodeSamples, rightNodeSamples;
        evalNode(data,sampleIndeces,leftNodeSamples,rightNodeSamples);

        m_leftChildNode->eval(data,leftNodeSamples,confidences,predictions);
        m_rightChildNode->eval(data,rightNodeSamples,confidences,predictions);
    }
}


#ifdef _USE_PCL_
#include "LidarRegistration.h"
#include <pcl/registration/icp.h>           //ICP类相关头文件
#include <pcl/registration/icp_nl.h>        //非线性ICP 相关头文件
#include <pcl/registration/transforms.h>      //变换矩阵类头文件

#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/fpfh_omp.h> //包含fpfh加速计算的omp(多核并行计算)
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_features.h> //特征的错误对应关系去除
#include <pcl/registration/correspondence_rejection_sample_consensus.h> //随机采样一致性去除

long LidarRegistration::LidarRegistration_ICP(pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                          const char* pathRegistration)
{
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;   //创建IterativeClosestPoint的对象
    icp.setInputCloud(input_cloud);                                 //cloud_in设置为点云的源点
    icp.setInputTarget(ref_cloud);                                  //cloud_out设置为与cloud_in对应的匹配目标
    icp.setMaxCorrespondenceDistance(800);
    icp.setMaximumIterations(30);

    pcl::PointCloud<pcl::PointXYZ> registration;                    //存储经过配准变换点云后的点云
    icp.align(registration);                                        //打印配准相关输入信息
    std::cout << "has converged:" << icp.hasConverged() <<" score: " <<icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    //配准点云保存到文件中
    pcl::io::savePCDFileASCII (pathRegistration, registration); //将点云保存到PCD文件中
    return 0;
}

long LidarRegistration::LidarRegistration_FPFH(pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                          const char* pathRegistration)
{
    clock_t start,end,time;
    start  = clock();
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh =  compute_fpfh_feature(input_cloud,tree);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_fpfh =  compute_fpfh_feature(ref_cloud,tree);

    //对齐(占用了大部分运行时间)
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ,pcl::PointXYZ,pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(input_cloud);
    sac_ia.setSourceFeatures(source_fpfh);
    sac_ia.setInputTarget(ref_cloud);
    sac_ia.setTargetFeatures(target_fpfh);
    pcl::PointCloud<pcl::PointXYZ>::Ptr align (new pcl::PointCloud<pcl::PointXYZ>());
    //sac_ia.setNumberOfSamples(20);  //设置每次迭代计算中使用的样本数量（可省）,可节省时间
    sac_ia.setCorrespondenceRandomness(6); //设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
    sac_ia.align(*align);
    end = clock();
    pcl::io::savePCDFile (pathRegistration, *align);
    printf("calculate time is: %lf\n",float (end-start)/CLOCKS_PER_SEC);
}

pcl::PointCloud<pcl::FPFHSignature33>::Ptr LidarRegistration::compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{
    //法向量
    pcl::PointCloud<pcl::Normal>::Ptr point_normal (new  pcl::PointCloud<pcl::Normal>());
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> est_normal;
    est_normal.setInputCloud(input_cloud);
    est_normal.setSearchMethod(tree);
    est_normal.setKSearch(10);
    est_normal.compute(*point_normal);
    //fpfh 估计
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh (new pcl::PointCloud<pcl::FPFHSignature33>);
    //pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_target_fpfh;
    pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_fpfh;
    est_fpfh.setNumberOfThreads(4); //指定4核计算
    // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree4 (new pcl::search::KdTree<pcl::PointXYZ> ());
    est_fpfh.setInputCloud(input_cloud);
    est_fpfh.setInputNormals(point_normal);
    est_fpfh.setSearchMethod(tree);
    est_fpfh.setKSearch(10);
    est_fpfh.compute(*fpfh);

    return fpfh;
}

#endif


// Copyright (C) 2019-2020 Zilliz. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance
// with the License. You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software distributed under the License
// is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
// or implied. See the License for the specific language governing permissions and limitations under the License

#include "SearchBruteForce.h"
#include <vector>
#include <common/Types.h>
#include <boost/dynamic_bitset.hpp>
#include <queue>
#include "SubSearchResult.h"

#include <faiss/utils/distances.h>
#include <faiss/utils/BinaryDistance.h>

namespace milvus::query {

// copy from knowhere
// disable lint to make further migration easier
static void
raw_search(MetricType metric_type,
           const uint8_t* xb,  //
           int64_t ntotal,     //
           int code_size,      //
           idx_t n,            // num_queries
           const uint8_t* x,   //
           idx_t k,            // topk
           float* D,
           idx_t* labels,
           const BitsetView bitset) {
    using namespace faiss;  // NOLINT
    if (metric_type == METRIC_Jaccard || metric_type == METRIC_Tanimoto) {
        float_maxheap_array_t res = {size_t(n), size_t(k), labels, D};
        binary_distance_knn_hc(METRIC_Jaccard, &res, x, xb, ntotal, code_size, bitset);

        if (metric_type == METRIC_Tanimoto) {
            for (int i = 0; i < k * n; i++) {
                D[i] = Jaccard_2_Tanimoto(D[i]);
            }
        }

    } else if (metric_type == METRIC_Hamming) {
        std::vector<int32_t> int_distances(n * k);
        int_maxheap_array_t res = {size_t(n), size_t(k), labels, int_distances.data()};
        binary_distance_knn_hc(METRIC_Hamming, &res, x, xb, ntotal, code_size, bitset);
        for (int i = 0; i < n * k; ++i) {
            D[i] = int_distances[i];
        }

    } else if (metric_type == METRIC_Substructure || metric_type == METRIC_Superstructure) {
        // only matched ids will be chosen, not to use heap
        binary_distance_knn_mc(metric_type, x, xb, n, ntotal, k, code_size, D, labels, bitset);
    } else {
        PanicInfo("unsupported");
    }
}

SubSearchResult
BinarySearchBruteForceFast(MetricType metric_type,
                           int64_t dim,
                           const uint8_t* binary_chunk,
                           int64_t size_per_chunk,
                           int64_t topk,
                           int64_t num_queries,
                           const uint8_t* query_data,
                           const faiss::BitsetView& bitset) {
    SubSearchResult sub_result(num_queries, topk, metric_type);
    float* result_distances = sub_result.get_values();
    idx_t* result_labels = sub_result.get_labels();

    int64_t code_size = dim / 8;
    const idx_t block_size = size_per_chunk;

    raw_search(metric_type, binary_chunk, size_per_chunk, code_size, num_queries, query_data, topk, result_distances,
               result_labels, bitset);

    return sub_result;
}

SubSearchResult
FloatSearchBruteForce(const dataset::SearchDataset& dataset,
                      const void* chunk_data_raw,
                      int64_t size_per_chunk,
                      const faiss::BitsetView& bitset) {
    auto metric_type = dataset.metric_type;
    auto num_queries = dataset.num_queries;
    auto topk = dataset.topk;
    auto dim = dataset.dim;
    SubSearchResult sub_qr(num_queries, topk, metric_type);
    auto query_data = reinterpret_cast<const float*>(dataset.query_data);
    auto chunk_data = reinterpret_cast<const float*>(chunk_data_raw);

    if (metric_type == MetricType::METRIC_L2) {
        faiss::float_maxheap_array_t buf{(size_t)num_queries, (size_t)topk, sub_qr.get_labels(), sub_qr.get_values()};
        faiss::knn_L2sqr(query_data, chunk_data, dim, num_queries, size_per_chunk, &buf, bitset);
        return sub_qr;
    } else {
        faiss::float_minheap_array_t buf{(size_t)num_queries, (size_t)topk, sub_qr.get_labels(), sub_qr.get_values()};
        faiss::knn_inner_product(query_data, chunk_data, dim, num_queries, size_per_chunk, &buf, bitset);
        return sub_qr;
    }
}

SubSearchResult
BinarySearchBruteForce(const dataset::SearchDataset& dataset,
                       const void* chunk_data_raw,
                       int64_t size_per_chunk,
                       const faiss::BitsetView& bitset) {
    // TODO: refactor the internal function
    auto query_data = reinterpret_cast<const uint8_t*>(dataset.query_data);
    auto chunk_data = reinterpret_cast<const uint8_t*>(chunk_data_raw);
    return BinarySearchBruteForceFast(dataset.metric_type, dataset.dim, chunk_data, size_per_chunk, dataset.topk,
                                      dataset.num_queries, query_data, bitset);
}
}  // namespace milvus::query


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef WEINBERG_ANGLE_H
#define WEINBERG_ANGLE_H

#include <Eigen/Core>

namespace flexiblesusy {

namespace weinberg_angle {

/**
 * @class Weinberg_angle
 * @brief Class to calculate the DR-bar weak mixing angle
 */
class Weinberg_angle {
public:
   /**
    * @class Data
    * @brief Model parameters necessary for calculating weak mixing angle
    *
    * @attention The W and Z self-energies are assumed to be
    * calculated using the top quark pole mass, instead of the top
    * quark DR-bar mass.
    */
   struct Data {
      Data();

      double scale;                  ///< renormalization scale
      double alpha_em_drbar;         ///< alpha_em(MZ, DR-bar, SUSY)
      double fermi_contant;          ///< Fermi constant
      double self_energy_z_at_mz;    ///< self-energy Z at p = MZ, mt = mt_pole
      double self_energy_w_at_0;     ///< self-energy W at p = 0, mt = mt_pole
      double self_energy_w_at_mw;    ///< self-energy W at p = MW, mt = mt_pole
      double mw_pole;                ///< W pole mass
      double mz_pole;                ///< Z pole mass
      double mt_pole;                ///< top quark pole mass
      double mh_drbar;               ///< lightest CP-even Higgs DR-bar mass
      double hmix_12;                ///< CP-even Higgs mixing Cos(alpha)
      double msel_drbar;             ///< left-handed selectron DR-bar mass
      double msmul_drbar;            ///< left-handed smuon DR-bar mass
      double msve_drbar;             ///< electron-sneutrino DR-bar mass
      double msvm_drbar;             ///< muon-sneutrino DR-bar mass
      Eigen::ArrayXd mn_drbar;       ///< Neutralino DR-bar mass
      Eigen::ArrayXd mc_drbar;       ///< Chargino DR-bar mass
      Eigen::MatrixXcd zn;           ///< Neutralino mixing matrix
      Eigen::MatrixXcd um;           ///< Chargino mixing matrix
      Eigen::MatrixXcd up;           ///< Chargino mixing matrix
      double gY;                     ///< U(1)_Y gauge coupling
      double g2;                     ///< SU(2)_L gauge coupling
      double g3;                     ///< SU(3)_c gauge coupling
      double tan_beta;               ///< tan(beta) = vu / vd
   };

   struct Self_energy_data {
      Self_energy_data();
      double scale;                  ///< renormalization scale
      double mt_pole;                ///< top quark pole mass
      double mt_drbar;               ///< top quark DR-bar mass
      double mb_drbar;               ///< bottom quark DR-bar mass
      double gY;                     ///< U(1)_Y gauge coupling
      double g2;                     ///< SU(2)_L gauge coupling
   };

   Weinberg_angle();

   void enable_susy_contributions(); ///< enable susy contributions
   void disable_susy_contributions(); ///< disable susy contributions

   void set_data(const Data&);       ///< set data necessary for the calculation
   void set_number_of_iterations(int); ///< maximum number of iterations
   void set_number_of_loops(int);    ///< set number of loops
   void set_precision_goal(double);  ///< set precision goal
   double get_rho_hat() const;       ///< returns the rho parameter
   double get_sin_theta() const;     ///< returns sin(theta_w)

   /// calculates the sinus of the Weinberg angle
   int calculate(double rho_start = 1.0, double sin_start = 0.48);

   static double replace_mtop_in_self_energy_z(double, double, const Self_energy_data&);
   static double replace_mtop_in_self_energy_w(double, double, const Self_energy_data&);

private:
   int number_of_iterations; ///< maximum number of iterations
   int number_of_loops;      ///< number of loops
   double precision_goal;         ///< precision goal
   double rho_hat;                ///< output rho-hat parameter
   double sin_theta;              ///< output sin(theta)
   Data data;
   bool susy_contributions;       ///< model type

   static double calculate_delta_r(double, double, const Data&, bool add_susy_contributions = true, int number_of_loops = 2);
   static double calculate_delta_rho(double, double, const Data&, bool add_susy_contributions = true, int number_of_loops = 2);
   static double calculate_delta_vb(double, double, const Data&, bool add_susy_contributions = true);
   static double calculate_delta_vb_sm(double, double, const Data&);
   static double calculate_delta_vb_susy(double, const Data&);
   static double rho_2(double);

   static double calculate_self_energy_z_top(double, double, const Self_energy_data&);
   static double calculate_self_energy_w_top(double, double, const Self_energy_data&);
};

} // namespace weinberg_angle

} // namespace flexiblesusy

#endif


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

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

#include <boost/simd.hpp>
#include <boost/simd/algorithm.hpp>
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/bitwise.hpp>
#include <boost/simd/boolean.hpp>
#include <boost/simd/constant.hpp>
#include <boost/simd/eulerian.hpp>
#include <boost/simd/exponential.hpp>
#include <boost/simd/hyperbolic.hpp>
#include <boost/simd/ieee.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/memory.hpp>
#include <boost/simd/operator.hpp>
#include <boost/simd/predicates.hpp>
#include <boost/simd/range.hpp>
#include <boost/simd/reduction.hpp>
#include <boost/simd/swar.hpp>
#include <boost/simd/trigonometric.hpp>

#include <simd_test.hpp>

STF_CASE("Check Main global include files")
{
  STF_PASS("Include files are OK");
}


// Copyright (c) 2012 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include "libmv/simple_pipeline/keyframe_selection.h"

#include "libmv/numeric/numeric.h"
#include "ceres/ceres.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/homography.h"
#include "libmv/multiview/fundamental.h"
#include "libmv/simple_pipeline/intersect.h"
#include "libmv/simple_pipeline/bundle.h"

#include <Eigen/Eigenvalues>

namespace libmv {
namespace {

Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) {
  Mat3 T = Mat3::Identity(), S = Mat3::Identity();

  T(0, 2) = -intrinsics.principal_point_x();
  T(1, 2) = -intrinsics.principal_point_y();

  S(0, 0) /= intrinsics.focal_length_x();
  S(1, 1) /= intrinsics.focal_length_y();

  return S * T;
}

// P.H.S. Torr
// Geometric Motion Segmentation and Model Selection
//
// http://reference.kfupm.edu.sa/content/g/e/geometric_motion_segmentation_and_model__126445.pdf
//
// d is the number of dimensions modeled
//     (d = 3 for a fundamental matrix or 2 for a homography)
// k is the number of degrees of freedom in the model
//     (k = 7 for a fundamental matrix or 8 for a homography)
// r is the dimension of the data
//     (r = 4 for 2D correspondences between two frames)
double GRIC(const Vec &e, int d, int k, int r) {
  int n = e.rows();
  double lambda1 = log(static_cast<double>(r));
  double lambda2 = log(static_cast<double>(r * n));

  // lambda3 limits the residual error, and this paper
  // http://elvera.nue.tu-berlin.de/files/0990Knorr2006.pdf
  // suggests using lambda3 of 2
  // same value is used in Torr's Problem of degeneracy in structure
  // and motion recovery from uncalibrated image sequences
  // http://www.robots.ox.ac.uk/~vgg/publications/papers/torr99.ps.gz
  double lambda3 = 2.0;

  // Variance of tracker position. Physically, this is typically about 0.1px,
  // and when squared becomes 0.01 px^2.
  double sigma2 = 0.01;

  // Finally, calculate the GRIC score.
  double gric = 0.0;
  for (int i = 0; i < n; i++) {
    gric += std::min(e(i) * e(i) / sigma2, lambda3 * (r - d));
  }
  gric += lambda1 * d * n;
  gric += lambda2 * k;
  return gric;
}

// Compute a generalized inverse using eigen value decomposition, clamping the
// smallest eigenvalues if requested. This is needed to compute the variance of
// reconstructed 3D points.
//
// TODO(keir): Consider moving this into the numeric code, since this is not
// related to keyframe selection.
Mat PseudoInverseWithClampedEigenvalues(const Mat &matrix,
                                        int num_eigenvalues_to_clamp) {
  Eigen::EigenSolver<Mat> eigen_solver(matrix);
  Mat D = eigen_solver.pseudoEigenvalueMatrix();
  Mat V = eigen_solver.pseudoEigenvectors();

  // Clamp too-small singular values to zero to prevent numeric blowup.
  double epsilon = std::numeric_limits<double>::epsilon();
  for (int i = 0; i < D.cols(); ++i) {
    if (D(i, i) > epsilon) {
      D(i, i) = 1.0 / D(i, i);
    } else {
      D(i, i) = 0.0;
    }
  }

  // Apply the clamp.
  for (int i = D.cols() - num_eigenvalues_to_clamp; i < D.cols(); ++i) {
    D(i, i) = 0.0;
  }
  return V * D * V.inverse();
}

void FilterZeroWeightMarkersFromTracks(const Tracks &tracks,
                                       Tracks *filtered_tracks) {
  vector<Marker> all_markers = tracks.AllMarkers();

  for (int i = 0; i < all_markers.size(); ++i) {
    Marker &marker = all_markers[i];
    if (marker.weight != 0.0) {
      filtered_tracks->Insert(marker.image,
                              marker.track,
                              marker.x,
                              marker.y,
                              marker.weight);
    }
  }
}

}  // namespace

void SelectKeyframesBasedOnGRICAndVariance(const Tracks &_tracks,
                                           const CameraIntrinsics &intrinsics,
                                           vector<int> &keyframes) {
  // Mirza Tahir Ahmed, Matthew N. Dailey
  // Robust key frame extraction for 3D reconstruction from video streams
  //
  // http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf

  Tracks filtered_tracks;
  FilterZeroWeightMarkersFromTracks(_tracks, &filtered_tracks);

  int max_image = filtered_tracks.MaxImage();
  int next_keyframe = 1;
  int number_keyframes = 0;

  // Limit correspondence ratio from both sides.
  // On the one hand if number of correspondent features is too low,
  // triangulation will suffer.
  // On the other hand high correspondence likely means short baseline.
  // which also will affect om accuracy
  const double Tmin = 0.8;
  const double Tmax = 1.0;

  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  Mat3 N_inverse = N.inverse();

  double Sc_best = std::numeric_limits<double>::max();
  double success_intersects_factor_best = 0.0f;

  while (next_keyframe != -1) {
    int current_keyframe = next_keyframe;
    double Sc_best_candidate = std::numeric_limits<double>::max();

    LG << "Found keyframe " << next_keyframe;

    number_keyframes++;
    next_keyframe = -1;

    for (int candidate_image = current_keyframe + 1;
         candidate_image <= max_image;
         candidate_image++) {
      // Conjunction of all markers from both keyframes
      vector<Marker> all_markers =
        filtered_tracks.MarkersInBothImages(current_keyframe,
                                            candidate_image);

      // Match keypoints between frames current_keyframe and candidate_image
      vector<Marker> tracked_markers =
        filtered_tracks.MarkersForTracksInBothImages(current_keyframe,
                                                     candidate_image);

      // Correspondences in normalized space
      Mat x1, x2;
      CoordinatesForMarkersInImage(tracked_markers, current_keyframe, &x1);
      CoordinatesForMarkersInImage(tracked_markers, candidate_image, &x2);

      LG << "Found " << x1.cols()
         << " correspondences between " << current_keyframe
         << " and " << candidate_image;

      // Not enough points to construct fundamental matrix
      if (x1.cols() < 8 || x2.cols() < 8)
        continue;

      // STEP 1: Correspondence ratio constraint
      int Tc = tracked_markers.size();
      int Tf = all_markers.size();
      double Rc = static_cast<double>(Tc) / Tf;

      LG << "Correspondence between " << current_keyframe
         << " and " << candidate_image
         << ": " << Rc;

      if (Rc < Tmin || Rc > Tmax)
        continue;

      Mat3 H, F;

      // Estimate homography using default options.
      EstimateHomographyOptions estimate_homography_options;
      EstimateHomography2DFromCorrespondences(x1,
                                              x2,
                                              estimate_homography_options,
                                              &H);

      // Convert homography to original pixel space.
      H = N_inverse * H * N;

      EstimateFundamentalOptions estimate_fundamental_options;
      EstimateFundamentalFromCorrespondences(x1,
                                             x2,
                                             estimate_fundamental_options,
                                             &F);

      // Convert fundamental to original pixel space.
      F = N_inverse * F * N;

      // TODO(sergey): STEP 2: Discard outlier matches

      // STEP 3: Geometric Robust Information Criteria

      // Compute error values for homography and fundamental matrices
      Vec H_e, F_e;
      H_e.resize(x1.cols());
      F_e.resize(x1.cols());
      for (int i = 0; i < x1.cols(); i++) {
        Vec2 current_x1, current_x2;

        intrinsics.NormalizedToImageSpace(x1(0, i), x1(1, i),
                                          &current_x1(0), &current_x1(1));

        intrinsics.NormalizedToImageSpace(x2(0, i), x2(1, i),
                                          &current_x2(0), &current_x2(1));

        H_e(i) = SymmetricGeometricDistance(H, current_x1, current_x2);
        F_e(i) = SymmetricEpipolarDistance(F, current_x1, current_x2);
      }

      LG << "H_e: " << H_e.transpose();
      LG << "F_e: " << F_e.transpose();

      // Degeneracy constraint
      double GRIC_H = GRIC(H_e, 2, 8, 4);
      double GRIC_F = GRIC(F_e, 3, 7, 4);

      LG << "GRIC values for frames " << current_keyframe
         << " and " << candidate_image
         << ", H-GRIC: " << GRIC_H
         << ", F-GRIC: " << GRIC_F;

      if (GRIC_H <= GRIC_F)
        continue;

      // TODO(sergey): STEP 4: PELC criterion

      // STEP 5: Estimation of reconstruction error
      //
      // Uses paper Keyframe Selection for Camera Motion and Structure
      // Estimation from Multiple Views
      // Uses ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf
      // Basically, equation (15)
      //
      // TODO(sergey): separate all the constraints into functions,
      //               this one is getting to much cluttered already

      // Definitions in equation (15):
      // - I is the number of 3D feature points
      // - A is the number of essential parameters of one camera

      EuclideanReconstruction reconstruction;

      // The F matrix should be an E matrix, but squash it just to be sure

      // Reconstruction should happen using normalized fundamental matrix
      Mat3 F_normal = N * F * N_inverse;

      Mat3 E;
      FundamentalToEssential(F_normal, &E);

      // Recover motion between the two images. Since this function assumes a
      // calibrated camera, use the identity for K
      Mat3 R;
      Vec3 t;
      Mat3 K = Mat3::Identity();

      if (!MotionFromEssentialAndCorrespondence(E,
                                                K, x1.col(0),
                                                K, x2.col(0),
                                                &R, &t)) {
        LG << "Failed to compute R and t from E and K";
        continue;
      }

      LG << "Camera transform between frames " << current_keyframe
         << " and " << candidate_image
         << ":\nR:\n" << R
         << "\nt:" << t.transpose();

      // First camera is identity, second one is relative to it
      reconstruction.InsertCamera(current_keyframe,
                                  Mat3::Identity(),
                                  Vec3::Zero());
      reconstruction.InsertCamera(candidate_image, R, t);

      // Reconstruct 3D points
      int intersects_total = 0, intersects_success = 0;
      for (int i = 0; i < tracked_markers.size(); i++) {
        if (!reconstruction.PointForTrack(tracked_markers[i].track)) {
          vector<Marker> reconstructed_markers;

          int track = tracked_markers[i].track;

          reconstructed_markers.push_back(tracked_markers[i]);

          // We know there're always only two markers for a track
          // Also, we're using brute-force search because we don't
          // actually know about markers layout in a list, but
          // at this moment this cycle will run just once, which
          // is not so big deal

          for (int j = i + 1; j < tracked_markers.size(); j++) {
            if (tracked_markers[j].track == track) {
              reconstructed_markers.push_back(tracked_markers[j]);
              break;
            }
          }

          intersects_total++;

          if (EuclideanIntersect(reconstructed_markers, &reconstruction)) {
            LG << "Ran Intersect() for track " << track;
            intersects_success++;
          } else {
            LG << "Filed to intersect track " << track;
          }
        }
      }

      double success_intersects_factor =
          (double) intersects_success / intersects_total;

      if (success_intersects_factor < success_intersects_factor_best) {
        LG << "Skip keyframe candidate because of "
              "lower successful intersections ratio";

        continue;
      }

      success_intersects_factor_best = success_intersects_factor;

      Tracks two_frames_tracks(tracked_markers);
      PolynomialCameraIntrinsics empty_intrinsics;
      BundleEvaluation evaluation;
      evaluation.evaluate_jacobian = true;

      EuclideanBundleCommonIntrinsics(two_frames_tracks,
                                      BUNDLE_NO_INTRINSICS,
                                      BUNDLE_NO_CONSTRAINTS,
                                      &reconstruction,
                                      &empty_intrinsics,
                                      &evaluation);

      Mat &jacobian = evaluation.jacobian;

      Mat JT_J = jacobian.transpose() * jacobian;
      // There are 7 degrees of freedom, so clamp them out.
      Mat JT_J_inv = PseudoInverseWithClampedEigenvalues(JT_J, 7);

      Mat temp_derived = JT_J * JT_J_inv * JT_J;
      bool is_inversed = (temp_derived - JT_J).cwiseAbs2().sum() <
          1e-4 * std::min(temp_derived.cwiseAbs2().sum(),
                          JT_J.cwiseAbs2().sum());

      LG << "Check on inversed: " << (is_inversed ? "true" : "false" )
         << ", det(JT_J): " << JT_J.determinant();

      if (!is_inversed) {
        LG << "Ignoring candidature due to poor jacobian stability";
        continue;
      }

      Mat Sigma_P;
      Sigma_P = JT_J_inv.bottomRightCorner(evaluation.num_points * 3,
                                           evaluation.num_points * 3);

      int I = evaluation.num_points;
      int A = 12;

      double Sc = static_cast<double>(I + A) / Square(3 * I) * Sigma_P.trace();

      LG << "Expected estimation error between "
         << current_keyframe << " and "
         << candidate_image << ": " << Sc;

      // Pairing with a lower Sc indicates a better choice
      if (Sc > Sc_best_candidate)
        continue;

      Sc_best_candidate = Sc;

      next_keyframe = candidate_image;
    }

    // This is a bit arbitrary and main reason of having this is to deal
    // better with situations when there's no keyframes were found for
    // current keyframe this could happen when there's no so much parallax
    // in the beginning of image sequence and then most of features are
    // getting occluded. In this case there could be good keyframe pair in
    // the middle of the sequence
    //
    // However, it's just quick hack and smarter way to do this would be nice
    if (next_keyframe == -1) {
      next_keyframe = current_keyframe + 10;
      number_keyframes = 0;

      if (next_keyframe >= max_image)
        break;

      LG << "Starting searching for keyframes starting from " << next_keyframe;
    } else {
      // New pair's expected reconstruction error is lower
      // than existing pair's one.
      //
      // For now let's store just one candidate, easy to
      // store more candidates but needs some thoughts
      // how to choose best one automatically from them
      // (or allow user to choose pair manually).
      if (Sc_best > Sc_best_candidate) {
        keyframes.clear();
        keyframes.push_back(current_keyframe);
        keyframes.push_back(next_keyframe);
        Sc_best = Sc_best_candidate;
      }
    }
  }
}

}  // namespace libmv


#pragma once

#include <type_traits>
#include "Legolas/EigenInterface.hxx"
#include <Eigen/Core>
#include "Legolas/Array/Block/BaseBlock.hxx"
#include "stringConvert.hxx"
#include "Legolas/Array/Block/BlockExpression.hxx"
#include <array>

namespace Legolas{

  template <int SIZE, int... SUB_SIZES>
  struct BlockShape{
    using Element=BlockShape<SUB_SIZES...>;

    constexpr static int elementFlatSize_=Element::flatSize_;
    constexpr static int size_=SIZE;
    constexpr static int flatSize_=size_*elementFlatSize_;

    template <class SCALAR_TYPE>
    constexpr static SCALAR_TYPE * getElement(int i,SCALAR_TYPE * data){
      return data+i*elementFlatSize_;
    }

    template <template <class,class> class BLOCK_VIEW, class SCALAR_TYPE>
    using BlockElement=BLOCK_VIEW<SCALAR_TYPE,Element>;

    constexpr static const bool is1D=false;
  };

  template <int SIZE>
  struct BlockShape<SIZE>{
    using Element=void;
    constexpr static int size_=SIZE;
    constexpr static int flatSize_=size_;
    constexpr static int elementFlatSize_=1;

    template <template <class,class> class BLOCK_VIEW, class SCALAR_TYPE>
    using BlockElement=SCALAR_TYPE &;

    template <class SCALAR_TYPE>
    constexpr static SCALAR_TYPE & getElement(int i,SCALAR_TYPE * data){
      return *(data+i);
    }
    constexpr static const bool is1D=true;
  };

  template <class SCALAR_TYPE, class BLOCK_SHAPE>
  struct BlockView: public BaseBlock<BlockView<SCALAR_TYPE,BLOCK_SHAPE>>{

    using ScalarType=SCALAR_TYPE;
    using Shape=BLOCK_SHAPE;
    using ElementShape=typename Shape::Element;

    //LP: If I remove the Legolas namespace: compilation fails... odd
    using Element=typename Shape::template BlockElement<Legolas::BlockView,ScalarType>;

    ScalarType * data_;
    BlockView(ScalarType * data):data_(data){}

    constexpr int size() const {return Shape::size_;}

    Element operator[](int i){return Shape::getElement(i,data_);}
    const Element operator[](int i) const {return Shape::getElement(i,data_);}

    using EigenBlock=Eigen::Array<ScalarType,Shape::flatSize_, 1>;
    using EigenBlockMap=typename Legolas::EigenBlockMap<EigenBlock>::Map;
    using ConstEigenBlockMap=typename Legolas::EigenBlockMap<const EigenBlock>::Map;

    constexpr EigenBlockMap getEigenView( void ){ return EigenBlockMap(data_);}
    constexpr ConstEigenBlockMap getEigenView( void ) const { return ConstEigenBlockMap(data_);}

    template <class DERIVED>
    BlockView & operator=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()=right.getBlockRef().getEigenView();
      return (*this);
    }
    template <class DERIVED>
    BlockView & operator+=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()+=right.getBlockRef().getEigenView();
      return (*this);
    }
    template <class DERIVED>
    BlockView & operator-=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()-=right.getBlockRef().getEigenView();
      return (*this);
    }

    BlockView & operator=(ScalarType value){
      (*this).getEigenView()=value;
      return (*this);
    }
    BlockView & operator*=(ScalarType value){
      (*this).getEigenView()*=value;
      return (*this);
    }

    std::ostream & display(std::ostream& os) const{
      os<<"[";
      for (int i=0 ; i<this->size();i++){
        os <<(*this)[i];
        if (i!=(this->size()-1)) os<<",";
      }
      os<<"]";
      return os;
    }

  };

  template <class SCALAR_TYPE, class BLOCK_SHAPE>
  std::ostream& operator << (std::ostream& os, const BlockView<SCALAR_TYPE,BLOCK_SHAPE> & b){
    return b.display(os);
    //    return os;
  }

  template <class SCALAR_TYPE, class BLOCK_SHAPE>
  struct Block: public BaseBlock<Block<SCALAR_TYPE,BLOCK_SHAPE>>{

    using ScalarType=SCALAR_TYPE;
    using Shape=BLOCK_SHAPE;
    using ElementShape=typename Shape::Element;

    //LP: If I remove the Legolas namespace: compilation fails... odd
    using Element=typename Shape::template BlockElement<Legolas::BlockView,ScalarType>;

    alignas(32) std::array<ScalarType,Shape::flatSize_> data_;
    Block( void ):data_(){}

    constexpr int size() const {return Shape::size_;}

    constexpr ScalarType * ptr() {return &data_[0];}
    constexpr const ScalarType * ptr() const  {return &data_[0];}

    Element operator[](int i){return Shape::getElement(i,ptr());}
    const Element operator[](int i) const {return const_cast<ScalarType *>(Shape::getElement(i,ptr()));}

    using EigenBlock=Eigen::Array<ScalarType,Shape::flatSize_, 1>;
    using EigenBlockMap=typename Legolas::EigenBlockMap<EigenBlock>::Map;
    using ConstEigenBlockMap=typename Legolas::EigenBlockMap<const EigenBlock>::Map;

    constexpr EigenBlockMap getEigenView( void ){ return EigenBlockMap(ptr());}
    constexpr ConstEigenBlockMap getEigenView( void ) const { return ConstEigenBlockMap(ptr());}

    template <class DERIVED>
    Block & operator=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()=right.getBlockRef().getEigenView();
      return (*this);
    }
    template <class DERIVED>
    Block & operator+=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()+=right.getBlockRef().getEigenView();
      return (*this);
    }
    template <class DERIVED>
    Block & operator-=(const BaseBlock<DERIVED> & right){
      (*this).getEigenView()-=right.getBlockRef().getEigenView();
      return (*this);
    }

    Block & operator=(ScalarType value){
      (*this).getEigenView()=value;
      return (*this);
    }
    Block & operator*=(ScalarType value){
      (*this).getEigenView()*=value;
      return (*this);
    }

    std::ostream & display(std::ostream& os) const{
      os<<"[";
      for (int i=0 ; i<this->size();i++){
        os <<(*this)[i];
        if (i!=(this->size()-1)) os<<",";
      }
      os<<"]";
      return os;
    }

  };

  template <class SCALAR_TYPE, class BLOCK_SHAPE>
  std::ostream& operator << (std::ostream& os, const Block<SCALAR_TYPE,BLOCK_SHAPE> & b){
    return b.display(os);
    //    return os;
  }

}


#include <iostream>                    // std::cout
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <chrono>                      // std::chrono
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // SegNet
#include "transforms.hpp"              // transforms_Compose
#include "datasets.hpp"                // datasets::ImageFolderSegmentWithPaths
#include "dataloader.hpp"              // DataLoader::ImageFolderSegmentWithPaths
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// ---------------
// Test Function
// ---------------
void test(po::variables_map &vm, torch::Device &device, SegNet &model, std::vector<transforms_Compose> &transformI, std::vector<transforms_Compose> &transformO){

    // (0) Initialization and Declaration
    size_t correct, correct_per_class, total_class_pixel, class_count;
    float ave_loss;
    double seconds, ave_time;
    double pixel_wise_accuracy, ave_pixel_wise_accuracy;
    double mean_accuracy, ave_mean_accuracy;
    std::string path, result_dir, fname;
    std::string input_dir, output_dir;
    std::ofstream ofs;
    std::chrono::system_clock::time_point start, end;
    std::tuple<torch::Tensor, torch::Tensor, std::vector<std::string>, std::vector<std::string>, std::vector<std::tuple<unsigned char, unsigned char, unsigned char>>> data;
    torch::Tensor image, label, output, output_argmax, answer_mask, response_mask;
    torch::Tensor loss;
    datasets::ImageFolderSegmentWithPaths dataset;
    DataLoader::ImageFolderSegmentWithPaths dataloader;

    // (1) Get Test Dataset
    input_dir = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_in_dir"].as<std::string>();
    output_dir = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_out_dir"].as<std::string>();
    dataset = datasets::ImageFolderSegmentWithPaths(input_dir, output_dir, transformI, transformO);
    dataloader = DataLoader::ImageFolderSegmentWithPaths(dataset, /*batch_size_=*/1, /*shuffle_=*/false, /*num_workers_=*/0);
    std::cout << "total test images : " << dataset.size() << std::endl << std::endl;

    // (2) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + ".pth";
    torch::load(model, path, device);

    // (3) Set Loss Function
    auto criterion = Loss();

    // (4) Initialization of Value
    ave_loss = 0.0;
    ave_pixel_wise_accuracy = 0.0;
    ave_mean_accuracy = 0.0;
    ave_time = 0.0;

    // (5) Tensor Forward
    model->eval();
    result_dir = vm["test_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ofs.open(result_dir + "/loss.txt", std::ios::out);
    while (dataloader(data)){

        image = std::get<0>(data).to(device);
        label = std::get<1>(data).to(device);

        torch::cuda::synchronize();
        start = std::chrono::system_clock::now();

        output = model->forward(image);

        torch::cuda::synchronize();
        end = std::chrono::system_clock::now();
        seconds = (double)std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 0.001 * 0.001;

        loss = criterion(output, label);

        output_argmax = output.exp().argmax(/*dim=*/1, /*keepdim=*/true);
        correct = (label == output_argmax).sum().item<long int>();
        pixel_wise_accuracy = (double)correct / (double)(label.size(0) * label.size(1) * label.size(2));

        class_count = 0;
        mean_accuracy = 0.0;
        for (size_t i = 0; i < std::get<4>(data).size(); i++){
            answer_mask = torch::full({label.size(0), label.size(1), label.size(2)}, /*value=*/(long int)i, torch::TensorOptions().dtype(torch::kLong)).to(device);
            total_class_pixel = (label == answer_mask).sum().item<long int>();
            if (total_class_pixel != 0){
                response_mask = torch::full({label.size(0), label.size(1), label.size(2)}, /*value=*/2, torch::TensorOptions().dtype(torch::kLong)).to(device);
                correct_per_class = (((label == output_argmax).to(torch::kLong) + (label == answer_mask).to(torch::kLong)) == response_mask).sum().item<long int>();
                mean_accuracy += (double)correct_per_class / (double)total_class_pixel;
                class_count++;
            }
        }
        mean_accuracy = mean_accuracy / (double)class_count;

        ave_loss += loss.item<float>();
        ave_pixel_wise_accuracy += pixel_wise_accuracy;
        ave_mean_accuracy += mean_accuracy;
        ave_time += seconds;

        std::cout << '<' << std::get<2>(data).at(0) << "> cross-entropy:" << loss.item<float>() << " pixel-wise-accuracy:" << pixel_wise_accuracy << " mean-accuracy:" << mean_accuracy << std::endl;
        ofs << '<' << std::get<2>(data).at(0) << "> cross-entropy:" << loss.item<float>() << " pixel-wise-accuracy:" << pixel_wise_accuracy << " mean-accuracy:" << mean_accuracy << std::endl;

        fname = result_dir + '/' + std::get<3>(data).at(0);
        visualizer::save_label(output_argmax.detach(), fname, std::get<4>(data), /*cols=*/1, /*padding=*/0);

    }

    // (6) Calculate Average
    ave_loss = ave_loss / (float)dataset.size();
    ave_pixel_wise_accuracy = ave_pixel_wise_accuracy / (double)dataset.size();
    ave_mean_accuracy = ave_mean_accuracy / (double)dataset.size();
    ave_time = ave_time / (double)dataset.size();

    // (7) Average Output
    std::cout << "<All> cross-entropy:" << ave_loss << " pixel-wise-accuracy:" << ave_pixel_wise_accuracy << " mean-accuracy:" << ave_mean_accuracy << " (time:" << ave_time << ')' << std::endl;
    ofs << "<All> cross-entropy:" << ave_loss << " pixel-wise-accuracy:" << ave_pixel_wise_accuracy << " mean-accuracy:" << ave_mean_accuracy << " (time:" << ave_time << ')' << std::endl;

    // Post Processing
    ofs.close();

    // End Processing
    return;

}


// PGASGraph
#include <pgas-graph/graph-utilities.hpp>
#include <pgas-graph/pgas-graph.h>

// CopyPasted
#include <pgas-graph/cppmemusage.hpp>

// STL
#include <chrono>
#include <iostream>
#include <random>

// Boost
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>

// System
#include <limits.h>
#include <unistd.h>

// Usings
namespace po = boost::program_options;

po::options_description createProgramOptions() {
  po::options_description desc("Allowed options");
  desc.add_options()("help", "produce help message")(
      "vertex-count", po::value<int>(),
      "set vertex count")("degree", po::value<int>(), "set degree")(
      "percentage", po::value<double>(), "set connectivity percentage");
  return desc;
}

using MyBundledVertex = PGASGraph::Graph<std::string, size_t>::Vertex;
using MyBundledEdge =
    PGASGraph::Graph<std::string, size_t>::Vertex::weight_node_t;

std::ostream &operator<<(std::ostream &os, const MyBundledVertex &e) noexcept {
  os << e.id;
  return os;
}

std::ostream &operator<<(std::ostream &os, const MyBundledEdge &e) noexcept {
  os << e.first;
  return os;
}

bool operator==(const MyBundledVertex &lhs,
                const MyBundledVertex &rhs) noexcept {
  return lhs.id == rhs.id;
}

bool operator!=(const MyBundledVertex &lhs,
                const MyBundledVertex &rhs) noexcept {
  return !(lhs == rhs);
}

template <typename Graph, typename BundledVertex>
typename boost::graph_traits<Graph>::vertex_descriptor
add_bundled_vertex(const BundledVertex &v, Graph &g) noexcept {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vd = boost::add_vertex(g);
  g[vd] = v;
  return vd;
}

template <typename Graph> void set_my_bundled_vertexes(Graph &g) {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vip = boost::vertices(g);
  const auto j = vip.second;

  PGASGraph::Id id{0};
  for (auto i = vip.first; i != j; ++i) {
    g[*i] = MyBundledVertex(id++);
  }
}

template <typename Graph, typename Function>
void for_each_bundled_vertex(Graph &g, Function f) {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vip = boost::vertices(g);
  const auto j = vip.second;

  PGASGraph::Id id{0};
  for (auto i = vip.first; i != j; ++i) {
    f(g[*i]);
  }
}

using PGASGraphType = PGASGraph::Graph<std::string, size_t>;
size_t generateRandomConnectedPGASGraph(const size_t vertexCount,
                                        double percentage,
                                        PGASGraphType &graph) {
  using PId = PGASGraph::Id;
  std::list<PId> unconnected;
  std::vector<PId> connected;
  connected.reserve(vertexCount);

  connected.push_back(1);
  for (int i = 2; i <= vertexCount; ++i) {
    unconnected.push_back(i);
  }

  size_t extraEdges{static_cast<size_t>((vertexCount * vertexCount / 2) *
                                        (percentage / 100.0))};
  size_t edges{0};

  std::random_device rnd;
  std::mt19937 gen{rnd()};
  std::uniform_int_distribution<int> dist(1, vertexCount);

  size_t weight{1};
  while (!unconnected.empty()) {
    int temp1 = std::uniform_int_distribution<int>(0, connected.size())(gen);
    int temp2 =
        std::uniform_int_distribution<int>(0, unconnected.size() - 1)(gen);

    PGASGraph::Id u{connected[temp1]};
    auto unconnectedItemIt{unconnected.begin()};
    std::advance(unconnectedItemIt, temp2);

    assert(unconnectedItemIt != unconnected.end());
    auto unconnectedItem{*unconnectedItemIt};
    assert(unconnectedItem != -1);
    PId v{unconnectedItem};
    if (u != v) {
      graph.AddEdge({u, v, weight++});
      edges++;
    }

    unconnected.erase(
        std::find(unconnected.begin(), unconnected.end(), unconnectedItem));
    connected.push_back(v);
  }

  while (extraEdges != 0) {
    PId temp1 = std::uniform_int_distribution<int>(1, vertexCount)(gen);
    PId temp2 = std::uniform_int_distribution<int>(1, vertexCount)(gen);
    if (temp1 != temp2) {
      graph.AddEdge({temp1, temp2, weight++});
      edges++;
      extraEdges--;
    }
  }

  return edges;
}

int main(int argc, char *argv[]) {
  using BoostGraph =
      boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                            MyBundledVertex, MyBundledEdge>;
  using SWGen = boost::small_world_iterator<boost::minstd_rand, BoostGraph>;

  // Initialize UPCXX.
  upcxx::init();

  // Parse command line options.
  auto optionsDesc = createProgramOptions();
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, optionsDesc), vm);
  if (vm.count("help")) {
    std::cout << optionsDesc << std::endl;
    return 1;
  }

  // Get vertex count.
  size_t totalNumberVertices = 256;
  if (vm.count("vertex-count")) {
    totalNumberVertices = vm["vertex-count"].as<int>();
  }

  // Get vertex count.
  size_t degree = 10;
  if (vm.count("degree")) {
    degree = vm["degree"].as<int>();
  }

  // Get vertex count.
  double percentage = 5.0;
  if (vm.count("percentage")) {
    percentage = vm["percentage"].as<double>();
  }

  // Distribute vertices over the ranks.
  const size_t verticesPerRank =
      (totalNumberVertices + upcxx::rank_n() - 1) / upcxx::rank_n();

  PGASGraphType pgasGraph(totalNumberVertices, verticesPerRank);

  if (0 == upcxx::rank_me()) {
    // Generate the graph.
    auto start = std::chrono::steady_clock::now();
    auto edgeCount = generateRandomConnectedPGASGraph(totalNumberVertices,
                                                      percentage, pgasGraph);
    // boost::minstd_rand gen;

    // BoostGraph boostGraph{
    //     GraphUtilities::generateRandomConnectedGraph<BoostGraph>(
    //         totalNumberVertices, percentage)};

    // // Assign ids to the vertices.
    // set_my_bundled_vertexes(boostGraph);

    // // Assign weights to the edges.
    // auto edges = boost::edges(boostGraph);
    // int weight{0};
    // for (auto it = edges.first; it != edges.second; ++it) {
    //   boostGraph[*it].first = weight++;
    // }

    // auto vs = boost::vertices(boostGraph);
    // weight = 0;
    // for (auto vit = vs.first; vit != vs.second; ++vit) {
    //   auto neighbors = boost::adjacent_vertices(*vit, boostGraph);
    //   for (auto nit = neighbors.first; nit != neighbors.second; ++nit) {
    //     pgasGraph.AddEdge({boostGraph[*vit].id, boostGraph[*nit].id,
    //     weight++}); if (boostGraph[*vit].id == boostGraph[*nit].id) {
    //       PGASGraph::logMsg("equal");
    //       upcxx::finalize();
    //       return 0;
    //     }
    //   }
    // }

    auto end = std::chrono::steady_clock::now();

    PGASGraph::logMsg("*********");
    PGASGraph::logMsg(
        "Graph generation elapsed time: " +
        std::to_string(std::chrono::duration<double>(end - start).count()));

    PGASGraph::logMsg("Vertex count: " + std::to_string(totalNumberVertices));

    PGASGraph::logMsg("Vertex count per rank: " +
                      std::to_string(verticesPerRank));

    PGASGraph::logMsg("Edge count: " + std::to_string(edgeCount));

    PGASGraph::logMsg("Connectivity percentage: " + std::to_string(percentage));

    PGASGraph::logMsg("UPCXX: Number of Ranks: " +
                      std::to_string(upcxx::rank_n()));
  }

  // Print hostname to make sure that the run is truly distributed.
  char hostname[HOST_NAME_MAX];
  gethostname(hostname, HOST_NAME_MAX);
  upcxx::rpc(0, [hostname]() {
    PGASGraph::logMsg(std::string{"Hostname: "} + hostname);
  }).wait();

  upcxx::barrier();

  auto start = std::chrono::steady_clock::now();
  auto mst = pgasGraph.MST();
  auto end = std::chrono::steady_clock::now();

  upcxx::barrier();
  PGASGraph::logMsg(
      "MST Elapsed time: " +
      std::to_string(std::chrono::duration<double>(end - start).count()));

  if (0 == upcxx::rank_me()) {
    upcxx::rpc(
        0,
        [](decltype(mst) &localMst) {
          size_t cost{0};
          auto begin = localMst->begin();
          for (; begin != localMst->end(); ++begin) {
            cost += begin->first;
          }
          PGASGraph::logMsg("MST Cost: " + std::to_string(cost));
        },
        mst)
        .wait();

    size_t peakSize = getPeakRSS();
    PGASGraph::logMsg("Peak Mem Usage: " + std::to_string(peakSize / 1000000) +
                      "MB");
  }
  PGASGraph::logMsg("*********");

  //  upcxx::barrier();
  //  return 0;

  upcxx::finalize();
  return 0;
};


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "BlitzArraySliceIterator.tcc"
#include "Randomized.h"

#include <boost/progress.hpp>

#include <list>

using namespace randomized;
using namespace std;

typedef IdxTiny<10> Size    ;
typedef IdxTiny< 6> DummyIdx;
typedef cppqedutils::MultiIndexIterator<6> MII6;
typedef tmptools::Vector<6,2,5,7> SliceVec;
typedef blitzplusplus::basi::Indexer<10,SliceVec> Indexer;
typedef blitzplusplus::basi::Iterator<10,SliceVec,true> BASI;
typedef blitzplusplus::SlicesData<10,SliceVec> SlicesData;
typedef blitzplusplus::basi_fast::Iterator<10,SliceVec,true> BASI_FAST;

const Size size(3,4,2,5,3,5,6,4,2,5);

const SliceVec sliceVec=SliceVec();

const size_t nRepeat=
#ifndef NDEBUG
  1
#else
  10000
#endif
  ;

int main()
{
  CArray<10> array(size);
  CArray< 4> arrayRes/*(6,2,5,4)*/;

  fillWithRandom(array);

  { // 1

  list<DummyIdx> dummyIdcs;

  const MII6
    begin(DummyIdx(0,0,0,0,0,0),DummyIdx(2,3,4,2,1,4),boost::mpl::false_()),
    end  (DummyIdx(0,0,0,0,0,0),DummyIdx(2,3,4,2,1,4),boost::mpl:: true_());

  copy(begin,end,back_inserter(dummyIdcs));

  // cout<<*begin<<endl<<*end<<endl;

  dcomp res;

  {
    boost::progress_timer t;
    for (size_t count=0; count<nRepeat; ++count) {
      for (MII6 i=begin; i!=end; ++i)
        res=Indexer::index(array,arrayRes,*i)(1,2,4,3);
    }
  }

  } // 1

  { // 2

  dcomp res;

  const BASI
    begin(array,boost::mpl::false_()),
    end  (array,boost::mpl:: true_());

  {
    boost::progress_timer t;
    for (size_t count=0; count<nRepeat; ++count) {
      for (BASI i=begin; i!=end; ++i)
        res=(*i)(2,1,4,3);
    }
  }

  } // 2

  { // 3

  const SlicesData sd(array);

  const BASI_FAST
    begin(array,sd,boost::mpl::false_()),
    end  (array,sd,boost::mpl:: true_());

  {
    dcomp res;

    boost::progress_timer t;
    for (size_t count=0; count<nRepeat; ++count) {
      for (BASI_FAST i=begin; i!=end; ++i)
        res=(*i)(2,1,4,3);
    }
  }

  } // 3

}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(
        group(list(int_<1>, long_<1>, type<int>, char_<'x'>, char_<'x'>))
        ==
        list(
            list(int_<1>, long_<1>),
            list(type<int>),
            list(char_<'x'>, char_<'x'>)
        )
    );
    //! [main]
}


#include <boost/text/string.hpp>
#include <iostream>

void usage_error()
{
    std::cerr << "code_point_to_utf8: error: code_point_to_utf8 takes\n"
                 "    exactly one parameter, a hexadecimal code point.\n";
    exit(1);
}

int main(int argc, char * argv[])
{
    if (argc != 2)
        usage_error();

    auto input = std::string(argv[1]);
    if (2 < input.size()) {
        if (input[0] == '0' && input[1] == 'x')
            input.erase(input.begin(), input.begin() + 2);
    }

    uint32_t code_point[1] = {0};
    for (auto c : input) {
        uint32_t value = 0;
        if ('a' <= c && c <= 'f')
            value = 10 + c - 'a';
        else if ('A' <= c && c <= 'F')
            value = 10 + c - 'A';
        else if ('0' <= c && c <= '9')
            value = c - '0';
        else
            usage_error();
        code_point[0] *= 16;
        code_point[0] += value;
    }

    boost::text::utf8::from_utf32_iterator<uint32_t const *> const cp_first(
        code_point, code_point, code_point + 1);
    boost::text::utf8::from_utf32_iterator<uint32_t const *> const cp_last(
        code_point, code_point + 1, code_point + 1);
    unsigned char code_units[4];
    auto code_units_end = std::copy(cp_first, cp_last, code_units);

    for (unsigned char * it = code_units; it != code_units_end; ++it) {
        std::cout << "0x" << std::hex << int(*it) << " ";
    }
    std::cout << "\n";

#if 0
    boost::text::utf8::to_utf32_iterator<unsigned char const *> const cu_first(
        code_units);
    boost::text::utf8::to_utf32_iterator<unsigned char const *> const cu_last(
        code_units_end);
    std::copy(cu_first, cu_last, code_point);
    std::cout << "Round-trips back to: 0x" << std::hex << code_point[0] << "\n";
#endif
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if neither x nor y is @ref Nan.

    @par Header <boost/simd/function/is_ord.hpp>

    @par Note

     Using `is_ord(x,y)` is similar to: `(x == x) && (y == y)`

    @par Example:

      @snippet is_ord.cpp is_ord

    @par Possible output:

      @snippet is_ord.txt is_ord

  **/
  as_logical_t<Value> is_ord(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/is_ord.hpp>
#include <boost/simd/function/simd/is_ord.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SIMD_COMMON_ELLIPKE_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SIMD_COMMON_ELLIPKE_HPP_INCLUDED
#include <nt2/toolbox/elliptic/functions/ellipke.hpp>
#include <nt2/sdk/simd/logical.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#include <nt2/include/functions/simd/ldexp.hpp>
#include <nt2/include/functions/simd/sqrt.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/average.hpp>
#include <nt2/include/functions/simd/oneminus.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/maximum.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::ellipke_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      )
  {
    typedef typename meta::as_floating<A0>::type         etype;
    typedef boost::fusion::tuple<etype, etype> result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<etype>::type setype;
      return ellipke(tofloat(a0), Eps<setype>());
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  //Implementation when type A0 is arithmetic_
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::ellipke_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<arithmetic_<A0>,X>))
                             ((scalar_<floating_<A1> >))
                             )
  {
    typedef typename meta::as_floating<A0>::type         etype;
    typedef boost::fusion::tuple<etype, etype> result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return ellipke(tofloat(a0), a1);
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is arithmetic_
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::ellipke_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<floating_<A0>,X>))
                             ((scalar_<floating_<A1> >))
                             )
  {
    typedef typename meta::strip<A0>::type              etype;
    typedef boost::fusion::tuple<etype, etype>    result_type;
    NT2_FUNCTOR_CALL(2)
      {
        result_type res;
        nt2::ellipke(a0, a1, boost::fusion::at_c<0>(res), boost::fusion::at_c<1>(res));
        return res;
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementations 1 input
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(  nt2::tag::ellipke_, tag::cpu_,
                               (A0)(A1)(X),
                               ((simd_<arithmetic_<A0>,X >))
                               ((simd_<floating_<A1>,X>))
                               ((simd_<floating_<A1>,X>))
                               )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
      {
        typedef typename meta::scalar_of<A1>::type sA1;
        nt2::ellipke(tofloat(a0), Eps<sA1>(), a1, a2);
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementations 2 inputs
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION_IF(nt2::tag::ellipke_, tag::cpu_,
                                (A0)(A1)(A2)(X),
                                (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                        , boost::simd::meta::cardinal_of<A2>
                                                        >
                                ),
                                ((simd_<arithmetic_<A0>,X >))
                                ((scalar_<floating_<A1> >))
                                ((simd_<floating_<A2>,X >))
                                ((simd_<floating_<A2>,X >))
                             )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0,
                                  A1 const& a1, A0 & a2,A0 & a3) const
      {
        nt2::ellipke(tofloat(a0),a1,a2,a3);
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementations 2 inputs
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(  nt2::tag::ellipke_, tag::cpu_,
                                (A0)(A1)(X),
                                ((simd_<floating_<A0>,X >))
                                ((scalar_<floating_<A1> >))
                                ((simd_<floating_<A0>,X >))
                                ((simd_<floating_<A0>,X >))
                                )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0,
                                  A1 const& a1, A0 & a2,A0 & a3) const
      {
        typedef typename meta::as_logical<A0>::type bA0;
        typedef typename meta::as_integer<A0>::type iA0;
        typedef typename meta::scalar_of<A0>::type sA0;
        bA0 isnan =  logical_or(is_ltz(a0), gt(a0, One<A0>()));
        A0 m = if_zero_else(isnan, a0);
        A0 aa0 = One<A0>();
        A0 bb0 = sqrt(oneminus(m));
        A0 s0 = m;
        int32_t i1 = 0;
        sA0 mm = One<sA0>();
        A0 aa1 = Zero<A0>();
        while (gt(mm, a1))
          {
            aa1 = average(aa0, bb0);
            A0 bb1 = sqrt(aa0*bb0);
            A0 cc1 = average(aa0, -bb0);
            ++i1;
            A0 w1 = ldexp(sqr(cc1), splat<iA0>(i1));
            mm =  maximum(w1);
            s0 += w1;
            aa0 = aa1;
            bb0 = bb1;
          };
        bA0 iseqm1 = eq(m, One<A0>());
        a2 = if_nan_else(isnan, sel(iseqm1,Inf<A0>(), nt2::Pio_2<A0>()/aa1));
        a3 = if_nan_else(isnan, sel(iseqm1,One<A0>(), a2*(One<A0>()-s0*Half<A0>())));
      }
  };

} }
#endif


#include <iostream>
#include "ros/ros.h"
#include <cv_bridge/cv_bridge.h>
#include <boost/thread/thread.hpp>

//PCL
#include <pcl/conversions.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/io/pcd_io.h>
#include <pcl/range_image/range_image.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/features/range_image_border_extractor.h>

#include <pcl/features/narf_descriptor.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>
#include "pcl_ros/point_cloud.h"
#include "pcl_conversions/pcl_conversions.h"

#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp.h>

//msgs
#include "geometry_msgs/Point.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/point_cloud_conversion.h"
#include "sensor_msgs/PointField.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"
#include <bits/stdc++.h>

// OpenCV headers
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

class NARFFeatures
{
    public:
		pcl::PointCloud<pcl::Narf36>::Ptr previous_Narf_features;
		pcl::PointCloud<pcl::PointXYZ>::Ptr previous_keypoint_cloud ;
		pcl::PointCloud<pcl::PointXYZ>::Ptr previous_cloud;
		pcl::PointCloud<pcl::PointXYZ>::Ptr result;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_rgb;
		Eigen::Matrix4f globaltransform;

        NARFFeatures()
        {
            //Assign subscriber
            this->subscriber = this->nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/points",5,&NARFFeatures::processPointCloud,this);
			this->publisher = this->nh.advertise<pcl::PointCloud<pcl::PointXYZRGB>> ("/map", 1);

			previous_Narf_features = pcl::PointCloud<pcl::Narf36>::Ptr (new pcl::PointCloud<pcl::Narf36>);
			previous_keypoint_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
			previous_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
			result = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
			result_rgb = pcl::PointCloud<pcl::PointXYZRGB>::Ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
			globaltransform = Eigen::Matrix4f::Identity();

        }

        void processPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){

		double start = std::clock();
		//Image
		sensor_msgs::Image image_;

        //Converting ROS point cloud to Image
		pcl::toROSMsg (*cloud_msg,image_);
		//sensor_msgs::ImageConstPtr img_msg = &image_;

		cv::Mat img,filtered_img,keypoints_mat, sharp_mat, contrast_mat, combi_mat;
		cv_bridge::CvImagePtr cvPtr;
		cvPtr = cv_bridge::toCvCopy(image_, sensor_msgs::image_encodings::BGR8);
		cvPtr->image.copyTo(img);

		cv::bilateralFilter(img,filtered_img,15,50,80,cv::BORDER_DEFAULT);
		std::vector<cv::KeyPoint> keypoints;
		cv::Ptr<cv::ORB> detector = cv::ORB::create(250);
		detector->detect(filtered_img, keypoints);
		cv::KeyPoint kk = keypoints[0];
		std::cout << "Resulting key points are of size: " << keypoints.size() <<std::endl;
		//std::cout << "First Point: " << kk.pt <<std::endl;
		cv::drawKeypoints(filtered_img, keypoints,keypoints_mat, cv::Scalar::all(-1), cv::DrawMatchesFlags::DEFAULT);
		cv::imshow("Keypoints", keypoints_mat);
		cv::waitKey(2);

		// creating point clouds......
		pcl::PCLPointCloud2 pcl_pc2;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_RGB (new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl_conversions::toPCL(*cloud_msg, pcl_pc2);
		pcl::fromPCLPointCloud2(pcl_pc2, *cloud_RGB);
		pcl::fromPCLPointCloud2(pcl_pc2, *cloud);

		std::vector<int> keypoint_indices2;
		keypoint_indices2.resize (keypoints.size ());
		pcl::PointCloud<pcl::PointXYZ>::Ptr keypoint_cloud (new pcl::PointCloud<pcl::PointXYZ>);

		pcl::PointXYZ point;
		for(int i =0; i< keypoints.size();i++){
			double xc =keypoints[i].pt.x;
			double yc =keypoints[i].pt.y;

			if(isnan(cloud->at(xc,yc).x)){
				continue;
			}else{

				point.x = cloud->at(xc,yc).x;
				point.y = cloud->at(xc,yc).y;
				point.z = cloud->at(xc,yc).z;
				keypoint_cloud->push_back(point);
				keypoint_indices2[i] = (((int)yc-1)*640)+(int)xc;
			}

		}

        // Keypoints captured//
        // --------------------------------------------------------------------------------------------//
        // Starting feature extraction

        // Eigen::Affine3f scene_sensor_pose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);

		// Parameters for narf computation
		 float support_size = 0.1f;
		// float maxAngleWidth = (float) (57.0f * (M_PI / 180.0f));
		// float maxAngleHeight = (float) (43.0f * (M_PI / 180.0f));
		// float angularResolution = (float)(57.0f / 640.0f * (M_PI/180.0f));

		Eigen::Affine3f sensorPose = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],
								 cloud->sensor_origin_[1],
								 cloud->sensor_origin_[2])) *
								 Eigen::Affine3f(cloud->sensor_orientation_);
		pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
		bool setUnseenToMaxRange = false;

		int imageSizeX = 640;
		int imageSizeY = 480;
		// Center of projection. here, we choose the middle of the image.
		float centerX = 640.0f / 2.0f;
		float centerY = 480.0f / 2.0f;
		// Focal length. The value seen here has been taken from the original depth images.
		// It is safe to use the same value vertically and horizontally.
		float focalLengthX = 525.0f, focalLengthY = focalLengthX;

        // -----------------------------------------------
		// -----Create RangeImage from the PointCloud-----
		// -----------------------------------------------
		float noise_level = 0.00;
		float min_range = 0;
		int border_size = 0;

		pcl::RangeImagePlanar rangeImagePlanar;
		rangeImagePlanar.createFromPointCloudWithFixedSize(*cloud, imageSizeX, imageSizeY,
		centerX, centerY, focalLengthX, focalLengthX,
		sensorPose, pcl::RangeImage::CAMERA_FRAME,
		noise_level, min_range);
		//range_image.integrateFarRanges (far_ranges);
		std::cout << "Range image size :" << rangeImagePlanar.height <<" x "<< rangeImagePlanar.width <<std::endl;
		rangeImagePlanar.setUnseenToMaxRange();

		//Show range image ///
		// pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
		// range_image_widget.showRangeImage (rangeImagePlanar);

        // calculating features....

        // ------------------------------------------------------
        // -----Extract NARF descriptors for interest points-----
        // ------------------------------------------------------
        bool rotation_invariant = true;
        pcl::NarfDescriptor narf_descriptor (&rangeImagePlanar, &keypoint_indices2);
        narf_descriptor.getParameters ().support_size = support_size;
        narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;
        pcl::PointCloud<pcl::Narf36>::Ptr narf_descriptors(new pcl::PointCloud<pcl::Narf36>);
        narf_descriptor.compute (*narf_descriptors);
        std::cout << "Extracted "<<narf_descriptors->size ()<<" descriptors for "<<keypoint_indices2.size ()<< " keypoints.\n";

		pcl::PointXYZ pointNarf;
		pcl::PointXYZ pointKey;

		pcl::PointCloud<pcl::PointXYZ>::Ptr keypoint_cloud_1to1 (new pcl::PointCloud<pcl::PointXYZ>);

		for (int j = 0; j<narf_descriptors->size(); j++){

				pointNarf.x = narf_descriptors->at(j).x;
				pointNarf.y = narf_descriptors->at(j).y;
				pointNarf.z = narf_descriptors->at(j).z;
				keypoint_cloud_1to1->push_back(pointNarf);

			}

		// Visualization of keypoints along with the original cloud
		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		double end = std::clock();
		double time_taken = double(end-start) / double(CLOCKS_PER_SEC);
		std::cout << "Time taken :" << time_taken <<std::endl;

		std::cout <<"filtered keypoints :" <<keypoint_cloud_1to1->size() << std::endl;
		std::cout <<"filtered features :" <<narf_descriptors->size() << std::endl;

		std::cout << previous_Narf_features->size() << std::endl;

		if(previous_Narf_features->size()> 0 && previous_keypoint_cloud->size()>0){

			std::cout << "hola" << std::endl;
           	double start_r = std::clock();
			// 1. initial alignment of point clouds
			Eigen::Matrix4f initTransform = Eigen::Matrix4f::Identity();
			pcl::SampleConsensusInitialAlignment<pcl::PointXYZ,pcl::PointXYZ,pcl::Narf36> initSAC;
			pcl::CorrespondencesPtr correspondences_filtered(new pcl::Correspondences());
			initSAC.setMinSampleDistance(0.01);
			initSAC.setMaxCorrespondenceDistance(0.5);
			initSAC.setMaximumIterations(1500);

			initSAC.setInputCloud(keypoint_cloud_1to1);
			initSAC.setSourceFeatures(narf_descriptors);

			initSAC.setInputTarget(previous_keypoint_cloud);
			initSAC.setTargetFeatures(previous_Narf_features);

			pcl::PointCloud<pcl::PointXYZ>::Ptr initTransCloud (new pcl::PointCloud<pcl::PointXYZ>);
			initSAC.align(*initTransCloud);
			initTransform = initSAC.getFinalTransformation();

			double end_r = std::clock();
			double time_r = double(end_r-start_r) / double(CLOCKS_PER_SEC);
			std::cout << "done init :" <<time_r<<" seconds"<<std::endl;
			// 2. Fine alignment using ICP
			Eigen::Matrix4f finalTransform = Eigen::Matrix4f::Identity();
			pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
			icp.setMaxCorrespondenceDistance(0.05);
			icp.setRANSACOutlierRejectionThreshold(0.2);
			icp.setTransformationEpsilon(0.01);
			icp.setMaximumIterations(10000);
			icp.setInputCloud(initTransCloud);
			icp.setInputTarget(previous_keypoint_cloud);

			pcl::PointCloud<pcl::PointXYZ>::Ptr finalCloud (new pcl::PointCloud<pcl::PointXYZ>);
			icp.align(*finalCloud);
			finalTransform = icp.getFinalTransformation() * initTransform;

			double end2_r = std::clock();
			double time2_r = double(end2_r-end_r) / double(CLOCKS_PER_SEC);
			std::cout << "done ICP :" <<time2_r<<" seconds"<<std::endl;

			double time2_reg = double(end2_r-start_r) / double(CLOCKS_PER_SEC);
			std::cout << "done registration :" <<time2_reg<<" seconds"<<std::endl;
			//global transform --
			globaltransform = globaltransform * finalTransform;
			std::cout << "Global Transform:" << std::endl<< globaltransform << std::endl;

			pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_source(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_source_RGB(new pcl::PointCloud<pcl::PointXYZRGB>);

			pcl::transformPointCloud(*cloud, *transformed_source, globaltransform);
			pcl::transformPointCloud(*cloud_RGB,*transformed_source_RGB,globaltransform);

			// viewer.setBackgroundColor(0, 0, 0);
			// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_source_cloud(transformed_source, 150, 80, 80);
			// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_source_keypoints(keypoint_cloud, 255, 0, 0);

			// viewer.addPointCloud<pcl::PointXYZ>(transformed_source, handler_source_cloud, "source_cloud");
			// viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source_cloud");
			// viewer.addPointCloud<pcl::PointXYZ>(keypoint_cloud, handler_source_keypoints, "source_keypoints");
			// viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "source_keypoints");
			// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_target_cloud(result, 80, 150, 80);
			// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_target_keypoints(previous_keypoint_cloud, 0, 255, 0);

			// viewer.addPointCloud<pcl::PointXYZ>(result, handler_target_cloud, "target_cloud");
			// viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target_cloud");
			// viewer.addPointCloud<pcl::PointXYZ>(previous_keypoint_cloud, handler_target_keypoints, "target_keypoints");
			// viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "target_keypoints");
			// viewer.addCorrespondences<pcl::PointXYZ>(previous_keypoint_cloud,keypoint_cloud, *correspondences_filtered, "correspondences");

			previous_Narf_features->clear();
			previous_keypoint_cloud->clear();
			pcl::copyPointCloud(*narf_descriptors,*previous_Narf_features);
			pcl::copyPointCloud(*keypoint_cloud_1to1,*previous_keypoint_cloud);
			result->operator+=(*transformed_source);
			result_rgb->operator+=(*transformed_source_RGB);
			std::cout<<"cloud size :"<<cloud->width<<"x"<<cloud->height<<std::endl;
			std::cout<<"Concatanated point cloud size :"<<result->width<<"x"<<result->height<<std::endl;

			publisher.publish(result_rgb);

		}
		else{
			std::cout << "first_time" << std::endl;
			pcl::copyPointCloud(*narf_descriptors,*previous_Narf_features);
			pcl::copyPointCloud(*keypoint_cloud_1to1,*previous_keypoint_cloud);
			pcl::copyPointCloud(*cloud,*result);
			pcl::copyPointCloud(*cloud_RGB,*result_rgb);
		}

		// while(!viewer.wasStopped())
		// {
		// 	viewer.spinOnce();
		// }

		// while (!ICPView.wasStopped())
		// {

		// 	ICPView.spinOnce();
		// }

		std::cout<<"***********************************************************"<<std::endl;

        }

    void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
	{
		Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
		Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
		Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
		viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
				    look_at_vector[0], look_at_vector[1], look_at_vector[2],
				    up_vector[0], up_vector[1], up_vector[2]);
	}

    private:
        ros::NodeHandle nh;
        ros::Subscriber subscriber;
        ros::Publisher publisher;
};

int main(int argc, char **argv)
{
    //initialize the node
    ros::init(argc,argv,"observe_PointCloud");
    std::cout << "Process_PointCloud under observation" << std::endl;

    // handle ROS communication events
    NARFFeatures PC_obj;
    //ros::spin();
    ros::Rate r(0.05);
    while (ros::ok())
    {
    	ros::spinOnce();
    	r.sleep();
     }

    return 0;
}


/*
   Copyright [2020] [IBM Corporation]

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

   The GNU C Library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   You should have received a copy of the GNU Lesser General Public
   License along with the GNU C Library; if not, see
   <http://www.gnu.org/licenses/>.

   As a special exception, if you link the code in this file with
   files compiled with a GNU compiler to produce an executable,
   that does not cause the resulting executable to be covered by
   the GNU Lesser General Public License.  This exception does not
   however invalidate any other reasons why the executable file
   might be covered by the GNU Lesser General Public License.
   This exception applies to code released by its copyright holders
   in files containing the exception.
*/

/*
  Author(s):
  Copyright (C) 2020, Daniel G. Waddington <daniel.waddington@ibm.com>
*/

#include <common/cycles.h>
#include <common/time.h>
#include <boost/numeric/conversion/cast.hpp>

using namespace std::chrono;

const common::Timespec Epoch_nil = common::Timespec();

namespace common
{

/**
 * Subtract timespec X from Y
 *
 * @param x
 * @param y
 *
 * @return Result
 */
struct timespec timespec_subtract(const struct timespec& x,
				  const struct timespec& y)
{
  struct timespec result;
  if (y.tv_nsec < x.tv_nsec) {
    result.tv_sec  = y.tv_sec - x.tv_sec - 1;
    result.tv_nsec =
      (1000000000L - x.tv_nsec) + y.tv_nsec;
  }
  else {
    result.tv_sec  = y.tv_sec - x.tv_sec;
    result.tv_nsec = y.tv_nsec - x.tv_nsec;
  }
  return result;
}

/**
 * Add timespec X and Y
 *
 * @param time
 * @param add
 *
 * @return
 */
struct timespec timespec_add(const struct timespec& x,
			     const struct timespec& y)
{
  struct timespec result = x;

  result.tv_sec += y.tv_sec;
  result.tv_nsec += y.tv_nsec;

  while ( result.tv_nsec >= 1000000000L ) {
    result.tv_nsec -= 1000000000L;
    result.tv_sec++;
  }

  return result;
}

static common::epoch_time_t Tsc_initialize();

/**
 * Static declaration
 *
 */
namespace tsc_static
{
static uint64_t             _ticks_per_second;
static double               _ticks_per_nanosecond;
static common::epoch_time_t _epoch_at_power_on = Tsc_initialize();
}

Tsc::Tsc() : _tsc(rdtsc()) {
}

Tsc::Tsc(const common::epoch_time_t& epoch) : _tsc(0) {

  if(epoch.tv_sec == 0 && epoch.tv_nsec == 0) {
    _tsc = 0;
    return;
  }

  assert(tsc_static::_epoch_at_power_on.tv_sec > 0);
  assert(tsc_static::_epoch_at_power_on <= epoch);

  auto delta = timespec_subtract(tsc_static::_epoch_at_power_on, epoch);

  _tsc = (boost::numeric_cast<uint64_t>(delta.tv_sec) * tsc_static::_ticks_per_second) +
    boost::numeric_cast<uint64_t>(boost::numeric_cast<double>(delta.tv_nsec) *
				  tsc_static::_ticks_per_nanosecond);
}

unsigned long Tsc::raw() const {
  return _tsc;
}

void Tsc::update() {
  _tsc = rdtsc();
}

static common::epoch_time_t Tsc_initialize()
{
  /* establish time datum */
  float freq_MHZ = common::get_rdtsc_frequency_mhz();

  tsc_static::_ticks_per_second = static_cast<uint64_t>(freq_MHZ * 1000000.0f);
  tsc_static::_ticks_per_nanosecond = static_cast<double>(freq_MHZ) / 1000.0f;

  auto tscA = rdtsc();
  struct timespec ts;
  if(clock_gettime(CLOCK_REALTIME, &ts) != 0)
    throw General_exception("clock_gettime failed");

  /* Note: an arbitrary period of time may elapse between time() and rdtsc().
   * Therefore users should not rely on epoch_now() in common/utils for a current
   * time consistent with timestamps used in mapstore. Rather, they could write
   * a kv pair and then retrieve that KV pair's timestamp.
   */
  auto tscB = rdtsc();
  auto nano_seconds_since_power_on = static_cast<double>((tscA + tscB) / 2) / tsc_static::_ticks_per_nanosecond;
  common::epoch_time_t tsc_ts(boost::numeric_cast<time_t>(nano_seconds_since_power_on / 1000000000.0),
			      boost::numeric_cast<long>(remainder(nano_seconds_since_power_on, 1000000000.0)));
  tsc_static::_epoch_at_power_on = timespec_subtract(tsc_ts, ts);
  assert(tsc_static::_epoch_at_power_on.seconds() > 0);

  return tsc_static::_epoch_at_power_on;
}

common::epoch_time_t Tsc::to_epoch() const {
  auto nano_seconds_since_power_on =
    static_cast<double>(_tsc) / tsc_static::_ticks_per_nanosecond;

  common::epoch_time_t tsc_ts
    (boost::numeric_cast<time_t>(nano_seconds_since_power_on / 1000000000.0),
     boost::numeric_cast<long>(remainder(nano_seconds_since_power_on, 1000000000.0)));

  assert(tsc_ts.tv_nsec < 1000000000L);

  return timespec_add(tsc_static::_epoch_at_power_on, tsc_ts);
}

constexpr nanoseconds timespec_to_duration(timespec ts)
{
    auto duration = seconds{ts.tv_sec} + nanoseconds{ts.tv_nsec};
    return duration_cast<nanoseconds>(duration);
}

/* Timepoint class */

Timepoint::Timepoint()
  : time_point_t(std::chrono::system_clock::now())
{
}

Timepoint::Timepoint(const common::epoch_time_t& epoch)
  : time_point_t(time_point<system_clock, nanoseconds>
   		 {duration_cast<system_clock::duration>(timespec_to_duration(epoch))})
{
}

unsigned long Timepoint::raw() const
{
  system_clock::duration dtn = this->time_since_epoch();
  return boost::numeric_cast<unsigned long>(dtn.count());
}

void Timepoint::update()
{
  time_point_t& tp = *this;
  tp = std::chrono::system_clock::now();
}

common::epoch_time_t Timepoint::to_epoch() const
{
  auto& tp = *this;
  auto secs = time_point_cast<seconds>(tp);
  auto ns = time_point_cast<nanoseconds>(tp) -
    time_point_cast<nanoseconds>(secs);

  return timespec{secs.time_since_epoch().count(), ns.count()};
}

/* Timespec class */
Timepoint Timespec::to_timepoint() const {
  Timepoint tp(*this);
  return tp;
}

} // common